Skip to content

Commit

Permalink
Refactor logging (#543)
Browse files Browse the repository at this point in the history 
* Refactor logging for example 1 - 6

* Update logger for remaining examples

* Revert change to copyright

* Revert "Reduce update rate to avoid clogging the log"

This reverts commit 10f2e22.

* Don't use throttling for example_6

* Update example_1/doc/userdoc.rst

Co-authored-by: Felix Exner (fexner) <felix_mauch@web.de>

* Update example_2/doc/userdoc.rst

Co-authored-by: Felix Exner (fexner) <felix_mauch@web.de>

* Updating docs

* Merge into one logging statement

* Remove throttled logging from example_14

* Apply suggestions from code review

Co-authored-by: Felix Exner (fexner) <felix_mauch@web.de>

---------

Co-authored-by: Felix Exner (fexner) <felix_mauch@web.de>
  • Loading branch information
@christophfroehlich @fmauch
christophfroehlich and fmauch authored Oct 14, 2024
1 parent 3f8265d commit 1596b79
Show file tree
Hide file tree
Showing 30 changed files with 483 additions and 636 deletions.
5 changes: 3 additions & 2 deletions example_1/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
70 changes: 30 additions & 40 deletions example_1/hardware/rrbot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,11 @@

#include <chrono>
#include <cmath>
#include <iomanip>
#include <limits>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
Expand Down Expand Up @@ -47,35 +50,32 @@ 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;
}

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;
}
Expand All @@ -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

Expand All @@ -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;
}
Expand All @@ -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

Expand All @@ -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;
}
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down
5 changes: 3 additions & 2 deletions example_10/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Loading

0 comments on commit 1596b79

Please sign in to comment.