adding regulated PP configs / updates

configuration/index.rst

@@ -21,4 +21,5 @@ the best navigation performance.
    packages/configuring-dwb-controller.rst
    packages/configuring-map-server.rst
    packages/configuring-amcl.rst
-   packages/configuring-recovery-server.rst
+   packages/configuring-recovery-server.rst
+   packages/configuring-regulated-pp.rst

configuration/packages/configuring-regulated-pp.rst

@@ -0,0 +1,299 @@
+.. _configuring_regulated_pure_puruit:
+
+Regulated Pure Pursuit
+######################
+
+Source code on Github_.
+
+.. _Github: https://github.com/ros-planning/navigation2/tree/main/nav2_regulated_pure_pursuit_controller
+
+The Regulated Pure Pursuit controller implements a variation on the Pure Pursuit controller that specifically targeting service / industrial robot needs.
+It regulates the linear velocities by curvature of the path to help reduce overshoot at high speeds around blind corners allowing operations to be much more safe.
+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.
+
+See the package's ``README`` for more complete information.
+
+
+Regulated Pure Pursuit Parameters
+*********************************
+
+:desired_linear_vel:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  double         0.5 
+  ============== ===========================
+
+  Description
+    The desired maximum linear velocity (m/s) to use.
+
+:max_linear_accel:
+
+  ============== ===================================
+  Type           Default                                               
+  -------------- -----------------------------------
+  double         2.5            
+  ============== ===================================
+
+  Description
+    Maximum acceleration for linear velocity (m/s/s)
+
+:max_linear_decel:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         2.5
+  ============== =============================
+
+  Description
+    Maximum deceleration for linear velocity (m/s/s)
+
+:lookahead_dist:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.6
+  ============== =============================
+
+  Description
+    The lookahead distance (m) to use to find the lookahead point when ``use_velocity_scaled_lookahead_dist`` is ``false``.
+
+:min_lookahead_dist:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.3 
+  ============== =============================
+
+  Description
+    The minimum lookahead distance (m) threshold when ``use_velocity_scaled_lookahead_dist`` is ``true``.
+
+:max_lookahead_dist:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.9 
+  ============== =============================
+
+  Description
+    The maximum lookahead distance (m) threshold when ``use_velocity_scaled_lookahead_dist`` is ``true``.
+
+:lookahead_time:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         1.5
+  ============== =============================
+
+  Description
+    The time (s) to project the velocity by when ``use_velocity_scaled_lookahead_dist`` is ``true``. Also known as the lookahead gain.
+
+:rotate_to_heading_angular_vel:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         1.8            
+  ============== =============================
+
+  Description
+    If ``use_rotate_to_heading`` is ``true``, this is the angular velocity to use.
+
+:transform_tolerance:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.1      
+  ============== =============================
+
+  Description
+    The TF transform tolerance (s).
+
+:use_velocity_scaled_lookahead_dist:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.4            
+  ============== =============================
+
+  Description
+    Whether to use the velocity scaled lookahead distances or constant ``lookahead_distance``.
+
+:min_approach_linear_velocity:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.05            
+  ============== =============================
+
+  Description
+    The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress, when ``use_approach_linear_velocity_scaling`` is ``true``.
+
+:use_approach_linear_velocity_scaling:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  bool           true            
+  ============== =============================
+
+  Description
+    Whether to scale the linear velocity down on approach to the goal for a smooth stop.
+
+:max_allowed_time_to_collision:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         1.0          
+  ============== =============================
+
+  Description
+    The time (s) to project a velocity command forward to check for collisions.
+
+:use_regulated_linear_velocity_scaling:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  bool           true           
+  ============== =============================
+
+  Description
+    Whether to use the regulated features for path curvature (e.g. slow on high curvature paths).
+
+:use_cost_regulated_linear_velocity_scaling:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  bool           true            
+  ============== =============================
+
+  Description
+    Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles).
+
+:regulated_linear_scaling_min_radius:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.90       
+  ============== =============================
+
+  Description
+    The turning radius (m) for which the regulation features are triggered when ``use_regulated_linear_velocity_scaling`` is ``tru``. Remember, sharper turns have smaller radii.
+
+:regulated_linear_scaling_min_speed:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.25            
+  ============== =============================
+
+  Description
+    The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature.
+
+:use_rotate_to_heading:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  bool           true            
+  ============== =============================
+
+  Description
+    Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place.
+
+:rotate_to_heading_min_angle:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.785            
+  ============== =============================
+
+  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 (m/s/s) while rotating to heading, if ``use_rotate_to_heading`` is ``true``.
+
+:goal_dist_tol:
+
+  ============== =============================
+  Type           Default                                               
+  -------------- -----------------------------
+  double         0.25          
+  ============== =============================
+
+  Description
+    XY tolerance from goal to rotate to the goal heading, if ``use_rotate_to_heading`` is ``true``. This should match or be smaller than the ``GoalChecker``'s translational goal tolerance.
+
+Example
+*******
+.. code-block:: yaml
+
+  controller_server:
+    ros__parameters:
+      use_sim_time: True
+      controller_frequency: 20.0
+      min_x_velocity_threshold: 0.001
+      min_y_velocity_threshold: 0.5
+      min_theta_velocity_threshold: 0.001
+      progress_checker_plugin: "progress_checker"
+      goal_checker_plugin: "goal_checker"
+      controller_plugins: ["FollowPath"]
+
+      progress_checker:
+        plugin: "nav2_controller::SimpleProgressChecker"
+        required_movement_radius: 0.5
+        movement_time_allowance: 10.0
+      goal_checker:
+        plugin: "nav2_controller::SimpleGoalChecker"
+        xy_goal_tolerance: 0.25
+        yaw_goal_tolerance: 0.25
+        stateful: True
+      FollowPath:
+        plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
+        desired_linear_vel: 0.5
+        max_linear_accel: 2.5
+        max_linear_decel: 2.5
+        lookahead_dist: 0.6
+        min_lookahead_dist: 0.3
+        max_lookahead_dist: 0.9
+        lookahead_time: 1.5
+        rotate_to_heading_angular_vel: 1.8
+        transform_tolerance: 0.1
+        use_velocity_scaled_lookahead_dist: false
+        min_approach_linear_velocity: 0.05
+        use_approach_linear_velocity_scaling: true
+        max_allowed_time_to_collision: 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_rotate_to_heading: true
+        rotate_to_heading_min_angle: 0.785
+        max_angular_accel: 3.2
+        goal_dist_tol: 0.25

