feat(hardware_interface): implement dynamic command toggling for MockHardware

hardware_interface/CMakeLists.txt

@@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   realtime_tools
   TinyXML2
   joint_limits
+  std_srvs
   urdf
   pal_statistics
   fmt
@@ -52,6 +53,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

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 <atomic>
 #include <string>
 #include <vector>
+
 #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<size_t> joint_control_mode_;
 
-  bool command_propagation_disabled_;
+  std::atomic<bool> command_propagation_disabled_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_command_propagation_srv_;
 };
 
 typedef GenericSystem GenericRobot;

hardware_interface/package.xml

@@ -23,6 +23,7 @@
   <depend>rcpputils</depend>
   <depend>realtime_tools</depend>
   <depend>sdformat_urdf</depend>
+  <depend>std_srvs</depend>
   <depend>tinyxml2</depend>
   <depend>urdf</depend>
   <depend>fmt</depend>

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<std_srvs::srv::SetBool>(
+      "~/set_command_propagation",
+      [this](
+        const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+        std::shared_ptr<std_srvs::srv::SetBool::Response> 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;

hardware_interface/test/mock_components/test_generic_system.cpp

@@ -19,15 +19,19 @@
 #include <unordered_map>
 #include <vector>
 
+#include <thread>
+
 #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<rclcpp::Node>("rm_node");
+  auto client_node = std::make_shared<rclcpp::Node>("service_client_node");
+  auto executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+  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<std_srvs::srv::SetBool>(
+      "/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<std_srvs::srv::SetBool::Request>();
+    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 =