From 4e84b06a3509b3d11490cbc741f76f189dec3ff4 Mon Sep 17 00:00:00 2001 From: Kenta Kato Date: Wed, 10 Jul 2024 17:52:40 +0900 Subject: [PATCH 1/4] Use non-blocking get_params function on update --- .../src/joint_trajectory_controller.cpp | 3 +-- pid_controller/src/pid_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..7fcb3a87f6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -128,9 +128,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_get_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 diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index b76926d5a0..790f48fa4a 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -419,7 +419,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_); if (params_.use_external_measured_states) { From 54291f334551e58e7767ab2ada827b109e306559 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 23 Jun 2025 12:08:51 +0000 Subject: [PATCH 2/4] pid: cleanup update_parameters --- .../include/pid_controller/pid_controller.hpp | 1 - pid_controller/src/pid_controller.cpp | 11 +---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 39ec58be6b..6e88fe64cb 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -109,7 +109,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: diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 745cd6f93d..48f7fdadb0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -89,18 +89,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()) { From 0dedf67055548927d3ab77351f46aa9803c05183 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 23 Jun 2025 12:09:01 +0000 Subject: [PATCH 3/4] Use new try_update_params --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index be46300ad3..940f179f3d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -137,7 +137,7 @@ controller_interface::return_type JointTrajectoryController::update( { auto logger = this->get_node()->get_logger(); // update dynamic parameters - if (param_listener_->try_get_params(params_)) + if (param_listener_->try_update_params(params_)) { default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains From c4ca428c7af6d6e503e02fef1363dd0067979961 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 23 Jun 2025 12:15:07 +0000 Subject: [PATCH 4/4] Update other controllers --- .../include/admittance_controller/admittance_rule_impl.hpp | 6 ++---- diff_drive_controller/src/diff_drive_controller.cpp | 3 +-- .../src/force_torque_sensor_broadcaster.cpp | 6 ++---- tricycle_controller/src/tricycle_controller.cpp | 3 +-- 4 files changed, 6 insertions(+), 12 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 859c7e3336..40ef3e139a 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -123,10 +123,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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 291071a3aa..2c876f0d58 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -309,9 +309,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"); } diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index ef5e74e89b..8fd289b7c2 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -166,10 +166,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; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 02f5449b4c..473b1dde45 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/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"); }