Implementation of Dynamic Window Pure Pursuit (DWPP)

nav2_regulated_pure_pursuit_controller/README.md

@@ -61,11 +61,26 @@ Mixing the proximity and curvature regulated linear velocities with the time-sca
 
 Note: The maximum allowed time to collision is thresholded by the lookahead point, starting in Humble. This is such that collision checking isn't significantly overshooting the path, which can cause issues in constrained environments. For example, if there were a straight-line path going towards a wall that then turned left, if this parameter was set to high, then it would detect a collision past the point of actual robot intended motion. Thusly, if a robot is moving fast, selecting further out lookahead points is not only a matter of behavioral stability for Pure Pursuit, but also gives a robot further predictive collision detection capabilities. The max allowable time parameter is still in place for slow commands, as described in detail above.
 
+## Dynamic Window Pure Pursuit Features
+
+This controller also implements the Dynamic Window Pure Pursuit (DWPP) algorithm, developed by [Fumiya Ohnishi](https://www.linkedin.com/in/fumiya-ohnishi-23b124202).
+Unlike the standard Pure Pursuit, DWPP enables the consideration of velocity and acceleration constraints when computing velocity commands.
+An overview of the algorithm can be found here: [DWPP Algorithm](https://github.com/Decwest/nav2_dynamic_window_pure_pursuit_controller/blob/main/algorithm.md).
+
+A link to the paper and its citation will be provided once it becomes publicly available.
+
 ## Configuration
 
 | Parameter | Description |
 |-----|----|
-| `desired_linear_vel` | The desired maximum linear velocity to use. |
+| `max_linear_vel` | The maximum linear velocity to use. |
+| `min_linear_vel` | The minimum linear velocity to use. |
+| `max_angular_vel` | The maximum angular velocity to use. |
+| `min_angular_vel` | The minimum angular velocity to use. |
+| `max_linear_accel` | The maximum linear acceleration to use. |
+| `max_linear_decel` | The maximum linear deceleration to use. |
+| `max_angular_accel` | The maximum angular acceleration to use. |
+| `max_angular_decel` | The maximum angular deceleration to use. |
 | `lookahead_dist` | The lookahead distance to use to find the lookahead point |
 | `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances |
 | `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances |
@@ -88,10 +103,11 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `curvature_lookahead_dist` | Distance to lookahead to determine curvature for velocity regulation purposes. Only used if `use_fixed_curvature_lookahead` is enabled. |
 | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. |
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. |
-| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled |
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. |
 | `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvature calculation (to avoid oscillations at the end of the path) |
 | `min_distance_to_obstacle` | The shortest distance at which the robot is allowed to be from an obstacle along its trajectory. Set <= 0.0 to disable. It is limited to maximum distance of lookahead distance selected. |
+| `use_dynamic_window` | Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. |
+| `velocity_feedback` | How the current velocity is obtained during dynamic window computation. "OPEN_LOOP" uses the last commanded velocity (recommended). "CLOSED_LOOP" uses odometry velocity (may hinder proper acceleration/deceleration) |
 
 Example fully-described XML with default parameter values:
 
@@ -117,7 +133,14 @@ controller_server:
       stateful: True
     FollowPath:
       plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-      desired_linear_vel: 0.5
+      max_linear_vel: 0.5
+      min_linear_vel: 0.0
+      max_angular_vel: 1.0
+      min_angular_vel: -1.0
+      max_linear_accel: 0.5
+      max_linear_decel: 0.5
+      max_angular_accel: 1.0
+      max_angular_decel: 1.0
       lookahead_dist: 0.6
       min_lookahead_dist: 0.3
       max_lookahead_dist: 0.9
@@ -126,23 +149,26 @@ controller_server:
       transform_tolerance: 0.1
       use_velocity_scaled_lookahead_dist: false
       min_approach_linear_velocity: 0.05
-      approach_velocity_scaling_dist: 1.0
+      approach_velocity_scaling_dist: 0.6
       use_collision_detection: true
       max_allowed_time_to_collision_up_to_carrot: 1.0
       use_regulated_linear_velocity_scaling: true
       use_cost_regulated_linear_velocity_scaling: false
       regulated_linear_scaling_min_radius: 0.9
       regulated_linear_scaling_min_speed: 0.25
       use_fixed_curvature_lookahead: false
-      curvature_lookahead_dist: 1.0
+      curvature_lookahead_dist: 0.6
       use_rotate_to_heading: true
+      allow_reversing: false
       rotate_to_heading_min_angle: 0.785
-      max_angular_accel: 3.2
       max_robot_pose_search_dist: 10.0
       interpolate_curvature_after_goal: false
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0
+      min_distance_to_obstacle: 0.0
+      use_dynamic_window: true
+      velocity_feedback: "OPEN_LOOP"
 ```
 
 ## Topics
@@ -165,3 +191,7 @@ To tune to get Pure Pursuit behaviors, set all boolean parameters to false and m
 Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way.
 
 The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it.
+
+When `use_dynamic_window` is set to True, the velocity, acceleration, and deceleration limits are enforced during the velocity command computation.
+Note that the velocity smoother clips the velocity commands output by this controller based on its own velocity and acceleration constraints before publishing cmd_vel.
+Therefore, the velocity smoother’s `max_velocity`, `min_velocity`, `max_accel`, and `max_decel` parameters must be consistent with this controller’s corresponding velocity, acceleration, and deceleration settings.

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America
+// Copyright (c) 2025 Fumiya Ohnishi
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -32,7 +33,8 @@ namespace nav2_regulated_pure_pursuit_controller
 
 struct Parameters
 {
-  double desired_linear_vel, base_desired_linear_vel;
+  double max_linear_vel, base_max_linear_vel, min_linear_vel;
+  double max_angular_vel, min_angular_vel;
   double lookahead_dist;
   double rotate_to_heading_angular_vel;
   double max_lookahead_dist;
@@ -53,7 +55,8 @@ struct Parameters
   bool use_fixed_curvature_lookahead;
   double curvature_lookahead_dist;
   bool use_rotate_to_heading;
-  double max_angular_accel;
+  double max_linear_accel, max_angular_accel;
+  double max_linear_decel, max_angular_decel;
   bool use_cancel_deceleration;
   double cancel_deceleration;
   double rotate_to_heading_min_angle;
@@ -63,6 +66,8 @@ struct Parameters
   bool use_collision_detection;
   double transform_tolerance;
   bool stateful;
+  bool use_dynamic_window;
+  std::string velocity_feedback;
 };
 
 /**

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -1,5 +1,6 @@
 // Copyright (c) 2020 Shrijit Singh
 // Copyright (c) 2020 Samsung Research America
+// Copyright (c) 2025 Fumiya Ohnishi
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -80,6 +81,55 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   void deactivate() override;
 
+  /**
+   * @brief Compute the dynamic window (feasible velocity bounds) based on the current speed and the given velocity and acceleration constraints.
+   * @param current_speed  Current linear and angular velocity of the robot
+   * @param dynamic_window_max_linear_vel  Computed upper bound of the linear velocity within the dynamic window
+   * @param dynamic_window_min_linear_vel  Computed lower bound of the linear velocity within the dynamic window
+   * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window
+   * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window
+   */
+  void computeDynamicWindow(
+    const geometry_msgs::msg::Twist & current_speed,
+    double & dynamic_window_max_linear_vel,
+    double & dynamic_window_min_linear_vel,
+    double & dynamic_window_max_angular_vel,
+    double & dynamic_window_min_angular_vel
+  );
+
+  /**
+   * @brief Apply regulated linear velocity to the dynamic window
+   * @param regulated_linear_vel   Regulated linear velocity
+   * @param dynamic_window_max_linear_vel  Computed upper bound of the linear velocity within the dynamic window
+   * @param dynamic_window_min_linear_vel  Computed lower bound of the linear velocity within the dynamic window
+   */
+  void applyRegulationToDynamicWindow(
+    const double & regulated_linear_vel,
+    double & dynamic_window_max_linear_vel,
+    double & dynamic_window_min_linear_vel);
+
+  /**
+   * @brief Compute the optimal velocity to follow the path within the dynamic window
+   * @param dynamic_window_max_linear_vel  Computed upper bound of the linear velocity within the dynamic window
+   * @param dynamic_window_min_linear_vel  Computed lower bound of the linear velocity within the dynamic window
+   * @param dynamic_window_max_angular_vel Computed upper bound of the angular velocity within the dynamic window
+   * @param dynamic_window_min_angular_vel Computed lower bound of the angular velocity within the dynamic window
+   * @param curvature      Curvature of the path to follow
+   * @param sign           Velocity sign (forward or backward)
+   * @param optimal_linear_vel   Optimal linear velocity to follow the path under velocity and acceleration constraints
+   * @param optimal_angular_vel   Optimal angular velocity to follow the path under velocity and acceleration constraints
+   */
+  void computeOptimalVelocityWithinDynamicWindow(
+    const double & dynamic_window_max_linear_vel,
+    const double & dynamic_window_min_linear_vel,
+    const double & dynamic_window_max_angular_vel,
+    const double & dynamic_window_min_angular_vel,
+    const double & curvature,
+    const double & sign,
+    double & optimal_linear_vel,
+    double & optimal_angular_vel
+  );
+
   /**
    * @brief Compute the best command given the current pose and velocity, with possible debug information
    *
@@ -195,6 +245,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   bool finished_cancelling_ = false;
   bool is_rotating_to_heading_ = false;
   bool has_reached_xy_tolerance_ = false;
+  geometry_msgs::msg::Twist last_command_velocity_;
 
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr global_path_pub_;
   nav2::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr carrot_pub_;

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America
+// Copyright (c) 2025 Fumiya Ohnishi
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -37,7 +38,13 @@ ParameterHandler::ParameterHandler(
   logger_ = logger;
 
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
+    node, plugin_name_ + ".max_linear_vel", rclcpp::ParameterValue(0.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".min_linear_vel", rclcpp::ParameterValue(0.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".min_angular_vel", rclcpp::ParameterValue(-1.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6));
   declare_parameter_if_not_declared(
@@ -88,7 +95,13 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+    node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(0.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(0.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
@@ -106,9 +119,16 @@ ParameterHandler::ParameterHandler(
     rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
       node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+      node, plugin_name_ + ".use_dynamic_window", rclcpp::ParameterValue(true));
+  declare_parameter_if_not_declared(
+      node, plugin_name_ + ".velocity_feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP")));
 
-  node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
-  params_.base_desired_linear_vel = params_.desired_linear_vel;
+  node->get_parameter(plugin_name_ + ".max_linear_vel", params_.max_linear_vel);
+  params_.base_max_linear_vel = params_.max_linear_vel;
+  node->get_parameter(plugin_name_ + ".min_linear_vel", params_.min_linear_vel);
+  node->get_parameter(plugin_name_ + ".max_angular_vel", params_.max_angular_vel);
+  node->get_parameter(plugin_name_ + ".min_angular_vel", params_.min_angular_vel);
   node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist);
   node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist);
   node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist);
@@ -163,7 +183,10 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading);
   node->get_parameter(
     plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
+  node->get_parameter(plugin_name_ + ".max_linear_accel", params_.max_linear_accel);
+  node->get_parameter(plugin_name_ + ".max_linear_decel", params_.max_linear_decel);
   node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
+  node->get_parameter(plugin_name_ + ".max_angular_decel", params_.max_angular_decel);
   node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration);
   node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration);
   node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
@@ -190,6 +213,8 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".use_collision_detection",
     params_.use_collision_detection);
   node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
+  node->get_parameter(plugin_name_ + ".use_dynamic_window", params_.use_dynamic_window);
+  node->get_parameter(plugin_name_ + ".velocity_feedback", params_.velocity_feedback);
 
   if (params_.inflation_cost_scaling_factor <= 0.0) {
     RCLCPP_WARN(
@@ -283,9 +308,9 @@ ParameterHandler::updateParametersCallback(
     if (param_type == ParameterType::PARAMETER_DOUBLE) {
       if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") {
         params_.inflation_cost_scaling_factor = parameter.as_double();
-      } else if (param_name == plugin_name_ + ".desired_linear_vel") {
-        params_.desired_linear_vel = parameter.as_double();
-        params_.base_desired_linear_vel = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".max_linear_vel") {
+        params_.max_linear_vel = parameter.as_double();
+        params_.base_max_linear_vel = parameter.as_double();
       } else if (param_name == plugin_name_ + ".lookahead_dist") {
         params_.lookahead_dist = parameter.as_double();
       } else if (param_name == plugin_name_ + ".max_lookahead_dist") {