Add missing methods for logging

ros-controls · Nov 2, 2024 · f32b440 · f32b440
1 parent c1f54e4
commit f32b440
Show file tree

Hide file tree

Showing 30 changed files with 317 additions and 28 deletions.
diff --git a/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp
@@ -23,6 +23,8 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
@@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
   hardware_interface::return_type write(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the SystemInterface.
+  /**
+   * \return logger of the SystemInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the SystemInterface.
+  /**
+   * \return clock of the SystemInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
   // Parameters for the RRBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
   double hw_slowdown_;
 
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   // Store the command for the simulated robot
   std::vector<double> hw_commands_;
   std::vector<double> hw_states_;

diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp
@@ -37,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
@@ -191,7 +194,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
     hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
 
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_states_[i] << " for joint '" << i << "'";
+       << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'";
   }
   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
@@ -210,7 +213,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
   {
     // Simulate sending commands to the hardware
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_commands_[i] << " for joint '" << i << "'";
+       << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'";
   }
   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

diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp
@@ -23,6 +23,8 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
@@ -56,8 +58,22 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface
   hardware_interface::return_type write(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the SystemInterface.
+  /**
+   * \return logger of the SystemInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the SystemInterface.
+  /**
+   * \return clock of the SystemInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
-  // Parameters for the RRBot simulation
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
 
   // Store the command and state interfaces for the simulated robot
   std::vector<double> hw_commands_;

diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp
@@ -36,6 +36,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
   hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp
@@ -37,6 +37,9 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.CarlikeBot"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
   // Check if the number of joints is correct based on the mode of operation
   if (info_.joints.size() != 2)

diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
@@ -27,6 +27,7 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
@@ -79,11 +80,27 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
   hardware_interface::return_type write(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the SystemInterface.
+  /**
+   * \return logger of the SystemInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the SystemInterface.
+  /**
+   * \return clock of the SystemInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
   // Parameters for the CarlikeBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
 
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   // std::vector<std::tuple<std::string, double, double>>
   //   hw_interfaces_;  // name of joint, state, command
   std::map<std::string, Joint> hw_interfaces_;

diff --git a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp
@@ -23,6 +23,8 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
@@ -56,12 +58,28 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
   hardware_interface::return_type write(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the SystemInterface.
+  /**
+   * \return logger of the SystemInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the SystemInterface.
+  /**
+   * \return clock of the SystemInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
   // Parameters for the RRBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
   double hw_slowdown_;
 
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   // Store the command for the simulated robot
   std::vector<double> hw_commands_;
   std::vector<double> hw_states_;

diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp
@@ -37,6 +37,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RRBot"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
@@ -192,7 +195,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
     hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
 
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name.c_str() << "'";
+       << "\t" << hw_states_[i] << " for joint '" << info_.joints[i].name << "'";
   }
   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
@@ -211,7 +214,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
   {
     // Simulate sending commands to the hardware
     ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name.c_str() << "'";
+       << "\t" << hw_commands_[i] << " for joint '" << info_.joints[i].name << "'";
   }
   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

diff --git a/example_14/doc/userdoc.rst b/example_14/doc/userdoc.rst
@@ -124,19 +124,19 @@ Tutorial steps
 
    .. code-block:: shell
 
-    [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] [INFO] [1728858168.276013464] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing...
+    [ros2_control_node-1] Writing command: 5.00 for joint 'joint1'
     [ros2_control_node-1] Sending data command: 5
     [ros2_control_node-1]
-    [ros2_control_node-1] [INFO] [1728858168.776052116] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Writing...
-    [ros2_control_node-1] Writing command: 5.00
+    [ros2_control_node-1] [INFO] [1728858168.776052116] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint]: Writing...
+    [ros2_control_node-1] Writing command: 5.00 for joint 'joint2'
     [ros2_control_node-1] Sending data command: 5
     [ros2_control_node-1]
-    [ros2_control_node-1] [INFO] [1728858169.275878132] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Reading...
+    [ros2_control_node-1] [INFO] [1728858169.275878132] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint]: Reading...
     [ros2_control_node-1] Got measured velocity 5.00
     [ros2_control_node-1] Got state 0.34 for joint 'joint1'
     [ros2_control_node-1]
-    [ros2_control_node-1] [INFO] [1728858169.775863217] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint2]: Reading...
+    [ros2_control_node-1] [INFO] [1728858169.775863217] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint]: Reading...
     [ros2_control_node-1] Got measured velocity 5.00
     [ros2_control_node-1] Got state 0.29 for joint 'joint2'
     [ros2_control_node-1]

diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp
@@ -29,6 +29,8 @@
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 
 namespace ros2_control_demo_example_14
@@ -60,11 +62,27 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac
   hardware_interface::return_type write(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the ActuatorInterface.
+  /**
+   * \return logger of the ActuatorInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the ActuatorInterface.
+  /**
+   * \return clock of the ActuatorInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
   // Parameters for the RRBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
 
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   // Store the command for the simulated robot
   double hw_joint_command_;
 

diff --git a/...e_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/...e_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp
@@ -31,6 +31,7 @@
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "rclcpp/clock.hpp"
+#include "rclcpp/logger.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/time.hpp"
 #include "realtime_tools/realtime_buffer.h"
@@ -62,19 +63,34 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface
   hardware_interface::return_type read(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  /// Get the logger of the SensorInterface.
+  /**
+   * \return logger of the SensorInterface.
+   */
+  rclcpp::Logger get_logger() const { return *logger_; }
+
+  /// Get the clock of the SensorInterface.
+  /**
+   * \return clock of the SensorInterface.
+   */
+  rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
+
 private:
   // Parameters for the RRBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
   double hw_slowdown_;
 
+  // Objects for logging
+  std::shared_ptr<rclcpp::Logger> logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
   // Store the command for the simulated robot
   double measured_velocity;  // Local variable, but avoid initialization on each read
   double last_measured_velocity_;
   double hw_joint_state_;
 
   // Timestamps to calculate position for velocity
-  rclcpp::Clock clock_;
   rclcpp::Time last_timestamp_;
   rclcpp::Time current_timestamp;  // Local variable, but avoid initialization on each read
 

diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp
@@ -45,6 +45,10 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(rclcpp::get_logger(
+    "controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
+
   // START: This part here is for exemplary purposes - Please do not copy to your production code
   hw_start_sec_ =
     hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
@@ -216,7 +220,8 @@ hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWitho
     std::stringstream ss;
     ss << "Writing..." << std::endl;
     ss << std::fixed << std::setprecision(2);
-    ss << "Writing command: " << hw_joint_command_ << std::endl;
+    ss << "Writing command: " << hw_joint_command_ << " for joint '" << info_.joints[0].name << "'"
+       << std::endl;
 
     std::ostringstream data;
     data << hw_joint_command_;

diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp
@@ -46,6 +46,11 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.sensor."
+                       "RRBotModularPositionSensorJoint"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
+
   // START: This part here is for exemplary purposes - Please do not copy to your production code
   hw_start_sec_ =
     hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
@@ -75,8 +80,6 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init(
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  clock_ = rclcpp::Clock();
-
   // START: This part here is for exemplary purposes - Please do not copy to your production code
   // Initialize objects for fake mechanical connection
   obj_socket_ = socket(AF_INET, SOCK_STREAM, 0);
@@ -201,7 +204,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure(
   last_measured_velocity_ = 0;
 
   // In general after a hardware is configured it can be read
-  last_timestamp_ = clock_.now();
+  last_timestamp_ = get_clock()->now();
 
   RCLCPP_INFO(get_logger(), "Configuration successful.");
   return hardware_interface::CallbackReturn::SUCCESS;
@@ -246,7 +249,7 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_deactivate(
 hardware_interface::return_type RRBotSensorPositionFeedback::read(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
-  current_timestamp = clock_.now();
+  current_timestamp = get_clock()->now();
   rclcpp::Duration duration = current_timestamp - last_timestamp_;
   last_timestamp_ = current_timestamp;
 

diff --git a/example_2/hardware/diffbot_system.cpp b/example_2/hardware/diffbot_system.cpp
@@ -38,6 +38,9 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init(
   {
     return hardware_interface::CallbackReturn::ERROR;
   }
+  logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.DiffBot"));
+  clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   hw_start_sec_ =