Skip to content
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -232,16 +232,16 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
{ 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(
Expand All @@ -255,10 +255,10 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
}
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(
Expand All @@ -272,10 +272,10 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
}
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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -215,9 +214,10 @@ 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;
}

Expand Down Expand Up @@ -262,9 +262,10 @@ 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;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -253,17 +253,17 @@ 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 << "`");
CLCPP_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(
Expand All @@ -277,10 +277,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(
Expand All @@ -294,10 +294,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;
}

Expand Down
Loading