From a0e32872a77baa5174245426a8af591703c1a97f Mon Sep 17 00:00:00 2001 From: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Date: Thu, 12 Sep 2024 08:28:07 -0700 Subject: [PATCH] [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy * Cleanup Signed-off-by: huiyulhy * fix linting Signed-off-by: huiyulhy * Update linting Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy * uncrustify Signed-off-by: huiyulhy --------- Signed-off-by: huiyulhy --- .../include/dwb_plugins/standard_traj_generator.hpp | 3 +++ .../dwb_plugins/src/standard_traj_generator.cpp | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index 87ba77baf02..db5467cd48d 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -147,6 +147,9 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator /// @brief the name of the overlying plugin ID std::string plugin_name_; + /// @brief Option to limit velocity in the trajectory generator by using current velocity + bool limit_vel_cmd_in_traj_; + /* Backwards Compatibility Parameter: include_last_point * * dwa had an off-by-one error built into it. diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 49a62de302e..3b57df08e1e 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -75,6 +75,10 @@ void StandardTrajectoryGenerator::initialize( nh, plugin_name + ".include_last_point", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + nh, + plugin_name + ".limit_vel_cmd_in_traj", rclcpp::ParameterValue(false)); + /* * If discretize_by_time, then sim_granularity represents the amount of time that should be between * two successive points on the trajectory. @@ -89,6 +93,7 @@ void StandardTrajectoryGenerator::initialize( nh->get_parameter(plugin_name + ".linear_granularity", linear_granularity_); nh->get_parameter(plugin_name + ".angular_granularity", angular_granularity_); nh->get_parameter(plugin_name + ".include_last_point", include_last_point_); + nh->get_parameter(plugin_name + ".limit_vel_cmd_in_traj", limit_vel_cmd_in_traj_); } void StandardTrajectoryGenerator::initializeIterator( @@ -156,9 +161,14 @@ dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory( double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); traj.poses.push_back(start_pose); + bool first_vel = false; for (double dt : steps) { // calculate velocities vel = computeNewVelocity(cmd_vel, vel, dt); + if (!first_vel && limit_vel_cmd_in_traj_) { + traj.velocity = vel; + first_vel = true; + } // update the position of the robot using the velocities passed in pose = computeNewPosition(pose, vel, dt);