Implementation of Dynamic Window Pure Pursuit (DWPP)

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.
@@ -33,6 +34,7 @@ namespace nav2_regulated_pure_pursuit_controller
 struct Parameters
 {
   double desired_linear_vel, base_desired_linear_vel;
+  double desired_angular_vel;
   double lookahead_dist;
   double rotate_to_heading_angular_vel;
   double max_lookahead_dist;
@@ -53,7 +55,7 @@ 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;
   bool use_cancel_deceleration;
   double cancel_deceleration;
   double rotate_to_heading_min_angle;
@@ -63,6 +65,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,22 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   void deactivate() override;
 
+  /**
+   * @brief Compute the optimal command given the current pose, velocity and acceleration constraints using dynamic window
+   * @param curvature      Curvature of the path to follow
+   * @param current_speed  Current robot velocity
+   * @param regulated_linear_vel   Regulated linear velocity
+   * @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 computeOptimalVelocityUsingDynamicWindow(
+    const double curvature,
+    const geometry_msgs::msg::Twist current_speed,
+    const double regulated_linear_vel,
+    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 +212,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.
@@ -38,6 +39,8 @@ ParameterHandler::ParameterHandler(
 
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".desired_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(
@@ -87,6 +90,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true));
   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_linear_accel", rclcpp::ParameterValue(0.5));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
   declare_parameter_if_not_declared(
@@ -106,9 +111,14 @@ 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_ + ".desired_angular_vel", params_.desired_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,6 +173,7 @@ 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_angular_accel", params_.max_angular_accel);
   node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration);
   node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration);
@@ -190,6 +201,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(

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -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.
@@ -162,6 +163,130 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point)
   }
 }
 
+void RegulatedPurePursuitController::computeOptimalVelocityUsingDynamicWindow(
+    const double curvature,
+    const geometry_msgs::msg::Twist current_speed,
+    const double regulated_linear_vel,
+    double & optimal_linear_vel,
+    double & optimal_angular_vel
+  )
+{
+  // ---- Parameters (assumed available in this scope) ----
+  const double A_MAX  = params_->max_linear_accel;      // A_MAX
+  const double V_MAX  = params_->desired_linear_vel;    // V_MAX
+  const double AW_MAX = params_->max_angular_accel;     // AW_MAX
+  const double W_MAX  = params_->desired_angular_vel;   // W_MAX
+  const double DT     = control_duration_;     // DT
+
+  const double V_MIN = -V_MAX;
+  const double W_MIN = -W_MAX;
+
+  // ---- 1) Dynamic Window ----
+  double dw_vmax = std::min(current_speed.linear.x  + A_MAX  * DT, V_MAX);
+  const double dw_vmin = std::max(current_speed.linear.x  - A_MAX  * DT, V_MIN);
+  double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX);
+  const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN);
+
+  // Reflect regulated v (tighten upper limit)
+  if (dw_vmax > regulated_linear_vel) {
+    dw_vmax = std::max(dw_vmin, regulated_linear_vel);
+  }
+
+  const double k = curvature;
+
+  // ---- Curvature is 0 (w = 0) ----
+  if (k == 0.0) {
+    // If w=0 is within DW, then the maximum linear speed is adopted as it is.
+    if (dw_wmin <= 0.0 && 0.0 <= dw_wmax) {
+      optimal_linear_vel  = dw_vmax; // Always maximum v
+      optimal_angular_vel = 0.0;
+      return;
+    }
+    // If w=0 is outside, choose the side closer to w=0 and with smaller |w|.
+    const double w_choice = (std::abs(dw_wmin) <= std::abs(dw_wmax)) ? dw_wmin : dw_wmax;
+    optimal_linear_vel  = dw_vmax;   // Always maximum v
+    optimal_angular_vel = w_choice;
+    return;
+  }
+
+  // ---- 2) Select 'max v' from the candidates in the DW among the intersections. ----
+  double best_v = -1e300;     // Initial value for maximization
+  double best_w = 0.0;
+
+  // Intersection with vertical edges
+  {
+    const double v1 = dw_vmin;
+    const double w1 = k * v1;
+    if (w1 >= dw_wmin && w1 <= dw_wmax) {
+      if (v1 > best_v) { best_v = v1; best_w = w1; }
+    }
+  }
+  {
+    const double v2 = dw_vmax;
+    const double w2 = k * v2;
+    if (w2 >= dw_wmin && w2 <= dw_wmax) {
+      if (v2 > best_v) { best_v = v2; best_w = w2; }
+    }
+  }
+
+  // Intersection with horizontal edge (k ! = 0)
+  {
+    const double v3 = dw_wmin / k;
+    if (v3 >= dw_vmin && v3 <= dw_vmax) {
+      const double w3 = dw_wmin;
+      if (v3 > best_v) { best_v = v3; best_w = w3; }
+    }
+  }
+  {
+    const double v4 = dw_wmax / k;
+    if (v4 >= dw_vmin && v4 <= dw_vmax) {
+      const double w4 = dw_wmax;
+      if (v4 > best_v) { best_v = v4; best_w = w4; }
+    }
+  }
+
+  if (best_v > -1e290) {
+    // Intersection found → Adopt max. v
+    optimal_linear_vel  = best_v;
+    optimal_angular_vel = best_w;
+    return;
+  }
+
+  // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners
+  struct Corner { double v; double w; };
+  const Corner corners[4] = {
+    {dw_vmin, dw_wmin},
+    {dw_vmin, dw_wmax},
+    {dw_vmax, dw_wmin},
+    {dw_vmax, dw_wmax}
+  };
+
+  const double denom = std::sqrt(k * k + 1.0); // Just sqrt once
+
+  auto euclid_dist = [&](const Corner &c) -> double {
+    // Distance from point (v, w) to line w - k v = 0
+    return std::abs(k * c.v - c.w) / denom;
+  };
+
+  double best_dist = 1e300;
+  best_v = corners[0].v;
+  best_w = corners[0].w;
+
+  for (int i = 0; i < 4; ++i) {
+    const double d = euclid_dist(corners[i]);
+    // 1) Smaller distance → Adopted
+    // 2) If distances are equal (~1e-12) → Choose the one with larger v (acceleration policy)
+    if (d < best_dist || (std::abs(d - best_dist) <= 1e-12 && corners[i].v > best_v)) {
+      best_dist = d;
+      best_v = corners[i].v;
+      best_w = corners[i].w;
+    }
+  }
+
+  optimal_linear_vel  = best_v;
+  optimal_angular_vel = best_w;
+}
+
 geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & pose,
   const geometry_msgs::msg::Twist & speed,
@@ -269,7 +394,21 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     }
 
     // Apply curvature to angular velocity after constraining linear velocity
-    angular_vel = linear_vel * regulation_curvature;
+    if (params_->use_dynamic_window == false){
+      angular_vel = linear_vel * regulation_curvature;
+    }
+    else{
+      // compute optimal path tracking velocity commands considering velocity and acceleration constraints
+      const double regulated_linear_vel = linear_vel;
+      if (params_->velocity_feedback == "CLOSED_LOOP"){
+        // using odom velocity as a current velocity (not recommended)
+        computeOptimalVelocityUsingDynamicWindow(regulation_curvature, speed, regulated_linear_vel, linear_vel, angular_vel);
+      }
+      else{
+        // using last command velocity as a current velocity (recommended)
+        computeOptimalVelocityUsingDynamicWindow(regulation_curvature, last_command_velocity_, regulated_linear_vel, linear_vel, angular_vel);
+      }
+    }
   }
 
   // Collision checking on this velocity heading
@@ -290,6 +429,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   cmd_vel.header = pose.header;
   cmd_vel.twist.linear.x = linear_vel;
   cmd_vel.twist.angular.z = angular_vel;
+
+  last_command_velocity_ = cmd_vel.twist;
+
   return cmd_vel;
 }