[ResourceManager] Propagate access to logger and clock interfaces to …

…HardwareComponent (#1585)

* Add the `init` API in the hardware interfaces to parse the logging and clock interfaces
* Change RCUTILS logging to RCLCPP logging
* rename `ResourceManager` to `resource_manager` in the logger
* support for the default `rm_logger_` if none provided

---------

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>

controller_manager/include/controller_manager/controller_manager.hpp

@@ -82,6 +82,13 @@ class ControllerManager : public rclcpp::Node
     const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
+  CONTROLLER_MANAGER_PUBLIC
+  ControllerManager(
+    std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
+    bool activate_all_hw_components, const std::string & manager_node_name = "controller_manager",
+    const std::string & node_namespace = "",
+    const rclcpp::NodeOptions & options = get_cm_node_options());
+
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 

controller_manager/src/controller_manager.cpp

@@ -179,15 +179,19 @@ rclcpp::NodeOptions get_cm_node_options()
   // Required for getting types of controllers to be loaded via service call
   node_options.allow_undeclared_parameters(true);
   node_options.automatically_declare_parameters_from_overrides(true);
+// \note The versions conditioning is added here to support the source-compatibility until Humble
+#if RCLCPP_VERSION_MAJOR >= 21
+  node_options.enable_logger_service(true);
+#endif
   return node_options;
 }
 
 ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   const std::string & node_namespace, const rclcpp::NodeOptions & options)
 : rclcpp::Node(manager_node_name, node_namespace, options),
-  resource_manager_(
-    std::make_unique<hardware_interface::ResourceManager>(this->get_node_clock_interface())),
+  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
+    this->get_node_clock_interface(), this->get_node_logging_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -198,7 +202,8 @@ ControllerManager::ControllerManager(
 {
   if (!get_parameter("update_rate", update_rate_))
   {
-    RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
+    RCLCPP_WARN(
+      get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
   }
 
   robot_description_ = "";
@@ -229,6 +234,40 @@ ControllerManager::ControllerManager(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
 }
 
+ControllerManager::ControllerManager(
+  std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
+  bool activate_all_hw_components, const std::string & manager_node_name,
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
+  update_rate_(get_parameter_or<int>("update_rate", 100)),
+  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
+    urdf, this->get_node_clock_interface(), this->get_node_logging_interface(),
+    activate_all_hw_components, update_rate_)),
+  diagnostics_updater_(this),
+  executor_(executor),
+  loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
+    kControllerInterfaceNamespace, kControllerInterfaceClassName)),
+  chainable_loader_(
+    std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+{
+  if (!get_parameter("update_rate", update_rate_))
+  {
+    RCLCPP_WARN(
+      get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
+  }
+
+  if (is_resource_manager_initialized())
+  {
+    init_services();
+  }
+  subscribe_to_robot_description_topic();
+
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
+}
+
 ControllerManager::ControllerManager(
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
@@ -245,7 +284,8 @@ ControllerManager::ControllerManager(
 {
   if (!get_parameter("update_rate", update_rate_))
   {
-    RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
+    RCLCPP_WARN(
+      get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_);
   }
 
   if (is_resource_manager_initialized())

controller_manager/test/controller_manager_test_common.hpp

@@ -74,7 +74,9 @@ class ControllerManagerFixture : public ::testing::Test
     if (robot_description_.empty())
     {
       cm_ = std::make_shared<CtrlMgr>(
-        std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+        std::make_unique<hardware_interface::ResourceManager>(
+          rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
+        executor_, TEST_CM_NAME);
     }
     else
     {
@@ -83,7 +85,9 @@ class ControllerManagerFixture : public ::testing::Test
       if (pass_urdf_as_parameter_)
       {
         cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(robot_description_, true, true),
+          std::make_unique<hardware_interface::ResourceManager>(
+            robot_description_, rm_node_->get_node_clock_interface(),
+            rm_node_->get_node_logging_interface(), true, 100),
           executor_, TEST_CM_NAME);
       }
       else
@@ -93,7 +97,9 @@ class ControllerManagerFixture : public ::testing::Test
 
         // this is just a workaround to skip passing
         cm_ = std::make_shared<CtrlMgr>(
-          std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+          std::make_unique<hardware_interface::ResourceManager>(
+            rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
+          executor_, TEST_CM_NAME);
         // mimic topic call
         auto msg = std_msgs::msg::String();
         msg.data = robot_description_;
@@ -158,6 +164,9 @@ class ControllerManagerFixture : public ::testing::Test
   const std::string robot_description_;
   const bool pass_urdf_as_parameter_;
   rclcpp::Time time_;
+
+protected:
+  rclcpp::Node::SharedPtr rm_node_ = std::make_shared<rclcpp::Node>("ResourceManager");
 };
 
 class TestControllerManagerSrvs

controller_manager/test/test_controller_manager_with_namespace.cpp

@@ -39,7 +39,8 @@ class TestControllerManagerWithNamespace
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<controller_manager::ControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::minimal_robot_urdf, true, true),
+        ros2_control_test_assets::minimal_robot_urdf, rm_node_->get_node_clock_interface(),
+        rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME, TEST_NAMESPACE);
     run_updater_ = false;
   }

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -111,7 +111,8 @@ class TestControllerChainingWithControllerManager
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     cm_ = std::make_shared<TestableControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::diffbot_urdf, true, true),
+        ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(),
+        rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME);
     run_updater_ = false;
   }

