diff --git a/configuration/packages/configuring-regulated-pp.rst b/configuration/packages/configuring-regulated-pp.rst index d7eb2bacd..b1d835e42 100644 --- a/configuration/packages/configuring-regulated-pp.rst +++ b/configuration/packages/configuring-regulated-pp.rst @@ -12,6 +12,7 @@ It regulates the linear velocities by curvature of the path to help reduce overs It also better follows paths than any other variation currently available of Pure Pursuit. It also has heuristics to slow in proximity to other obstacles so that you can slow the robot automatically when nearby potential collisions. It also implements the Adaptive lookahead point features to be scaled by velocities to enable more stable behavior in a larger range of translational speeds. +It also considers the robot’s velocity and acceleration constraints during velocity command computation using `Dynamic Window Pure Pursuit `_ algorithm. See the package's ``README`` for more complete information. @@ -22,7 +23,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this Regulated Pure Pursuit Parameters ********************************* -:desired_linear_vel: +:max_linear_vel: ============== =========================== Type Default @@ -31,7 +32,84 @@ Regulated Pure Pursuit Parameters ============== =========================== Description - The desired maximum linear velocity (m/s) to use. + The maximum linear velocity (m/s) to use. Previously `desired_linear_vel` + +:min_linear_vel: + + ============== =========================== + Type Default + -------------- --------------------------- + double -0.5 + ============== =========================== + + Description + The minimum linear velocity (m/s) used when `use_dynamic_window` is `true`. + +:max_angular_vel: + + ============== =========================== + Type Default + -------------- --------------------------- + double 2.5 + ============== =========================== + + Description + The maximum angular velocity (rad/s) used when `use_dynamic_window` is `true`. + +:min_angular_vel: + + ============== =========================== + Type Default + -------------- --------------------------- + double -2.5 + ============== =========================== + + Description + The minimum angular velocity (rad/s) used when `use_dynamic_window` is `true`. + +:max_linear_accel: + + ============== =========================== + Type Default + -------------- --------------------------- + double 2.5 + ============== =========================== + + Description + The maximum linear acceleration (m/s^2) used when `use_dynamic_window` is `true`. + +:max_linear_decel: + + ============== =========================== + Type Default + -------------- --------------------------- + double -2.5 + ============== =========================== + + Description + The maximum linear deceleration (m/s^2) used when `use_dynamic_window` is `true`. + +:max_angular_accel: + + ============== =========================== + Type Default + -------------- --------------------------- + double 3.2 + ============== =========================== + + Description + The maximum angular acceleration (rad/s^2) to use. + +:max_angular_decel: + + ============== =========================== + Type Default + -------------- --------------------------- + double -3.2 + ============== =========================== + + Description + The maximum angular deceleration (rad/s^2) used when `use_dynamic_window` is `true`. :lookahead_dist: @@ -266,17 +344,6 @@ Regulated Pure Pursuit Parameters Description The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place, if ``use_rotate_to_heading`` is ``true``. -:max_angular_accel: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 3.2 - ============== ============================= - - Description - Maximum allowable angular acceleration (rad/s/s) while rotating to heading, if ``use_rotate_to_heading`` is ``true``. - :use_cancel_deceleration: ============== ============================= @@ -345,6 +412,19 @@ Regulated Pure Pursuit Parameters Description 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: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool false + ============== ============================= + + Description + Whether to use the Dynamic Window Pure Pursuit (DWPP) Algorithm. This algorithm computes optimal path tracking velocity commands under velocity and acceleration constraints. + Fumiya Ohnishi and Masaki Takahashi, "Dynamic Window Pure Pursuit for Robot Path Tracking Considering Velocity and Acceleration Constraints", the 19th International Conference on Intelligent Autonomous Systems (IAS-19), 2025. + + Example ******* .. code-block:: yaml @@ -370,7 +450,14 @@ Example 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 @@ -392,6 +479,6 @@ Example use_rotate_to_heading: true allow_reversing: false rotate_to_heading_min_angle: 0.785 - max_angular_accel: 3.2 min_distance_to_obstacle: 0.0 stateful: true + use_dynamic_window: false diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 8bf06d7c0..59f10d01e 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -563,6 +563,30 @@ The UI workflow is now organized into two primary navigation modes: GIF above shows how multiple-goal navigation is configured mixing visual goal setting and file loading for NavigateThroughPoses and Waypoint Following actions. +Add Dynamic Window Pure Pursuit Option to Regulated Pure Pursuit Controller +--------------------------------------------------------------------------- + +In `PR #5783 `_, an option was added to enable the Dynamic Window Pure Pursuit (DWPP) algorithm in the Regulated Pure Pursuit controller. When this option is enabled, velocity and acceleration constraints are explicitly considered when computing command velocities. See the Configuration Guide for the new parameters associated with this feature. + +- Fumiya Ohnishi and 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, Genoa, Italy, 2025. + +The following parameters are updated for this feature. + +:max_linear_vel (renamed): + + ============== =========================== + Type Default + -------------- --------------------------- + double 0.5 + ============== =========================== + + Description + The maximum linear velocity (m/s) to use. **Previously named `desired_linear_vel`** + + +**Note:** The velocity smoother clips velocity commands produced by this controller according to its own velocity and acceleration limits before publishing `cmd_vel`. +Therefore, the velocity smoother parameters `max_velocity`, `min_velocity`, `max_accel`, and `max_decel` must be set to values consistent with, or greater than, the corresponding velocity, acceleration, and deceleration parameters of this controller. + Bond Heartbeat Period Default Value Change ------------------------------------------