From d9a6bb43e312d8d74e52eddfb700e5249c18f4f5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Sun, 19 Nov 2023 20:18:46 +0100
Subject: [PATCH] [JTC] Fix dynamic reconfigure of tolerances (#849)

* Fix dynamic reconfigure of tolerances

* Fix static cast syntax
---
 .../src/joint_trajectory_controller.cpp       |  5 +-
 .../test/test_trajectory_controller.cpp       | 53 +++++++++++++++++++
 .../test/test_trajectory_controller_utils.hpp |  5 ++
 3 files changed, 61 insertions(+), 2 deletions(-)

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 18f61f0e49..dae7f13148 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -119,11 +119,12 @@ controller_interface::return_type JointTrajectoryController::update(
   if (param_listener_->is_old(params_))
   {
     params_ = param_listener_->get_params();
-    // use_closed_loop_pid_adapter_ is updated in on_configure only
+    default_tolerances_ = get_segment_tolerances(params_);
+    // update the PID gains
+    // variable use_closed_loop_pid_adapter_ is updated in on_configure only
     if (use_closed_loop_pid_adapter_)
     {
       update_pids();
-      default_tolerances_ = get_segment_tolerances(params_);
     }
   }
 
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index 7842d50434..8b29c2cb16 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -449,6 +449,59 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
   executor.cancel();
 }
 
+/**
+ * @brief check if dynamic tolerances are updated
+ */
+TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+
+  SetUpAndActivateTrajectoryController(executor);
+
+  updateController();
+
+  // test default parameters
+  {
+    auto tols = traj_controller_->get_tolerances();
+    EXPECT_EQ(tols.goal_time_tolerance, 0.0);
+    for (size_t i = 0; i < joint_names_.size(); ++i)
+    {
+      EXPECT_EQ(tols.state_tolerance.at(i).position, 0.0);
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 0.0);
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.01);
+    }
+  }
+
+  // change parameters, update and see what happens
+  std::vector<rclcpp::Parameter> new_tolerances{
+    rclcpp::Parameter("constraints.goal_time", 1.0),
+    rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.02),
+    rclcpp::Parameter("constraints.joint1.trajectory", 1.0),
+    rclcpp::Parameter("constraints.joint2.trajectory", 2.0),
+    rclcpp::Parameter("constraints.joint3.trajectory", 3.0),
+    rclcpp::Parameter("constraints.joint1.goal", 10.0),
+    rclcpp::Parameter("constraints.joint2.goal", 20.0),
+    rclcpp::Parameter("constraints.joint3.goal", 30.0)};
+  for (const auto & param : new_tolerances)
+  {
+    traj_controller_->get_node()->set_parameter(param);
+  }
+  updateController();
+
+  {
+    auto tols = traj_controller_->get_tolerances();
+    EXPECT_EQ(tols.goal_time_tolerance, 1.0);
+    for (size_t i = 0; i < joint_names_.size(); ++i)
+    {
+      EXPECT_EQ(tols.state_tolerance.at(i).position, static_cast<double>(i) + 1.0);
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).position, 10.0 * (static_cast<double>(i) + 1.0));
+      EXPECT_EQ(tols.goal_state_tolerance.at(i).velocity, 0.02);
+    }
+  }
+
+  executor.cancel();
+}
+
 /**
  * @brief check if hold on startup is deactivated
  */
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 224265ad83..fdf4305cec 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -132,6 +132,11 @@ class TestableJointTrajectoryController
 
   std::vector<PidPtr> get_pids() const { return pids_; }
 
+  joint_trajectory_controller::SegmentTolerances get_tolerances() const
+  {
+    return default_tolerances_;
+  }
+
   bool has_active_traj() const { return has_active_trajectory(); }
 
   bool has_trivial_traj() const