Add two_odrive_joints robot #2

Author: Roelemans

march_hardware_builder/include/march_hardware_builder/allowed_robot.h

@@ -18,11 +18,13 @@ class AllowedRobot
     odrive_test_joint_rotational,
     test_joint_linear,
     pdb,
+    two_odrive_joints,
   };
 
   AllowedRobot() = default;
   explicit AllowedRobot(const std::string& robot_name)
   {
+      ROS_WARN("Used robot %s", robot_name);
     if (robot_name == "march4")
     {
       this->value = march4;
@@ -39,6 +41,10 @@ class AllowedRobot
     {
       this->value = odrive_test_joint_rotational;
     }
+    else if (robot_name == "two_odrive_joints")
+    {
+        this->value = two_odrive_joints;
+    }
     else if (robot_name == "test_joint_linear")
     {
       this->value = test_joint_linear;
@@ -69,6 +75,10 @@ class AllowedRobot
     {
       return base_path.append("/robots/odrive_test_joint_rotational.yaml");
     }
+    else if (this->value == AllowedRobot::two_odrive_joints)
+    {
+        return base_path.append("/robots/two_odrive_joints.yaml");
+    }
     else if (this->value == AllowedRobot::odrive_test_joint_rotational)
     {
       return base_path.append("/robots/odrive_test_joint_rotational.yaml");

march_hardware_interface/src/march_hardware_interface.cpp

@@ -277,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat
       }
       else if (joint.getActuationMode() == march::ActuationMode::torque)
       {
-        joint.actuateTorque(joint_effort_command_[i]));
+        joint.actuateTorque(joint_effort_command_[i]);
       }
     }
   }