Fix configurations with non-joints via joint tag -> gpio

ros-controls · Dec 28, 2023 · 8ce02e6 · 8ce02e6
1 parent 391c9e3
commit 8ce02e6
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 25 deletions.
diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
@@ -142,12 +142,12 @@ class TestGenericSystem : public ::testing::Test
         <param name="initial_value">0.2</param>
       </state_interface>
     </joint>
-    <joint name="voltage_output">
+    <gpio name="voltage_output">
       <command_interface name="voltage"/>
       <state_interface name="voltage">
         <param name="initial_value">0.5</param>
       </state_interface>
-    </joint>
+    </gpio>
   </ros2_control>
 )";
 

diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp
@@ -32,22 +32,19 @@ class TestSystem : public SystemInterface
     std::vector<StateInterface> state_interfaces;
     for (auto i = 0u; i < info_.joints.size(); ++i)
     {
-      if (info_.joints[i].name != "configuration")
-      {
-        state_interfaces.emplace_back(hardware_interface::StateInterface(
-          info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i]));
-        state_interfaces.emplace_back(hardware_interface::StateInterface(
-          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i]));
-        state_interfaces.emplace_back(hardware_interface::StateInterface(
-          info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i]));
-      }
+      state_interfaces.emplace_back(hardware_interface::StateInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i]));
+      state_interfaces.emplace_back(hardware_interface::StateInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i]));
+      state_interfaces.emplace_back(hardware_interface::StateInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i]));
     }
 
-    if (info_.joints.size() > 2)
+    if (info_.gpios.size() > 0)
     {
       // Add configuration/max_tcp_jerk interface
       state_interfaces.emplace_back(hardware_interface::StateInterface(
-        info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_));
+        info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_));
     }
 
     return state_interfaces;
@@ -58,22 +55,19 @@ class TestSystem : public SystemInterface
     std::vector<CommandInterface> command_interfaces;
     for (auto i = 0u; i < info_.joints.size(); ++i)
     {
-      if (info_.joints[i].name != "configuration")
-      {
-        command_interfaces.emplace_back(hardware_interface::CommandInterface(
-          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i]));
-      }
+      command_interfaces.emplace_back(hardware_interface::CommandInterface(
+        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i]));
     }
     // Add max_acceleration command interface
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
       info_.joints[0].name, info_.joints[0].command_interfaces[1].name,
       &max_acceleration_command_));
 
-    if (info_.joints.size() > 2)
+    if (info_.gpios.size() > 0)
     {
       // Add configuration/max_tcp_jerk interface
       command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_));
+        info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_));
     }
 
     return command_interfaces;

diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
@@ -113,6 +113,34 @@ const auto urdf_head =
       </geometry>
     </collision>
   </link>
+  <joint name="joint3" type="revolute">
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
+    <parent link="link2"/>
+    <child link="link3"/>
+    <limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
+  </joint>
+  <link name="link3">
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="1" radius="0.1"/>
+      </geometry>
+      <material name="DarkGrey">
+        <color rgba="0.4 0.4 0.4 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="1" radius="0.1"/>
+      </geometry>
+    </collision>
+  </link>
   <joint name="tool_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 1"/>
     <parent link="link2"/>
@@ -169,10 +197,10 @@ const auto hardware_resources =
       <state_interface name="velocity"/>
       <state_interface name="acceleration"/>
     </joint>
-    <joint name="configuration">
+    <gpio name="configuration">
       <command_interface name="max_tcp_jerk"/>
       <state_interface name="max_tcp_jerk"/>
-    </joint>
+    </gpio>
   </ros2_control>
 )";
 
@@ -807,10 +835,10 @@ const auto minimal_robot_missing_command_keys_urdf =
 [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system";
 [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system";
 [[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = {
-  "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"};
+  "joint2/velocity", "joint2/max_acceleration", "joint3/velocity"};
 [[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_STATE_INTERFACES = {
-  "joint2/position", "joint2/velocity",     "joint2/acceleration",       "joint3/position",
-  "joint3/velocity", "joint3/acceleration", "configuration/max_tcp_jerk"};
+  "joint2/position", "joint2/velocity", "joint2/acceleration",
+  "joint3/position", "joint3/velocity", "joint3/acceleration"};
 
 }  // namespace ros2_control_test_assets