diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index dc1555b698..846dc2aa42 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -192,7 +192,7 @@ controller_interface::return_type DiffDriveController::update() if (std::isnan(left_position) || std::isnan(right_position)) { RCLCPP_ERROR( - logger, "Either the left or right wheel position is invalid for index [%d]", + logger, "Either the left or right wheel position is invalid for index [%zu]", index); return controller_interface::return_type::ERROR; } @@ -285,7 +285,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State & if (left_wheel_names_.size() != right_wheel_names_.size()) { RCLCPP_ERROR( logger, - "The number of left wheels [%d] and the number of right wheels [%d] are different", + "The number of left wheels [%zu] and the number of right wheels [%zu] are different", left_wheel_names_.size(), right_wheel_names_.size()); return CallbackReturn::ERROR; @@ -575,9 +575,7 @@ CallbackReturn DiffDriveController::configure_side( auto logger = node_->get_logger(); if (wheel_names.empty()) { - std::stringstream ss; - ss << "No " << side << " wheel names specified."; - RCLCPP_ERROR(logger, ss.str().c_str()); + RCLCPP_ERROR(logger, "No '%s' wheel names specified", side.c_str()); return CallbackReturn::ERROR; } diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 5d79a2870d..cfbe66b479 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -82,7 +82,7 @@ CallbackReturn ForwardCommandController::on_configure( rt_command_ptr_.writeFromNonRT(msg); }); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -138,7 +138,7 @@ CallbackReturn ForwardCommandController::on_activate( { RCLCPP_ERROR( node_->get_logger(), - "Expected %u position command interfaces, got %u", + "Expected %zu position command interfaces, got %zu", joint_names_.size(), ordered_interfaces.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -162,12 +162,11 @@ controller_interface::return_type ForwardCommandController::update() } if ((*joint_commands)->data.size() != command_interfaces_.size()) { - RCLCPP_ERROR_STREAM_THROTTLE( + RCLCPP_ERROR_THROTTLE( get_node()->get_logger(), *node_->get_clock(), 1000, - "command size (" << (*joint_commands)->data.size() << - ") does not match number of interfaces (" << - command_interfaces_.size() << ")"); + "command size (%zu) does not match number of interfaces (%zu)", + (*joint_commands)->data.size(), command_interfaces_.size()); return controller_interface::return_type::ERROR; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index fbe96fea51..7bb47f445a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -166,24 +166,27 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR_STREAM(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "Path state tolerances failed:"); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { - RCLCPP_ERROR_STREAM( - logger, "Position Error: " << error_position << - " Position Tolerance: " << state_tolerance.position); + RCLCPP_ERROR( + logger, + "Position Error: %f, Position Tolerance: %f", + error_position, state_tolerance.position); } if (state_tolerance.velocity > 0.0 && abs(error_velocity) > state_tolerance.velocity) { - RCLCPP_ERROR_STREAM( - logger, "Velocity Error: " << error_velocity << - " Velocity Tolerance: " << state_tolerance.velocity); + RCLCPP_ERROR( + logger, + "Velocity Error: %f, Velocity Tolerance: %f", + error_velocity, state_tolerance.velocity); } if (state_tolerance.acceleration > 0.0 && abs(error_acceleration) > state_tolerance.acceleration) { - RCLCPP_ERROR_STREAM( - logger, "Acceleration Error: " << error_acceleration << - " Acceleration Tolerance: " << state_tolerance.acceleration); + RCLCPP_ERROR( + logger, + "Acceleration Error: %f, Acceleration Tolerance: %f", + error_acceleration, state_tolerance.acceleration); } } return false; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f91b2cbabb..9f8e8b2c6f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -299,9 +299,10 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) // State publisher const double state_publish_rate = node_->get_parameter("state_publish_rate").get_value(); - RCLCPP_INFO_STREAM( - logger, "Controller state will be published at " << - state_publish_rate << "Hz."); + RCLCPP_INFO( + logger, + "Controller state will be published at %fHz.", + state_publish_rate); if (state_publish_rate > 0.0) { state_publisher_period_ = rclcpp::Duration::from_seconds(1.0 / state_publish_rate); @@ -338,9 +339,10 @@ JointTrajectoryController::on_configure(const rclcpp_lifecycle::State &) const double action_monitor_rate = node_->get_parameter("action_monitor_rate") .get_value(); - RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << - action_monitor_rate << "Hz."); + RCLCPP_INFO( + logger, + "Action status changes will be monitored at %fHz.", + action_monitor_rate); action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / action_monitor_rate); using namespace std::placeholders; @@ -387,7 +389,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_ERROR( node_->get_logger(), - "Expected %u position command interfaces, got %u", + "Expected %zu position command interfaces, got %zu", joint_names_.size(), joint_position_command_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -397,7 +399,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_ERROR( node_->get_logger(), - "Expected %u position state interfaces, got %u", + "Expected %zu position state interfaces, got %zu", joint_names_.size(), joint_position_state_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -407,7 +409,7 @@ JointTrajectoryController::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_ERROR( node_->get_logger(), - "Expected %u velocity state interfaces, got %u", + "Expected %zu velocity state interfaces, got %zu", joint_names_.size(), joint_velocity_state_interface_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } @@ -656,7 +658,8 @@ void JointTrajectoryController::sort_to_local_joint_order( if (to_remap.size() != mapping.size()) { RCLCPP_WARN( node_->get_logger(), - "Invalid input size (%d) for sorting", to_remap.size()); + "Invalid input size (%zu) for sorting", + to_remap.size()); return to_remap; } std::vector output; @@ -694,7 +697,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( if (joint_names_size != vector_field.size()) { RCLCPP_ERROR( node_->get_logger(), - "Mismatch between joint_names (%u) and %s (%u) at point #%u.", + "Mismatch between joint_names (%zu) and %s (%zu) at point #%zu.", joint_names_size, string_for_vector_field.c_str(), vector_field.size(), i); return false; } @@ -758,7 +761,7 @@ bool JointTrajectoryController::validate_trajectory_msg( if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( node_->get_logger(), - "Time between points %u and %u is not strictly increasing, it is %f and %f respectively", + "Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively", i - 1, i, previous_traj_time.seconds(), rclcpp::Duration(trajectory.points[i].time_from_start).seconds()); return false;