Skip to content

Added acceleration, velocity and jerk as dynamic parameters to diff_drive_controller #570

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

Open
wants to merge 1 commit into
base: noetic-devel
Choose a base branch
from
Open

Added acceleration, velocity and jerk as dynamic parameters to diff_drive_controller #570

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
5 changes: 5 additions & 0 deletions diff_drive_controller/cfg/DiffDriveController.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,11 @@ gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multipli
gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)

# Acceleration, velocity, jerk parameters
gen.add("max_acceleration_z", double_t, 0, "Max acceleration.", 1.0, 0.1, 10.0)
gen.add("max_velocity", double_t, 0, "Max velocity.", 1.0, 0.1, 10.0)
gen.add("max_jerk", double_t, 0, "Max jerk.", 1.0, 0.1, 10.0)

# Publication related
gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0)
gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", True)
Expand Down
17 changes: 17 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,11 @@ namespace diff_drive_controller{
double left_wheel_radius_multiplier_;
double right_wheel_radius_multiplier_;

// Acceleration, velocity and jerk
double max_acceleration_z_;
double max_velocity_;
double max_jerk_;

/// Timeout to consider cmd_vel commands old:
double cmd_vel_timeout_;

Expand Down Expand Up @@ -197,6 +202,10 @@ namespace diff_drive_controller{
double right_wheel_radius_multiplier;
double wheel_separation_multiplier;

double max_acceleration_z;
double max_velocity;
double max_jerk;

bool publish_cmd;

double publish_rate;
Expand All @@ -206,6 +215,9 @@ namespace diff_drive_controller{
: left_wheel_radius_multiplier(1.0)
, right_wheel_radius_multiplier(1.0)
, wheel_separation_multiplier(1.0)
, max_acceleration_z(1.0)
, max_velocity(1.0)
, max_jerk(1.0)
, publish_cmd(false)
, publish_rate(50)
, enable_odom_tf(true)
Expand All @@ -220,6 +232,11 @@ namespace diff_drive_controller{
<< "\t\tright wheel radius multiplier: " << params.right_wheel_radius_multiplier << "\n"
<< "\t\twheel separation multiplier: " << params.wheel_separation_multiplier << "\n"
//
<< "\tVelocity, acceleration, jerk parameters:\n"
<< "\t\tmax_acceleration_z: " << params.max_acceleration_z << "\n"
<< "\t\tmax_velocity: " << params.max_velocity << "\n"
<< "\t\tmax_jerk: " << params.max_jerk << "\n"
//
<< "\tPublication parameters:\n"
<< "\t\tPublish executed velocity command: " << (params.publish_cmd?"enabled":"disabled") << "\n"
<< "\t\tPublication rate: " << params.publish_rate << "\n"
Expand Down
22 changes: 22 additions & 0 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 @@ -152,6 +152,9 @@ namespace diff_drive_controller{
, wheel_separation_multiplier_(1.0)
, left_wheel_radius_multiplier_(1.0)
, right_wheel_radius_multiplier_(1.0)
, max_acceleration_z_(1.0)
, max_velocity_(1.0)
, max_jerk_(1.0)
, cmd_vel_timeout_(0.5)
, allow_multiple_cmd_vel_publishers_(true)
, base_frame_id_("base_link")
Expand Down Expand Up @@ -361,6 +364,10 @@ namespace diff_drive_controller{
dynamic_params.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
dynamic_params.wheel_separation_multiplier = wheel_separation_multiplier_;

dynamic_params.max_acceleration_z = limiter_ang_.max_acceleration;
dynamic_params.max_velocity = limiter_lin_.max_velocity;
dynamic_params.max_jerk = limiter_lin_.max_jerk;

dynamic_params.publish_rate = publish_rate;
dynamic_params.enable_odom_tf = enable_odom_tf_;

Expand All @@ -372,6 +379,10 @@ namespace diff_drive_controller{
config.right_wheel_radius_multiplier = right_wheel_radius_multiplier_;
config.wheel_separation_multiplier = wheel_separation_multiplier_;

config.max_acceleration_z = limiter_ang_.max_acceleration;
config.max_velocity = limiter_lin_.max_velocity;
config.max_jerk = limiter_lin_.max_jerk;

config.publish_rate = publish_rate;
config.enable_odom_tf = enable_odom_tf_;

Expand Down Expand Up @@ -739,6 +750,10 @@ namespace diff_drive_controller{
dynamic_params.right_wheel_radius_multiplier = config.right_wheel_radius_multiplier;
dynamic_params.wheel_separation_multiplier = config.wheel_separation_multiplier;

dynamic_params.max_acceleration_z = config.max_acceleration_z;
dynamic_params.max_velocity = config.max_velocity;
dynamic_params.max_jerk = config.max_jerk;

dynamic_params.publish_rate = config.publish_rate;

dynamic_params.enable_odom_tf = config.enable_odom_tf;
Expand All @@ -757,6 +772,13 @@ namespace diff_drive_controller{
right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;
wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier;

limiter_ang_.max_acceleration = dynamic_params.max_acceleration_z;
limiter_ang_.min_acceleration = -dynamic_params.max_acceleration_z;
limiter_lin_.max_velocity = dynamic_params.max_velocity;
limiter_lin_.min_velocity = -dynamic_params.max_velocity;
limiter_lin_.max_jerk = dynamic_params.max_jerk;
limiter_lin_.min_jerk = -dynamic_params.max_jerk;

publish_period_ = ros::Duration(1.0 / dynamic_params.publish_rate);
enable_odom_tf_ = dynamic_params.enable_odom_tf;
}
Expand Down