Implementation of Dynamic Window Pure Pursuit (DWPP)

nav2_regulated_pure_pursuit_controller/README.md

@@ -61,11 +61,25 @@ 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).
+*Fumiya Ohnishi, Masaki Takahashi, "Dynamic Window Pure Pursuit for Robot Path Tracking Considering Velocity and Acceleration Constraints", Proceedings of the 19th International Conference on Intelligent Autonomous Systems (IAS-19), Genoa, Italy, 2025.
+
 ## Configuration
 
 | Parameter | Description |
 |-----|----|
-| `desired_linear_vel` | The desired maximum linear velocity to use. |
+| `max_linear_vel` | The maximum linear velocity to use. Previously `desired_linear_vel` |
+| `max_angular_accel` | The maximum angular acceleration to use. |
+| `min_linear_vel` | The minimum linear velocity used when `use_dynamic_window` is `true`. |
+| `max_angular_vel` | The maximum angular velocity used when `use_dynamic_window` is `true`. |
+| `min_angular_vel` | The minimum angular velocity used when `use_dynamic_window` is `true`. |
+| `max_linear_accel` | The maximum linear acceleration used when `use_dynamic_window` is `true`. |
+| `max_linear_decel` | The maximum linear deceleration used when `use_dynamic_window` is `true`. |
+| `max_angular_decel` | The maximum angular deceleration used when `use_dynamic_window` is `true`. |
 | `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 |
@@ -87,9 +101,9 @@ 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 |
 | `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. |
 
 Example fully-described XML with default parameter values:
 
@@ -115,7 +129,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.5
+      max_angular_vel: 2.5
+      min_angular_vel: -2.5
+      max_linear_accel: 2.5
+      max_linear_decel: -2.5
+      max_angular_accel: 3.2
+      max_angular_decel: -3.2
       lookahead_dist: 0.6
       min_lookahead_dist: 0.3
       max_lookahead_dist: 0.9
@@ -134,11 +155,11 @@ controller_server:
       curvature_lookahead_dist: 1.0
       use_rotate_to_heading: true
       rotate_to_heading_min_angle: 0.785
