[JTC] Activate update of dynamic parameters

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -267,6 +267,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);
 
 private:
+  void update_pids();
+
   bool contains_interface_type(
     const std::vector<std::string> & interface_type_list, const std::string & interface_type);
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -124,6 +124,16 @@ controller_interface::return_type JointTrajectoryController::update(
   {
     return controller_interface::return_type::OK;
   }
+  // update dynamic parameters
+  if (param_listener_->is_old(params_))
+  {
+    params_ = param_listener_->get_params();
+    // use_closed_loop_pid_adapter_ is updated in on_configure only
+    if (use_closed_loop_pid_adapter_)
+    {
+      update_pids();
+    }
+  }
 
   auto compute_error_for_joint = [&](
                                    JointTrajectoryPoint & error, size_t index,
@@ -713,15 +723,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     ff_velocity_scale_.resize(dof_);
     tmp_command_.resize(dof_, 0.0);
 
-    // Init PID gains from ROS parameters
-    for (size_t i = 0; i < dof_; ++i)
-    {
-      const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
-      pids_[i] = std::make_shared<control_toolbox::Pid>(
-        gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
-
-      ff_velocity_scale_[i] = gains.ff_velocity_scale;
-    }
+    update_pids();
   }
 
   // Configure joint position error normalization from ROS parameters
@@ -1530,6 +1532,26 @@ bool JointTrajectoryController::has_active_trajectory() const
   return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();
 }
 
+void JointTrajectoryController::update_pids()
+{
+  for (size_t i = 0; i < dof_; ++i)
+  {
+    const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
+    if (pids_.at(i))
+    {
+      // update PIDs with gains from ROS parameters
+      pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+    }
+    else
+    {
+      // Init PIDs with gains from ROS parameters
+      pids_[i] = std::make_shared<control_toolbox::Pid>(
+        gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+    }
+    ff_velocity_scale_[i] = gains.ff_velocity_scale;
+  }
+}
+
 }  // namespace joint_trajectory_controller
 
 #include "pluginlib/class_list_macros.hpp"

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -418,6 +418,42 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
   }
 }
 
+/**
+ * @brief check if dynamic parameters are updated
+ */
+TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+
+  SetUpAndActivateTrajectoryController(executor, true);
+
+  updateController();
+  auto pids = traj_controller_->get_pids();
+
+  if (traj_controller_->use_closed_loop_pid_adapter())
+  {
+    EXPECT_EQ(pids.size(), 3);
+    auto gain_0 = pids.at(0)->getGains();
+    EXPECT_EQ(gain_0.p_gain_, 0.0);
+
+    double kp = 1.0;
+    SetPidParameters(kp);
+    updateController();
+
+    pids = traj_controller_->get_pids();
+    EXPECT_EQ(pids.size(), 3);
+    gain_0 = pids.at(0)->getGains();
+    EXPECT_EQ(gain_0.p_gain_, kp);
+  }
+  else
+  {
+    // nothing to check here, skip further test
+    EXPECT_EQ(pids.size(), 0);
+  }
+
+  executor.cancel();
+}
+
 /**
  * @brief check if hold on startup is deactivated
  */

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -124,6 +124,8 @@ class TestableJointTrajectoryController
 
   bool is_open_loop() const { return params_.open_loop_control; }
 
+  std::vector<PidPtr> get_pids() const { return pids_; }
+
   bool has_active_traj() const { return has_active_trajectory(); }
 
   bool has_trivial_traj() const