Skip to content

Commit

Permalink
Merge into one logging statement
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Oct 13, 2024
1 parent 4a9f372 commit 91efe03
Show file tree
Hide file tree
Showing 3 changed files with 36 additions and 23 deletions.
28 changes: 16 additions & 12 deletions example_14/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,18 +124,22 @@ Tutorial steps

.. code-block:: shell
[ros2_control_node-1] [INFO] [1721766743.716253088] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Writing...
[ros2_control_node-1] [INFO] [1721766743.716266749] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Writing command: 5.000000
[ros2_control_node-1] [INFO] [1721766743.716286822] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint1]: Sending data command: 5
[ros2_control_node-1] [INFO] [1721766743.716253088] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Writing...
[ros2_control_node-1] [INFO] [1721766743.716266749] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Writing command: 5.000000
[ros2_control_node-1] [INFO] [1721766743.716286822] [controller_manager.resource_manager.hardware_component.actuator.RRBotModularJoint2]: Sending data command: 5
[ros2_control_node-1] [INFO] [1721766742.706166134] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Reading...
[ros2_control_node-1] [INFO] [1721766742.706232033] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Got measured velocity 5.00000
[ros2_control_node-1] [INFO] [1721766742.706250200] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Got state 0.25300 for joint 'joint1'!
[ros2_control_node-1] [INFO] [1721766742.706166134] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint1]: Reading...
[ros2_control_node-1] [INFO] [1721766742.706232033] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint2]: Got measured velocity 5.00000
[ros2_control_node-1] [INFO] [1721766742.706250200] [controller_manager.resource_manager.hardware_component.sensor.RRBotModularPositionSensorJoint2]: Got state 0.25300 for joint 'joint2'!
[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.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]
[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]
Files used for this demos
Expand Down
15 changes: 10 additions & 5 deletions example_14/hardware/rrbot_actuator_without_feedback.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,10 @@
#include <chrono>
#include <cmath>
#include <cstring>
#include <iomanip>
#include <limits>
#include <memory>
#include <sstream>
#include <vector>

#include "hardware_interface/actuator_interface.hpp"
Expand Down Expand Up @@ -209,14 +211,17 @@ 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_THROTTLE(get_logger(), *get_clock(), 500, "Writing...");
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "Writing command: %f", hw_joint_command_);
std::stringstream ss;
ss << "Writing..." << std::endl;
ss << std::fixed << std::setprecision(2) << std::endl;
ss << "Writing command: " << hw_joint_command_ << std::endl;

// Simulate sending commands to the hardware
std::ostringstream data;
data << hw_joint_command_;
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 500, "Sending data command: %s", data.str().c_str());
ss << "Sending data command: " << data.str() << std::endl;
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, ss.str().c_str());

// 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

Expand Down
16 changes: 10 additions & 6 deletions example_14/hardware/rrbot_sensor_for_position_feedback.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,10 @@
#include <chrono>
#include <cmath>
#include <cstring>
#include <iomanip>
#include <limits>
#include <memory>
#include <sstream>
#include <thread>
#include <vector>

Expand Down Expand Up @@ -249,20 +251,22 @@ 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_THROTTLE(get_logger(), *get_clock(), 500, "Reading...");
std::stringstream ss;
ss << "Reading..." << std::endl;

// Simulate RRBot's movement
measured_velocity = *(rt_incomming_data_ptr_.readFromRT());
if (!std::isnan(measured_velocity))
{
last_measured_velocity_ = measured_velocity;
}
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 500, "Got measured velocity %.5f", measured_velocity);
hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_;
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 500, "Got state %.5f for joint '%s'!", hw_joint_state_,
info_.joints[0].name.c_str());

ss << std::fixed << std::setprecision(2) << std::endl;
ss << "Got measured velocity " << measured_velocity << std::endl;
ss << "Got state " << hw_joint_state_ << " for joint '" << info_.joints[0].name << "'"
<< std::endl;
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, 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

0 comments on commit 91efe03

Please sign in to comment.