diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index e540b06e51..4c0a8bb3f8 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -150,7 +150,7 @@ inline bool check_state_tolerance_per_joint( if (show_errors) { const auto logger = rclcpp::get_logger("tolerances"); - RCLCPP_ERROR(logger, "Path state tolerances failed:"); + RCLCPP_ERROR(logger, "State tolerances failed for joint %d:", joint_idx); if (state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0865c13716..301c0909f1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -259,20 +259,21 @@ controller_interface::return_type JointTrajectoryController::update( // Always check the state tolerance on the first sample in case the first sample // is the last point + // print output per default, goal will be aborted afterwards if ( - (before_last_point || first_sample) && + (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.state_tolerance[index], + true /* show_errors */)) { tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance if ( - !before_last_point && + !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, default_tolerances_.goal_state_tolerance[index], + false /* show_errors */)) { outside_goal_tolerance = true; @@ -281,6 +282,10 @@ controller_interface::return_type JointTrajectoryController::update( if (time_difference > default_tolerances_.goal_time_tolerance) { within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + state_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); } } }