Use ParamListener::try_get_params to Avoid Blocking in Real-Time Contexts (#1198)

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>
Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>

Author: 4 people

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -120,10 +120,8 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints)
 
 void AdmittanceRule::apply_parameters_update()
 {
-  if (parameter_handler_->is_old(parameters_))
-  {
-    parameters_ = parameter_handler_->get_params();
-  }
+  parameter_handler_->try_get_params(parameters_);
+
   // update param values
   end_effector_weight_[2] = -parameters_.gravity_compensation.CoG.force;
   vec_to_eigen(parameters_.gravity_compensation.CoG.pos, cog_pos_);

diff_drive_controller/src/diff_drive_controller.cpp

@@ -311,9 +311,8 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   auto logger = get_node()->get_logger();
 
   // update parameters if they have changed
-  if (param_listener_->is_old(params_))
+  if (param_listener_->try_update_params(params_))
   {
-    params_ = param_listener_->get_params();
     RCLCPP_INFO(logger, "Parameters were updated");
   }
 

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -144,10 +144,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate
 controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  if (param_listener_->is_old(params_))
-  {
-    params_ = param_listener_->get_params();
-  }
+  param_listener_->try_get_params(params_);
+
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
     realtime_publisher_->msg_.header.stamp = time;

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -159,9 +159,8 @@ controller_interface::return_type JointTrajectoryController::update(
 
   auto logger = this->get_node()->get_logger();
   // update dynamic parameters
-  if (param_listener_->is_old(params_))
+  if (param_listener_->try_update_params(params_))
   {
-    params_ = param_listener_->get_params();
     default_tolerances_ = get_segment_tolerances(logger, params_);
     // update the PID gains
     // variable use_closed_loop_pid_adapter_ is updated in on_configure only

pid_controller/include/pid_controller/pid_controller.hpp

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

pid_controller/src/pid_controller.cpp

@@ -87,18 +87,9 @@ controller_interface::CallbackReturn PidController::on_init()
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-void PidController::update_parameters()
-{
-  if (!param_listener_->is_old(params_))
-  {
-    return;
-  }
-  params_ = param_listener_->get_params();
-}
-
 controller_interface::CallbackReturn PidController::configure_parameters()
 {
-  update_parameters();
+  params_ = param_listener_->get_params();
 
   if (!params_.reference_and_state_dof_names.empty())
   {
@@ -466,7 +457,7 @@ controller_interface::return_type PidController::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   // check for any parameter updates
-  update_parameters();
+  param_listener_->try_get_params(params_);
 
   // Update feedback either from external measured state or from state interfaces
   if (params_.use_external_measured_states)

tricycle_controller/src/tricycle_controller.cpp

@@ -221,9 +221,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
   auto logger = get_node()->get_logger();
 
   // update parameters if they have changed
-  if (param_listener_->is_old(params_))
+  if (param_listener_->try_update_params(params_))
   {
-    params_ = param_listener_->get_params();
     RCLCPP_INFO(logger, "Parameters were updated");
   }