diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 14e9858df5..1681e7e037 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,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 89cd93985e..8542068b56 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -253,20 +253,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; @@ -275,6 +276,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 */); } } }