diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 888b139ca8..c9e8e0bd89 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 2742c39695..e559958244 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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"); } 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 9605cd8c2a..6ddd94ca72 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/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; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 8e0f1b894b..65c4709b5f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/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 diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index bc3f823dfd..12007297c0 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/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: diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c49cffff01..c35a637d51 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/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) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 6d32b8265a..e7433b59be 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"); }