From 2b758eb7470478d6c240cf9be7908acbc16c21a0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jul 2024 18:06:58 +0200 Subject: [PATCH] [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (#1585) * Add the `init` API in the hardware interfaces to parse the logging and clock interfaces * Change RCUTILS logging to RCLCPP logging * rename `ResourceManager` to `resource_manager` in the logger * support for the default `rm_logger_` if none provided --------- Co-authored-by: Dr. Denis --- .../controller_manager/controller_manager.hpp | 7 + controller_manager/src/controller_manager.cpp | 48 ++- .../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 +- doc/release_notes/Jazzy.rst | 1 + .../include/hardware_interface/actuator.hpp | 6 +- .../hardware_interface/actuator_interface.hpp | 43 ++- .../hardware_interface/resource_manager.hpp | 21 +- .../include/hardware_interface/sensor.hpp | 6 +- .../hardware_interface/sensor_interface.hpp | 43 ++- .../include/hardware_interface/system.hpp | 6 +- .../hardware_interface/system_interface.hpp | 43 ++- hardware_interface/src/actuator.cpp | 6 +- .../src/mock_components/generic_system.cpp | 32 +- hardware_interface/src/resource_manager.cpp | 295 ++++++++++-------- hardware_interface/src/sensor.cpp | 6 +- hardware_interface/src/system.cpp | 6 +- .../mock_components/test_generic_system.cpp | 57 ++-- .../test/test_component_interfaces.cpp | 27 +- .../test/test_resource_manager.cpp | 48 +-- .../test/test_resource_manager.hpp | 15 +- ...esource_manager_prepare_perform_switch.cpp | 10 +- 24 files changed, 514 insertions(+), 239 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4865c1a7a6..f83339edc7 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 bdd30cb515..80e635db7f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -179,6 +179,10 @@ rclcpp::NodeOptions get_cm_node_options() // Required for getting types of controllers to be loaded via service call node_options.allow_undeclared_parameters(true); node_options.automatically_declare_parameters_from_overrides(true); +// \note The versions conditioning is added here to support the source-compatibility until Humble +#if RCLCPP_VERSION_MAJOR >= 21 + node_options.enable_logger_service(true); +#endif return node_options; } @@ -186,8 +190,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>( @@ -198,7 +202,8 @@ ControllerManager::ControllerManager( { if (!get_parameter("update_rate", update_rate_)) { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + RCLCPP_WARN( + get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); } robot_description_ = ""; @@ -229,6 +234,40 @@ ControllerManager::ControllerManager( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); } +ControllerManager::ControllerManager( + std::shared_ptr 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 of %d Hz.", update_rate_); + } + + if (is_resource_manager_initialized()) + { + init_services(); + } + subscribe_to_robot_description_topic(); + + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); +} + ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, @@ -245,7 +284,8 @@ ControllerManager::ControllerManager( { if (!get_parameter("update_rate", update_rate_)) { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + RCLCPP_WARN( + get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); } if (is_resource_manager_initialized()) 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 cd2485e60e..946ffbb4dc 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/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index aab17febbe..5d1773afe8 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -97,6 +97,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 ************ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index b23b913d75..3a1d7a5974 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -44,7 +46,9 @@ class Actuator final ~Actuator() = default; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & initialize(const HardwareInfo & actuator_info); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & actuator_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1ae05aa5f7..88b36d460b 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -73,7 +75,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod public: ActuatorInterface() : lifecycle_state_(rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)) + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + actuator_logger_(rclcpp::get_logger("actuator_interface")) { } @@ -88,15 +91,33 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod virtual ~ActuatorInterface() = default; - /// Initialization of the hardware interface from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + clock_interface_ = clock_interface; + actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; + return on_init(hardware_info); + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + { return CallbackReturn::SUCCESS; }; @@ -209,8 +230,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } protected: + /// Get the logger of the ActuatorInterface. + /** + * \return logger of the ActuatorInterface. + */ + rclcpp::Logger get_logger() const { return actuator_logger_; } + + /// Get the clock of the ActuatorInterface. + /** + * \return clock of the ActuatorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger actuator_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 58710b8563..5313162c29 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, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); /// Constructor for the Resource Manager. /** @@ -67,8 +68,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * used for triggering async components. */ explicit ResourceManager( - const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + const std::string & urdf, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface, + bool activate_all = false, const unsigned int update_rate = 100); ResourceManager(const ResourceManager &) = delete; @@ -461,6 +464,18 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager bool state_interface_exists(const std::string & key) const; protected: + /// Gets the logger for the resource manager + /** + * \return logger of the resource manager + */ + rclcpp::Logger get_logger() const; + + /// Gets the clock for the resource manager + /** + * \return clock of the resource manager + */ + rclcpp::Clock::SharedPtr get_clock() const; + bool components_are_loaded_and_initialized_ = false; mutable std::recursive_mutex resource_interfaces_lock_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4c267bef77..d8e55aa4ad 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -45,7 +47,9 @@ class Sensor final ~Sensor() = default; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & initialize(const HardwareInfo & sensor_info); + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & sensor_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); HARDWARE_INTERFACE_PUBLIC const rclcpp_lifecycle::State & configure(); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 5a3601afa8..79f689d0c6 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -73,7 +75,8 @@ class 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")) { } @@ -88,15 +91,33 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SensorInterface() = default; - /// Initialization of the hardware interface from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + clock_interface_ = clock_interface; + sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; + return on_init(hardware_info); + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + { return CallbackReturn::SUCCESS; }; @@ -148,8 +169,24 @@ 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 sensor_logger_; } + + /// Get the clock of the SensorInterface. + /** + * \return clock of the SensorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger sensor_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index fb28929948..1ca4260750 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -45,7 +47,9 @@ 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::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 6b2c38b915..8e41438ba5 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/logging.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -74,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")) { } @@ -89,15 +92,33 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI virtual ~SystemInterface() = default; - /// Initialization of the hardware interface from data parsed from the robot's URDF. + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. /** * \param[in] hardware_info structure with data from URDF. + * \param[in] clock_interface pointer to the clock interface. + * \param[in] logger_interface pointer to the logger interface. * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. * \returns CallbackReturn::ERROR if any error happens or data are missing. */ - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + clock_interface_ = clock_interface; + system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; + return on_init(hardware_info); + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + virtual CallbackReturn on_init(const HardwareInfo & /*hardware_info*/) + { return CallbackReturn::SUCCESS; }; @@ -210,8 +231,24 @@ 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 system_logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + rclcpp::Logger system_logger_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index b80f76ebf5..7e50c07eb0 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -36,11 +36,13 @@ 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::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(actuator_info)) + switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( 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 f3c0d1d954..a6eadd49fe 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -99,12 +99,23 @@ 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, + 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) + clock_interface_(clock_interface), + rm_logger_(rclcpp::get_logger("resource_manager")) { + 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"); + } } template @@ -115,15 +126,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)); @@ -143,26 +153,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; @@ -171,37 +180,36 @@ 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 { - const rclcpp_lifecycle::State new_state = hardware.initialize(hardware_info); + const rclcpp_lifecycle::State new_state = + hardware.initialize(hardware_info, rm_logger_, clock_interface_); result = new_state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; 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()); } @@ -220,14 +228,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()); } @@ -247,16 +255,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()); @@ -273,16 +281,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()); @@ -316,16 +324,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()); @@ -340,16 +348,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()); @@ -369,14 +377,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()); } @@ -403,14 +411,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()); } @@ -442,14 +450,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()); } @@ -477,14 +485,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()); } @@ -523,8 +531,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; } @@ -543,8 +551,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; } @@ -567,8 +575,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; } @@ -615,15 +623,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()); } @@ -640,15 +648,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()); } @@ -754,9 +762,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; } @@ -787,9 +794,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; } @@ -821,9 +827,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; } @@ -853,9 +858,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()); } }; @@ -882,9 +886,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()); } }; @@ -912,9 +915,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()); } }; @@ -970,11 +972,28 @@ class ResourceStorage return hw_group_state_.at(group_name); } + /// Gets the logger for the resource storage + /** + * \return logger of the resource storage + */ + const rclcpp::Logger & get_logger() const { return rm_logger_; } + + /// Gets the clock for the resource storage + /** + * \return clock of the resource storage + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + // hardware plugins pluginlib::ClassLoader actuator_loader_; 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_; + rclcpp::Logger rm_logger_; + std::vector actuators_; std::vector sensors_; std::vector systems_; @@ -1012,21 +1031,22 @@ 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_; }; 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)) { } 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)) + 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); @@ -1271,9 +1291,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()); } } } @@ -1487,8 +1506,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; @@ -1513,14 +1532,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) @@ -1535,9 +1555,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; @@ -1545,8 +1564,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(), @@ -1555,8 +1574,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(), @@ -1585,7 +1604,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) @@ -1600,16 +1620,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(), @@ -1618,8 +1637,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", @@ -1650,9 +1669,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; } @@ -1770,15 +1788,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; } @@ -1831,15 +1849,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; } @@ -1894,6 +1912,13 @@ 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::SharedPtr ResourceManager::get_clock() const +{ + return resource_storage_->get_clock(); +} + // BEGIN: private methods bool ResourceManager::validate_storage( diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 2da627f892..9b7f1f69d6 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -34,11 +34,13 @@ 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::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(sensor_info)) + 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 8e950faa89..ba327d8ab2 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,11 +34,13 @@ 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::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_init(system_info)) + switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: impl_->set_state(rclcpp_lifecycle::State( 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/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 6dc1c394b0..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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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()); 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(); +}