From c2eaa3517cdc54cd3743e2f7b08155a0506f5971 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:54:27 +0100 Subject: [PATCH] Refactor logging (backport #543) (#619) (#638) --- example_1/doc/userdoc.rst | 5 +- .../ros2_control_demo_example_1/rrbot.hpp | 18 +++ example_1/hardware/rrbot.cpp | 73 +++++------ example_10/doc/userdoc.rst | 5 +- .../ros2_control_demo_example_10/rrbot.hpp | 18 ++- example_10/hardware/rrbot.cpp | 80 ++++++------ example_11/doc/userdoc.rst | 5 +- example_11/hardware/carlikebot_system.cpp | 116 +++++++++--------- .../carlikebot_system.hpp | 17 +++ example_12/doc/userdoc.rst | 5 +- .../ros2_control_demo_example_12/rrbot.hpp | 18 +++ example_12/hardware/rrbot.cpp | 72 +++++------ example_14/doc/userdoc.rst | 30 ++--- .../rrbot_actuator_without_feedback.hpp | 18 +++ .../rrbot_sensor_for_position_feedback.hpp | 18 ++- .../rrbot_actuator_without_feedback.cpp | 66 +++++----- .../rrbot_sensor_for_position_feedback.cpp | 84 ++++++------- example_2/doc/userdoc.rst | 5 +- example_2/hardware/diffbot_system.cpp | 70 ++++++----- .../diffbot_system.hpp | 17 +++ ...t_multi_interface_forward_controllers.yaml | 2 +- example_3/doc/userdoc.rst | 10 +- .../rrbot_system_multi_interface.hpp | 17 +++ .../hardware/rrbot_system_multi_interface.cpp | 79 ++++++------ .../config/rrbot_with_sensor_controllers.yaml | 2 +- example_4/doc/userdoc.rst | 5 +- .../rrbot_system_with_sensor.hpp | 18 +++ .../hardware/rrbot_system_with_sensor.cpp | 79 ++++++------ ...rbot_with_external_sensor_controllers.yaml | 2 +- example_5/doc/userdoc.rst | 5 +- .../external_rrbot_force_torque_sensor.cpp | 38 +++--- .../external_rrbot_force_torque_sensor.hpp | 18 +++ .../ros2_control_demo_example_5/rrbot.hpp | 18 +++ example_5/hardware/rrbot.cpp | 72 +++++------ example_6/doc/userdoc.rst | 10 +- .../rrbot_actuator.hpp | 18 +++ example_6/hardware/rrbot_actuator.cpp | 57 +++++---- example_8/doc/userdoc.rst | 12 +- .../ros2_control_demo_example_8/rrbot.hpp | 72 ----------- ...bot_transmissions_system_position_only.hpp | 19 ++- ...bot_transmissions_system_position_only.cpp | 37 +++--- .../ros2_control_demo_example_9/rrbot.hpp | 18 +++ example_9/hardware/rrbot.cpp | 72 +++++------ 43 files changed, 778 insertions(+), 642 deletions(-) delete mode 100644 example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp diff --git a/example_1/doc/userdoc.rst b/example_1/doc/userdoc.rst index 06fbe33d0..65321fbde 100644 --- a/example_1/doc/userdoc.rst +++ b/example_1/doc/userdoc.rst @@ -208,8 +208,9 @@ Tutorial steps .. code-block:: shell - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + [ros2_control_node-1] [INFO] [1721763082.437870177] [controller_manager.resource_manager.hardware_component.system.RRBot]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint2/position' + [ros2_control_node-1] 0.50 for joint 'joint1/position' If you echo the ``/joint_states`` or ``/dynamic_joint_states`` topics you should now get similar values, namely the simulated states of the robot diff --git a/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp index b55a821a6..717b3308a 100644 --- a/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp +++ b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp @@ -23,6 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_commands_; std::vector hw_states_; diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 40b110be2..4d27c0a46 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -49,26 +55,24 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -76,8 +80,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -90,15 +93,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -108,8 +108,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure hw_states_[i] = 0; hw_commands_[i] = 0; } - - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -144,15 +143,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,7 +158,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -171,18 +167,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivat const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -192,17 +185,18 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -212,17 +206,16 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_10/doc/userdoc.rst b/example_10/doc/userdoc.rst index 4ab38dd7c..d7c442977 100644 --- a/example_10/doc/userdoc.rst +++ b/example_10/doc/userdoc.rst @@ -88,8 +88,9 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. .. code-block:: shell - [RRBotSystemWithGPIOHardware]: Got command 0.5 for GP output 0! - [RRBotSystemWithGPIOHardware]: Got command 0.7 for GP output 1! + [ros2_control_node-1] [INFO] [1721765648.271058850] [controller_manager.resource_manager.hardware_component.system.RRBot]: Writing commands: + [ros2_control_node-1] 0.50 for GPIO output '0' + [ros2_control_node-1] 0.70 for GPIO output '1' 7. Let's introspect the ros2_control hardware component. Calling diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp index 11bc5e1eb..68aaf5427 100644 --- a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp +++ b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp @@ -23,6 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -56,8 +58,22 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: - // Parameters for the RRBot simulation + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; // Store the command and state interfaces for the simulated robot std::vector hw_commands_; diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index d8e9bd84a..83cf7f81c 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -16,8 +16,10 @@ #include #include +#include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,6 +36,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot")); + clock_ = std::make_shared(rclcpp::Clock()); hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -44,26 +49,24 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -71,8 +74,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -82,9 +84,8 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( if (info_.gpios.size() != 2) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "RRBotSystemWithGPIOHardware has '%ld' GPIO components, '%d' expected.", info_.gpios.size(), - 2); + get_logger(), "RRBotSystemWithGPIOHardware has '%ld' GPIO components, '%d' expected.", + info_.gpios.size(), 2); return hardware_interface::CallbackReturn::ERROR; } // with exactly 1 command interface @@ -93,8 +94,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( if (info_.gpios[i].command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "GPIO component %s has '%ld' command interfaces, '%d' expected.", + get_logger(), "GPIO component %s has '%ld' command interfaces, '%d' expected.", info_.gpios[i].name.c_str(), info_.gpios[i].command_interfaces.size(), 1); return hardware_interface::CallbackReturn::ERROR; } @@ -103,17 +103,15 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( if (info_.gpios[0].state_interfaces.size() != 3) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "GPIO component %s has '%ld' state interfaces, '%d' expected.", info_.gpios[0].name.c_str(), - info_.gpios[0].state_interfaces.size(), 3); + get_logger(), "GPIO component %s has '%ld' state interfaces, '%d' expected.", + info_.gpios[0].name.c_str(), info_.gpios[0].state_interfaces.size(), 3); return hardware_interface::CallbackReturn::ERROR; } if (info_.gpios[1].state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), - "GPIO component %s has '%ld' state interfaces, '%d' expected.", info_.gpios[0].name.c_str(), - info_.gpios[0].state_interfaces.size(), 1); + get_logger(), "GPIO component %s has '%ld' state interfaces, '%d' expected.", + info_.gpios[0].name.c_str(), info_.gpios[0].state_interfaces.size(), 1); return hardware_interface::CallbackReturn::ERROR; } @@ -124,7 +122,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); // END: This part here is for exemplary purposes - Please do not copy to your production code // reset values always when configuring hardware @@ -133,7 +131,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0); std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0); - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -148,7 +146,7 @@ RRBotSystemWithGPIOHardware::export_state_interfaces() info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:"); + RCLCPP_INFO(get_logger(), "State interfaces:"); hw_gpio_in_.resize(4); size_t ct = 0; for (size_t i = 0; i < info_.gpios.size(); i++) @@ -158,8 +156,7 @@ RRBotSystemWithGPIOHardware::export_state_interfaces() state_interfaces.emplace_back(hardware_interface::StateInterface( info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++])); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), state_if.name.c_str()); + get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), state_if.name.c_str()); } } @@ -175,7 +172,7 @@ RRBotSystemWithGPIOHardware::export_command_interfaces() command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:"); + RCLCPP_INFO(get_logger(), "Command interfaces:"); hw_gpio_out_.resize(2); size_t ct = 0; for (size_t i = 0; i < info_.gpios.size(); i++) @@ -185,8 +182,7 @@ RRBotSystemWithGPIOHardware::export_command_interfaces() command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++])); RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", - info_.gpios.at(i).name.c_str(), command_if.name.c_str()); + get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), command_if.name.c_str()); } } @@ -197,7 +193,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); // END: This part here is for exemplary purposes - Please do not copy to your production code // command and state should be equal when starting @@ -206,7 +202,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -215,7 +211,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -225,7 +221,8 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_states_.size(); i++) { @@ -244,11 +241,10 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read( for (uint i = 0; i < hw_gpio_in_.size(); i++) { - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!", - hw_gpio_in_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_gpio_in_[i] << " from GPIO input '" << static_cast(i) << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -258,15 +254,15 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_gpio_out_.size(); i++) { - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!", - hw_gpio_out_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_gpio_out_[i] << " for GPIO output '" << static_cast(i) << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index 6e8eec1fd..f661dd6a0 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -111,8 +111,9 @@ Tutorial steps .. code-block:: shell - [CarlikeBotSystemHardware]: Got position command: 0.03 for joint 'virtual_front_wheel_joint'. - [CarlikeBotSystemHardware]: Got velocity command: 20.01 for joint 'virtual_rear_wheel_joint'. + [ros2_control_node-1] [INFO] [1721766165.108212153] [controller_manager.resource_manager.hardware_component.system.CarlikeBot]: Writing commands: + [ros2_control_node-1] position: 0.03 for joint 'virtual_front_wheel_joint' + [ros2_control_node-1] velocity: 20.00 for joint 'virtual_rear_wheel_joint' Files used for this demos -------------------------- diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index b91abc036..1bc1d2c96 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -17,8 +17,10 @@ #include #include #include +#include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -35,12 +37,15 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.CarlikeBot")); + clock_ = std::make_shared(rclcpp::Clock()); // Check if the number of joints is correct based on the mode of operation if (info_.joints.size() != 2) { RCLCPP_ERROR( - rclcpp::get_logger("CarlikeBotSystemHardware"), + get_logger(), "CarlikeBotSystemHardware::on_init() - Failed to initialize, " "because the number of joints %ld is not 2.", info_.joints.size()); @@ -54,24 +59,20 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( // Steering joints have a position command interface and a position state interface if (joint_is_steering) { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a steering joint.", - joint.name.c_str()); + RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str()); if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -79,8 +80,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -88,33 +88,28 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } } else { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a drive joint.", - joint.name.c_str()); + RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str()); // Drive joints have a velocity command interface and a velocity state interface if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %s command interface. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } @@ -122,8 +117,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint.state_interfaces.size() != 2) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -131,8 +125,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } @@ -140,8 +133,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( if (joint.state_interfaces[1].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -177,15 +169,11 @@ std::vector CarlikeBotSystemHardware::export } } - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu state interfaces.", - state_interfaces.size()); + RCLCPP_INFO(get_logger(), "Exported %zu state interfaces.", state_interfaces.size()); for (auto s : state_interfaces) { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported state interface '%s'.", - s.get_name().c_str()); + RCLCPP_INFO(get_logger(), "Exported state interface '%s'.", s.get_name().c_str()); } return state_interfaces; @@ -212,15 +200,12 @@ CarlikeBotSystemHardware::export_command_interfaces() } } - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu command interfaces.", - command_interfaces.size()); + RCLCPP_INFO(get_logger(), "Exported %zu command interfaces.", command_interfaces.size()); for (auto i = 0u; i < command_interfaces.size(); i++) { RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported command interface '%s'.", - command_interfaces[i].get_name().c_str()); + get_logger(), "Exported command interface '%s'.", command_interfaces[i].get_name().c_str()); } return command_interfaces; @@ -229,13 +214,12 @@ CarlikeBotSystemHardware::export_command_interfaces() hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (auto i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } for (auto & joint : hw_interfaces_) @@ -254,7 +238,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate( } } - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -263,16 +247,15 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (auto i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -288,13 +271,21 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( hw_interfaces_["traction"].state.position += hw_interfaces_["traction"].state.velocity * period.seconds(); - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position state: %.2f for joint '%s'.", - hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); + std::stringstream ss; + ss << "Reading states:"; + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" + << "position: " << hw_interfaces_["steering"].state.position << " for joint '" + << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "\t" + << "position: " << hw_interfaces_["traction"].state.position << " for joint '" + << hw_interfaces_["traction"].joint_name.c_str() << "'" << std::endl + << "\t" + << "velocity: " << hw_interfaces_["traction"].state.velocity << " for joint '" + << hw_interfaces_["traction"].joint_name.c_str() << "'"; - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity state: %.2f for joint '%s'.", - hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -305,15 +296,18 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position command: %.2f for joint '%s'.", - hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); - - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity command: %.2f for joint '%s'.", - hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); - + std::stringstream ss; + ss << "Writing commands:"; + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" + << "position: " << hw_interfaces_["steering"].command.position << " for joint '" + << hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl + << "\t" + << "velocity: " << hw_interfaces_["traction"].command.velocity << " for joint '" + << hw_interfaces_["traction"].joint_name.c_str() << "'"; + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp index 44ab8a1f5..e833ffceb 100644 --- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp +++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -79,11 +80,27 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the CarlikeBot simulation double hw_start_sec_; double hw_stop_sec_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // std::vector> // hw_interfaces_; // name of joint, state, command std::map hw_interfaces_; diff --git a/example_12/doc/userdoc.rst b/example_12/doc/userdoc.rst index 75abec7dd..d4e979219 100644 --- a/example_12/doc/userdoc.rst +++ b/example_12/doc/userdoc.rst @@ -146,8 +146,9 @@ Tutorial steps .. code-block:: shell - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + [ros2_control_node-1] [INFO] [1721766407.439574931] [controller_manager.resource_manager.hardware_component.system.RRBot]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint1' + [ros2_control_node-1] 0.50 for joint 'joint2' If you echo the ``/joint_states`` or ``/dynamic_joint_states`` topics you should now get similar values, namely the simulated states of the robot diff --git a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp index ac116044e..3e7632537 100644 --- a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp +++ b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp @@ -23,6 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_commands_; std::vector hw_states_; diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp index a7d3a66cc..5b12fdd4a 100644 --- a/example_12/hardware/rrbot.cpp +++ b/example_12/hardware/rrbot.cpp @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -49,26 +55,24 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -76,8 +80,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -90,15 +93,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -109,7 +109,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure hw_commands_[i] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -144,15 +144,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,7 +159,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -171,18 +168,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivat const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -192,17 +186,18 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -212,17 +207,16 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_14/doc/userdoc.rst b/example_14/doc/userdoc.rst index 1ebfd57f4..d9405b082 100644 --- a/example_14/doc/userdoc.rst +++ b/example_14/doc/userdoc.rst @@ -124,20 +124,22 @@ Tutorial steps .. code-block:: shell - [RRBotActuatorWithoutFeedback]: Writing command: 5.000000 - [RRBotActuatorWithoutFeedback]: Sending data command: 5 - [RRBotActuatorWithoutFeedback]: Joints successfully written! - [RRBotActuatorWithoutFeedback]: Writing command: 5.000000 - [RRBotActuatorWithoutFeedback]: Sending data command: 5 - [RRBotActuatorWithoutFeedback]: Joints successfully written! - [RRBotSensorPositionFeedback]: Reading... - [RRBotSensorPositionFeedback]: Got measured velocity 5.00000 - [RRBotSensorPositionFeedback]: Got state 0.25300 for joint 'joint1'! - [RRBotSensorPositionFeedback]: Joints successfully read! - [RRBotSensorPositionFeedback]: Reading... - [RRBotSensorPositionFeedback]: Got measured velocity 5.00000 - [RRBotSensorPositionFeedback]: Got state 0.25300 for joint 'joint2'! - [RRBotSensorPositionFeedback]: Joints successfully read! + [ros2_control_node-1] [INFO] [1728858168.276013464] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing... + [ros2_control_node-1] Writing command: 5.00 for joint 'joint1' + [ros2_control_node-1] Sending data command: 5 + [ros2_control_node-1] + [ros2_control_node-1] [INFO] [1728858168.776052116] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing... + [ros2_control_node-1] Writing command: 5.00 for joint 'joint2' + [ros2_control_node-1] Sending data command: 5 + [ros2_control_node-1] + [ros2_control_node-1] [INFO] [1728858169.275878132] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint]: Reading... + [ros2_control_node-1] Got measured velocity 5.00 + [ros2_control_node-1] Got state 0.34 for joint 'joint1' + [ros2_control_node-1] + [ros2_control_node-1] [INFO] [1728858169.775863217] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint]: Reading... + [ros2_control_node-1] Got measured velocity 5.00 + [ros2_control_node-1] Got state 0.29 for joint 'joint2' + [ros2_control_node-1] Files used for this demos diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 837d3aa2e..8bec697f9 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -29,6 +29,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" namespace ros2_control_demo_example_14 @@ -60,11 +62,27 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the ActuatorInterface. + /** + * \return logger of the ActuatorInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the ActuatorInterface. + /** + * \return clock of the ActuatorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot double hw_joint_command_; diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 5644d714d..9716755d2 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -31,6 +31,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "realtime_tools/realtime_buffer.h" @@ -62,19 +63,34 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SensorInterface. + /** + * \return logger of the SensorInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SensorInterface. + /** + * \return clock of the SensorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot double measured_velocity; // Local variable, but avoid initialization on each read double last_measured_velocity_; double hw_joint_state_; // Timestamps to calculate position for velocity - rclcpp::Clock clock_; rclcpp::Time last_timestamp_; rclcpp::Time current_timestamp; // Local variable, but avoid initialization on each read diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 04d18af3f..7bb98833d 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -23,8 +23,10 @@ #include #include #include +#include #include #include +#include #include #include "hardware_interface/actuator_interface.hpp" @@ -43,6 +45,10 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint")); + clock_ = std::make_shared(rclcpp::Clock()); + // START: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -58,8 +64,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -67,9 +72,9 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } @@ -78,7 +83,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( sock_ = socket(AF_INET, SOCK_STREAM, 0); if (sock_ < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Creating socket failed."); + RCLCPP_FATAL(get_logger(), "Creating socket failed."); return hardware_interface::CallbackReturn::ERROR; } @@ -93,9 +98,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( const int max_retries = 5; const int initial_delay_ms = 1000; // Initial delay of 1 second - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Trying to connect to port %d.", - socket_port_); + RCLCPP_INFO(get_logger(), "Trying to connect to port %d.", socket_port_); int retries = 0; int delay_ms = initial_delay_ms; @@ -110,9 +113,8 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( } RCLCPP_WARN( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), - "Connection attempt %d failed: %s. Retrying in %d ms...", retries + 1, strerror(errno), - delay_ms); + get_logger(), "Connection attempt %d failed: %s. Retrying in %d ms...", retries + 1, + strerror(errno), delay_ms); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); delay_ms *= 2; // Exponential backoff @@ -122,15 +124,13 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( if (!connected) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), - "Connection over socket failed after %d attempts: %s", retries, strerror(errno)); + get_logger(), "Connection over socket failed after %d attempts: %s", retries, + strerror(errno)); return hardware_interface::CallbackReturn::ERROR; } else { - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully connected to port %d.", - socket_port_); + RCLCPP_INFO(get_logger(), "Successfully connected to port %d.", socket_port_); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -167,14 +167,12 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -184,7 +182,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( hw_joint_command_ = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -193,16 +191,15 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "%.1f seconds left...", hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -220,18 +217,19 @@ hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWitho if (std::isfinite(hw_joint_command_)) { // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Writing command: %f", hw_joint_command_); + std::stringstream ss; + ss << "Writing..." << std::endl; + ss << std::fixed << std::setprecision(2); + ss << "Writing command: " << hw_joint_command_ << " for joint '" << info_.joints[0].name << "'" + << std::endl; - // Simulate sending commands to the hardware std::ostringstream data; data << hw_joint_command_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Sending data command: %s", - data.str().c_str()); - send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); + ss << "Sending data command: " << data.str() << std::endl; + RCLCPP_INFO(get_logger(), ss.str().c_str()); - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Joints successfully written!"); + // Simulate sending commands to the hardware + send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); // END: This part here is for exemplary purposes - Please do not copy to your production code } return hardware_interface::return_type::OK; diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 96e7ee866..457e3eaf5 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -23,8 +23,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -44,6 +46,11 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.sensor." + "RRBotModularPositionSensorJoint")); + clock_ = std::make_shared(rclcpp::Clock()); + // START: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -60,8 +67,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSensorPositionFeedback"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -69,27 +75,24 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSensorPositionFeedback"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } - clock_ = rclcpp::Clock(); - // START: This part here is for exemplary purposes - Please do not copy to your production code // Initialize objects for fake mechanical connection obj_socket_ = socket(AF_INET, SOCK_STREAM, 0); if (obj_socket_ < 0) { - RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Creating socket failed."); + RCLCPP_FATAL(get_logger(), "Creating socket failed."); return hardware_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Setting socket options."); + RCLCPP_INFO(get_logger(), "Setting socket options."); if (setsockopt(obj_socket_, SOL_SOCKET, SO_REUSEADDR, &sockoptval_, sizeof(sockoptval_))) { - RCLCPP_FATAL(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Setting socket failed."); + RCLCPP_FATAL(get_logger(), "Setting socket failed."); return hardware_interface::CallbackReturn::ERROR; } @@ -99,11 +102,11 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( address_.sin_addr.s_addr = INADDR_ANY; address_.sin_port = htons(socket_port_); - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket address."); + RCLCPP_INFO(get_logger(), "Binding to socket address."); if (bind(obj_socket_, reinterpret_cast(&address_), sizeof(address_)) < 0) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Binding to socket failed: %s", + get_logger(), "Binding to socket failed: %s", strerror(errno)); // Print the error message return hardware_interface::CallbackReturn::ERROR; } @@ -114,13 +117,10 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( [this]() { // Await and accept connection - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Listening for connection on port %d.", - socket_port_); + RCLCPP_INFO(get_logger(), "Listening for connection on port %d.", socket_port_); if (listen(obj_socket_, 1) < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Cannot listen from the server."); + RCLCPP_FATAL(get_logger(), "Cannot listen from the server."); return hardware_interface::CallbackReturn::ERROR; } @@ -129,15 +129,14 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( reinterpret_cast(&address_length_)); if (sock_ < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Cannot accept on the server."); + RCLCPP_FATAL(get_logger(), "Cannot accept on the server."); return hardware_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Accepting on socket."); + RCLCPP_INFO(get_logger(), "Accepting on socket."); int incoming_data_read_rate = 1000; // Hz RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), + get_logger(), "Creating thread for incoming data and read them with %d Hz to not miss any data.", incoming_data_read_rate); @@ -148,22 +147,18 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( // Use nanoseconds to avoid chrono's rounding std::this_thread::sleep_for(std::chrono::nanoseconds(1000000000 / incoming_data_read_rate)); - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Receiving data"); + RCLCPP_INFO(get_logger(), "Receiving data"); while (rclcpp::ok()) { if (recv(sock_, buffer, reading_size_bytes, 0) > 0) { - RCLCPP_DEBUG( - rclcpp::get_logger("RRBotSensorPositionFeedback"), - "Read form buffer sockets data: '%s'", buffer); + RCLCPP_DEBUG(get_logger(), "Read form buffer sockets data: '%s'", buffer); rt_incomming_data_ptr_.writeFromNonRT(hardware_interface::stod(buffer)); } else { - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), - "Data not yet received from socket."); + RCLCPP_INFO(get_logger(), "Data not yet received from socket."); rt_incomming_data_ptr_.writeFromNonRT(std::numeric_limits::quiet_NaN()); } @@ -209,9 +204,9 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( last_measured_velocity_ = 0; // In general after a hardware is configured it can be read - last_timestamp_ = clock_.now(); + last_timestamp_ = get_clock()->now(); - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Configuration successful."); + RCLCPP_INFO(get_logger(), "Configuration successful."); return hardware_interface::CallbackReturn::SUCCESS; } @@ -219,17 +214,16 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "%.1f seconds left...", hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -238,16 +232,15 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "%.1f seconds left...", hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -256,12 +249,13 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_deactivate( hardware_interface::return_type RRBotSensorPositionFeedback::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - current_timestamp = clock_.now(); + current_timestamp = get_clock()->now(); rclcpp::Duration duration = current_timestamp - last_timestamp_; last_timestamp_ = current_timestamp; // START: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Reading..."); + std::stringstream ss; + ss << "Reading..." << std::endl; // Simulate RRBot's movement measured_velocity = *(rt_incomming_data_ptr_.readFromRT()); @@ -269,15 +263,13 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( { last_measured_velocity_ = measured_velocity; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got measured velocity %.5f", - measured_velocity); hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got state %.5f for joint '%s'!", - hw_joint_state_, info_.joints[0].name.c_str()); - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Joints successfully read!"); + ss << std::fixed << std::setprecision(2); + ss << "Got measured velocity " << measured_velocity << std::endl; + ss << "Got state " << hw_joint_state_ << " for joint '" << info_.joints[0].name << "'" + << std::endl; + RCLCPP_INFO(get_logger(), ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_2/doc/userdoc.rst b/example_2/doc/userdoc.rst index 55de949e0..1e1536a04 100644 --- a/example_2/doc/userdoc.rst +++ b/example_2/doc/userdoc.rst @@ -104,8 +104,9 @@ Tutorial steps .. code-block:: shell - [DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! - [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'! + [ros2_control_node-1] [INFO] [1721762311.808415917] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 43.33 for 'left_wheel_joint'! + [ros2_control_node-1] command 50.00 for 'right_wheel_joint'! 6. Let's introspect the ros2_control hardware component. Calling diff --git a/example_2/hardware/diffbot_system.cpp b/example_2/hardware/diffbot_system.cpp index 177b3a4f5..d957088a6 100644 --- a/example_2/hardware/diffbot_system.cpp +++ b/example_2/hardware/diffbot_system.cpp @@ -17,8 +17,10 @@ #include #include #include +#include #include #include +#include #include #include "hardware_interface/lexical_casts.hpp" @@ -36,6 +38,9 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.DiffBot")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = @@ -53,26 +58,24 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 2) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), - "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -80,18 +83,18 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), - "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { RCLCPP_FATAL( - rclcpp::get_logger("DiffBotSystemHardware"), - "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); + get_logger(), "Joint '%s' have '%s' as second state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[1].name.c_str(), + hardware_interface::HW_IF_VELOCITY); return hardware_interface::CallbackReturn::ERROR; } } @@ -129,13 +132,12 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (auto i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -150,7 +152,7 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( } } - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -159,17 +161,16 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (auto i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -178,6 +179,8 @@ hardware_interface::return_type DiffBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Reading states:"; for (std::size_t i = 0; i < hw_velocities_.size(); i++) { // Simulate DiffBot wheels's movement as a first-order system @@ -185,11 +188,13 @@ hardware_interface::return_type DiffBotSystemHardware::read( // Simply integrates hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i]; - RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), - "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], - hw_velocities_[i], info_.joints[i].name.c_str()); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" + "position " + << hw_positions_[i] << " and velocity " << hw_velocities_[i] << " for '" + << info_.joints[i].name.c_str() << "'!"; } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -199,18 +204,17 @@ hardware_interface::return_type ros2_control_demo_example_2 ::DiffBotSystemHardw const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing..."); - + std::stringstream ss; + ss << "Writing commands:"; for (auto i = 0u; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("DiffBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i], - info_.joints[i].name.c_str()); - hw_velocities_[i] = hw_commands_[i]; + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << "command " << hw_commands_[i] << " for '" << info_.joints[i].name.c_str() << "'!"; } - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp index 1e383abd9..dfa9c898d 100644 --- a/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp +++ b/example_2/hardware/include/ros2_control_demo_example_2/diffbot_system.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -56,11 +57,27 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the DiffBot simulation double hw_start_sec_; double hw_stop_sec_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_commands_; std::vector hw_positions_; diff --git a/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml b/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml index 6bffd2754..0b832d944 100644 --- a/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml +++ b/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 10 # Hz + update_rate: 100 # Hz forward_position_controller: type: position_controllers/JointGroupPositionController diff --git a/example_3/doc/userdoc.rst b/example_3/doc/userdoc.rst index 65a4745d0..ac3625669 100644 --- a/example_3/doc/userdoc.rst +++ b/example_3/doc/userdoc.rst @@ -135,10 +135,12 @@ Tutorial steps .. code-block:: shell - [RRBotSystemMultiInterfaceHardware]: Got the commands pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 0, control_lvl:1 - [RRBotSystemMultiInterfaceHardware]: Got the commands pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 1, control_lvl:1 - [RRBotSystemMultiInterfaceHardware]: Got pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 0! - [RRBotSystemMultiInterfaceHardware]: Got pos: 0.78500, vel: 0.00000, acc: 0.00000 for joint 1! + [ros2_control_node-1] [INFO] [1728857332.160329225] [controller_manager.resource_manager.hardware_component.system.RRBotSystemMultiInterface]: Writing commands: + [ros2_control_node-1] command pos: 0.00, vel: 5.00, acc: 0.00 for joint 0, control lvl: 2 + [ros2_control_node-1] command pos: 0.00, vel: 5.00, acc: 0.00 for joint 1, control lvl: 2 + [ros2_control_node-1] [INFO] [1728857332.320242591] [controller_manager.resource_manager.hardware_component.system.RRBotSystemMultiInterface]: Reading states: + [ros2_control_node-1] pos: 0.67, vel: 5.00, acc: 0.00 for joint 0 + [ros2_control_node-1] pos: 0.67, vel: 5.00, acc: 0.00 for joint 1 6. Now you can also switch controllers during runtime, which also changes the command mode automatically. First, you have to load the new controller, for example the ``forward_position_controller`` if you haven't changed the launch file argument. diff --git a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp index 7469de7c0..4af2d0a38 100644 --- a/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp +++ b/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -62,12 +63,28 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the commands for the simulated robot std::vector hw_commands_positions_; std::vector hw_commands_velocities_; diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index ee6f4d37b..b8f1cf292 100644 --- a/example_3/hardware/rrbot_system_multi_interface.cpp +++ b/example_3/hardware/rrbot_system_multi_interface.cpp @@ -16,8 +16,10 @@ #include #include +#include #include #include +#include #include #include @@ -35,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.system.RRBotSystemMultiInterface")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -56,8 +61,7 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( if (joint.command_interfaces.size() != 3) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Joint '%s' has %zu command interfaces. 3 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu command interfaces. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -67,18 +71,17 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( joint.command_interfaces[0].name == hardware_interface::HW_IF_ACCELERATION)) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + get_logger(), "Joint '%s' has %s command interface. Expected %s, %s, or %s.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Joint '%s'has %zu state interfaces. 3 expected.", joint.name.c_str(), + get_logger(), "Joint '%s'has %zu state interfaces. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -88,10 +91,10 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( joint.state_interfaces[0].name == hardware_interface::HW_IF_ACCELERATION)) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + get_logger(), "Joint '%s' has %s state interface. Expected %s, %s, or %s.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, + hardware_interface::HW_IF_ACCELERATION); return hardware_interface::CallbackReturn::ERROR; } } @@ -201,15 +204,12 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Activating... please wait..."); + RCLCPP_INFO(get_logger(), "Activating... please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -243,9 +243,7 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat control_level_[i] = integration_level_t::UNDEFINED; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully activated! %u", - control_level_[0]); + RCLCPP_INFO(get_logger(), "System successfully activated! %u", control_level_[0]); return hardware_interface::CallbackReturn::SUCCESS; } @@ -253,18 +251,15 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactiv const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Deactivating... please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating... please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -273,14 +268,15 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactiv hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Reading states:"; for (std::size_t i = 0; i < hw_states_positions_.size(); i++) { switch (control_level_[i]) { case integration_level_t::UNDEFINED: - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Nothing is using the hardware interface!"); + RCLCPP_INFO(get_logger(), "Nothing is using the hardware interface!"); return hardware_interface::return_type::OK; break; case integration_level_t::POSITION: @@ -300,13 +296,13 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; break; } - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %lu!", hw_states_positions_[i], - hw_states_velocities_[i], hw_states_accelerations_[i], i); - // END: This part here is for exemplary purposes - Please do not copy to your production code + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" + << "pos: " << hw_states_positions_[i] << ", vel: " << hw_states_velocities_[i] + << ", acc: " << hw_states_accelerations_[i] << " for joint " << i; } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; } @@ -314,15 +310,18 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Writing commands:"; for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %lu, control_lvl:%u", - hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i, - control_level_[i]); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" + << "command pos: " << hw_commands_positions_[i] << ", vel: " << hw_commands_velocities_[i] + << ", acc: " << hw_commands_accelerations_[i] << " for joint " << i + << ", control lvl: " << static_cast(control_level_[i]); } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_4/bringup/config/rrbot_with_sensor_controllers.yaml b/example_4/bringup/config/rrbot_with_sensor_controllers.yaml index bbd2c5d8e..c086d4e40 100644 --- a/example_4/bringup/config/rrbot_with_sensor_controllers.yaml +++ b/example_4/bringup/config/rrbot_with_sensor_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 10 # Hz + update_rate: 100 # Hz forward_position_controller: type: forward_command_controller/ForwardCommandController diff --git a/example_4/doc/userdoc.rst b/example_4/doc/userdoc.rst index efe61c051..92cf35ee5 100644 --- a/example_4/doc/userdoc.rst +++ b/example_4/doc/userdoc.rst @@ -123,8 +123,9 @@ Tutorial steps .. code-block:: shell - [RRBotSystemWithSensorHardware]: Got command 0.50000 for joint 0! - [RRBotSystemWithSensorHardware]: Got command 0.50000 for joint 1! + [ros2_control_node-1] [INFO] [1721763738.761847562] [controller_manager.resource_manager.hardware_component.system.RRBotSystemWithSensor]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint1' + [ros2_control_node-1] 0.50 for joint 'joint2' 6. Access wrench data from 2D FTS via diff --git a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp index e166c9906..6730addd8 100644 --- a/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp +++ b/example_4/hardware/include/ros2_control_demo_example_4/rrbot_system_with_sensor.hpp @@ -27,6 +27,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -59,6 +61,18 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; @@ -66,6 +80,10 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface double hw_slowdown_; double hw_sensor_change_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_joint_commands_; std::vector hw_joint_states_; diff --git a/example_4/hardware/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index eea370e1f..14c9f0807 100644 --- a/example_4/hardware/rrbot_system_with_sensor.cpp +++ b/example_4/hardware/rrbot_system_with_sensor.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -38,6 +40,9 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.system.RRBotSystemWithSensor")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -57,26 +62,24 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -84,8 +87,7 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -98,14 +100,12 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -116,7 +116,7 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_configure( hw_joint_commands_[i] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -158,14 +158,12 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -181,7 +179,7 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( hw_sensor_states_[0] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -190,18 +188,15 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -211,29 +206,33 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Reading...please wait..."); + std::stringstream ss; + ss << "Reading states from joints:"; for (uint i = 0; i < hw_joint_states_.size(); i++) { // Simulate RRBot's movement hw_joint_states_[i] += (hw_joint_commands_[i] - hw_joint_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %u!", - hw_joint_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_joint_states_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + ss.str(""); + ss << "Reading states from sensors:"; for (uint i = 0; i < hw_sensor_states_.size(); i++) { // Simulate RRBot's sensor data unsigned int seed = time(NULL) + i; hw_sensor_states_[i] = static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got value %e for interface %s!", - hw_sensor_states_[i], info_.sensors[0].state_interfaces[i].name.c_str()); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_sensor_states_[i] << " for sensor '" + << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Sensors successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -243,16 +242,16 @@ hardware_interface::return_type ros2_control_demo_example_4::RRBotSystemWithSens const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Writing...please wait..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_joint_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got command %.5f for joint %u!", - hw_joint_commands_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_joint_commands_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml b/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml index 5b650ea7e..c7759ea67 100644 --- a/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 10 # Hz + update_rate: 100 # Hz forward_position_controller: type: forward_command_controller/ForwardCommandController diff --git a/example_5/doc/userdoc.rst b/example_5/doc/userdoc.rst index 69f7f9f6c..a44cb3711 100644 --- a/example_5/doc/userdoc.rst +++ b/example_5/doc/userdoc.rst @@ -127,8 +127,9 @@ Tutorial steps .. code-block:: shell - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! - [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + [ros2_control_node-1] [INFO] [1721764191.201301188] [controller_manager.resource_manager.hardware_component.system.RRBotSystemPositionOnly]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint1' + [ros2_control_node-1] 0.50 for joint 'joint2' 6. Access wrench data from 2D FTS via diff --git a/example_5/hardware/external_rrbot_force_torque_sensor.cpp b/example_5/hardware/external_rrbot_force_torque_sensor.cpp index 3cd56adfc..4db2ffc58 100644 --- a/example_5/hardware/external_rrbot_force_torque_sensor.cpp +++ b/example_5/hardware/external_rrbot_force_torque_sensor.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -38,6 +40,9 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_in { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.sensor.ExternalRRBotFTSensor")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -70,19 +75,15 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_ac const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -92,19 +93,15 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_de const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -114,7 +111,8 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_sensor_states_.size(); i++) { @@ -122,12 +120,12 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read( unsigned int seed = time(NULL) + i; hw_sensor_states_[i] = static_cast(rand_r(&seed)) / (static_cast(RAND_MAX / hw_sensor_change_)); - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %u!", - hw_sensor_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_sensor_states_[i] << " for sensor '" + << info_.sensors[0].state_interfaces[i].name.c_str() << "'"; } - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp index 3f0a3a941..c9ca307ed 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/external_rrbot_force_torque_sensor.hpp @@ -27,6 +27,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" namespace ros2_control_demo_example_5 @@ -50,12 +52,28 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SensorInterface. + /** + * \return logger of the SensorInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SensorInterface. + /** + * \return clock of the SensorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_sensor_change_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the sensor states for the simulated robot std::vector hw_sensor_states_; }; diff --git a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp index 3fdac8de1..a4bfa5f04 100644 --- a/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp +++ b/example_5/hardware/include/ros2_control_demo_example_5/rrbot.hpp @@ -23,6 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_commands_; std::vector hw_states_; diff --git a/example_5/hardware/rrbot.cpp b/example_5/hardware/rrbot.cpp index 35fe1c0f2..754e02892 100644 --- a/example_5/hardware/rrbot.cpp +++ b/example_5/hardware/rrbot.cpp @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.system.RRBotSystemPositionOnly")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -49,26 +55,24 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -76,8 +80,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -90,15 +93,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -109,7 +109,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure hw_commands_[i] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -144,15 +144,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,7 +159,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -171,18 +168,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivat const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -192,17 +186,18 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -212,17 +207,16 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_6/doc/userdoc.rst b/example_6/doc/userdoc.rst index 357cb18cd..7c074bf6b 100644 --- a/example_6/doc/userdoc.rst +++ b/example_6/doc/userdoc.rst @@ -119,12 +119,10 @@ Tutorial steps .. code-block:: shell - [RRBotModularJoint]: Writing...please wait... - [RRBotModularJoint]: Got command 0.50000 for joint 'joint1'! - [RRBotModularJoint]: Joints successfully written! - [RRBotModularJoint]: Writing...please wait... - [RRBotModularJoint]: Got command 0.50000 for joint 'joint2'! - [RRBotModularJoint]: Joints successfully written! + [ros2_control_node-1] [INFO] [1721764663.304187517] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint1' + [ros2_control_node-1] [INFO] [1721764663.304196897] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint2' Files used for this demos diff --git a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp index 4874bd47d..a502b5b1e 100644 --- a/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp +++ b/example_6/hardware/include/ros2_control_demo_example_6/rrbot_actuator.hpp @@ -28,6 +28,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" namespace ros2_control_demo_example_6 @@ -56,12 +58,28 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the ActuatorInterface. + /** + * \return logger of the ActuatorInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the ActuatorInterface. + /** + * \return clock of the ActuatorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot double hw_joint_command_; double hw_joint_state_; diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index b48e6c0d7..b1683ba62 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include #include #include "hardware_interface/actuator_interface.hpp" @@ -39,6 +41,10 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared(rclcpp::get_logger( + "controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint")); + clock_ = std::make_shared(rclcpp::Clock()); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); @@ -53,8 +59,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotModularJoint"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -62,26 +67,25 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_init( if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotModularJoint"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotModularJoint"), "Joint '%s' has %zu state interface. 1 expected.", - joint.name.c_str(), joint.state_interfaces.size()); + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotModularJoint"), "Joint '%s' have %s state interface. '%s' expected.", - joint.name.c_str(), joint.state_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -112,12 +116,12 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "%.1f seconds left...", hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -128,7 +132,7 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_activate( hw_joint_command_ = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -137,15 +141,15 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "%.1f seconds left...", hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -155,15 +159,16 @@ hardware_interface::return_type RRBotModularJoint::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; // Simulate RRBot's movement hw_joint_state_ = hw_joint_state_ + (hw_joint_command_ - hw_joint_state_) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotModularJoint"), "Got state %.5f for joint '%s'!", hw_joint_state_, - info_.joints[0].name.c_str()); - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Joints successfully read!"); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_joint_state_ << " for joint '" << info_.joints[0].name << "'"; + + RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -173,14 +178,14 @@ hardware_interface::return_type ros2_control_demo_example_6::RRBotModularJoint:: const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Writing...please wait..."); + std::stringstream ss; + ss << "Writing commands:"; // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotModularJoint"), "Got command %.5f for joint '%s'!", hw_joint_command_, - info_.joints[0].name.c_str()); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_joint_command_ << " for joint '" << info_.joints[0].name << "'"; - RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Joints successfully written!"); + RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_8/doc/userdoc.rst b/example_8/doc/userdoc.rst index 13dbb0408..75f439004 100644 --- a/example_8/doc/userdoc.rst +++ b/example_8/doc/userdoc.rst @@ -92,12 +92,12 @@ Tutorial steps .. code-block:: shell - [RRBotTransmissionsSystemPositionOnlyHardware]: Command data: - joint1: 0.5 --> transmission1(R=2) --> actuator1: 1 - joint2: 0.5 --> transmission2(R=4) --> actuator2: 2 - [RRBotTransmissionsSystemPositionOnlyHardware]: State data: - joint1: 0.383253 <-- transmission1(R=2) <-- actuator1: 0.766505 - joint2: 0.383253 <-- transmission2(R=4) <-- actuator2: 1.53301 + [ros2_control_node-1] [INFO] [1728857106.562714002] [controller_manager.resource_manager.hardware_component.system.RRBotTransmissionsSystemPositionOnly]: Command data: + [ros2_control_node-1] joint1: 0.5 --> transmission1(R=2) --> actuator1: 1 + [ros2_control_node-1] joint2: 0.5 --> transmission2(R=4) --> actuator2: 2 + [ros2_control_node-1] [INFO] [1728857106.762624114] [controller_manager.resource_manager.hardware_component.system.RRBotTransmissionsSystemPositionOnly]: State data: + [ros2_control_node-1] joint1: 0.166196 <-- transmission1(R=2) <-- actuator1: 0.332392 + [ros2_control_node-1] joint2: 0.166196 <-- transmission2(R=4) <-- actuator2: 0.664784 Files used for this demos diff --git a/example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp b/example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp deleted file mode 100644 index 5c96b9bc7..000000000 --- a/example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 2020 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROS2_CONTROL_DEMO_EXAMPLE_8__RRBOT_HPP_ -#define ROS2_CONTROL_DEMO_EXAMPLE_8__RRBOT_HPP_ - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace ros2_control_demo_example_8 -{ -class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); - - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - -private: - // Parameters for the RRBot simulation - double hw_start_sec_; - double hw_stop_sec_; - double hw_slowdown_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_states_; -}; - -} // namespace ros2_control_demo_example_8 - -#endif // ROS2_CONTROL_DEMO_EXAMPLE_8__RRBOT_HPP_ diff --git a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp index 453e2a146..d09a3ddf9 100644 --- a/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp +++ b/example_8/hardware/include/ros2_control_demo_example_8/rrbot_transmissions_system_position_only.hpp @@ -57,10 +57,19 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; -private: - std::unique_ptr logger_; - std::unique_ptr clock_; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } +private: // parameters for the RRBot simulation double actuator_slowdown_; @@ -80,6 +89,10 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: }; std::vector joint_interfaces_; std::vector actuator_interfaces_; + + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace ros2_control_demo_example_8 diff --git a/example_8/hardware/rrbot_transmissions_system_position_only.cpp b/example_8/hardware/rrbot_transmissions_system_position_only.cpp index 6cf3b2428..0f95137c3 100644 --- a/example_8/hardware/rrbot_transmissions_system_position_only.cpp +++ b/example_8/hardware/rrbot_transmissions_system_position_only.cpp @@ -24,7 +24,6 @@ #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/logging.hpp" #include "transmission_interface/simple_transmission_loader.hpp" #include "transmission_interface/transmission.hpp" @@ -38,12 +37,12 @@ constexpr double kNaN = std::numeric_limits::quiet_NaN(); hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { - logger_ = std::make_unique( - rclcpp::get_logger("RRBotTransmissionsSystemPositionOnlyHardware")); + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system." + "RRBotTransmissionsSystemPositionOnly")); + clock_ = std::make_shared(rclcpp::Clock()); - clock_ = std::make_unique(); - - RCLCPP_INFO(*logger_, "Initializing..."); + RCLCPP_INFO(get_logger(), "Initializing..."); if ( hardware_interface::SystemInterface::on_init(info) != @@ -75,7 +74,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: if (transmission_info.type != "transmission_interface/SimpleTransmission") { RCLCPP_FATAL( - *logger_, "Transmission '%s' of type '%s' not supported in this demo", + get_logger(), "Transmission '%s' of type '%s' not supported in this demo", transmission_info.name.c_str(), transmission_info.type.c_str()); return hardware_interface::CallbackReturn::ERROR; } @@ -88,7 +87,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: catch (const transmission_interface::TransmissionInterfaceException & exc) { RCLCPP_FATAL( - *logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + get_logger(), "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); return hardware_interface::CallbackReturn::ERROR; } @@ -102,7 +101,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: joint_info.command_interfaces[0] == hardware_interface::HW_IF_POSITION)) { RCLCPP_FATAL( - *logger_, "Invalid transmission joint '%s' configuration for this demo", + get_logger(), "Invalid transmission joint '%s' configuration for this demo", joint_info.name.c_str()); return hardware_interface::CallbackReturn::ERROR; } @@ -139,14 +138,14 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: catch (const transmission_interface::TransmissionInterfaceException & exc) { RCLCPP_FATAL( - *logger_, "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); + get_logger(), "Error while configuring %s: %s", transmission_info.name.c_str(), exc.what()); return hardware_interface::CallbackReturn::ERROR; } transmissions_.push_back(transmission); } - RCLCPP_INFO(*logger_, "Initialization successful"); + RCLCPP_INFO(get_logger(), "Initialization successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -154,7 +153,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Configuring..."); + RCLCPP_INFO(get_logger(), "Configuring..."); auto reset_interfaces = [](std::vector & interfaces) { @@ -169,7 +168,7 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: reset_interfaces(joint_interfaces_); reset_interfaces(actuator_interfaces_); - RCLCPP_INFO(*logger_, "Configuration successful"); + RCLCPP_INFO(get_logger(), "Configuration successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -211,8 +210,8 @@ RRBotTransmissionsSystemPositionOnlyHardware::export_command_interfaces() hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Activating..."); - RCLCPP_INFO(*logger_, "Activation successful"); + RCLCPP_INFO(get_logger(), "Activating..."); + RCLCPP_INFO(get_logger(), "Activation successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -220,8 +219,8 @@ hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware: hardware_interface::CallbackReturn RRBotTransmissionsSystemPositionOnlyHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(*logger_, "Deactivating..."); - RCLCPP_INFO(*logger_, "Deactivation successful"); + RCLCPP_INFO(get_logger(), "Deactivating..."); + RCLCPP_INFO(get_logger(), "Deactivation successful"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -266,7 +265,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::re << transmission_info.name << "(R=" << reduction << ") <-- " << actuator_interface->name_ << ": " << actuator_interface->state_; } - RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); return hardware_interface::return_type::OK; } @@ -321,7 +320,7 @@ hardware_interface::return_type RRBotTransmissionsSystemPositionOnlyHardware::wr << transmission_info.name << "(R=" << reduction << ") --> " << actuator_interface->name_ << ": " << actuator_interface->command_; } - RCLCPP_INFO_THROTTLE(*logger_, *clock_, 1000, "%s", ss.str().c_str()); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); return hardware_interface::return_type::OK; } diff --git a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp index f9b3ea82d..de982a0be 100644 --- a/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp +++ b/example_9/hardware/include/ros2_control_demo_example_9/rrbot.hpp @@ -23,6 +23,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + /// Get the logger of the SystemInterface. + /** + * \return logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return *logger_; } + + /// Get the clock of the SystemInterface. + /** + * \return clock of the SystemInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + private: // Parameters for the RRBot simulation double hw_start_sec_; double hw_stop_sec_; double hw_slowdown_; + // Objects for logging + std::shared_ptr logger_; + rclcpp::Clock::SharedPtr clock_; + // Store the command for the simulated robot std::vector hw_commands_; std::vector hw_states_; diff --git a/example_9/hardware/rrbot.cpp b/example_9/hardware/rrbot.cpp index 90b9be9f3..4123cc1b6 100644 --- a/example_9/hardware/rrbot.cpp +++ b/example_9/hardware/rrbot.cpp @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -34,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_shared( + rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot")); + clock_ = std::make_shared(rclcpp::Clock()); // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -49,26 +55,24 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -76,8 +80,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + get_logger(), "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return hardware_interface::CallbackReturn::ERROR; } @@ -90,15 +93,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -109,7 +109,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure hw_commands_[i] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -144,15 +144,12 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait..."); + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_start_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); } // END: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,7 +159,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( hw_commands_[i] = hw_states_[i]; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -171,18 +168,15 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivat const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", - hw_stop_sec_ - i); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -192,17 +186,18 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); + std::stringstream ss; + ss << "Reading states:"; for (uint i = 0; i < hw_states_.size(); i++) { // Simulate RRBot's movement hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", - hw_states_[i], i); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -212,17 +207,16 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); + std::stringstream ss; + ss << "Writing commands:"; for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", - hw_commands_[i], i); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'"; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK;