[MockHardware] Enable disabling of command to simulate HW failures. (r…

…os-controls#1027)

hardware_interface/doc/mock_components_userdoc.rst

@@ -27,6 +27,11 @@ Features:
 Parameters
 ,,,,,,,,,,
 
+disable_commands (optional; boolean; default: false)
+  Disables mirroring commands to states. 
+  This option is helpful to simulate an erroneous connection to the hardware when nothing breaks, but suddenly there is no feedback from a hardware interface.
+  Or it can help you to test your setup when the hardware is running without feedback, i.e., in open loop configuration.
+
 mock_sensor_commands (optional; boolean; default: false)
   Creates fake command interfaces for faking sensor measurements with an external command.
   Those interfaces are usually used by a :ref:`forward controller <forward_command_controller_userdoc>` to provide access from ROS-world.

hardware_interface/include/mock_components/generic_system.hpp

@@ -114,6 +114,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
   double position_state_following_offset_;
   std::string custom_interface_with_following_offset_;
   size_t index_custom_interface_with_following_offset_;
+
+  bool command_propagation_disabled_;
 };
 
 typedef GenericSystem GenericRobot;

hardware_interface/src/mock_components/generic_system.cpp

@@ -119,6 +119,18 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
     }
   }
 
+  // check if there is parameter that disables commands
+  // this way we simulate disconnected driver
+  it = info_.hardware_parameters.find("disable_commands");
+  if (it != info.hardware_parameters.end())
+  {
+    command_propagation_disabled_ = hardware_interface::parse_bool(it->second);
+  }
+  else
+  {
+    command_propagation_disabled_ = false;
+  }
+
   // process parameters about state following
   position_state_following_offset_ = 0.0;
   custom_interface_with_following_offset_ = "";
@@ -359,6 +371,13 @@ std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_
 
 return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
+  if (command_propagation_disabled_)
+  {
+    RCUTILS_LOG_WARN_NAMED(
+      "mock_generic_system", "Command propagation is disabled - no values will be returned!");
+    return return_type::OK;
+  }
+
   auto mirror_command_to_state = [](auto & states_, auto commands_, size_t start_index = 0)
   {
     for (size_t i = start_index; i < states_.size(); ++i)

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -483,6 +483,24 @@ class TestGenericSystem : public ::testing::Test
     </gpio>
   </ros2_control>
 )";
+
+    disabled_commands_ =
+      R"(
+  <ros2_control name="GenericSystem2dof" type="system">
+    <hardware>
+      <plugin>fake_components/GenericSystem</plugin>
+      <param name="disable_commands">True</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <state_interface name="position">
+        <param name="initial_value">3.45</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+)";
   }
 
   std::string hardware_robot_2dof_;
@@ -503,6 +521,7 @@ class TestGenericSystem : public ::testing::Test
   std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_;
   std::string sensor_with_initial_value_;
   std::string gpio_with_initial_value_;
+  std::string disabled_commands_;
 };
 
 // Forward declaration
@@ -1604,3 +1623,51 @@ TEST_F(TestGenericSystem, gpio_with_initial_value)
 
   ASSERT_EQ(1, state.get_value());
 }
+
+TEST_F(TestGenericSystem, disabled_commands_flag_is_active)
+{
+  auto urdf =
+    ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail;
+  TestableResourceManager rm(urdf);
+  // Activate components to get all interfaces available
+  activate_components(rm);
+
+  // Check interfaces
+  EXPECT_EQ(1u, rm.system_components_size());
+  ASSERT_EQ(2u, rm.state_interface_keys().size());
+  EXPECT_TRUE(rm.state_interface_exists("joint1/position"));
+  EXPECT_TRUE(rm.state_interface_exists("joint1/velocity"));
+
+  ASSERT_EQ(2u, rm.command_interface_keys().size());
+  EXPECT_TRUE(rm.command_interface_exists("joint1/position"));
+  EXPECT_TRUE(rm.command_interface_exists("joint1/velocity"));
+
+  // Check initial values
+  hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position");
+  hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity");
+  hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position");
+
+  ASSERT_EQ(3.45, j1p_s.get_value());
+  ASSERT_EQ(0.0, j1v_s.get_value());
+  ASSERT_TRUE(std::isnan(j1p_c.get_value()));
+
+  // set some new values in commands
+  j1p_c.set_value(0.11);
+
+  // State values should not be changed
+  ASSERT_EQ(3.45, j1p_s.get_value());
+  ASSERT_EQ(0.0, j1v_s.get_value());
+  ASSERT_EQ(0.11, j1p_c.get_value());
+
+  // write() does not change values
+  rm.write(TIME, PERIOD);
+  ASSERT_EQ(3.45, j1p_s.get_value());
+  ASSERT_EQ(0.0, j1v_s.get_value());
+  ASSERT_EQ(0.11, j1p_c.get_value());
+
+  // read() also does not change values
+  rm.read(TIME, PERIOD);
+  ASSERT_EQ(3.45, j1p_s.get_value());
+  ASSERT_EQ(0.0, j1v_s.get_value());
+  ASSERT_EQ(0.11, j1p_c.get_value());
+}