From 8bf1a8a1b3dd2533c93d3cc0591abf4560199b23 Mon Sep 17 00:00:00 2001
From: "Dr. Denis" <denis@stoglrobotics.de>
Date: Mon, 6 Nov 2023 19:18:20 +0100
Subject: [PATCH] [MockHardware] Fix the issues where hardware with multiple
 interfaces can not be started because of a logical bug added when adding
 dynamics calculation functionality. (#1151)

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
---
 .../src/mock_components/generic_system.cpp    |  5 ++
 .../mock_components/test_generic_system.cpp   | 81 +++++++++++++++++++
 2 files changed, 86 insertions(+)

diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
index 3ae3568a9e..1b339c71ce 100644
--- 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
index cd344b600e..9556ed6f8b 100644
--- 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,43 @@ 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 = [&](const std::string & urdf)
+  {
+    TestableResourceManager rm(
+      ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail);
+    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(hardware_system_2dof_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_asymetric_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_other_interface_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_));
+  ASSERT_TRUE(
+    check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_));
+  ASSERT_TRUE(
+    check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    valid_urdf_ros2_control_system_robot_with_gpio_mock_command_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(gpio_with_initial_value_));
+  ASSERT_FALSE(check_prepare_command_mode_switch(
+    hardware_system_2dof_standard_interfaces_with_different_control_modes_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(
+    valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_));
+  ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_));
+}