Update PIDController to enable parameter updates.

ros-controls · Sep 12, 2023 · 05607da · 05607da
1 parent ff17e8a
commit 05607da
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 1 deletion.
diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp
@@ -64,6 +64,10 @@ class PidController : public controller_interface::ChainableControllerInterface
   PID_CONTROLLER__VISIBILITY_PUBLIC
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
+  PID_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
   PID_CONTROLLER__VISIBILITY_PUBLIC
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
@@ -123,6 +127,7 @@ class PidController : public controller_interface::ChainableControllerInterface
   bool on_set_chained_mode(bool chained_mode) override;
 
   // internal methods
+  void update_parameters();
   controller_interface::CallbackReturn configure_parameters();
 
 private:

diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp
@@ -74,9 +74,18 @@ controller_interface::CallbackReturn PidController::on_init()
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn PidController::configure_parameters()
+void PidController::update_parameters()
 {
+  if (!param_listener_->is_old(params_))
+  {
+    return;
+  }
   params_ = param_listener_->get_params();
+}
+
+controller_interface::CallbackReturn PidController::configure_parameters()
+{
+  update_parameters();
 
   if (!params_.reference_and_state_dof_names.empty())
   {
@@ -125,6 +134,15 @@ controller_interface::CallbackReturn PidController::configure_parameters()
   return CallbackReturn::SUCCESS;
 }
 
+controller_interface::CallbackReturn PidController::on_cleanup(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  reference_and_state_dof_names_.clear();
+  pids_.clear();
+
+  return CallbackReturn::SUCCESS;
+}
+
 controller_interface::CallbackReturn PidController::on_configure(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
@@ -384,6 +402,9 @@ controller_interface::return_type PidController::update_reference_from_subscribe
 controller_interface::return_type PidController::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  // check for any parameter updates
+  update_parameters();
+
   if (params_.use_external_measured_states)
   {
     const auto measured_state = *(measured_state_.readFromRT());