-      max_angular_accel: 3.2
       interpolate_curvature_after_goal: false
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0
+      use_dynamic_window: false
 ```
 
 ## Topics
@@ -161,3 +182,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 or greater than this controller’s corresponding velocity, acceleration, and deceleration settings.

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/dynamic_window_pure_pursuit_functions.hpp

@@ -0,0 +1,318 @@
+// 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_
+#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_
+
+#include <string>
+#include <vector>
+#include <algorithm>
+#include <tuple>
+#include <utility>
+#include <limits>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace nav2_regulated_pure_pursuit_controller
+{
+
+namespace dynamic_window_pure_pursuit
+{
+
+struct DynamicWindowBounds
+{
+  double max_linear_vel;
+  double min_linear_vel;
+  double max_angular_vel;
+  double min_angular_vel;
+};
+
+/**
+ * @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 max_linear_vel    Maximum allowable linear velocity
+ * @param min_linear_vel    Minimum allowable linear velocity
+ * @param max_angular_vel   Maximum allowable angular velocity
+ * @param min_angular_vel   Minimum allowable angular velocity
+ * @param max_linear_accel  Maximum allowable linear acceleration
+ * @param max_linear_decel  Maximum allowable linear deceleration
+ * @param max_angular_accel Maximum allowable angular acceleration
+ * @param max_angular_decel Maximum allowable angular deceleration
+ * @param dt                Control duration
+ * @return                  Computed dynamic window's velocity bounds
+ */
+inline DynamicWindowBounds computeDynamicWindow(
+  const geometry_msgs::msg::Twist & current_speed,
+  const double & max_linear_vel,
+  const double & min_linear_vel,
+  const double & max_angular_vel,
+  const double & min_angular_vel,
+  const double & max_linear_accel,
+  const double & max_linear_decel,
+  const double & max_angular_accel,
+  const double & max_angular_decel,
+  const double & dt
+)
+{
+  DynamicWindowBounds dynamic_window;
+  constexpr double Eps = 1e-3;
+
+  // function to compute dynamic window for a single dimension
+  auto compute_window = [&](const double & current_vel, const double & max_vel,
+    const double & min_vel, const double & max_accel, const double & max_decel)
+    {
+      double candidate_max_vel = 0.0;
+      double candidate_min_vel = 0.0;
+
+      if (current_vel > Eps) {
+      // if the current velocity is positive, acceleration means an increase in speed
+        candidate_max_vel = current_vel + max_accel * dt;
+        candidate_min_vel = current_vel + max_decel * dt;
+      } else if (current_vel < -Eps) {
+      // if the current velocity is negative, acceleration means a decrease in speed
+        candidate_max_vel = current_vel - max_decel * dt;
+        candidate_min_vel = current_vel - max_accel * dt;
+      } else {
+      // if the current velocity is zero, allow acceleration in both directions.
+        candidate_max_vel = current_vel + max_accel * dt;
+        candidate_min_vel = current_vel - max_accel * dt;
+      }
+
+      // clip to max/min velocity limits
+      double dynamic_window_max_vel = std::min(candidate_max_vel, max_vel);
+      double dynamic_window_min_vel = std::max(candidate_min_vel, min_vel);
+      return std::make_tuple(dynamic_window_max_vel, dynamic_window_min_vel);
+    };
+
+  // linear velocity
+  std::tie(dynamic_window.max_linear_vel, dynamic_window.min_linear_vel) =
+    compute_window(current_speed.linear.x, max_linear_vel, min_linear_vel,
+    max_linear_accel, max_linear_decel);
+
+  // angular velocity
+  std::tie(dynamic_window.max_angular_vel, dynamic_window.min_angular_vel) =
+    compute_window(current_speed.angular.z, max_angular_vel, min_angular_vel,
+    max_angular_accel, max_angular_decel);
+
+  return dynamic_window;
+}
+
+/**
+ * @brief                        Apply regulated linear velocity to the dynamic window
+ * @param regulated_linear_vel   Regulated linear velocity
+ * @param dynamic_window         Dynamic window to be regulated
+ */
+inline void applyRegulationToDynamicWindow(
+  const double & regulated_linear_vel,
+  DynamicWindowBounds & dynamic_window)
+{
+  // Create regulated bounds [0, v_reg] or [v_reg, 0]
+  double v_reg_min = std::min(0.0, regulated_linear_vel);
+  double v_reg_max = std::max(0.0, regulated_linear_vel);
+
+  // Intersect the dynamic window with the regulated bounds
+  dynamic_window.min_linear_vel = std::max(dynamic_window.min_linear_vel, v_reg_min);
+  dynamic_window.max_linear_vel = std::min(dynamic_window.max_linear_vel, v_reg_max);
+
+  // If min > max, collapse to the nearest boundary
+  if (dynamic_window.min_linear_vel > dynamic_window.max_linear_vel) {
+    if (dynamic_window.min_linear_vel > v_reg_max) {
+      dynamic_window.max_linear_vel = dynamic_window.min_linear_vel;
+    } else {
+      dynamic_window.min_linear_vel = dynamic_window.max_linear_vel;
+    }
+  }
+}
+
+/**
+ * @brief                Compute the optimal velocity to follow the path within the dynamic window
+ * @param dynamic_window Dynamic window defining feasible velocity bounds
+ * @param curvature      Curvature of the path to follow
+ * @param sign           Velocity sign (forward or backward)
+ * @return               Optimal linear and angular velocity
+ */
+inline std::tuple<double, double> computeOptimalVelocityWithinDynamicWindow(
+  const DynamicWindowBounds & dynamic_window,
+  const double & curvature,
+  const double & sign
+)
+{
+  double optimal_linear_vel;
+  double optimal_angular_vel;
+
+  // consider linear_vel - angular_vel space (horizontal and vertical axes respectively)
+  // Select the closest point to the line
+  // angular_vel = curvature * linear_vel within the dynamic window.
+  // If multiple points are equally close, select the one with the largest linear_vel.
+
+  // When curvature == 0, the line is angular_vel = 0
+  if (abs(curvature) < 1e-3) {
+    // linear velocity
+    if (sign >= 0.0) {
+      // If moving forward, select the max linear vel
+      optimal_linear_vel = dynamic_window.max_linear_vel;
+    } else {
+      // If moving backward, select the min linear vel
+      optimal_linear_vel = dynamic_window.min_linear_vel;
+    }
+
+    // angular velocity
+    // If the line angular_vel = 0 intersects the dynamic window,angular_vel = 0.0
+    if (dynamic_window.min_angular_vel <= 0.0 && 0.0 <= dynamic_window.max_angular_vel) {
+      optimal_angular_vel = 0.0;
+    } else {
+      // If not, select angular vel within dynamic window closest to 0
+      if (std::abs(dynamic_window.min_angular_vel) <= std::abs(dynamic_window.max_angular_vel)) {
+        optimal_angular_vel = dynamic_window.min_angular_vel;
+      } else {
+        optimal_angular_vel = dynamic_window.max_angular_vel;
+      }
+    }
+    return std::make_tuple(optimal_linear_vel, optimal_angular_vel);
+  }
+
+  // When the dynamic window and the line angular_vel = curvature * linear_vel intersect,
+  // select the intersection point that yields the highest linear velocity.
+
+  // List the four candidate intersection points
+  std::pair<double, double> candidates[] = {
+    {dynamic_window.min_linear_vel, curvature * dynamic_window.min_linear_vel},
+    {dynamic_window.max_linear_vel, curvature * dynamic_window.max_linear_vel},
+    {dynamic_window.min_angular_vel / curvature, dynamic_window.min_angular_vel},
+    {dynamic_window.max_angular_vel / curvature, dynamic_window.max_angular_vel}
+  };
+
+  double best_linear_vel = -std::numeric_limits<double>::max() * sign;
+  double best_angular_vel = 0.0;
+
+  for (auto [linear_vel, angular_vel] : candidates) {
+    // Check whether the candidate lies within the dynamic window
+    if (linear_vel >= dynamic_window.min_linear_vel &&
+      linear_vel <= dynamic_window.max_linear_vel &&
+      angular_vel >= dynamic_window.min_angular_vel &&
+      angular_vel <= dynamic_window.max_angular_vel)
+    {
+      // Select the candidate with the largest linear velocity (considering moving direction)
+      if (linear_vel * sign > best_linear_vel * sign) {
+        best_linear_vel = linear_vel;
+        best_angular_vel = angular_vel;
+      }
+    }
+  }
+
+  // If best_linear_vel was updated, it means that a valid intersection exists
+  if (best_linear_vel != -std::numeric_limits<double>::max() * sign) {
+    optimal_linear_vel = best_linear_vel;
+    optimal_angular_vel = best_angular_vel;
+    return std::make_tuple(optimal_linear_vel, optimal_angular_vel);
+  }
+
+  // When the dynamic window and the line angular_vel = curvature * linear_vel have no intersection,
+  // select the point within the dynamic window that is closest to the line.
+
+  // Because the dynamic window is a convex region,
+  // the closest point must be one of its four corners.
+  const std::array<std::array<double, 2>, 4> corners = {{
+    {dynamic_window.min_linear_vel, dynamic_window.min_angular_vel},
+    {dynamic_window.min_linear_vel, dynamic_window.max_angular_vel},
+    {dynamic_window.max_linear_vel, dynamic_window.min_angular_vel},
+    {dynamic_window.max_linear_vel, dynamic_window.max_angular_vel}
+  }};
+
+  // Compute the distance from a point (linear_vel, angular_vel)
+  // to the line angular_vel = curvature * linear_vel
+  const double denom = std::sqrt(curvature * curvature + 1.0);
+  auto compute_dist = [&](const std::array<double, 2> & corner) -> double {
+      return std::abs(curvature * corner[0] - corner[1]) / denom;
+    };
+
+  double closest_dist = std::numeric_limits<double>::max();
+  best_linear_vel = -std::numeric_limits<double>::max() * sign;
+  best_angular_vel = 0.0;
+
+  for (const auto & corner : corners) {
+    const double dist = compute_dist(corner);
+    // Update if this corner is closer to the line,
+    // or equally close but has a larger linear velocity (considering moving direction)
+    if (dist < closest_dist ||
+      (std::abs(dist - closest_dist) <= 1e-3 && corner[0] * sign > best_linear_vel * sign))
+    {
+      closest_dist = dist;
+      best_linear_vel = corner[0];
+      best_angular_vel = corner[1];
+    }
+  }
+
+  optimal_linear_vel = best_linear_vel;
+  optimal_angular_vel = best_angular_vel;
+
+  return std::make_tuple(optimal_linear_vel, optimal_angular_vel);
+}
+
+/**
+ * @brief Compute velocity commands using Dynamic Window Pure Pursuit method
+ * @param current_speed         Current linear and angular velocity of the robot
+ * @param max_linear_vel        Maximum allowable linear velocity
+ * @param min_linear_vel        Minimum allowable linear velocity
+ * @param max_angular_vel       Maximum allowable angular velocity
+ * @param min_angular_vel       Minimum allowable angular velocity
+ * @param max_linear_accel      Maximum allowable linear acceleration
+ * @param max_linear_decel      Maximum allowable linear deceleration
+ * @param max_angular_accel     Maximum allowable angular acceleration
+ * @param max_angular_decel     Maximum allowable angular deceleration
+ * @param regulated_linear_vel  Regulated linear velocity
+ * @param curvature             Curvature of the path to follow
+ * @param sign                  Velocity sign (forward or backward)
+ * @param dt                    Control duration
+ * @return                      Optimal linear and angular velocity
+ */
+inline std::tuple<double, double> computeDynamicWindowVelocities(
+  const geometry_msgs::msg::Twist & current_speed,
+  const double & max_linear_vel,
+  const double & min_linear_vel,
+  const double & max_angular_vel,
+  const double & min_angular_vel,
+  const double & max_linear_accel,
+  const double & max_linear_decel,
+  const double & max_angular_accel,
+  const double & max_angular_decel,
+  const double & regulated_linear_vel,
+  const double & curvature,
+  const double & sign,
+  const double & dt
+)
+{
+  // compute Dynamic Window
+  DynamicWindowBounds dynamic_window = computeDynamicWindow(
+    current_speed, max_linear_vel, min_linear_vel, max_angular_vel, min_angular_vel,
+    max_linear_accel, max_linear_decel, max_angular_accel, max_angular_decel, dt);
+
+  // apply regulation to Dynamic Window
+  applyRegulationToDynamicWindow(regulated_linear_vel, dynamic_window);
+
+  // compute optimal velocity within Dynamic Window
+  auto [linear_vel, angular_vel] = computeOptimalVelocityWithinDynamicWindow(
+    dynamic_window, curvature, sign);
+
+  return std::make_tuple(linear_vel, angular_vel);
+}
+
+
+}  // namespace dynamic_window_pure_pursuit
+
+}  // namespace nav2_regulated_pure_pursuit_controller
+
+#endif  // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__DYNAMIC_WINDOW_PURE_PURSUIT_FUNCTIONS_HPP_