[DWB] Option to limit velocity commands in trajectory generator (#4663)

* Option to limit vel cmd through traj generator Signed-off-by: huiyulhy <[email protected]> * Cleanup Signed-off-by: huiyulhy <[email protected]> * fix linting Signed-off-by: huiyulhy <[email protected]> * Update linting Signed-off-by: huiyulhy <[email protected]> * uncrustify Signed-off-by: huiyulhy <[email protected]> * uncrustify Signed-off-by: huiyulhy <[email protected]> --------- Signed-off-by: huiyulhy <[email protected]>
ros-navigation · Sep 12, 2024 · 271d2d6 · 271d2d6
1 parent 7eb47d8
commit 271d2d6
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
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
@@ -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
@@ -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<double> 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);