From cd7fe3099ccb865eadbd96b02b24909430e09665 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 22 Jun 2024 13:17:20 +0200 Subject: [PATCH 01/22] Add logging interface as an argument into the ResourceManager constructor --- controller_manager/src/controller_manager.cpp | 4 ++-- .../hardware_interface/resource_manager.hpp | 6 ++++-- hardware_interface/src/resource_manager.cpp | 17 +++++++++++------ 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f55f23190e..e7eb170817 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -187,8 +187,8 @@ ControllerManager::ControllerManager( std::shared_ptr 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(this->get_node_clock_interface())), + resource_manager_(std::make_unique( + this->get_node_clock_interface(), this->get_node_logging_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 37baf96388..26fc7b58f5 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/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 = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); /// Constructor for the Resource Manager. /** @@ -68,7 +69,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ explicit ResourceManager( const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); ResourceManager(const ResourceManager &) = delete; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7be0cd0cb6..a191195d3a 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -99,11 +99,13 @@ class ResourceStorage // TODO(VX792): Change this when HW ifs get their own update rate, // because the ResourceStorage really shouldn't know about the cm's parameters explicit ResourceStorage( - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), - clock_interface_(clock_interface) + clock_interface_(clock_interface), + logger_interface_(logger_interface) { } @@ -970,11 +972,13 @@ class ResourceStorage // Used by async components. unsigned int cm_update_rate_ = 100; rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; }; ResourceManager::ResourceManager( - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: resource_storage_(std::make_unique(clock_interface)) + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) +: resource_storage_(std::make_unique(clock_interface, logger_interface)) { } @@ -982,8 +986,9 @@ ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( const std::string & urdf, bool activate_all, const unsigned int update_rate, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: resource_storage_(std::make_unique(clock_interface)) + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) +: resource_storage_(std::make_unique(clock_interface, logger_interface)) { load_and_initialize_components(urdf, update_rate); From a424ec2f703b4508c26a9ce5babc44c229e3c3ec Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 22 Jun 2024 13:29:45 +0200 Subject: [PATCH 02/22] Add get_logger and get_clock methods in the ResourceManager and ResourceStorage --- .../hardware_interface/resource_manager.hpp | 12 +++++++ hardware_interface/src/resource_manager.cpp | 36 +++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 26fc7b58f5..e3476f1472 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -410,6 +410,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 get_clock() const; + bool components_are_loaded_and_initialized_ = false; mutable std::recursive_mutex resource_interfaces_lock_; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a191195d3a..95f2f9c6f1 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -931,6 +931,38 @@ class ResourceStorage return hw_group_state_.at(group_name); } + /// Gets the logger for the resource storage + /** + * \return logger of the resource storage + */ + rclcpp::Logger get_logger() const + { + if (logger_interface_) + { + return logger_interface_->get_logger().get_child("ResourceManager"); + } + else + { + return rclcpp::get_logger("ResourceManager"); + } + } + + /// Gets the clock for the resource storage + /** + * \return clock of the resource storage + */ + rclcpp::Clock get_clock() const + { + if (clock_interface_) + { + return *clock_interface_->get_clock(); + } + else + { + return rclcpp::Clock(RCL_ROS_TIME); + } + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -1792,6 +1824,10 @@ bool ResourceManager::state_interface_exists(const std::string & key) const // END: "used only in tests and locally" +rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); } + +rclcpp::Clock ResourceManager::get_clock() const { return resource_storage_->get_clock(); } + // BEGIN: private methods bool ResourceManager::validate_storage( From 806430e24218ed582e87166d927c01b6ec256600 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 10:43:05 +0200 Subject: [PATCH 03/22] Propagate the interfaces to the HW component --- hardware_interface/include/hardware_interface/actuator.hpp | 7 ++++++- hardware_interface/include/hardware_interface/sensor.hpp | 7 ++++++- hardware_interface/include/hardware_interface/system.hpp | 7 ++++++- hardware_interface/src/actuator.cpp | 5 ++++- hardware_interface/src/resource_manager.cpp | 3 ++- hardware_interface/src/sensor.cpp | 5 ++++- hardware_interface/src/system.cpp | 5 ++++- 7 files changed, 32 insertions(+), 7 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index b23b913d75..d640ca1343 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -44,7 +46,10 @@ 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::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4c267bef77..e57dfad26a 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -45,7 +47,10 @@ 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::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index fb28929948..4b695f6efb 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -45,7 +47,10 @@ class System final ~System() = default; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & initialize(const HardwareInfo & system_info); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & system_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index b80f76ebf5..d84f903f24 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -36,7 +36,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} -const rclcpp_lifecycle::State & Actuator::initialize(const HardwareInfo & actuator_info) +const rclcpp_lifecycle::State & Actuator::initialize( + const HardwareInfo & actuator_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 95f2f9c6f1..b7c5cb6988 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -179,7 +179,8 @@ class ResourceStorage bool result = false; try { - const rclcpp_lifecycle::State new_state = hardware.initialize(hardware_info); + const rclcpp_lifecycle::State new_state = + hardware.initialize(hardware_info, clock_interface_, logger_interface_); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2da627f892..72b69d7799 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -34,7 +34,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} -const rclcpp_lifecycle::State & Sensor::initialize(const HardwareInfo & sensor_info) +const rclcpp_lifecycle::State & Sensor::initialize( + const HardwareInfo & sensor_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 8e950faa89..b9bd718e26 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,7 +34,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} -const rclcpp_lifecycle::State & System::initialize(const HardwareInfo & system_info) +const rclcpp_lifecycle::State & System::initialize( + const HardwareInfo & system_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { From 73cc79487746e26d2a9cb00cf89b8e795bfbe650 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 11:49:07 +0200 Subject: [PATCH 04/22] Add the `init` API in the hardware interfaces to parse the logging and clock interfaces --- .../hardware_interface/actuator_interface.hpp | 35 ++++++++++++++++++- .../hardware_interface/sensor_interface.hpp | 35 ++++++++++++++++++- .../hardware_interface/system_interface.hpp | 35 ++++++++++++++++++- hardware_interface/src/actuator.cpp | 2 +- hardware_interface/src/sensor.cpp | 2 +- hardware_interface/src/system.cpp | 2 +- 6 files changed, 105 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1ae05aa5f7..258ba5df2e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -88,6 +90,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual ~ActuatorInterface() = default; + /// 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. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + { + clock_interface_ = clock_interface; + logger_interface_ = logger_interface; + 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. @@ -96,7 +118,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { - info_ = hardware_info; return CallbackReturn::SUCCESS; }; @@ -209,8 +230,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + rclcpp::Logger get_logger() const + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.actuator." + info_.name); + } + + rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 5a3601afa8..831b3b1d39 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -88,6 +90,26 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SensorInterface() = default; + /// 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. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + { + clock_interface_ = clock_interface; + logger_interface_ = logger_interface; + 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. @@ -96,7 +118,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { - info_ = hardware_info; return CallbackReturn::SUCCESS; }; @@ -148,8 +169,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + rclcpp::Logger get_logger() const + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.sensor." + info_.name); + } + + rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 6b2c38b915..f6b591fde0 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_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/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -89,6 +91,26 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SystemInterface() = default; + /// 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. + */ + CallbackReturn init( + const HardwareInfo & hardware_info, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + { + clock_interface_ = clock_interface; + logger_interface_ = logger_interface; + 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. @@ -97,7 +119,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { - info_ = hardware_info; return CallbackReturn::SUCCESS; }; @@ -210,8 +231,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + rclcpp::Logger get_logger() const + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.system." + info_.name); + } + + rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index d84f903f24..a00e129772 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -43,7 +43,7 @@ const rclcpp_lifecycle::State & Actuator::initialize( { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(actuator_info)) + switch (impl_->init(actuator_info, clock_interface, logger_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 72b69d7799..814a17def7 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -41,7 +41,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(sensor_info)) + switch (impl_->init(sensor_info, clock_interface, logger_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index b9bd718e26..5cd20af739 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -41,7 +41,7 @@ const rclcpp_lifecycle::State & System::initialize( { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(system_info)) + switch (impl_->init(system_info, clock_interface, logger_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( From a39155875158d06417ed670d190dfe9fa978a859 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 11:50:51 +0200 Subject: [PATCH 05/22] comment to avoid unused parameter warning --- .../include/hardware_interface/actuator_interface.hpp | 2 +- .../include/hardware_interface/sensor_interface.hpp | 2 +- .../include/hardware_interface/system_interface.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 258ba5df2e..3085d17a2c 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -116,7 +116,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \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) + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { return CallbackReturn::SUCCESS; }; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 831b3b1d39..f25092c018 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -116,7 +116,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \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) + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { return CallbackReturn::SUCCESS; }; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f6b591fde0..721ea2a26f 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -117,7 +117,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \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) + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) { return CallbackReturn::SUCCESS; }; From 4dbcf0c85b6da3e590beda1ce0a166c988224674 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 12:40:08 +0200 Subject: [PATCH 06/22] Add documentation to the get_logger and get_clock methods --- .../include/hardware_interface/actuator_interface.hpp | 8 ++++++++ .../include/hardware_interface/sensor_interface.hpp | 8 ++++++++ .../include/hardware_interface/system_interface.hpp | 8 ++++++++ 3 files changed, 24 insertions(+) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 3085d17a2c..93179da76e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -230,12 +230,20 @@ 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 logger_interface_->get_logger().get_child( "ResourceManager.hardware_component.actuator." + info_.name); } + /// Get the clock of the ActuatorInterface. + /** + * \return clock of the ActuatorInterface. + */ rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index f25092c018..166564f89c 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -169,12 +169,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + /// Get the logger of the SensorInterface. + /** + * \return logger of the SensorInterface. + */ rclcpp::Logger get_logger() const { return logger_interface_->get_logger().get_child( "ResourceManager.hardware_component.sensor." + info_.name); } + /// Get the clock of the SensorInterface. + /** + * \return clock of the SensorInterface. + */ rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } HardwareInfo info_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 721ea2a26f..871278eda9 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -231,12 +231,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ rclcpp::Logger get_logger() const { return logger_interface_->get_logger().get_child( "ResourceManager.hardware_component.system." + info_.name); } + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } HardwareInfo info_; From 3565f8be19ef51ec435acb2ee7334810f6cea31a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 13:22:17 +0200 Subject: [PATCH 07/22] remove the default nullptr args to the logging and clock interface --- .../include/hardware_interface/actuator.hpp | 4 ++-- .../include/hardware_interface/sensor.hpp | 4 ++-- .../include/hardware_interface/system.hpp | 4 ++-- .../test/test_component_interfaces.cpp | 18 +++++++++--------- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index d640ca1343..0d941be6db 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -48,8 +48,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( const HardwareInfo & actuator_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e57dfad26a..7ff2d41f32 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -49,8 +49,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( const HardwareInfo & sensor_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 4b695f6efb..e4c3a17fd8 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -49,8 +49,8 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( const HardwareInfo & system_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 6dc1c394b0..ecfd06002e 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -425,7 +425,7 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); + auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -515,7 +515,7 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); + auto state = sensor_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -546,7 +546,7 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -670,7 +670,7 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::System system_hw( std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -701,7 +701,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); + auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -760,7 +760,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info); + auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -819,7 +819,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info); + auto state = sensor_hw.initialize(mock_hw_info, nullptr, nullptr); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -883,7 +883,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -947,7 +947,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info); + auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); From 7760461cf31aad18c7da66197fee6e6bdf1644b4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 13:22:41 +0200 Subject: [PATCH 08/22] Consider the nullptr and return valid Logger and Clock --- .../hardware_interface/actuator_interface.hpp | 23 ++++++++++++++++--- .../hardware_interface/sensor_interface.hpp | 23 ++++++++++++++++--- .../hardware_interface/system_interface.hpp | 23 ++++++++++++++++--- 3 files changed, 60 insertions(+), 9 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 93179da76e..6ef10aa0f8 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -236,15 +236,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ rclcpp::Logger get_logger() const { - return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.actuator." + info_.name); + if (logger_interface_) + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.actuator." + info_.name); + } + else + { + return rclcpp::get_logger("ResourceManager.hardware_component.actuator." + info_.name); + } } /// Get the clock of the ActuatorInterface. /** * \return clock of the ActuatorInterface. */ - rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + rclcpp::Clock get_clock() const + { + if (clock_interface_) + { + return *(clock_interface_->get_clock()); + } + else + { + return rclcpp::Clock(RCL_ROS_TIME); + } + } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 166564f89c..8fe1e79dfc 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -175,15 +175,32 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Logger get_logger() const { - return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.sensor." + info_.name); + if (logger_interface_) + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.sensor." + info_.name); + } + else + { + return rclcpp::get_logger("ResourceManager.hardware_component.sensor." + info_.name); + } } /// Get the clock of the SensorInterface. /** * \return clock of the SensorInterface. */ - rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + rclcpp::Clock get_clock() const + { + if (clock_interface_) + { + return *(clock_interface_->get_clock()); + } + else + { + return rclcpp::Clock(RCL_ROS_TIME); + } + } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 871278eda9..2c6ce6ff2d 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -237,15 +237,32 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Logger get_logger() const { - return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.system." + info_.name); + if (logger_interface_) + { + return logger_interface_->get_logger().get_child( + "ResourceManager.hardware_component.system." + info_.name); + } + else + { + return rclcpp::get_logger("ResourceManager.hardware_component.system." + info_.name); + } } /// Get the clock of the SystemInterface. /** * \return clock of the SystemInterface. */ - rclcpp::Clock get_clock() const { return *(clock_interface_->get_clock()); } + rclcpp::Clock get_clock() const + { + if (clock_interface_) + { + return *(clock_interface_->get_clock()); + } + else + { + return rclcpp::Clock(RCL_ROS_TIME); + } + } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; From fe469061a4952318571dacb05e84d41123424d25 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 18:20:53 +0200 Subject: [PATCH 09/22] Change RCUTILS logging to RCLCPP logging --- .../src/mock_components/generic_system.cpp | 32 ++- hardware_interface/src/resource_manager.cpp | 240 +++++++++--------- 2 files changed, 129 insertions(+), 143 deletions(-) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 162c3aa60d..67f9ed56da 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -158,15 +158,15 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i { index_custom_interface_with_following_offset_ = std::distance(other_interfaces_.begin(), if_it); - RCUTILS_LOG_INFO_NAMED( - "mock_generic_system", "Custom interface with following offset '%s' found at index: %zu.", + RCLCPP_INFO( + get_logger(), "Custom interface with following offset '%s' found at index: %zu.", custom_interface_with_following_offset_.c_str(), index_custom_interface_with_following_offset_); } else { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", + RCLCPP_WARN( + get_logger(), "Custom interface with following offset '%s' does not exist. Offset will not be applied", custom_interface_with_following_offset_.c_str()); } @@ -356,8 +356,8 @@ return_type GenericSystem::prepare_command_mode_switch( { if (!calculate_dynamics_) { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", + RCLCPP_WARN( + get_logger(), "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " "might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); @@ -368,8 +368,8 @@ return_type GenericSystem::prepare_command_mode_switch( { if (!calculate_dynamics_) { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", + RCLCPP_WARN( + get_logger(), "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " "this might lead to wrong feedback and unexpected behavior.", info_.joints[joint_index].name.c_str()); @@ -379,9 +379,8 @@ return_type GenericSystem::prepare_command_mode_switch( } else { - RCUTILS_LOG_DEBUG_NAMED( - "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", - key.c_str()); + RCLCPP_DEBUG( + get_logger(), "Got interface '%s' that is not joint - nothing to do!", key.c_str()); } } @@ -390,8 +389,8 @@ return_type GenericSystem::prepare_command_mode_switch( // There has to always be at least one control mode from the above three set if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) { - RCUTILS_LOG_ERROR_NAMED( - "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + RCLCPP_ERROR( + get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); ret_val = hardware_interface::return_type::ERROR; @@ -400,8 +399,8 @@ return_type GenericSystem::prepare_command_mode_switch( // Currently we don't support multiple interface request if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) { - RCUTILS_LOG_ERROR_NAMED( - "mock_generic_system", + RCLCPP_ERROR( + get_logger(), "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); @@ -454,8 +453,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur { if (command_propagation_disabled_) { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", "Command propagation is disabled - no values will be returned!"); + RCLCPP_WARN(get_logger(), "Command propagation is disabled - no values will be returned!"); return return_type::OK; } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b7c5cb6988..aad0d0049d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -117,15 +117,14 @@ class ResourceStorage bool is_loaded = false; try { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); + RCLCPP_INFO(get_logger(), "Loading hardware '%s' ", hardware_info.name.c_str()); // hardware_plugin_name has to match class name in plugin xml description auto interface = std::unique_ptr( loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); if (interface) { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Loaded hardware '%s' from plugin '%s'", hardware_info.name.c_str(), + RCLCPP_INFO( + get_logger(), "Loaded hardware '%s' from plugin '%s'", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); HardwareT hardware(std::move(interface)); container.emplace_back(std::move(hardware)); @@ -145,26 +144,25 @@ class ResourceStorage } else { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Failed to load hardware '%s' from plugin '%s'", - hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + RCLCPP_ERROR( + get_logger(), "Failed to load hardware '%s' from plugin '%s'", hardware_info.name.c_str(), + hardware_info.hardware_plugin_name.c_str()); } } catch (const pluginlib::PluginlibException & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception while loading hardware: %s", ex.what()); + RCLCPP_ERROR(get_logger(), "Exception while loading hardware: %s", ex.what()); } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while loading hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while loading hardware '%s': %s", hardware_info.name.c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while loading hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while loading hardware '%s'", hardware_info.name.c_str()); } return is_loaded; @@ -173,8 +171,7 @@ class ResourceStorage template bool initialize_hardware(const HardwareInfo & hardware_info, HardwareT & hardware) { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Initialize hardware '%s' ", hardware_info.name.c_str()); + RCLCPP_INFO(get_logger(), "Initialize hardware '%s' ", hardware_info.name.c_str()); bool result = false; try @@ -185,26 +182,25 @@ class ResourceStorage if (result) { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Successful initialization of hardware '%s'", - hardware_info.name.c_str()); + RCLCPP_INFO( + get_logger(), "Successful initialization of hardware '%s'", hardware_info.name.c_str()); } else { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Failed to initialize hardware '%s'", hardware_info.name.c_str()); + RCLCPP_ERROR( + get_logger(), "Failed to initialize hardware '%s'", hardware_info.name.c_str()); } } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while initializing hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while initializing hardware '%s': %s", hardware_info.name.c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while initializing hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while initializing hardware '%s'", hardware_info.name.c_str()); } @@ -223,14 +219,14 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while configuring hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while configuring hardware '%s': %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while configuring hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while configuring hardware '%s'", hardware.get_name().c_str()); } @@ -250,16 +246,16 @@ class ResourceStorage if (found_it == available_state_interfaces_.end()) { available_state_interfaces_.emplace_back(interface); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' state interface added into available list", + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' state interface added into available list", hardware.get_name().c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", + RCLCPP_WARN( + get_logger(), "(hardware '%s'): '%s' state interface already in available list." " This can happen due to multiple calls to 'configure'", hardware.get_name().c_str(), interface.c_str()); @@ -276,16 +272,16 @@ class ResourceStorage if (found_it == available_command_interfaces_.end()) { available_command_interfaces_.emplace_back(interface); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' command interface added into available list", + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' command interface added into available list", hardware.get_name().c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", + RCLCPP_WARN( + get_logger(), "(hardware '%s'): '%s' command interface already in available list." " This can happen due to multiple calls to 'configure'", hardware.get_name().c_str(), interface.c_str()); @@ -319,16 +315,16 @@ class ResourceStorage if (found_it != available_command_interfaces_.end()) { available_command_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' command interface removed from available list", + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' command interface removed from available list", hardware_name.c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", + RCLCPP_WARN( + get_logger(), "(hardware '%s'): '%s' command interface not in available list. " "This should not happen (hint: multiple cleanup calls).", hardware_name.c_str(), interface.c_str()); @@ -343,16 +339,16 @@ class ResourceStorage if (found_it != available_state_interfaces_.end()) { available_state_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "(hardware '%s'): '%s' state interface removed from available list", + RCLCPP_DEBUG( + get_logger(), "(hardware '%s'): '%s' state interface removed from available list", hardware_name.c_str(), interface.c_str()); } else { // TODO(destogl): do here error management if interfaces are only partially added into // "available" list - this should never be the case! - RCUTILS_LOG_WARN_NAMED( - "resource_manager", + RCLCPP_WARN( + get_logger(), "(hardware '%s'): '%s' state interface not in available list. " "This should not happen (hint: multiple cleanup calls).", hardware_name.c_str(), interface.c_str()); @@ -372,14 +368,14 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while cleaning up hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while cleaning up hardware '%s': %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while cleaning up hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while cleaning up hardware '%s'", hardware.get_name().c_str()); } @@ -406,14 +402,14 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while shutting down hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while shutting down hardware '%s': %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while shutting down hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while shutting down hardware '%s'", hardware.get_name().c_str()); } @@ -445,14 +441,14 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while activating hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while activating hardware '%s': %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while activating hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while activating hardware '%s'", hardware.get_name().c_str()); } @@ -480,14 +476,14 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception occurred while deactivating hardware '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception occurred while deactivating hardware '%s': %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception occurred while deactivating hardware '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while deactivating hardware '%s'", hardware.get_name().c_str()); } @@ -526,8 +522,8 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; - RCUTILS_LOG_WARN_NAMED( - "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", hardware.get_name().c_str()); break; } @@ -546,8 +542,8 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; - RCUTILS_LOG_WARN_NAMED( - "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", hardware.get_name().c_str()); break; } @@ -570,8 +566,8 @@ class ResourceStorage break; case State::PRIMARY_STATE_FINALIZED: result = false; - RCUTILS_LOG_WARN_NAMED( - "resource_manager", "hardware '%s' is in finalized state and can be only destroyed.", + RCLCPP_WARN( + get_logger(), "hardware '%s' is in finalized state and can be only destroyed.", hardware.get_name().c_str()); break; } @@ -618,15 +614,15 @@ class ResourceStorage } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + get_logger(), "Exception occurred while importing state interfaces for the hardware '%s' : %s", hardware.get_name().c_str(), e.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while importing state interfaces for the hardware '%s'", hardware.get_name().c_str()); } @@ -643,15 +639,15 @@ class ResourceStorage } catch (const std::exception & ex) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + get_logger(), "Exception occurred while importing command interfaces for the hardware '%s' : %s", hardware.get_name().c_str(), ex.what()); } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + get_logger(), "Unknown exception occurred while importing command interfaces for the hardware '%s'", hardware.get_name().c_str()); } @@ -716,9 +712,8 @@ class ResourceStorage } else { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); return false; } @@ -749,9 +744,8 @@ class ResourceStorage } else { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); return false; } @@ -783,9 +777,8 @@ class ResourceStorage } else { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "System hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); return false; } @@ -815,9 +808,8 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "Actuator hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "Actuator hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); } }; @@ -844,9 +836,8 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "Sensor hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); } }; @@ -874,9 +865,8 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( - "resource_manager", - "System hardware component '%s' from plugin '%s' failed to initialize.", + RCLCPP_WARN( + get_logger(), "System hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); } }; @@ -1203,9 +1193,8 @@ void ResourceManager::make_controller_reference_interfaces_unavailable( if (found_it != resource_storage_->available_command_interfaces_.end()) { resource_storage_->available_command_interfaces_.erase(found_it); - RCUTILS_LOG_DEBUG_NAMED( - "resource_manager", "'%s' command interface removed from available list", - interface.c_str()); + RCLCPP_DEBUG( + get_logger(), "'%s' command interface removed from available list", interface.c_str()); } } } @@ -1419,8 +1408,8 @@ bool ResourceManager::prepare_command_mode_switch( if (!(check_exist(start_interfaces) && check_exist(stop_interfaces))) { ss_not_existing << "]" << std::endl; - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + RCLCPP_ERROR( + get_logger(), "Not acceptable command interfaces combination: \n%s%s", interfaces_to_string(start_interfaces, stop_interfaces).c_str(), ss_not_existing.str().c_str()); return false; @@ -1445,14 +1434,15 @@ bool ResourceManager::prepare_command_mode_switch( if (!(check_available(start_interfaces) && check_available(stop_interfaces))) { ss_not_available << "]" << std::endl; - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Not acceptable command interfaces combination: \n%s%s", + RCLCPP_ERROR( + get_logger(), "Not acceptable command interfaces combination: \n%s%s", interfaces_to_string(start_interfaces, stop_interfaces).c_str(), ss_not_available.str().c_str()); return false; } - auto call_prepare_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) + auto call_prepare_mode_switch = + [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) { bool ret = true; for (auto & component : components) @@ -1467,9 +1457,8 @@ bool ResourceManager::prepare_command_mode_switch( return_type::OK != component.prepare_command_mode_switch(start_interfaces, stop_interfaces)) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", - "Component '%s' did not accept command interfaces combination: \n%s", + RCLCPP_ERROR( + logger, "Component '%s' did not accept command interfaces combination: \n%s", component.get_name().c_str(), interfaces_to_string(start_interfaces, stop_interfaces).c_str()); ret = false; @@ -1477,8 +1466,8 @@ bool ResourceManager::prepare_command_mode_switch( } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + logger, "Exception occurred while preparing command mode switch for component '%s' for the " "interfaces: \n %s : %s", component.get_name().c_str(), @@ -1487,8 +1476,8 @@ bool ResourceManager::prepare_command_mode_switch( } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + logger, "Unknown exception occurred while preparing command mode switch for component '%s' for " "the interfaces: \n %s", component.get_name().c_str(), @@ -1517,7 +1506,8 @@ bool ResourceManager::perform_command_mode_switch( return true; } - auto call_perform_mode_switch = [&start_interfaces, &stop_interfaces](auto & components) + auto call_perform_mode_switch = + [&start_interfaces, &stop_interfaces, logger = get_logger()](auto & components) { bool ret = true; for (auto & component : components) @@ -1532,16 +1522,15 @@ bool ResourceManager::perform_command_mode_switch( return_type::OK != component.perform_command_mode_switch(start_interfaces, stop_interfaces)) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Component '%s' could not perform switch", - component.get_name().c_str()); + RCLCPP_ERROR( + logger, "Component '%s' could not perform switch", component.get_name().c_str()); ret = false; } } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + logger, "Exception occurred while performing command mode switch for component '%s' for the " "interfaces: \n %s : %s", component.get_name().c_str(), @@ -1550,8 +1539,8 @@ bool ResourceManager::perform_command_mode_switch( } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", + RCLCPP_ERROR( + logger, "Unknown exception occurred while performing command mode switch for component '%s' " "for " "the interfaces: \n %s", @@ -1582,9 +1571,8 @@ return_type ResourceManager::set_component_state( if (found_it == resource_storage_->hardware_info_map_.end()) { - RCUTILS_LOG_INFO_NAMED( - "resource_manager", "Hardware Component with name '%s' does not exists", - component_name.c_str()); + RCLCPP_INFO( + get_logger(), "Hardware Component with name '%s' does not exists", component_name.c_str()); return return_type::ERROR; } @@ -1701,15 +1689,15 @@ HardwareReadWriteStatus ResourceManager::read( } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception thrown durind read of the component '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception thrown durind read of the component '%s': %s", component.get_name().c_str(), e.what()); ret_val = return_type::ERROR; } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception thrown during read of the component '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception thrown during read of the component '%s'", component.get_name().c_str()); ret_val = return_type::ERROR; } @@ -1762,15 +1750,15 @@ HardwareReadWriteStatus ResourceManager::write( } catch (const std::exception & e) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Exception thrown during write of the component '%s': %s", + RCLCPP_ERROR( + get_logger(), "Exception thrown during write of the component '%s': %s", component.get_name().c_str(), e.what()); ret_val = return_type::ERROR; } catch (...) { - RCUTILS_LOG_ERROR_NAMED( - "resource_manager", "Unknown exception thrown during write of the component '%s'", + RCLCPP_ERROR( + get_logger(), "Unknown exception thrown during write of the component '%s'", component.get_name().c_str()); ret_val = return_type::ERROR; } From e037d10e7a3f614c4766d4e0462182901473a8d5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 24 Jun 2024 18:32:24 +0200 Subject: [PATCH 10/22] rename ResourceManager to resource_manager in the logger --- .../include/hardware_interface/actuator_interface.hpp | 4 ++-- .../include/hardware_interface/sensor_interface.hpp | 4 ++-- .../include/hardware_interface/system_interface.hpp | 4 ++-- hardware_interface/src/resource_manager.cpp | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 6ef10aa0f8..36b1a56fc8 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -239,11 +239,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod if (logger_interface_) { return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.actuator." + info_.name); + "resource_manager.hardware_component.actuator." + info_.name); } else { - return rclcpp::get_logger("ResourceManager.hardware_component.actuator." + info_.name); + return rclcpp::get_logger("resource_manager.hardware_component.actuator." + info_.name); } } diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 8fe1e79dfc..d3dda154d6 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -178,11 +178,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI if (logger_interface_) { return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.sensor." + info_.name); + "resource_manager.hardware_component.sensor." + info_.name); } else { - return rclcpp::get_logger("ResourceManager.hardware_component.sensor." + info_.name); + return rclcpp::get_logger("resource_manager.hardware_component.sensor." + info_.name); } } diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 2c6ce6ff2d..e6f69b1066 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -240,11 +240,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI if (logger_interface_) { return logger_interface_->get_logger().get_child( - "ResourceManager.hardware_component.system." + info_.name); + "resource_manager.hardware_component.system." + info_.name); } else { - return rclcpp::get_logger("ResourceManager.hardware_component.system." + info_.name); + return rclcpp::get_logger("resource_manager.hardware_component.system." + info_.name); } } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index aad0d0049d..935725e7d7 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -930,11 +930,11 @@ class ResourceStorage { if (logger_interface_) { - return logger_interface_->get_logger().get_child("ResourceManager"); + return logger_interface_->get_logger().get_child("resource_manager"); } else { - return rclcpp::get_logger("ResourceManager"); + return rclcpp::get_logger("resource_manager"); } } From b313af4adfea823b81fe75211c0e9b658675efc9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 05:10:27 +0200 Subject: [PATCH 11/22] change the variables order for the vtable --- hardware_interface/src/resource_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 935725e7d7..2a00695bc8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -959,6 +959,10 @@ class ResourceStorage pluginlib::ClassLoader sensor_loader_; pluginlib::ClassLoader system_loader_; + // Logger and Clock interfaces + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + std::vector actuators_; std::vector sensors_; std::vector systems_; @@ -994,8 +998,6 @@ class ResourceStorage // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; }; ResourceManager::ResourceManager( From 578408daa16550d99574fc547aba41eb746de1dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 05:20:24 +0200 Subject: [PATCH 12/22] Add rm_logger variable to avoid asking for logger every instance --- hardware_interface/src/resource_manager.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2a00695bc8..891d3b9a75 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -104,9 +104,12 @@ class ResourceStorage : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), - clock_interface_(clock_interface), - logger_interface_(logger_interface) + clock_interface_(clock_interface) { + if (logger_interface) + { + rm_logger_ = logger_interface->get_logger().get_child("resource_manager"); + } } template @@ -926,17 +929,7 @@ class ResourceStorage /** * \return logger of the resource storage */ - rclcpp::Logger get_logger() const - { - if (logger_interface_) - { - return logger_interface_->get_logger().get_child("resource_manager"); - } - else - { - return rclcpp::get_logger("resource_manager"); - } - } + const rclcpp::Logger & get_logger() const { return rm_logger_; } /// Gets the clock for the resource storage /** @@ -962,6 +955,7 @@ class ResourceStorage // Logger and Clock interfaces rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Logger rm_logger_ = rclcpp::get_logger("resource_manager"); std::vector actuators_; std::vector sensors_; From 1fdcbbb1c7fcb69d3eecc787c80f4a910b8ad726 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 06:08:45 +0200 Subject: [PATCH 13/22] remove NodeLoggingInterface and parse Logger directly --- .../include/hardware_interface/actuator.hpp | 5 ++-- .../hardware_interface/actuator_interface.hpp | 25 +++++------------ .../include/hardware_interface/sensor.hpp | 5 ++-- .../hardware_interface/sensor_interface.hpp | 25 +++++------------ .../include/hardware_interface/system.hpp | 5 ++-- .../hardware_interface/system_interface.hpp | 25 +++++------------ hardware_interface/src/actuator.cpp | 7 +++-- hardware_interface/src/resource_manager.cpp | 2 +- hardware_interface/src/sensor.cpp | 7 +++-- hardware_interface/src/system.cpp | 7 +++-- .../test/test_component_interfaces.cpp | 27 ++++++++++++------- 11 files changed, 55 insertions(+), 85 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 0d941be6db..78e0dc7ae4 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -47,9 +47,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( - const HardwareInfo & actuator_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); + const HardwareInfo & actuator_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 36b1a56fc8..fb3a21369e 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -75,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")) { } @@ -100,12 +101,11 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { clock_interface_ = clock_interface; - logger_interface_ = logger_interface; + actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; return on_init(hardware_info); }; @@ -234,18 +234,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return logger of the ActuatorInterface. */ - rclcpp::Logger get_logger() const - { - if (logger_interface_) - { - return logger_interface_->get_logger().get_child( - "resource_manager.hardware_component.actuator." + info_.name); - } - else - { - return rclcpp::get_logger("resource_manager.hardware_component.actuator." + info_.name); - } - } + rclcpp::Logger get_logger() const { return actuator_logger_; } /// Get the clock of the ActuatorInterface. /** @@ -268,7 +257,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Logger actuator_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 7ff2d41f32..646f18cab9 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -48,9 +48,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( - const HardwareInfo & sensor_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); + const HardwareInfo & sensor_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index d3dda154d6..22ec0df006 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -75,7 +75,8 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI public: SensorInterface() : 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)), + sensor_logger_(rclcpp::get_logger("sensor_interface")) { } @@ -100,12 +101,11 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { clock_interface_ = clock_interface; - logger_interface_ = logger_interface; + sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; return on_init(hardware_info); }; @@ -173,18 +173,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return logger of the SensorInterface. */ - rclcpp::Logger get_logger() const - { - if (logger_interface_) - { - return logger_interface_->get_logger().get_child( - "resource_manager.hardware_component.sensor." + info_.name); - } - else - { - return rclcpp::get_logger("resource_manager.hardware_component.sensor." + info_.name); - } - } + rclcpp::Logger get_logger() const { return sensor_logger_; } /// Get the clock of the SensorInterface. /** @@ -207,7 +196,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Logger sensor_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index e4c3a17fd8..c262fcaf4e 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -48,9 +48,8 @@ class System final HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & initialize( - const HardwareInfo & system_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); + const HardwareInfo & system_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index e6f69b1066..401b0e7c43 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -76,7 +76,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI public: SystemInterface() : 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)), + system_logger_(rclcpp::get_logger("system_interface")) { } @@ -101,12 +102,11 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI * \returns CallbackReturn::ERROR if any error happens or data are missing. */ CallbackReturn init( - const HardwareInfo & hardware_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { clock_interface_ = clock_interface; - logger_interface_ = logger_interface; + system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; return on_init(hardware_info); }; @@ -235,18 +235,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return logger of the SystemInterface. */ - rclcpp::Logger get_logger() const - { - if (logger_interface_) - { - return logger_interface_->get_logger().get_child( - "resource_manager.hardware_component.system." + info_.name); - } - else - { - return rclcpp::get_logger("resource_manager.hardware_component.system." + info_.name); - } - } + rclcpp::Logger get_logger() const { return system_logger_; } /// Get the clock of the SystemInterface. /** @@ -269,7 +258,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; + rclcpp::Logger system_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index a00e129772..7e50c07eb0 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -37,13 +37,12 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} const rclcpp_lifecycle::State & Actuator::initialize( - const HardwareInfo & actuator_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & actuator_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(actuator_info, clock_interface, logger_interface)) + switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 891d3b9a75..26138f4800 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -180,7 +180,7 @@ class ResourceStorage try { const rclcpp_lifecycle::State new_state = - hardware.initialize(hardware_info, clock_interface_, logger_interface_); + hardware.initialize(hardware_info, rm_logger_, clock_interface_); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; if (result) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 814a17def7..9b7f1f69d6 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -35,13 +35,12 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} const rclcpp_lifecycle::State & Sensor::initialize( - const HardwareInfo & sensor_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & sensor_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(sensor_info, clock_interface, logger_interface)) + switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 5cd20af739..ba327d8ab2 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -35,13 +35,12 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} const rclcpp_lifecycle::State & System::initialize( - const HardwareInfo & system_info, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const HardwareInfo & system_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->init(system_info, clock_interface, logger_interface)) + switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index ecfd06002e..42ccdae8fa 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -425,7 +425,8 @@ TEST(TestComponentInterfaces, dummy_actuator) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); + auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -515,7 +516,8 @@ TEST(TestComponentInterfaces, dummy_sensor) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); + auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -546,7 +548,8 @@ TEST(TestComponentInterfaces, dummy_system) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); + auto state = system_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -670,7 +673,8 @@ TEST(TestComponentInterfaces, dummy_command_mode_system) hardware_interface::System system_hw( std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); + auto state = system_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -701,7 +705,8 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); + auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -760,7 +765,8 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) hardware_interface::Actuator actuator_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = actuator_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_actuator_components"); + auto state = actuator_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -819,7 +825,8 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) hardware_interface::Sensor sensor_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = sensor_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_sensor_components"); + auto state = sensor_hw.initialize(mock_hw_info, logger, nullptr); auto state_interfaces = sensor_hw.export_state_interfaces(); // Updated because is is INACTIVE @@ -883,7 +890,8 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); + auto state = system_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -947,7 +955,8 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) hardware_interface::System system_hw(std::make_unique()); hardware_interface::HardwareInfo mock_hw_info{}; - auto state = system_hw.initialize(mock_hw_info, nullptr, nullptr); + rclcpp::Logger logger = rclcpp::get_logger("test_system_components"); + auto state = system_hw.initialize(mock_hw_info, logger, nullptr); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); From 4eecf45217bdc9ca2e98540097ea344e5396d53b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 06:40:44 +0200 Subject: [PATCH 14/22] change the include headers of unused logging interface --- hardware_interface/include/hardware_interface/actuator.hpp | 2 +- .../include/hardware_interface/actuator_interface.hpp | 2 +- hardware_interface/include/hardware_interface/sensor.hpp | 2 +- .../include/hardware_interface/sensor_interface.hpp | 2 +- hardware_interface/include/hardware_interface/system.hpp | 2 +- .../include/hardware_interface/system_interface.hpp | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 78e0dc7ae4..3a1d7a5974 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -24,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index fb3a21369e..de98c2321c 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -25,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 646f18cab9..d8e55aa4ad 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -25,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 22ec0df006..66a3c636ea 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -25,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index c262fcaf4e..1ca4260750 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -25,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 401b0e7c43..f16df4e441 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,8 +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/node_interfaces/node_logging_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" From 2baf0b1bf29afeea88f785da5a74de050ce6e1a6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 06:44:45 +0200 Subject: [PATCH 15/22] enable logger services for the controller_manager --- controller_manager/src/controller_manager.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e7eb170817..ed312aa47c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -180,6 +180,9 @@ 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); +#if RCLCPP_VERSION_MAJOR >= 21 + node_options.enable_logger_service(true); +#endif return node_options; } From 812d103bb54dccf43934aa3a7daa056752c78c6e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 12:22:40 +0200 Subject: [PATCH 16/22] Update Release Notes --- doc/release_notes/Jazzy.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4074cbca63..67c432716c 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -94,6 +94,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) +* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) joint_limits ************ From 4ff14e5109b60fe57b7b8f6916a39ab9e831f0d9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Jun 2024 13:49:14 +0200 Subject: [PATCH 17/22] return shared pointer of clock instead of clock itself --- .../hardware_interface/actuator_interface.hpp | 12 +----------- .../hardware_interface/resource_manager.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 12 +----------- .../hardware_interface/system_interface.hpp | 12 +----------- hardware_interface/src/resource_manager.cpp | 17 +++++------------ 5 files changed, 9 insertions(+), 46 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index de98c2321c..88b36d460b 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -240,17 +240,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return clock of the ActuatorInterface. */ - rclcpp::Clock get_clock() const - { - if (clock_interface_) - { - return *(clock_interface_->get_clock()); - } - else - { - return rclcpp::Clock(RCL_ROS_TIME); - } - } + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index e3476f1472..8448942e78 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -420,7 +420,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * \return clock of the resource manager */ - rclcpp::Clock get_clock() const; + rclcpp::Clock::SharedPtr get_clock() const; bool components_are_loaded_and_initialized_ = false; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 66a3c636ea..79f689d0c6 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -179,17 +179,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return clock of the SensorInterface. */ - rclcpp::Clock get_clock() const - { - if (clock_interface_) - { - return *(clock_interface_->get_clock()); - } - else - { - return rclcpp::Clock(RCL_ROS_TIME); - } - } + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f16df4e441..8e41438ba5 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -241,17 +241,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return clock of the SystemInterface. */ - rclcpp::Clock get_clock() const - { - if (clock_interface_) - { - return *(clock_interface_->get_clock()); - } - else - { - return rclcpp::Clock(RCL_ROS_TIME); - } - } + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 26138f4800..0427af2c2b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -935,17 +935,7 @@ class ResourceStorage /** * \return clock of the resource storage */ - rclcpp::Clock get_clock() const - { - if (clock_interface_) - { - return *clock_interface_->get_clock(); - } - else - { - return rclcpp::Clock(RCL_ROS_TIME); - } - } + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } // hardware plugins pluginlib::ClassLoader actuator_loader_; @@ -1811,7 +1801,10 @@ bool ResourceManager::state_interface_exists(const std::string & key) const rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); } -rclcpp::Clock ResourceManager::get_clock() const { return resource_storage_->get_clock(); } +rclcpp::Clock::SharedPtr ResourceManager::get_clock() const +{ + return resource_storage_->get_clock(); +} // BEGIN: private methods From 5bc3c93e15d27556cdb0fca33ce1a8e856523177 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 27 Jun 2024 14:08:32 +0200 Subject: [PATCH 18/22] make the clock and logger interfaces mandatory arguments and fix tests --- .../controller_manager/controller_manager.hpp | 7 +++ controller_manager/src/controller_manager.cpp | 33 +++++++++++ .../test/controller_manager_test_common.hpp | 15 ++++- ...test_controller_manager_with_namespace.cpp | 3 +- ...llers_chaining_with_controller_manager.cpp | 3 +- .../test/test_hardware_management_srvs.cpp | 6 +- .../hardware_interface/resource_manager.hpp | 11 ++-- hardware_interface/src/resource_manager.cpp | 15 +++-- .../mock_components/test_generic_system.cpp | 57 ++++++++++++------- .../test/test_resource_manager.cpp | 48 +++++++++------- .../test/test_resource_manager.hpp | 15 ++++- ...esource_manager_prepare_perform_switch.cpp | 10 +++- 12 files changed, 160 insertions(+), 63 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b2cf2197ea..37503a85c9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/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 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; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ed312aa47c..c8abf010d5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -233,6 +233,39 @@ ControllerManager::ControllerManager( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); } +ControllerManager::ControllerManager( + std::shared_ptr 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("update_rate", 100)), + resource_manager_(std::make_unique( + 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>( + kControllerInterfaceNamespace, kControllerInterfaceClassName)), + chainable_loader_( + std::make_shared>( + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) +{ + if (!get_parameter("update_rate", update_rate_)) + { + RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + } + + 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 resource_manager, std::shared_ptr executor, const std::string & manager_node_name, diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index ac66392a13..f8cf9a7c11 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/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( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique( + 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( - std::make_unique(robot_description_, true, true), + std::make_unique( + 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( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique( + 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("ResourceManager"); }; class TestControllerManagerSrvs diff --git a/controller_manager/test/test_controller_manager_with_namespace.cpp b/controller_manager/test/test_controller_manager_with_namespace.cpp index 7d3d11c2e0..b83eca0c55 100644 --- a/controller_manager/test/test_controller_manager_with_namespace.cpp +++ b/controller_manager/test/test_controller_manager_with_namespace.cpp @@ -39,7 +39,8 @@ class TestControllerManagerWithNamespace executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( - 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; } diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index eadca39756..03a63346a6 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -111,7 +111,8 @@ class TestControllerChainingWithControllerManager executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( - 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; } diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index cbfbef5c30..c0c9bbd9a4 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -63,8 +63,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs void SetUp() override { executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; cm_->set_parameter( @@ -369,8 +368,7 @@ class TestControllerManagerHWManagementSrvsWithoutParams void SetUp() override { executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; // TODO(destogl): separate this to init_tests method where parameter can be set for each test diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 8448942e78..10ce685035 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -50,8 +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::NodeLoggingInterface::SharedPtr logger_interface = nullptr); + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); /// Constructor for the Resource Manager. /** @@ -68,9 +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, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_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; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0427af2c2b..ada54ea65d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -99,13 +99,18 @@ class ResourceStorage // TODO(VX792): Change this when HW ifs get their own update rate, // because the ResourceStorage really shouldn't know about the cm's parameters explicit ResourceStorage( - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface = nullptr) + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), clock_interface_(clock_interface) { + if (!clock_interface_) + { + throw std::invalid_argument( + "Clock interface is nullptr. ResourceManager needs a valid clock interface."); + } if (logger_interface) { rm_logger_ = logger_interface->get_logger().get_child("resource_manager"); @@ -994,9 +999,9 @@ ResourceManager::ResourceManager( ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool activate_all, const unsigned int update_rate, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface) + const std::string & urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, bool activate_all, + const unsigned int update_rate) : resource_storage_(std::make_unique(clock_interface, logger_interface)) { load_and_initialize_components(urdf, update_rate); diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 08a3640bd8..c90bc85055 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -26,6 +26,7 @@ #include "hardware_interface/resource_manager.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" #include "ros2_control_test_assets/descriptions.hpp" @@ -670,6 +671,7 @@ class TestGenericSystem : public ::testing::Test std::string disabled_commands_; std::string hardware_system_2dof_standard_interfaces_with_same_hardware_group_; std::string hardware_system_2dof_standard_interfaces_with_two_diff_hw_groups_; + rclcpp::Node node_ = rclcpp::Node("TestGenericSystem"); }; // Forward declaration @@ -694,10 +696,16 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command); FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True); - TestableResourceManager() : hardware_interface::ResourceManager() {} + explicit TestableResourceManager(rclcpp::Node & node) + : hardware_interface::ResourceManager( + node.get_node_clock_interface(), node.get_node_logging_interface()) + { + } - explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, activate_all) + explicit TestableResourceManager( + rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager( + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) { } }; @@ -744,7 +752,7 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(TestableResourceManager rm(urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(node_, urdf)); } // Test inspired by hardware_interface/test_resource_manager.cpp @@ -752,7 +760,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -783,7 +791,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -831,7 +839,8 @@ void generic_system_functional_test( const std::string & urdf, const std::string component_name = "GenericSystem2dof", const double offset = 0) { - TestableResourceManager rm(urdf); + rclcpp::Node node("test_generic_system"); + TestableResourceManager rm(node, urdf); // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -929,7 +938,8 @@ void generic_system_functional_test( void generic_system_error_group_test( const std::string & urdf, const std::string component_prefix, bool validate_same_group) { - TestableResourceManager rm(urdf); + rclcpp::Node node("test_generic_system"); + TestableResourceManager rm(node, urdf); const std::string component1 = component_prefix + "1"; const std::string component2 = component_prefix + "2"; // check is hardware is configured @@ -1108,7 +1118,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -1191,7 +1201,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -1290,7 +1300,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) void TestGenericSystem::test_generic_system_with_mock_sensor_commands( std::string & urdf, const std::string & component_name) { - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {component_name}); @@ -1430,7 +1440,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) void TestGenericSystem::test_generic_system_with_mimic_joint( std::string & urdf, const std::string & component_name) { - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {component_name}); @@ -1527,7 +1537,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i const double offset = -3; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); const std::string hardware_name = "MockHardwareSystem"; @@ -1640,7 +1650,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) { auto urdf = ros2_control_test_assets::urdf_head + valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); const std::string hardware_name = "MockHardwareSystem"; @@ -1737,7 +1747,7 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) void TestGenericSystem::test_generic_system_with_mock_gpio_commands( std::string & urdf, const std::string & component_name) { - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // check is hardware is started auto status_map = rm.get_components_status(); @@ -1863,7 +1873,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -1891,7 +1901,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -1912,7 +1922,7 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) hardware_system_2dof_standard_interfaces_with_different_control_modes_ + ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -2106,7 +2116,7 @@ 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); + TestableResourceManager rm(node_, urdf); // Activate components to get all interfaces available activate_components(rm, {"MockHardwareSystem"}); @@ -2155,7 +2165,7 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag [&]( const std::string & urdf, const std::string & urdf_head = ros2_control_test_assets::urdf_head) { - TestableResourceManager rm(urdf_head + urdf + ros2_control_test_assets::urdf_tail); + TestableResourceManager rm(node_, urdf_head + urdf + ros2_control_test_assets::urdf_tail); rclcpp_lifecycle::State state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active"); rm.set_component_state("MockHardwareSystem", state); auto start_interfaces = rm.command_interface_keys(); @@ -2192,3 +2202,10 @@ TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tag valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5a2ea0210c..5fb155fa3a 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -88,23 +88,24 @@ auto shutdown_components = TEST_F(ResourceManagerTest, initialization_empty) { - ASSERT_ANY_THROW(TestableResourceManager rm("")); + ASSERT_ANY_THROW(TestableResourceManager rm(node_, "");); } TEST_F(ResourceManagerTest, initialization_with_urdf) { - ASSERT_NO_THROW(TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf);); } TEST_F(ResourceManagerTest, post_initialization_with_urdf) { - TestableResourceManager rm; + TestableResourceManager rm(node_); ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); } void test_load_and_initialized_components_failure(const std::string & urdf) { - TestableResourceManager rm; + rclcpp::Node node = rclcpp::Node("TestableResourceManager"); + TestableResourceManager rm(node); ASSERT_FALSE(rm.load_and_initialize_components(urdf)); ASSERT_FALSE(rm.are_components_initialized()); @@ -148,7 +149,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware) TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) { // we validate the results manually - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf, false); EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); @@ -185,7 +186,7 @@ TEST_F(ResourceManagerTest, expect_validation_failure_if_not_all_interfaces_are_ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) { // we validate the results manually - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); auto command_interface_keys = rm.command_interface_keys(); for (const auto & key : command_interface_keys) @@ -203,7 +204,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { - TestableResourceManager rm; + TestableResourceManager rm(node_); ASSERT_FALSE(rm.are_components_initialized()); } @@ -254,19 +255,19 @@ TEST_F( TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_async_urdf_is_valid) { - TestableResourceManager rm(ros2_control_test_assets::minimal_async_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_async_robot_urdf); ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { - TestableResourceManager rm; + TestableResourceManager rm(node_); ASSERT_FALSE(rm.are_components_initialized()); rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.are_components_initialized()); @@ -274,7 +275,7 @@ TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) TEST_F(ResourceManagerTest, resource_claiming) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -388,7 +389,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -432,7 +433,7 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) TEST_F(ResourceManagerTest, default_prepare_perform_switch) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -443,7 +444,7 @@ TEST_F(ResourceManagerTest, default_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); auto status_map = rm.get_components_status(); @@ -516,7 +517,7 @@ TEST_F(ResourceManagerTest, resource_status) TEST_F(ResourceManagerTest, lifecycle_all_resources) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -659,7 +660,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) TEST_F(ResourceManagerTest, lifecycle_individual_resources) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -872,7 +873,7 @@ TEST_F(ResourceManagerTest, lifecycle_individual_resources) TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { using std::placeholders::_1; - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); auto check_interfaces = [](const std::vector & interface_names, auto check_method, bool expected_result) @@ -1220,7 +1221,7 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); std::string CONTROLLER_NAME = "test_controller"; std::vector REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"}; @@ -1340,7 +1341,7 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest void setup_resource_manager_and_do_initial_checks() { rm = std::make_shared( - ros2_control_test_assets::minimal_robot_urdf, false); + node_, ros2_control_test_assets::minimal_robot_urdf, false); activate_components(*rm); auto status_map = rm->get_components_status(); @@ -1689,7 +1690,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) { - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf, false); activate_components(rm); static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; @@ -1731,3 +1732,10 @@ TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); } } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 4eb7b8a799..18e51342f6 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -19,6 +19,7 @@ #include +#include #include #include @@ -31,6 +32,8 @@ class ResourceManagerTest : public ::testing::Test static void SetUpTestCase() {} void SetUp() {} + + rclcpp::Node node_{"ResourceManagerTest"}; }; // Forward declaration @@ -50,10 +53,16 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); FRIEND_TEST(ResourceManagerTest, test_uninitializable_hardware_no_validation); - TestableResourceManager() : hardware_interface::ResourceManager() {} + explicit TestableResourceManager(rclcpp::Node & node) + : hardware_interface::ResourceManager( + node.get_node_clock_interface(), node.get_node_logging_interface()) + { + } - explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, activate_all) + explicit TestableResourceManager( + rclcpp::Node & node, const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager( + urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100) { } }; diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index 8f6ba8f99a..d730029d90 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -64,7 +64,7 @@ class ResourceManagerPreparePerformTest : public ResourceManagerTest { ResourceManagerTest::SetUp(); - rm_ = std::make_unique(command_mode_urdf); + rm_ = std::make_unique(node_, command_mode_urdf); ASSERT_EQ(1u, rm_->actuator_components_size()); ASSERT_EQ(1u, rm_->system_components_size()); @@ -104,6 +104,7 @@ class ResourceManagerPreparePerformTest : public ResourceManagerTest std::unique_ptr claimed_actuator_position_state_; // Scenarios defined by example criteria + rclcpp::Node node_{"ResourceManagerPreparePerformTest"}; std::vector empty_keys = {}; std::vector non_existing_keys = {"elbow_joint/position", "should_joint/position"}; std::vector legal_keys_system = {"joint1/position", "joint2/position"}; @@ -387,3 +388,10 @@ TEST_F( EXPECT_EQ(claimed_system_acceleration_state_->get_value(), 0.0); EXPECT_EQ(claimed_actuator_position_state_->get_value(), 0.0); }; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 038945ccdac90941b29dccada2d947e18b402399 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jul 2024 19:36:57 +0200 Subject: [PATCH 19/22] add similar comment on the version conditioning --- controller_manager/src/controller_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c8abf010d5..88b71c59aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -180,6 +180,7 @@ 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 From 303c846c81c21db05159ab300e2d2f92f048ead9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jul 2024 19:41:52 +0200 Subject: [PATCH 20/22] add the default rm_logger_ to the if-else as per MR suggestion --- hardware_interface/src/resource_manager.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 225244e771..1aaef2e528 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -104,7 +104,8 @@ class ResourceStorage : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), - clock_interface_(clock_interface) + clock_interface_(clock_interface), + rm_logger_(rclcpp::get_logger("resource_manager")) { if (!clock_interface_) { @@ -950,7 +951,7 @@ class ResourceStorage // Logger and Clock interfaces rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface_; - rclcpp::Logger rm_logger_ = rclcpp::get_logger("resource_manager"); + rclcpp::Logger rm_logger_; std::vector actuators_; std::vector sensors_; From ec89bf52a1d3951861b1fbe9c46d212e6d96b362 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jul 2024 22:58:44 +0200 Subject: [PATCH 21/22] print default value of the update_rate Co-authored-by: Dr. Denis --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 88b71c59aa..b8e807c2c7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -253,7 +253,7 @@ 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()) From 9d7250049b7e22d2c0e2705d829e81ad434654eb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Jul 2024 23:05:05 +0200 Subject: [PATCH 22/22] print default update rate in other instances as well --- controller_manager/src/controller_manager.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b8e807c2c7..48c7f3a222 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -203,7 +203,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_ = ""; @@ -253,7 +254,8 @@ ControllerManager::ControllerManager( { if (!get_parameter("update_rate", update_rate_)) { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); + RCLCPP_WARN( + get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); } if (is_resource_manager_initialized()) @@ -283,7 +285,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())