Move parameter read and check into separate method for easier inherit…

…ance.
ros-controls · Nov 29, 2022 · c28a4c9 · c28a4c9
1 parent 273b16a
commit c28a4c9
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 6 deletions.
diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp
@@ -99,6 +99,11 @@ class PidController : public controller_interface::ChainableControllerInterface
   std::vector<std::string> reference_and_state_dof_names_;
   size_t dof_;
 
+  using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
+  std::vector<PidPtr> pids_;
+  // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+  std::vector<double> feedforward_gain_;
+
   // Command subscribers and Controller State publisher
   rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
   realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
@@ -119,10 +124,8 @@ class PidController : public controller_interface::ChainableControllerInterface
 
   bool on_set_chained_mode(bool chained_mode) override;
 
-  using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
-  std::vector<PidPtr> pids_;
-  // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
-  std::vector<double> feedforward_gain_;
+  // internal methods
+  controller_interface::CallbackReturn configure_parameters();
 
 private:
   // callback for topic interface

diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp
@@ -93,8 +93,7 @@ controller_interface::CallbackReturn PidController::on_init()
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn PidController::on_configure(
-  const rclcpp_lifecycle::State & /*previous_state*/)
+controller_interface::CallbackReturn PidController::configure_parameters()
 {
   params_ = param_listener_->get_params();
 
@@ -137,6 +136,12 @@ controller_interface::CallbackReturn PidController::on_configure(
       std::make_shared<control_toolbox::PidROS>(get_node(), "gains." + params_.dof_names[i]);
     pids_[i]->initPid();
   }
+}
+
+controller_interface::CallbackReturn PidController::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  configure_parameters();
 
   // topics QoS
   auto subscribers_qos = rclcpp::SystemDefaultsQoS();