diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index b4c0586473..38f091b939 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -299,10 +299,11 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty()) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " - << msg.header.frame_id << ". Expected reference frame: " - << admittance_->parameters_.ft_sensor.frame.id); + RCLCPP_ERROR( + get_node()->get_logger(), + "Ignoring wrench reference as it is on the wrong frame: %s. Expected reference frame: " + "%s", + msg.header.frame_id.c_str(), admittance_->parameters_.ft_sensor.frame.id.c_str()); return; } input_wrench_command_.writeFromNonRT(msg); diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 034f7e43dd..a94e163d7b 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -210,8 +210,8 @@ controller_interface::CallbackReturn GripperActionController: // Action status checking update rate action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); - RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + RCLCPP_INFO( + logger, "Action status changes will be monitored at %f Hz.", params_.action_monitor_rate); // Controlled joint if (params_.joint.empty()) @@ -232,16 +232,14 @@ controller_interface::CallbackReturn GripperActionController: { return command_interface.get_interface_name() == HardwareInterface; }); if (command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Expected 1 " << HardwareInterface << " command interface"); + RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 %s command interface", HardwareInterface); return controller_interface::CallbackReturn::ERROR; } if (command_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Command interface is different than joint name `" - << command_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface is different than joint name `%s` != `%s`", + command_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -255,10 +253,10 @@ controller_interface::CallbackReturn GripperActionController: } if (position_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Position state interface is different than joint name `%s` != `%s`", + position_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -272,10 +270,10 @@ controller_interface::CallbackReturn GripperActionController: } if (velocity_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Velocity command interface is different than joint name `%s` != `%s`", + velocity_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp index df8c9da0c4..f357a312c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/interpolation_methods.hpp @@ -53,7 +53,7 @@ const std::unordered_map InterpolationMethodMa // Default else { - RCLCPP_INFO_STREAM( + RCLCPP_INFO( LOGGER, "No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE."); return InterpolationMethod::VARIABLE_DEGREE_SPLINE; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index fbd11b9cdb..a89991bd55 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -175,10 +175,9 @@ SegmentTolerances get_segment_tolerances( } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger, "Specified illegal goal_time_tolerance: " - << rclcpp::Duration(goal.goal_time_tolerance).seconds() - << ". Using default tolerances"); + RCLCPP_ERROR( + logger, "Specified illegal goal_time_tolerance: %f. Using default tolerances", + rclcpp::Duration(goal.goal_time_tolerance).seconds()); return default_tolerances; } RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); @@ -215,9 +214,11 @@ SegmentTolerances get_segment_tolerances( } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " - << interface << " tolerance. Using default tolerances."); + RCLCPP_ERROR( + logger, + "joint '%s' specified in goal.path_tolerance has a invalid %s tolerance. Using default " + "tolerances.", + joint.c_str(), interface.c_str()); return default_tolerances; } @@ -262,9 +263,11 @@ SegmentTolerances get_segment_tolerances( } catch (const std::runtime_error & e) { - RCLCPP_ERROR_STREAM( - logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " - << interface << " tolerance. Using default tolerances."); + RCLCPP_ERROR( + logger, + "joint '%s' specified in goal.goal_tolerance has a invalid %s tolerance. Using default " + "tolerances.", + joint.c_str(), interface.c_str()); return default_tolerances; } diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index 5b7e760ca2..50f91678cb 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -230,8 +230,8 @@ controller_interface::CallbackReturn GripperActionController::on_configure( // Action status checking update rate action_monitor_period_ = rclcpp::Duration::from_seconds(1.0 / params_.action_monitor_rate); - RCLCPP_INFO_STREAM( - logger, "Action status changes will be monitored at " << params_.action_monitor_rate << "Hz."); + RCLCPP_INFO( + logger, "Action status changes will be monitored at %f Hz.", params_.action_monitor_rate); // Controlled joint if (params_.joint.empty()) @@ -253,17 +253,16 @@ controller_interface::CallbackReturn GripperActionController::on_activate( { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); if (command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Expected 1 " << hardware_interface::HW_IF_POSITION << " command interface"); + RCLCPP_ERROR( + get_node()->get_logger(), "Expected 1 %s command interface", + hardware_interface::HW_IF_POSITION); return controller_interface::CallbackReturn::ERROR; } if (command_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Command interface is different than joint name `" - << command_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), "Command interface is different than joint name `%s` != `%s`", + command_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto position_state_interface_it = std::find_if( @@ -277,10 +276,10 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } if (position_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position state interface is different than joint name `" - << position_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Position state interface is different than joint name `%s` != `%s`", + position_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; } const auto velocity_state_interface_it = std::find_if( @@ -294,10 +293,10 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } if (velocity_state_interface_it->get_prefix_name() != params_.joint) { - RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Velocity command interface is different than joint name `" - << velocity_state_interface_it->get_prefix_name() << "` != `" - << params_.joint << "`"); + RCLCPP_ERROR( + get_node()->get_logger(), + "Velocity command interface is different than joint name `%s` != `%s`", + velocity_state_interface_it->get_prefix_name().c_str(), params_.joint.c_str()); return controller_interface::CallbackReturn::ERROR; }