From 45a5c3631e20ff5872f2c53e28cda7fbb7a8aa6a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 22 Jan 2021 14:42:29 -0800 Subject: [PATCH] adding regulated PP configs / updates --- configuration/index.rst | 3 +- .../packages/configuring-regulated-pp.rst | 299 ++++++++++++++++++ migration/Foxy.rst | 18 +- .../writing_new_nav2controller_plugin.rst | 3 + plugins/index.rst | 7 +- 5 files changed, 325 insertions(+), 5 deletions(-) create mode 100644 configuration/packages/configuring-regulated-pp.rst diff --git a/configuration/index.rst b/configuration/index.rst index a436fd7863..6dfb7d7b56 100644 --- a/configuration/index.rst +++ b/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 \ No newline at end of file + packages/configuring-recovery-server.rst + packages/configuring-regulated-pp.rst diff --git a/configuration/packages/configuring-regulated-pp.rst b/configuration/packages/configuring-regulated-pp.rst new file mode 100644 index 0000000000..4087aefb8f --- /dev/null +++ b/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 diff --git a/migration/Foxy.rst b/migration/Foxy.rst index 59ef4dc4b2..af98971f4e 100644 --- a/migration/Foxy.rst +++ b/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 `_, 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 `_, 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 **************************** diff --git a/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst b/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst index 8afe269176..aad962dbb2 100644 --- a/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst +++ b/plugin_tutorials/docs/writing_new_nav2controller_plugin.rst @@ -21,6 +21,9 @@ This tutorial shows how to create your own controller `plugin `_. 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 `_. + Requirements ============ diff --git a/plugins/index.rst b/plugins/index.rst index dbb53eebba..34bd0df8b4 100644 --- a/plugins/index.rst +++ b/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 ========