From e10569469f67181e20056e3905d0aaa59669ea41 Mon Sep 17 00:00:00 2001 From: Ivan Shalnov Date: Wed, 28 Jul 2021 17:53:24 +0300 Subject: [PATCH] added acc, vel and jerk as dynamic parameters --- .../cfg/DiffDriveController.cfg | 5 +++++ .../diff_drive_controller.h | 17 ++++++++++++++ .../src/diff_drive_controller.cpp | 22 +++++++++++++++++++ 3 files changed, 44 insertions(+) diff --git a/diff_drive_controller/cfg/DiffDriveController.cfg b/diff_drive_controller/cfg/DiffDriveController.cfg index 31a166830..0de1e1084 100755 --- a/diff_drive_controller/cfg/DiffDriveController.cfg +++ b/diff_drive_controller/cfg/DiffDriveController.cfg @@ -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) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index 798e6fa93..953f8b89f 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -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_; @@ -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; @@ -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) @@ -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" diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 018bd177c..bf4aab8f1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -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") @@ -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_; @@ -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_; @@ -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; @@ -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; }