diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index f899b60236..abc6c860aa 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -48,6 +48,59 @@ The following is a step-by-step guide to create source files, basic tests, and c In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``. If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise. + #. **(Optional) Adding Publisher, Services, etc.** + + A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop. + + This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are two primary ways to achieve this. + + **Method 1: Using the Framework-Managed Node (Recommended & Simplest)** + + The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it. + + #. **Access and using the Default Node**: You can get a ``shared_ptr`` to the node by calling the ``get_node()`` method and use it just like any other ``rclcpp::Node::SharedPtr`` to create publishers, timers, etc. + + .. code-block:: cpp + + // Continuing inside on_configure() + if (get_node()) + { + my_publisher_ = get_node()->create_publisher("~/status", 10); + + using namespace std::chrono_literals; + my_timer_ = get_node()->create_wall_timer(1s, [this]() { + std_msgs::msg::String msg; + msg.data = "Hardware status update!"; + my_publisher_->publish(msg); + }); + } + + **Method 2: Using the Executor from `HardwareComponentParams`** + + For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor. + + #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentParams``. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareComponentParams & params) override; + + #. **Lock and Use the Executor**: Inside ``on_init``, you must safely "lock" the ``weak_ptr`` to get a usable ``shared_ptr``. You can then create your own node and add it to the executor. + + .. code-block:: cpp + + // In your .cpp, inside on_init(params) + if (auto locked_executor = params.executor.lock()) + { + my_custom_node_ = std::make_shared("my_custom_node"); + locked_executor->add_node(my_custom_node_->get_node_base_interface()); + // ... create publishers/timers on my_custom_node_ ... + } + + For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in example 17. + #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``. diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 459d8fa14e..99ddf44226 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -203,6 +203,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod info_.thread_priority); async_handler_->start_thread(); } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = params.hardware_info.name; + std::transform( + node_name.begin(), node_name.end(), node_name.begin(), + [](unsigned char c) { return std::tolower(c); }); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared(node_name); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + hardware_interface::HardwareComponentInterfaceParams interface_params; interface_params.hardware_info = info_; interface_params.executor = params.executor; @@ -663,6 +683,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; } + /// Get the default node of the ActuatorInterface. + /** + * \return node of the ActuatorInterface. + */ + rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } + /// Get the hardware info of the ActuatorInterface. /** * \return hardware info of the ActuatorInterface. @@ -719,6 +745,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod private: rclcpp::Clock::SharedPtr actuator_clock_; rclcpp::Logger actuator_logger_; + rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index fc35d74d61..c5ff7b7feb 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -166,6 +166,26 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI info_.thread_priority); read_async_handler_->start_thread(); } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = params.hardware_info.name; + std::transform( + node_name.begin(), node_name.end(), node_name.begin(), + [](unsigned char c) { return std::tolower(c); }); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared(node_name); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + hardware_interface::HardwareComponentInterfaceParams interface_params; interface_params.hardware_info = info_; interface_params.executor = params.executor; @@ -436,6 +456,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; } + /// Get the default node of the ActuatorInterface. + /** + * \return node of the ActuatorInterface. + */ + rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } + /// Get the hardware info of the SensorInterface. /** * \return hardware info of the SensorInterface. @@ -476,6 +502,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::Clock::SharedPtr sensor_clock_; rclcpp::Logger sensor_logger_; + rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; // interface names to Handle accessed through getters/setters std::unordered_map sensor_states_map_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 01710aba12..1a4bb9cc0f 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -207,6 +207,26 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI info_.thread_priority); async_handler_->start_thread(); } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = params.hardware_info.name; + std::transform( + node_name.begin(), node_name.end(), node_name.begin(), + [](unsigned char c) { return std::tolower(c); }); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared(node_name); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + hardware_interface::HardwareComponentInterfaceParams interface_params; interface_params.hardware_info = info_; interface_params.executor = params.executor; @@ -694,6 +714,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; } + /// Get the default node of the ActuatorInterface. + /** + * \return node of the ActuatorInterface. + */ + rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } + /// Get the hardware info of the SystemInterface. /** * \return hardware info of the SystemInterface. @@ -760,6 +786,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::Clock::SharedPtr system_clock_; rclcpp::Logger system_logger_; + rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 8caddffb4f..688f2d9b94 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1342,6 +1342,49 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } +class MockExecutor : public rclcpp::executors::SingleThreadedExecutor +{ +public: + explicit MockExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()) + : rclcpp::executors::SingleThreadedExecutor(options) + { + } + + void add_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) override + { + rclcpp::executors::SingleThreadedExecutor::add_node(node_ptr, notify); + added_node_names.push_back(node_ptr->get_name()); + } + + std::vector added_node_names; +}; + +TEST_F(ResourceManagerTest, hardware_nodes_are_added_to_mock_executor_on_load) +{ + auto mock_executor = std::make_shared(); + + hardware_interface::ResourceManagerParams params; + params.robot_description = ros2_control_test_assets::minimal_robot_urdf; + params.clock = node_.get_clock(); + params.logger = node_.get_logger(); + params.executor = mock_executor; + TestableResourceManager rm(params); + auto to_lower = [](const std::string & s) + { + std::string result = s; + std::transform( + result.begin(), result.end(), result.begin(), + [](unsigned char c) { return std::tolower(c); }); + return result; + }; + EXPECT_THAT( + mock_executor->added_node_names, + testing::UnorderedElementsAre( + to_lower(TEST_ACTUATOR_HARDWARE_NAME), to_lower(TEST_SENSOR_HARDWARE_NAME), + to_lower(TEST_SYSTEM_HARDWARE_NAME))); +} + class ResourceManagerTestReadWriteError : public ResourceManagerTest { public: diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index c252c3a5be..64f171b81f 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -64,6 +64,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) { } + + explicit TestableResourceManager(const hardware_interface::ResourceManagerParams & params) + : hardware_interface::ResourceManager(params, true) + { + } }; std::vector set_components_state(