controller_manager/test/test_hardware_management_srvs.cpp

@@ -63,8 +63,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
   void SetUp() override
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
     run_updater_ = false;
 
     cm_->set_parameter(
@@ -369,8 +368,7 @@ class TestControllerManagerHWManagementSrvsWithoutParams
   void SetUp() override
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
     run_updater_ = false;
 
     // TODO(destogl): separate this to init_tests method where parameter can be set for each test

doc/release_notes/Jazzy.rst

@@ -97,6 +97,7 @@ hardware_interface
     </ros2_control>
 
 * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 <https://github.com/ros-controls/ros2_control/pull/1488>`_)
+* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 <https://github.com/ros-controls/ros2_control/pull/1585>`_)
 
 joint_limits
 ************

hardware_interface/include/hardware_interface/actuator.hpp

@@ -24,6 +24,8 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
@@ -44,7 +46,9 @@ class Actuator final
   ~Actuator() = default;
 
   HARDWARE_INTERFACE_PUBLIC
-  const rclcpp_lifecycle::State & initialize(const HardwareInfo & actuator_info);
+  const rclcpp_lifecycle::State & initialize(
+    const HardwareInfo & actuator_info, rclcpp::Logger logger,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface);
 
   HARDWARE_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & configure();

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -25,6 +25,8 @@
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
@@ -73,7 +75,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 public:
   ActuatorInterface()
   : lifecycle_state_(rclcpp_lifecycle::State(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN))
+      lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)),
+    actuator_logger_(rclcpp::get_logger("actuator_interface"))
   {
   }
 
@@ -88,15 +91,33 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 
   virtual ~ActuatorInterface() = default;
 
-  /// Initialization of the hardware interface from data parsed from the robot's URDF.
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
   /**
    * \param[in] hardware_info structure with data from URDF.
+   * \param[in] clock_interface pointer to the clock interface.
+   * \param[in] logger_interface pointer to the logger interface.
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
-  virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
+  CallbackReturn init(
+    const HardwareInfo & hardware_info, rclcpp::Logger logger,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   {
+    clock_interface_ = clock_interface;
+    actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
     info_ = hardware_info;
+    return on_init(hardware_info);
+  };
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF.
+  /**
+   * \param[in] hardware_info structure with data from URDF.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/)
+  {
     return CallbackReturn::SUCCESS;
   };
 
@@ -209,8 +230,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
 
 protected:
+  /// Get the logger of the ActuatorInterface.
+  /**
+   * \return logger of the ActuatorInterface.
+   */
+  rclcpp::Logger get_logger() const { return actuator_logger_; }
+
+  /// Get the clock of the ActuatorInterface.
+  /**
+   * \return clock of the ActuatorInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); }
+
   HardwareInfo info_;
   rclcpp_lifecycle::State lifecycle_state_;
+
+private:
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+  rclcpp::Logger actuator_logger_;
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -50,7 +50,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 public:
   /// Default constructor for the Resource Manager.
   explicit ResourceManager(
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface);
 
   /// Constructor for the Resource Manager.
   /**
@@ -67,8 +68,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * used for triggering async components.
    */
   explicit ResourceManager(
-    const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100,
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    const std::string & urdf,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+    bool activate_all = false, const unsigned int update_rate = 100);
 
   ResourceManager(const ResourceManager &) = delete;
 
@@ -461,6 +464,18 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   bool state_interface_exists(const std::string & key) const;
 
 protected:
+  /// Gets the logger for the resource manager
+  /**
+   * \return logger of the resource manager
+   */
+  rclcpp::Logger get_logger() const;
+
+  /// Gets the clock for the resource manager
+  /**
+   * \return clock of the resource manager
+   */
+  rclcpp::Clock::SharedPtr get_clock() const;
+
   bool components_are_loaded_and_initialized_ = false;
 
   mutable std::recursive_mutex resource_interfaces_lock_;

hardware_interface/include/hardware_interface/sensor.hpp

@@ -25,6 +25,8 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/visibility_control.h"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/logging.hpp"
+#include "rclcpp/node_interfaces/node_clock_interface.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
@@ -45,7 +47,9 @@ class Sensor final
   ~Sensor() = default;
 
   HARDWARE_INTERFACE_PUBLIC
-  const rclcpp_lifecycle::State & initialize(const HardwareInfo & sensor_info);
+  const rclcpp_lifecycle::State & initialize(
+    const HardwareInfo & sensor_info, rclcpp::Logger logger,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface);
 
   HARDWARE_INTERFACE_PUBLIC
   const rclcpp_lifecycle::State & configure();