[JTC] Activate update of dynamic parameters (#761)

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -277,6 +277,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

@@ -115,6 +115,17 @@ 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();
+      default_tolerances_ = get_segment_tolerances(params_);
+    }
+  }
 
   auto compute_error_for_joint = [&](
                                    JointTrajectoryPoint & error, size_t index,
@@ -704,15 +715,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 (angle_wraparound)
@@ -787,8 +790,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     get_interface_list(params_.command_interfaces).c_str(),
     get_interface_list(params_.state_interfaces).c_str());
 
-  default_tolerances_ = get_segment_tolerances(params_);
-
+  // parse remaining parameters
   const std::string interpolation_string =
     get_node()->get_parameter("interpolation_method").as_string();
   interpolation_method_ = interpolation_methods::from_string(interpolation_string);
@@ -874,32 +876,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     std::string(get_node()->get_name()) + "/query_state",
     std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));
 
-  if (params_.cmd_timeout > 0.0)
-  {
-    if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
-    {
-      cmd_timeout_ = params_.cmd_timeout;
-    }
-    else
-    {
-      // deactivate timeout
-      RCLCPP_WARN(
-        logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)",
-        params_.cmd_timeout, default_tolerances_.goal_time_tolerance);
-      cmd_timeout_ = 0.0;
-    }
-  }
-  else
-  {
-    cmd_timeout_ = 0.0;
-  }
-
   return CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   const rclcpp_lifecycle::State &)
 {
+  // update the dynamic map parameters
+  param_listener_->refresh_dynamic_parameters();
+
+  // get parameters from the listener in case they were updated
+  params_ = param_listener_->get_params();
+
+  // parse remaining parameters
+  default_tolerances_ = get_segment_tolerances(params_);
+
   // order all joints in the storage
   for (const auto & interface : params_.command_interfaces)
   {
@@ -974,6 +965,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   }
   rt_is_holding_.writeFromNonRT(true);
 
+  // parse timeout parameter
+  if (params_.cmd_timeout > 0.0)
+  {
+    if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance)
+    {
+      cmd_timeout_ = params_.cmd_timeout;
+    }
+    else
+    {
+      // deactivate timeout
+      RCLCPP_WARN(
+        get_node()->get_logger(),
+        "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout,
+        default_tolerances_.goal_time_tolerance);
+      cmd_timeout_ = 0.0;
+    }
+  }
+  else
+  {
+    cmd_timeout_ = 0.0;
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -1530,6 +1543,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_[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;
+  }
+}
+
 void JointTrajectoryController::init_hold_position_msg()
 {
   hold_position_msg_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -413,6 +413,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);
+
+  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

@@ -130,6 +130,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