migration/Foxy.rst

@@ -46,7 +46,6 @@ Several example implementations are included in ``nav2_waypoint_follower``. ``Wa
 
 Loading a plugin of this type is done through ``nav2_bringup/params/nav2_param.yaml``, by specifying plugin's name, type and it's used parameters. 
 
-For instance; 
 .. code-block:: yaml
 
     waypoint_follower:
@@ -85,12 +84,25 @@ High-level design of this concept could be found `here <https://github.com/ros-p
 SmacPlanner
 ***********
 
-A new package, ``SmacPlanner`` was added containing 4 or 8 connected 2D A*, and Dubin and Reed-shepp model hybrid-A* with smoothing, multi-resolution query, and more.
+A new package, ``nav2_smac_planner`` was added containing 4 or 8 connected 2D A*, and Dubin and Reed-shepp model hybrid-A* with smoothing, multi-resolution query, and more.
 
 The ``nav2_smac_planner`` package contains an optimized templated A* search algorithm used to create multiple A*-based planners for multiple types of robot platforms. We support differential-drive and omni-directional drive robots using the ``SmacPlanner2D`` planner which implements a cost-aware A* planner. We support cars, car-like, and ackermann vehicles using the ``SmacPlanner`` plugin which implements a Hybrid-A* planner. This plugin is also useful for curvature constrained planning, like when planning robot at high speeds to make sure they don't flip over or otherwise skid out of control.
 
-The `SmacPlanner` fully-implements the Hybrid-A* planner as proposed in `Practical Search Techniques in Path Planning for Autonomous Driving <https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf>`_, including hybrid searching, CG smoothing, analytic expansions and hueristic functions.
+The ``SmacPlanner`` fully-implements the Hybrid-A* planner as proposed in `Practical Search Techniques in Path Planning for Autonomous Driving <https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf>`_, including hybrid searching, CG smoothing, analytic expansions and hueristic functions.
 
+RegulatedPurePursuitController
+******************************
+
+A new package, ``nav2_regulated_pure_pursuit_controller`` was added containing a novel varient of the Pure Pursuit algorithm.
+It also includes configurations to enable Pure Pursuit and Adaptive Pure Pursuit variations as well.
+
+This variation is specifically targeting service / industrial robot needs.
+It regulates the linear velocities by curvature of the path to help reduce overshoot at high speeds around blind corners allowing operations to be much more safe.
+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.
+
+There's more this does, that that's the general information. See the package's ``README`` for more.
 
 Costmap2D ``current_`` Usage
 ****************************

plugin_tutorials/docs/writing_new_nav2controller_plugin.rst

@@ -21,6 +21,9 @@ This tutorial shows how to create your own controller `plugin <https://index.ros
 In this tutorial, we will be implementing the pure pursuit path tracking algorithm based on this `paper <https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf>`_. 
 It is recommended you go through it.
 
+Note: This tutorial is based on a previously existing simplifed version of the Regulated Pure Pursuit controller now in the Nav2 stack.
+You can find the source code matching this tutorial `here <https://github.com/ros-planning/navigation2_tutorials/tree/126902457c5c646b136569886d6325f070c1073d/nav2_pure_pursuit_controller>`_.
+
 Requirements
 ============
 

plugins/index.rst

@@ -82,11 +82,16 @@ Controllers
 +--------------------------+--------------------+----------------------------------+-----------------------+
 |  `TEB Controller`_       | Christoph Rösmann  | A MPC-like controller suitable   | **Ackermann**, Legged,|
 |                          |                    | for ackermann, differential, and | Omnidirectional,      |
-|                          |                    | holonomic robots.                | Legged,               |
+|                          |                    | holonomic robots.                | Differential          |
++--------------------------+--------------------+----------------------------------+-----------------------+
+| `Regulated Pure Pursuit`_| Steve Macenski     | A service / industrial robot     | **Ackermann**, Legged,|
+|                          |                    | variation on the pure pursuit    | Omnidirectional,      |
+|                          |                    | algorithm with adaptive features.| Differential          |
 +--------------------------+--------------------+----------------------------------+-----------------------+
 
 .. _DWB Controller: https://github.com/ros-planning/navigation2/tree/main/nav2_dwb_controller
 .. _TEB Controller: https://github.com/rst-tu-dortmund/teb_local_planner
+.. _Regulated Pure Pursuit: https://github.com/navigation2/tree/main/nav2_regulated_pure_persuit_controller
 
 Planners
 ========