diff --git a/example_14/doc/userdoc.rst b/example_14/doc/userdoc.rst index e123351fb..5e1c2b836 100644 --- a/example_14/doc/userdoc.rst +++ b/example_14/doc/userdoc.rst @@ -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 diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index e1bd0c4ea..800db4858 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" @@ -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 diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 100937b82..eb0b93ea2 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 @@ -249,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_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()); @@ -257,12 +260,13 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( { 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;