[JTC] Fix dynamic reconfigure of tolerances (backport ros-controls#849)

* Fix dynamic reconfigure of tolerances * Fix static cast syntax
pac48 · Dec 6, 2023 · d5dd094 · d5dd094
1 parent a04a926
commit d5dd094
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 2 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -131,11 +131,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
@@ -501,6 +501,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
@@ -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