Fix the issues where hardware with multiple interfaces can not be sta…

…rted because of a logical bug added when adding dynamics calculation functionality.
ros-controls · Nov 6, 2023 · 2410274 · 2410274
1 parent 863f161
commit 2410274
Show file tree

Hide file tree

Showing 2 changed files with 121 additions and 0 deletions.
diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
@@ -388,6 +388,11 @@ return_type GenericSystem::prepare_command_mode_switch(
 {
   hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
 
+  if (!calculate_dynamics_)
+  {
+    return ret_val;
+  }
+
   const size_t FOUND_ONCE_FLAG = 1000000;
 
   std::vector<size_t> joint_found_in_x_requests_;

diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp
@@ -518,6 +518,46 @@ class TestGenericSystem : public ::testing::Test
   </ros2_control>
 )";
 
+    valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ =
+      R"(
+  <ros2_control name="HardwareSystem2dofStandardInterfacesWithDifferentControlModes" type="system">
+    <hardware>
+      <plugin>mock_components/GenericSystem</plugin>
+      <param name="calculate_dynamics">true</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">3.45</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <joint name="joint2">
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <param name="initial_value">2.78</param>
+      </state_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <joint name="joint3">
+      <command_interface name="acceleration"/>
+      <state_interface name="position">
+        <param name="initial_value">2.78</param>
+      </state_interface>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <gpio name="flange_vacuum">
+      <command_interface name="vacuum"/>
+      <state_interface name="vacuum" data_type="double"/>
+    </gpio>
+  </ros2_control>
+)";
+
     disabled_commands_ =
       R"(
   <ros2_control name="GenericSystem2dof" type="system">
@@ -556,6 +596,7 @@ class TestGenericSystem : public ::testing::Test
   std::string sensor_with_initial_value_;
   std::string gpio_with_initial_value_;
   std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_;
+  std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_;
   std::string disabled_commands_;
 };
 
@@ -1901,3 +1942,78 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active)
   ASSERT_EQ(0.0, j1v_s.get_value());
   ASSERT_EQ(0.11, j1p_c.get_value());
 }
+
+TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags)
+{
+  auto check_prepare_command_mode_switch_works = [](const std::string & urdf)
+  {
+    TestableResourceManager rm(urdf);
+    auto start_interfaces = rm.command_interface_keys();
+    std::vector<std::string> stop_interfaces;
+    return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces);
+  };
+
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_True_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_with_offset_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + valid_urdf_ros2_control_system_robot_with_gpio_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + sensor_with_initial_value_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + gpio_with_initial_value_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_FALSE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    hardware_system_2dof_standard_interfaces_with_different_control_modes_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head +
+    valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ +
+    ros2_control_test_assets::urdf_tail));
+  ASSERT_TRUE(check_prepare_command_mode_switch_works(
+    ros2_control_test_assets::urdf_head + disabled_commands_ +
+    ros2_control_test_assets::urdf_tail));
+}