From d0d5b7d86fe0107f2ecfeffdfe3265f64a0fb2b1 Mon Sep 17 00:00:00 2001 From: shlok-mehndiratta Date: Wed, 22 Apr 2026 01:43:09 +0530 Subject: [PATCH] feat(hardware_interface): implement dynamic command toggling for MockHardware --- hardware_interface/CMakeLists.txt | 2 + .../mock_components/generic_system.hpp | 7 +- hardware_interface/package.xml | 1 + .../src/mock_components/generic_system.cpp | 23 ++++- .../mock_components/test_generic_system.cpp | 96 +++++++++++++++++++ 5 files changed, 125 insertions(+), 4 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 453c0c0258..6d164532cb 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS realtime_tools TinyXML2 joint_limits + std_srvs urdf pal_statistics fmt @@ -53,6 +54,7 @@ target_link_libraries(hardware_interface PUBLIC ${pal_statistics_LIBRARIES} ${control_msgs_TARGETS} ${lifecycle_msgs_TARGETS} + ${std_srvs_TARGETS} fmt::fmt) add_library(mock_components SHARED diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 25c3219dde..b6d9b39222 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -17,13 +17,17 @@ #ifndef MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ #define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_ +#include #include #include + #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/set_bool.hpp" using hardware_interface::return_type; @@ -89,7 +93,8 @@ class GenericSystem : public hardware_interface::SystemInterface bool calculate_dynamics_; std::vector joint_control_mode_; - bool command_propagation_disabled_; + std::atomic command_propagation_disabled_; + rclcpp::Service::SharedPtr set_command_propagation_srv_; }; typedef GenericSystem GenericRobot; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 4580c9284c..e168a392ab 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -23,6 +23,7 @@ rcpputils realtime_tools sdformat_urdf + std_srvs tinyxml2 urdf fmt diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 29f137b701..b8b97cdb2b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -66,11 +66,28 @@ CallbackReturn GenericSystem::on_init( it = get_hardware_info().hardware_parameters.find("disable_commands"); if (it != get_hardware_info().hardware_parameters.end()) { - command_propagation_disabled_ = hardware_interface::parse_bool(it->second); + command_propagation_disabled_.store( + hardware_interface::parse_bool(it->second), std::memory_order_relaxed); } else { - command_propagation_disabled_ = false; + command_propagation_disabled_.store(false, std::memory_order_relaxed); + } + + if (get_node()) + { + set_command_propagation_srv_ = get_node()->create_service( + "~/set_command_propagation", + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + command_propagation_disabled_.store(!request->data, std::memory_order_release); + response->success = true; + response->message = + request->data ? "Command propagation enabled" : "Command propagation disabled"; + RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); + }); } // check if there is parameter that enables dynamic calculation @@ -330,7 +347,7 @@ hardware_interface::CallbackReturn GenericSystem::on_configure( return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - if (command_propagation_disabled_) + if (command_propagation_disabled_.load(std::memory_order_acquire)) { RCLCPP_WARN(get_logger(), "Command propagation is disabled - no values will be returned!"); return return_type::OK; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index a8052448c0..a767ae3086 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -19,15 +19,19 @@ #include #include +#include + #include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "std_srvs/srv/set_bool.hpp" namespace { @@ -860,6 +864,25 @@ class TestableResourceManager : public hardware_interface::ResourceManager cm_update_rate) { } + + explicit TestableResourceManager( + rclcpp::Node::SharedPtr node, rclcpp::Executor::SharedPtr executor, const std::string & urdf, + bool activate_all = false, unsigned int cm_update_rate = 100) + : hardware_interface::ResourceManager( + [&]() + { + hardware_interface::ResourceManagerParams params; + params.robot_description = urdf; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + params.activate_all = activate_all; + params.update_rate = cm_update_rate; + params.executor = executor; + return params; + }(), + true) + { + } }; void set_components_state( @@ -2703,6 +2726,79 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) EXPECT_EQ(0.11, j1p_c.get_optional().value()); } +TEST_F(TestGenericSystem, toggle_command_propagation_service) +{ + auto urdf = + ros2_control_test_assets::urdf_head + hw_sys_2dof_ + ros2_control_test_assets::urdf_tail; + auto rm_node = std::make_shared("rm_node"); + auto client_node = std::make_shared("service_client_node"); + auto executor = std::make_shared(); + executor->add_node(rm_node); + executor->add_node(client_node); + + std::thread executor_thread([executor]() { executor->spin(); }); + + { + TestableResourceManager rm(rm_node, executor, urdf); + activate_components(rm, {"MockHardwareSystem"}); + + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1p_c = + rm.claim_command_interface("joint1/position"); + + // Initialize state + ASSERT_TRUE(j1p_c.set_value(1.1)); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + ASSERT_EQ(1.1, j1p_s.get_optional().value()); + + // Disable via service + auto client = client_node->create_client( + "/mockhardwaresystem/set_command_propagation"); + + if (!client->wait_for_service(std::chrono::seconds(10))) + { + executor->cancel(); + executor_thread.join(); + FAIL() << "Service not available"; + } + + auto request = std::make_shared(); + request->data = false; + auto result_future = client->async_send_request(request); + + if (result_future.wait_for(std::chrono::seconds(10)) != std::future_status::ready) + { + executor->cancel(); + executor_thread.join(); + FAIL() << "Service call timed out"; + } + ASSERT_TRUE(result_future.get()->success); + + // Verify Disabled - set new command but state should stay at 1.1 + ASSERT_TRUE(j1p_c.set_value(2.2)); + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(1.1, j1p_s.get_optional().value()); // Frozen + + // Enable via service + request->data = true; + result_future = client->async_send_request(request); + if (result_future.wait_for(std::chrono::seconds(10)) != std::future_status::ready) + { + executor->cancel(); + executor_thread.join(); + FAIL() << "Service call timed out"; + } + ASSERT_TRUE(result_future.get()->success); + + // Verify Enabled again - state should now update to 2.2 + ASSERT_EQ(rm.read(TIME, PERIOD).result, hardware_interface::return_type::OK); + EXPECT_EQ(2.2, j1p_s.get_optional().value()); + } + + executor->cancel(); + executor_thread.join(); +} + TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) { auto check_prepare_command_mode_switch =