From c76d084d87cb5d3a90f9aeaa637fdefc472e7595 Mon Sep 17 00:00:00 2001 From: adhithiyan110 Date: Mon, 30 Jun 2025 14:20:32 +0530 Subject: [PATCH 1/3] add read_only parameters in diff_drive_controller --- .../src/diff_drive_controller_parameter.yaml | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 1fc8b66e88..1887764413 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -3,6 +3,7 @@ diff_drive_controller: type: string_array, default_value: [], description: "Names of the left side wheels' joints", + read_only: true, validation: { not_empty<>: [] } @@ -11,6 +12,7 @@ diff_drive_controller: type: string_array, default_value: [], description: "Names of the right side wheels' joints", + read_only: true, validation: { not_empty<>: [] } @@ -19,6 +21,7 @@ diff_drive_controller: type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", + read_only: true, validation: { gt<>: [0.0] } @@ -27,6 +30,7 @@ diff_drive_controller: type: double, default_value: 0.0, description: "Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations. If this parameter is wrong the robot will move faster or slower then expected.", + read_only: true, validation: { gt<>: [0.0] } @@ -35,87 +39,104 @@ diff_drive_controller: type: double, default_value: 1.0, description: "Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)", + read_only: true, } left_wheel_radius_multiplier: { type: double, default_value: 1.0, description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.", + read_only: true, } right_wheel_radius_multiplier: { type: double, default_value: 1.0, description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.", + read_only: true, } tf_frame_prefix_enable: { type: bool, default_value: true, description: "Enables or disables appending tf_prefix to tf frame id's.", + read_only: true, } tf_frame_prefix: { type: string, default_value: "", description: "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + read_only: true, } odom_frame_id: { type: string, default_value: "odom", description: "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.", + read_only: true, } base_frame_id: { type: string, default_value: "base_link", description: "Name of the robot's base frame that is child of the odometry frame.", + read_only: true, } pose_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the pose. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, } twist_covariance_diagonal: { type: double_array, default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], description: "Odometry covariance for the encoder output of the robot for the speed. These values should be tuned to your robot's sample odometry data, but these values are a good place to start: ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.", + read_only: true, } open_loop: { type: bool, default_value: false, description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", + read_only: false, } position_feedback: { type: bool, default_value: true, description: "Is there position feedback from hardware.", + read_only: false, } enable_odom_tf: { type: bool, default_value: true, description: "Publish transformation between ``odom_frame_id`` and ``base_frame_id``.", + read_only: false, } cmd_vel_timeout: { type: double, default_value: 0.5, # seconds description: "Timeout in seconds, after which input command on ``cmd_vel`` topic is considered staled.", + read_only: false, } publish_limited_velocity: { type: bool, default_value: false, description: "Publish limited velocity value.", + read_only: false, } velocity_rolling_window_size: { type: int, default_value: 10, description: "Size of the rolling window for calculation of mean velocity use in odometry.", + read_only: true, } publish_rate: { type: double, default_value: 50.0, # Hz description: "Publishing rate (Hz) of the odometry and TF messages.", + read_only: false, } linear: x: max_velocity: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -123,6 +144,7 @@ diff_drive_controller: min_velocity: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -131,6 +153,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum acceleration in forward direction.", + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -139,6 +162,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum deceleration in forward direction.", + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -147,6 +171,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -155,6 +180,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -162,6 +188,7 @@ diff_drive_controller: max_jerk: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -169,6 +196,7 @@ diff_drive_controller: min_jerk: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -178,6 +206,7 @@ diff_drive_controller: max_velocity: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -185,6 +214,7 @@ diff_drive_controller: min_velocity: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -193,6 +223,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum acceleration in positive direction.", + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -201,6 +232,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum deceleration in positive direction.", + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -209,6 +241,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.", + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } @@ -217,6 +250,7 @@ diff_drive_controller: type: double, default_value: .NAN, description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.", + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -224,6 +258,7 @@ diff_drive_controller: max_jerk: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::gt_eq_or_nan<>": [0.0] } @@ -231,6 +266,7 @@ diff_drive_controller: min_jerk: { type: double, default_value: .NAN, + read_only: false, validation: { "control_filters::lt_eq_or_nan<>": [0.0] } From 88404fd80a6e3d14efe46f69260014160e2a0ef9 Mon Sep 17 00:00:00 2001 From: adhithiyan110 Date: Sun, 6 Jul 2025 10:38:37 +0530 Subject: [PATCH 2/3] Refactor diff_drive_controller parameter config --- .../src/diff_drive_controller_parameter.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 1887764413..327695f2ce 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -93,13 +93,13 @@ diff_drive_controller: type: bool, default_value: false, description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.", - read_only: false, + read_only: true, } position_feedback: { type: bool, default_value: true, description: "Is there position feedback from hardware.", - read_only: false, + read_only: true, } enable_odom_tf: { type: bool, @@ -129,7 +129,7 @@ diff_drive_controller: type: double, default_value: 50.0, # Hz description: "Publishing rate (Hz) of the odometry and TF messages.", - read_only: false, + read_only: true, } linear: x: From f44bf43c8901137bc5da439aff11bed494599c2b Mon Sep 17 00:00:00 2001 From: adhithiyan110 Date: Wed, 30 Jul 2025 14:34:36 +0530 Subject: [PATCH 3/3] add try_update_params to the update_reference_from_subscribers --- .../src/diff_drive_controller.cpp | 63 ++++++++++--------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index e559958244..f5bab7fa2f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -107,6 +107,37 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub { auto logger = get_node()->get_logger(); + // update parameters if they have changed + if (param_listener_->try_update_params(params_)) + { + RCLCPP_INFO(logger, "Parameters were updated"); + } + + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); + publish_limited_velocity_ = params_.publish_limited_velocity; + // limit the publication on the topics /odom and /tf + publish_rate_ = params_.publish_rate; + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; + + odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); + odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); + + limiter_linear_ = std::make_unique( + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); + + limiter_angular_ = std::make_unique( + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); + + auto current_ref_op = received_velocity_msg_.try_get(); if (current_ref_op.has_value()) { @@ -310,12 +341,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( { auto logger = get_node()->get_logger(); - // update parameters if they have changed - if (param_listener_->try_update_params(params_)) - { - RCLCPP_INFO(logger, "Parameters were updated"); - } - if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) { RCLCPP_ERROR( @@ -324,32 +349,10 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; - const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - - odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius); - odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); - - cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); - publish_limited_velocity_ = params_.publish_limited_velocity; - // Allocate reference interfaces if needed const int nr_ref_itfs = 2; reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); - limiter_linear_ = std::make_unique( - params_.linear.x.min_velocity, params_.linear.x.max_velocity, - params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, - params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, - params_.linear.x.min_jerk, params_.linear.x.max_jerk); - - limiter_angular_ = std::make_unique( - params_.angular.z.min_velocity, params_.angular.z.max_velocity, - params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, - params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, - params_.angular.z.min_jerk, params_.angular.z.max_jerk); - if (!reset()) { return controller_interface::CallbackReturn::ERROR; @@ -443,9 +446,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_message.header.frame_id = odom_frame_id; odometry_message.child_frame_id = base_frame_id; - // limit the publication on the topics /odom and /tf - publish_rate_ = params_.publish_rate; - publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + // initialize odom values zeros odometry_message.twist =