diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 7fd50fc9c8..f143ea816c 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/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 ` to provide access from ROS-world. diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 9506139c4a..c244ee1254 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/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; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index d490784dbd..13a5a8b679 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/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 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) diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index e82d7d1fb2..8e78ade7fb 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -483,6 +483,24 @@ class TestGenericSystem : public ::testing::Test )"; + + disabled_commands_ = + R"( + + + fake_components/GenericSystem + True + + + + + + 3.45 + + + + +)"; } 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()); +}