Skip to content

add read_only parameters to diff_drive_controller #1781

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 7 commits into
base: master
Choose a base branch
from
Draft

add read_only parameters to diff_drive_controller #1781

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c76d084
add read_only parameters in diff_drive_controller
adhithiyan110 Jun 30, 2025
6d210f4
Merge branch 'ros-controls:master' into master
adhithiyan110 Jul 6, 2025
88404fd
Refactor diff_drive_controller parameter config
adhithiyan110 Jul 6, 2025
a138243
Merge branch 'master' into master
christophfroehlich Jul 7, 2025
f44bf43
add try_update_params to the update_reference_from_subscribers
adhithiyan110 Jul 30, 2025
aa90421
Merge branch 'master' of github.com:adhithiyan110/ros2_controllers
adhithiyan110 Jul 30, 2025
582fa70
Merge branch 'ros-controls:master' into master
adhithiyan110 Jul 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 32 additions & 31 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>(params_.velocity_rolling_window_size));
Comment on lines +118 to +126
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

these parameters are read-only, better leave them in on_configure


limiter_linear_ = std::make_unique<SpeedLimiter>(
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<SpeedLimiter>(
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);
Comment on lines +128 to +138
Copy link
Contributor

@christophfroehlich christophfroehlich Jul 30, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This method is part of the RT loop, we must not allocate memory here. Better leave the initial construction of the objects in the on_configure, but update the parameters (only if they changed, i.e., inside the if clause above).
To achieve this, we have to expose the control_toolbox::RateLimiter::set_params method in the diff_drive_controller::SpeedLimiter class.
https://github.com/ros-controls/control_toolbox/blob/c46e3c5b56d62c30e30e42d3658821fbf909dd3e/control_toolbox/include/control_toolbox/rate_limiter.hpp#L107-L132

Furthermore, we should change the rate_limiter class to throw the errors of the input checks before the member variables are set. So we can catch the errors in the diff_drive_controller::SpeedLimiter::set_params method, otherwise we have to reimplement the checks to avoid crashing the controller: ros-controls/control_toolbox#437



auto current_ref_op = received_velocity_msg_.try_get();
if (current_ref_op.has_value())
{
Expand Down Expand Up @@ -311,12 +342,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(
Expand All @@ -325,32 +350,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<size_t>(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<double>::quiet_NaN());

limiter_linear_ = std::make_unique<SpeedLimiter>(
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<SpeedLimiter>(
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;
Expand Down Expand Up @@ -444,9 +447,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 =
Expand Down
36 changes: 36 additions & 0 deletions diff_drive_controller/src/diff_drive_controller_parameter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<>: []
}
Expand All @@ -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<>: []
}
Expand All @@ -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]
}
Expand All @@ -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]
}
Expand All @@ -35,94 +39,112 @@ 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: true,
}
position_feedback: {
type: bool,
default_value: true,
description: "Is there position feedback from hardware.",
read_only: true,
}
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: true,
}
linear:
x:
max_velocity: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::gt_eq_or_nan<>": [0.0]
}
}
min_velocity: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::lt_eq_or_nan<>": [0.0]
}
Expand All @@ -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]
}
Expand All @@ -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]
}
Expand All @@ -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]
}
Expand All @@ -155,20 +180,23 @@ 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]
}
}
max_jerk: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::gt_eq_or_nan<>": [0.0]
}
}
min_jerk: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::lt_eq_or_nan<>": [0.0]
}
Expand All @@ -178,13 +206,15 @@ diff_drive_controller:
max_velocity: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::gt_eq_or_nan<>": [0.0]
}
}
min_velocity: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::lt_eq_or_nan<>": [0.0]
}
Expand All @@ -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]
}
Expand All @@ -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]
}
Expand All @@ -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]
}
Expand All @@ -217,20 +250,23 @@ 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]
}
}
max_jerk: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::gt_eq_or_nan<>": [0.0]
}
}
min_jerk: {
type: double,
default_value: .NAN,
read_only: false,
validation: {
"control_filters::lt_eq_or_nan<>": [0.0]
}
Expand Down
Loading