diff --git a/example_1/doc/userdoc.rst b/example_1/doc/userdoc.rst index 4bec46299..de4824166 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/rrbot.cpp b/example_1/hardware/rrbot.cpp index 4e7664649..28b07d4a3 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" @@ -47,26 +50,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; } @@ -74,8 +75,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; } @@ -88,15 +88,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 { set_command(name, 0.0); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + RCLCPP_INFO(get_logger(), "Successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -118,15 +115,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 @@ -136,7 +130,7 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( set_command(name, get_state(name)); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + RCLCPP_INFO(get_logger(), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -145,18 +139,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; @@ -166,18 +157,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 (const auto & [name, descr] : joint_state_interfaces_) { + // Simulate RRBot's movement auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; set_state(name, new_value); - // Simulate RRBot's movement - RCLCPP_INFO_STREAM( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Got state " << get_state(name) << " for joint: " << name << "!"); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_state(name) << " for joint '" << 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; @@ -187,17 +178,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 (const auto & [name, descr] : joint_command_interfaces_) { // Simulate sending commands to the hardware - RCLCPP_INFO_STREAM( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Got command" << get_command(name) << " for joint: " << name << "!"); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_command(name) << " for joint '" << 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/bringup/config/rrbot_forward_position_publisher.yaml b/example_10/bringup/config/rrbot_forward_position_publisher.yaml index d0fd6330a..879ad34ab 100644 --- a/example_10/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_10/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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..53e20ab7c 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" @@ -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..91e7742cb 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.RRBotModularJoint1]: Writing... + [ros2_control_node-1] Writing command: 5.00 + [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.RRBotModularJoint2]: Writing... + [ros2_control_node-1] Writing command: 5.00 + [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.RRBotModularPositionSensorJoint1]: 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.RRBotModularPositionSensorJoint2]: 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/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index ac416eb34..4608e8a43 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" @@ -58,8 +60,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 +68,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 +79,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 +94,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 +109,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 +120,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 +163,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 +178,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 +187,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; @@ -218,18 +211,18 @@ hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWitho const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // 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_ << 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..50a7393e2 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 @@ -60,8 +62,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,8 +70,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; } @@ -82,14 +82,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; } @@ -99,11 +99,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 +114,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 +126,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 +144,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()); } @@ -211,7 +203,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; } @@ -219,17 +211,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 +229,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; @@ -261,7 +251,8 @@ 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..."); + std::stringstream ss; + ss << "Reading..." << std::endl; // Simulate RRBot's movement measured_velocity = *(rt_incomming_data_ptr_.readFromRT()); @@ -269,15 +260,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_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml b/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml index 2730478bc..2ea75637d 100644 --- a/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml +++ b/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml @@ -7,6 +7,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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..87a21f53a 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" @@ -53,26 +55,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 +80,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 +129,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 +149,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 +158,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 +176,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 +185,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 +201,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_3/bringup/config/rrbot_forward_position_publisher.yaml b/example_3/bringup/config/rrbot_forward_position_publisher.yaml index d0fd6330a..879ad34ab 100644 --- a/example_3/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_3/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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 9ac21ff51..2494f5b63 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 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/example_3/doc/userdoc.rst b/example_3/doc/userdoc.rst index dbdbdf908..29570224d 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. To demonstrate illegal controller configuration, use one of the following launch file arguments: diff --git a/example_3/hardware/rrbot_system_multi_interface.cpp b/example_3/hardware/rrbot_system_multi_interface.cpp index ee6f4d37b..0818056a4 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 @@ -56,8 +58,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 +68,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 +88,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 +201,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 +240,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 +248,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 +265,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 +293,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 +307,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_forward_position_publisher.yaml b/example_4/bringup/config/rrbot_forward_position_publisher.yaml index fde53b780..a282ab70b 100644 --- a/example_4/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_4/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] diff --git a/example_4/bringup/config/rrbot_with_sensor_controllers.yaml b/example_4/bringup/config/rrbot_with_sensor_controllers.yaml index 4692c0bbc..6dce7c162 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 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster 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/rrbot_system_with_sensor.cpp b/example_4/hardware/rrbot_system_with_sensor.cpp index eea370e1f..751cbc512 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" @@ -57,26 +59,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 +84,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 +97,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 +113,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 +155,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 +176,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 +185,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 +203,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.c_str() << "'"; } - 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 +239,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.c_str() << "'"; } - 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_forward_position_publisher.yaml b/example_5/bringup/config/rrbot_forward_position_publisher.yaml index fde53b780..a282ab70b 100644 --- a/example_5/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_5/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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 d4e43a1d2..7a4b500e7 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 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster 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..aadc299cd 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" @@ -70,19 +72,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 +90,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 +108,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 +117,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/rrbot.cpp b/example_5/hardware/rrbot.cpp index 35fe1c0f2..6c395451b 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" @@ -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_6/bringup/config/rrbot_forward_position_publisher.yaml b/example_6/bringup/config/rrbot_forward_position_publisher.yaml index d0fd6330a..879ad34ab 100644 --- a/example_6/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_6/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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 b48e6c0d7..a1528b623 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" @@ -53,8 +55,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 +63,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 +112,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 +128,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 +137,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 +155,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.c_str() << "'"; + + 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 +174,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.c_str() << "'"; - 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/bringup/config/rrbot_forward_position_publisher.yaml b/example_8/bringup/config/rrbot_forward_position_publisher.yaml index d0fd6330a..879ad34ab 100644 --- a/example_8/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_8/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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..fc01cb2b8 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 @@ -58,9 +58,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/bringup/config/rrbot_forward_position_publisher.yaml b/example_9/bringup/config/rrbot_forward_position_publisher.yaml index d0fd6330a..879ad34ab 100644 --- a/example_9/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_9/bringup/config/rrbot_forward_position_publisher.yaml @@ -6,6 +6,6 @@ publisher_forward_position_controller: goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] - pos2: [0, 0] + pos2: [0.0, 0.0] pos3: [-0.785, -0.785] - pos4: [0, 0] + pos4: [0.0, 0.0] 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;