From 915c3cb966481bc4fb02185595c269572be5ddd8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 21 Dec 2023 12:29:46 +0100 Subject: [PATCH 1/2] [JTC] Add console output for tolerance checks (#932) (cherry picked from commit 4c6d7236245c527f275b1e03a7e5242b658dc782) --- .../joint_trajectory_controller/tolerances.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) 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 */); } } } From add28363d35a6e1dcc7df7044948c2a57fdd2698 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 21 Dec 2023 12:22:32 +0000 Subject: [PATCH 2/2] fix typo --- joint_trajectory_controller/CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 418324b5ac..54e3b6d5e0 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -26,7 +26,7 @@ Changelog for package joint_trajectory_controller * [JTC] Tolerance tests + Hold on time violation (backport `#613 `_) * [JTC] Explicitly set hold position (backport `#558 `_) * [Doc] Fix links (backport `#715 `_) -* Contributors: Christoph Fröhlich, Dr Denis Stogl, Bence Magyar, Abishalini Sivaraman +* Contributors: Christoph Fröhlich, Dr Denis Stogl, Bence Magyar, Abishalini Sivaraman 2.29.0 (2023-12-05) -------------------