From 54d279cde54e425117a965c2429fccef49810247 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 22 Jun 2024 10:12:16 +0000 Subject: [PATCH] Remove duplicate check --- .../src/joint_trajectory_controller.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2f34fdf33d..76e6387817 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -225,8 +225,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, active_tol->state_tolerance[index], true /* show_errors */) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -234,9 +233,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, active_tol->goal_state_tolerance[index], - false /* show_errors */) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true;