From 4e84b06a3509b3d11490cbc741f76f189dec3ff4 Mon Sep 17 00:00:00 2001 From: Kenta Kato Date: Wed, 10 Jul 2024 17:52:40 +0900 Subject: [PATCH] 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) {