From 812243c3ed3c41a3b980067d87931766d14490be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 25 Jun 2024 06:08:45 +0200 Subject: [PATCH] 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 0d941be6dbd..78e0dc7ae48 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 36b1a56fc8b..fb3a21369e4 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 7ff2d41f320..646f18cab91 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 d3dda154d6c..22ec0df0067 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 e4c3a17fd82..c262fcaf4e3 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 e6f69b10662..401b0e7c435 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 a00e129772a..7e50c07eb05 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 d47f2e4eef2..7573e698159 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -182,7 +182,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 814a17def77..9b7f1f69d66 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 5cd20af7396..ba327d8ab24 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 ecfd06002eb..42ccdae8fa5 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());