From e8b0a76478efeb3e64ae94dc73d9d6cbd659dc05 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Jul 2024 20:36:11 +0000 Subject: [PATCH] Update logger for remaining examples --- example_10/doc/userdoc.rst | 5 +- example_10/hardware/rrbot.cpp | 77 ++++++------ example_11/doc/userdoc.rst | 5 +- example_11/hardware/carlikebot_system.cpp | 113 ++++++++---------- example_12/doc/userdoc.rst | 5 +- example_12/hardware/rrbot.cpp | 71 +++++------ example_14/doc/userdoc.rst | 26 ++-- .../rrbot_actuator_without_feedback.cpp | 47 +++----- .../rrbot_sensor_for_position_feedback.cpp | 71 +++++------ example_6/doc/userdoc.rst | 10 +- example_6/hardware/rrbot_actuator.cpp | 6 +- .../ros2_control_demo_example_8/rrbot.hpp | 81 ------------- ...bot_transmissions_system_position_only.hpp | 3 - ...bot_transmissions_system_position_only.cpp | 33 +++-- example_9/hardware/rrbot.cpp | 69 +++++------ 15 files changed, 236 insertions(+), 386 deletions(-) delete mode 100644 example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp 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/rrbot.cpp b/example_10/hardware/rrbot.cpp index d8e9bd84a..9c0d2836b 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" @@ -44,26 +46,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 +71,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 +81,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 +91,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 +100,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 +119,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 +128,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 +143,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 +153,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 +169,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 +179,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 +190,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 +199,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 +208,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 +218,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 +238,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 +251,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..b8deca75c 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" @@ -40,7 +42,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init( 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 +56,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 +77,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 +85,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 +114,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 +122,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 +130,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 +166,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 +197,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 +211,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 +235,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 +244,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 +268,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 +293,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_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/rrbot.cpp b/example_12/hardware/rrbot.cpp index a7d3a66cc..031cda494 100644 --- a/example_12/hardware/rrbot.cpp +++ b/example_12/hardware/rrbot.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 ros2_control Development Team +// 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. @@ -16,8 +16,11 @@ #include #include +#include #include #include +#include +#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -49,26 +52,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 +77,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 +90,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 +106,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 +141,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 +156,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 +165,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 +183,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.c_str() << "'"; } - 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 +204,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.c_str() << "'"; } - 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..e123351fb 100644 --- a/example_14/doc/userdoc.rst +++ b/example_14/doc/userdoc.rst @@ -124,20 +124,18 @@ 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] [1721766743.716253088] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Writing... + [ros2_control_node-1] [INFO] [1721766743.716266749] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Writing command: 5.000000 + [ros2_control_node-1] [INFO] [1721766743.716286822] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Sending data command: 5 + [ros2_control_node-1] [INFO] [1721766743.716253088] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Writing... + [ros2_control_node-1] [INFO] [1721766743.716266749] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Writing command: 5.000000 + [ros2_control_node-1] [INFO] [1721766743.716286822] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Sending data command: 5 + [ros2_control_node-1] [INFO] [1721766742.706166134] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Reading... + [ros2_control_node-1] [INFO] [1721766742.706232033] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Got measured velocity 5.00000 + [ros2_control_node-1] [INFO] [1721766742.706250200] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Got state 0.25300 for joint 'joint1'! + [ros2_control_node-1] [INFO] [1721766742.706166134] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Reading... + [ros2_control_node-1] [INFO] [1721766742.706232033] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint2]: Got measured velocity 5.00000 + [ros2_control_node-1] [INFO] [1721766742.706250200] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint2]: Got state 0.25300 for joint 'joint2'! Files used for this demos diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 6feb8067c..d67531cb0 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -56,8 +56,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; } @@ -65,9 +64,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; } @@ -76,7 +75,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; } @@ -88,16 +87,13 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( server->h_length); address_.sin_port = htons(socket_port_); - 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_); if (connect(sock_, (struct sockaddr *)&address_, sizeof(address_)) < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connection over socket failed."); + RCLCPP_FATAL(get_logger(), "Connection over socket failed."); return hardware_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Connected to socket"); + RCLCPP_INFO(get_logger(), "Connected to socket"); // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::CallbackReturn::SUCCESS; @@ -133,14 +129,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 @@ -150,7 +144,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; } @@ -159,16 +153,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; @@ -186,18 +179,16 @@ 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_); + + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "Writing..."); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "Writing command: %f", hw_joint_command_); // 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()); + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 500, "Sending data command: %s", data.str().c_str()); send(sock_, data.str().c_str(), strlen(data.str().c_str()), 0); - - RCLCPP_INFO(rclcpp::get_logger("RRBotActuatorWithoutFeedback"), "Joints successfully written!"); // 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 395c922c6..aebfefc13 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -59,8 +59,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; } @@ -68,8 +67,7 @@ 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; } @@ -81,14 +79,14 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( 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; } @@ -98,10 +96,10 @@ 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."); + RCLCPP_FATAL(get_logger(), "Binding to socket failed."); return hardware_interface::CallbackReturn::ERROR; } @@ -111,13 +109,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; } @@ -126,15 +121,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); @@ -145,22 +139,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()); } @@ -208,7 +198,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( // In general after a hardware is configured it can be read last_timestamp_ = clock_.now(); - RCLCPP_INFO(rclcpp::get_logger("RRBotSensorPositionFeedback"), "Configuration successful."); + RCLCPP_INFO(get_logger(), "Configuration successful."); return hardware_interface::CallbackReturn::SUCCESS; } @@ -216,17 +206,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; } @@ -235,16 +224,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; @@ -258,7 +246,7 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( 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..."); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "Reading..."); // Simulate RRBot's movement measured_velocity = *(rt_incomming_data_ptr_.readFromRT()); @@ -266,15 +254,12 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( { last_measured_velocity_ = measured_velocity; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSensorPositionFeedback"), "Got measured velocity %.5f", - measured_velocity); + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 500, "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!"); + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 500, "Got state %.5f for joint '%s'!", hw_joint_state_, + info_.joints[0].name.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..2ee04f202 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.RRBotModularJoint1]: 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.RRBotModularJoint2]: Writing commands: + [ros2_control_node-1] 0.50 for joint 'joint2' Files used for this demos diff --git a/example_6/hardware/rrbot_actuator.cpp b/example_6/hardware/rrbot_actuator.cpp index 4e859e6ab..1adeb8d12 100644 --- a/example_6/hardware/rrbot_actuator.cpp +++ b/example_6/hardware/rrbot_actuator.cpp @@ -164,8 +164,7 @@ hardware_interface::return_type RRBotModularJoint::read( ss << std::fixed << std::setprecision(2) << std::endl << "\t" << hw_joint_state_ << " for joint '" << info_.joints[0].name.c_str() << "'"; - // RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); - RCLCPP_INFO(get_logger(), "%s", ss.str().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; @@ -182,8 +181,7 @@ hardware_interface::return_type ros2_control_demo_example_6::RRBotModularJoint:: ss << std::fixed << std::setprecision(2) << std::endl << "\t" << hw_joint_command_ << " for joint '" << info_.joints[0].name.c_str() << "'"; - // RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); - RCLCPP_INFO(get_logger(), "%s", ss.str().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_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 ac5eeb0c1..000000000 --- a/example_8/hardware/include/ros2_control_demo_example_8/rrbot.hpp +++ /dev/null @@ -1,81 +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" -#include "ros2_control_demo_example_8/visibility_control.h" - -namespace ros2_control_demo_example_8 -{ -class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - hardware_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - std::vector export_state_interfaces() override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - std::vector export_command_interfaces() override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - ROS2_CONTROL_DEMO_EXAMPLE_8_PUBLIC - 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 c873a2815..705bd6d3e 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 @@ -67,9 +67,6 @@ class RRBotTransmissionsSystemPositionOnlyHardware : public hardware_interface:: const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - std::unique_ptr logger_; - std::unique_ptr clock_; - // parameters for the RRBot simulation double actuator_slowdown_; diff --git a/example_8/hardware/rrbot_transmissions_system_position_only.cpp b/example_8/hardware/rrbot_transmissions_system_position_only.cpp index 6cf3b2428..594ec13f8 100644 --- a/example_8/hardware/rrbot_transmissions_system_position_only.cpp +++ b/example_8/hardware/rrbot_transmissions_system_position_only.cpp @@ -38,12 +38,7 @@ 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")); - - clock_ = std::make_unique(); - - RCLCPP_INFO(*logger_, "Initializing..."); + RCLCPP_INFO(get_logger(), "Initializing..."); if ( hardware_interface::SystemInterface::on_init(info) != @@ -75,7 +70,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 +83,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 +97,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 +134,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 +149,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 +164,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 +206,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 +215,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 +261,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 +316,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/rrbot.cpp b/example_9/hardware/rrbot.cpp index 90b9be9f3..ba1dc6cec 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" @@ -49,26 +52,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 +77,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 +90,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 +106,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 +141,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 +156,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 +165,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 +183,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.c_str() << "'"; } - 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 +204,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.c_str() << "'"; } - 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;