diff --git a/configuration/index.rst b/configuration/index.rst index 9d98fc6da4..a436fd7863 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -16,6 +16,7 @@ the best navigation performance. packages/configuring-lifecycle.rst packages/configuring-planner-server.rst packages/configuring-navfn.rst + packages/configuring-smac-planner.rst packages/configuring-controller-server.rst packages/configuring-dwb-controller.rst packages/configuring-map-server.rst diff --git a/configuration/packages/configuring-smac-planner.rst b/configuration/packages/configuring-smac-planner.rst new file mode 100644 index 0000000000..c8807dac9e --- /dev/null +++ b/configuration/packages/configuring-smac-planner.rst @@ -0,0 +1,424 @@ +.. _configuring_smac_planner: + +Smac Planner +############ + +Source code and README with design, explanations, and metrics can be found on Github_. + +.. _Github: https://github.com/ros-planning/navigation2/tree/main/smac_planner + +The Smac Planner plugin implements a 2D A* and Hybrid-A* path planners. +An example of the Hyrid-A* planner can be seen below, planning a 85 meter path in 33ms. + +.. image:: smac/path.png + :width: 640px + :align: center + :alt: Path generated by the Hybrid-A* planner + + +```` is the corresponding planner plugin ID selected for this type. + +Parameters +********** + +:````.tolerance: + + ============== ======= + Type Default + -------------- ------- + double 0.5 + ============== ======= + + Description + Tolerance in meters between requested goal pose and end of path. + +:````.downsample_costmap: + + ==== ======= + Type Default + ---- ------- + bool False + ==== ======= + + Description + Whether to downsample costmap to another resolution for search. + +:````.downsampling_factor: + + ==== ======= + Type Default + ---- ------- + int 1 + ==== ======= + + Description + Multiplier factor to downsample costmap by (e.g. if 5cm costmap at 2 ``downsample_factor``, 10cm output). + +:````.allow_unknown: + + ==== ======= + Type Default + ---- ------- + bool True + ==== ======= + + Description + Whether to allow traversing/search in unknown space. + +:````.max_iterations: + + ==== ======= + Type Default + ---- ------- + int -1 + ==== ======= + + Description + Maximum number of search iterations before failing to limit compute time, disabled by -1. + +:````.max_on_approach_iterations: + + ==== ======= + Type Default + ---- ------- + int 1000 + ==== ======= + + Description + Maximum number of iterations after the search is within tolerance before returning approximate path with best heuristic if exact path not found. + +:````.max_planning_time_ms: + + ====== ======= + Type Default + ------ ------- + double 5000.0 + ====== ======= + + Description + Maximum planning time in ms. + +:````.analytic_expansion_ratio: + + ====== ======= + Type Default + ------ ------- + double 2.0 + ====== ======= + + Description + SE2 node will attempt to complete an analytic expansions proportional to this value and the minimum heuristic. + +:````.smooth_path: + + ==== ======= + Type Default + ---- ------- + bool False + ==== ======= + + Description + Whether to smooth path with CG smoother. + +:````.motion_model_for_search: + + ====== ======= + Type Default + ------ ------- + string "DUBIN" + ====== ======= + + Description + Motion model enum string to search with. For SE2 node, default is "DUBIN". For 2D it is "MOORE". Options for SE2 are DUBIN or REEDS_SHEPP. Options for 2D is MOORE and VON_NEUMANN. + +:````.angle_quantization_bins: + + ==== ======= + Type Default + ---- ------- + int 1 + ==== ======= + + Description + Number of angular bins to use for SE2 search. For 2D this must be 1, for SE2 it can be any number, but a good baseline is 72 (for 5 degree increments). + +:````.minimum_turning_radius: + + ====== ======= + Type Default + ------ ------- + double 0.2 + ====== ======= + + Description + Minimum turning radius in meters of vehicle. Also used in the smoother to compute maximum curvature. + +:````.reverse_penalty: + + ====== ======= + Type Default + ------ ------- + double 2.0 + ====== ======= + + Description + Heuristic penalty to apply to SE2 node if searching in reverse direction. Only used in ``REEDS_SHEPP`` motion model. + +:````.change_penalty: + + ====== ======= + Type Default + ------ ------- + double 0.5 + ====== ======= + + Description + Heuristic penalty to apply to SE2 node if changing direction in search. + +:````.non_straight_penalty: + + ====== ======= + Type Default + ------ ------- + double 1.05 + ====== ======= + + Description + Heuristic penalty to apply to SE2 node if searching in non-straight direction. + +:````.cost_penalty: + + ====== ======= + Type Default + ------ ------- + double 1.2 + ====== ======= + + Description + Heuristic penalty to apply to SE2 node for cost at pose. Allows Hybrid-A* to be cost aware. + +:````.smoother.smoother.w_curve: + + ====== ======= + Type Default + ------ ------- + double 1.5 + ====== ======= + + Description + CG smoother cost function weight on the curvature of path. + +:````.smoother.smoother.w_dist: + + ====== ======= + Type Default + ------ ------- + double 0.0 + ====== ======= + + Description + CG smoother cost function weight on the distance from the original path. Disabled by default. + +:````.smoother.smoother.w_smooth: + + ====== ======= + Type Default + ------ ------- + double 15000.0 + ====== ======= + + Description + CG smoother cost function weight on the distance between nodes. + +:````.smoother.smoother.w_cost: + + ====== ======= + Type Default + ------ ------- + double 1.5 + ====== ======= + + Description + CG smoother cost function weight on the costmap's cost. + +:````.smoother.smoother.cost_scaling_factor: + + ====== ======= + Type Default + ------ ------- + double 10.0 + ====== ======= + + Description + Scale factor for the inflation layer. Must be the same as your inflation layer's value. Used to approximate a Voronoi field. + +:````.smoother.optimizer.max_time: + + ====== ======= + Type Default + ------ ------- + double 0.10 + ====== ======= + + Description + Maximum time spent smoothing, in seconds. If planning takes too long, this can be dynamically adjusted to ensure the planner meets ``max_planning_time_ms``. + +:````.smoother.optimizer.max_iterations: + + ====== ======= + Type Default + ------ ------- + int 500 + ====== ======= + + Description + Maximum number of iterations we can run the CG smoother. + +:````.smoother.optimizer.debug_optimizer: + + ====== ======= + Type Default + ------ ------- + bool False + ====== ======= + + Description + Whether to print debug info from Ceres. + +:````.smoother.optimizer.gradient_tol: + + ====== ======= + Type Default + ------ ------- + double 1e-10 + ====== ======= + + Description + Minimum change in gradient to terminate smoothing. + +:````.smoother.optimizer.fn_tol: + + ====== ======= + Type Default + ------ ------- + double 1e-7 + ====== ======= + + Description + Minimum change in function to terminate smoothing. + +:````.smoother.optimizer.param_tol: + + ====== ======= + Type Default + ------ ------- + double 1e-15 + ====== ======= + + Description + Minimum change in parameter blocks to terminate smoothing. + +:````.smoother.optimizer.advanced.min_line_search_step_size: + + ====== ======= + Type Default + ------ ------- + double 1e-20 + ====== ======= + + Description + Terminate smoothing iteration if change in parameter block less than this. + +:````.smoother.optimizer.advanced.max_num_line_search_step_size_iterations: + + ====== ======= + Type Default + ------ ------- + int 50 + ====== ======= + + Description + Maximum iterations for line search in CG smoother. + +:````.smoother.optimizer.advanced.line_search_sufficient_function_decrease: + + ====== ======= + Type Default + ------ ------- + double 1e-20 + ====== ======= + + Description + Function decrease amount to terminate current line search iteration. + +:````.smoother.optimizer.advanced.max_num_line_search_direction_restarts: + + ====== ======= + Type Default + ------ ------- + int 10 + ====== ======= + + Description + Maximum umber of restarts of line search when over-shoots. + +:````.smoother.optimizer.advanced.max_line_search_step_expansion: + + ====== ======= + Type Default + ------ ------- + int 50 + ====== ======= + + Description + Step size multiplier at each iteration of line search. + +Example +******* +.. code-block:: yaml + + planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: True + + GridBased: + plugin: "smac_planner/SmacPlanner" + tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters, for 2D node + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: false # allow traveling in unknown space + max_iterations: -1 # maximum total iterations to search for before failing + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only + max_planning_time_ms: 2000.0 # max time in ms for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. + smooth_path: false # Whether to smooth searched path + motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp + angle_quantization_bins: 72 # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search) + minimum_turning_radius: 0.20 # For SE2 node & smoother: minimum turning radius in m of path / vehicle + reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.20 # For SE2 node: penalty to apply if motion is changing directions, must be >= 0 + non_straight_penalty: 1.05 # For SE2 node: penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 1.3 # For SE2 node: penalty to apply to higher cost zones + + smoother: + smoother: + w_curve: 30.0 # weight to minimize curvature of path + w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight + w_smooth: 30000.0 # weight to maximize smoothness of path + w_cost: 0.025 # weight to steer robot away from collision and cost + cost_scaling_factor: 10.0 # this should match the inflation layer's parameter + + # I do not recommend users mess with this unless they're doing production tuning + optimizer: + max_time: 0.10 # maximum compute time for smoother + max_iterations: 500 # max iterations of smoother + debug_optimizer: false # print debug info + gradient_tol: 1.0e-10 + fn_tol: 1.0e-20 + param_tol: 1.0e-15 + advanced: + min_line_search_step_size: 1.0e-20 + max_num_line_search_step_size_iterations: 50 + line_search_sufficient_function_decrease: 1.0e-20 + max_num_line_search_direction_restarts: 10 + max_line_search_step_expansion: 50 diff --git a/configuration/packages/smac/path.png b/configuration/packages/smac/path.png new file mode 100644 index 0000000000..fc98e7709b Binary files /dev/null and b/configuration/packages/smac/path.png differ diff --git a/migration/Foxy.rst b/migration/Foxy.rst index cd1c05e673..d2b2bd4b3f 100644 --- a/migration/Foxy.rst +++ b/migration/Foxy.rst @@ -38,3 +38,12 @@ Architecturally, costmap filters consists from ``CostmapFilter`` class which is Each costmap filter subscribes to filter info topic (publishing by `Costmap Filter Info Publisher Server `_) having all necessary information for loaded costmap filter and filter mask topic. High-level design of this concept could be found `here `_. The functionality of costmap filters is being disscussed in `the ticket #1263 `_ and carried out by `PR #1882 `_. The following tutorial: :ref:`navigation2_with_keepout_filter` will help to easily get involved with ``KeepoutFilter`` functionality. + +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. + +The ``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 `_, including hybrid searching, CG smoothing, analytic expansions and hueristic functions. diff --git a/plugins/index.rst b/plugins/index.rst index 4a7d59fb64..79d1ad14e8 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -9,7 +9,7 @@ A list of all known plugins are listed here below for ROS 2 Navigation. If you know of a plugin, or you have created a new plugin, please consider submitting a pull request with that information. This file can be found and editted under ``sphinx_docs/plugins/index.rst``. -For tutorials on creating your own plugins, please see :ref:`writing_new_costmap2d_plugin`. +For tutorials on creating your own plugins, please see :ref:`writing_new_costmap2d_plugin`, :ref:`writing_new_nbt_plugin`, :ref:`writing_new_nav2controller_plugin`, :ref:`writing_new_nav2planner_plugin`, or :ref:`writing_new_recovery_plugin`. Costmap Layers ============== @@ -69,17 +69,17 @@ Costmap Filters Controllers =========== -+--------------------------+--------------------+----------------------------------+ -| Plugin Name | Creator | Description | -+==========================+====================+==================================+ -| `DWB Controller`_ | David Lu!! | A highly configurable DWA | -| | | implementation with plugin | -| | | interfaces | -+--------------------------+--------------------+----------------------------------+ -| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | -| | | for ackermann, differential, and | -| | | holonomic robots. | -+--------------------------+--------------------+----------------------------------+ ++--------------------------+--------------------+----------------------------------+-----------------------+ +| Plugin Name | Creator | Description | Drivetrain support | ++==========================+====================+==================================+=======================+ +| `DWB Controller`_ | David Lu!! | A highly configurable DWA | Differential, | +| | | implementation with plugin | Omnidirectional, | +| | | interfaces | Legged | ++--------------------------+--------------------+----------------------------------+-----------------------+ +| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | **Ackermann**, Legged,| +| | | for ackermann, differential, and | Omnidirectional, | +| | | holonomic robots. | Legged, | ++--------------------------+--------------------+----------------------------------+-----------------------+ .. _DWB Controller: https://github.com/ros-planning/navigation2/tree/main/nav2_dwb_controller .. _TEB Controller: https://github.com/rst-tu-dortmund/teb_local_planner @@ -87,16 +87,32 @@ Controllers Planners ======== -+-------------------+---------------------------------------+------------------------------+ -| Plugin Name | Creator | Description | -+===================+=======================================+==============================+ -| `NavFn Planner`_ | Eitan Marder-Eppstein & Kurt Konolige | A navigation function | -| | | using A* or Dijkstras | -| | | expansion, assumes 2D | -| | | holonomic particle | -+-------------------+---------------------------------------+------------------------------+ ++-------------------+---------------------------------------+------------------------------+---------------------+ +| Plugin Name | Creator | Description | Drivetrain support | ++===================+=======================================+==============================+=====================+ +| `NavFn Planner`_ | Eitan Marder-Eppstein & Kurt Konolige | A navigation function | Differential, | +| | | using A* or Dijkstras | Omnidirectional, | +| | | expansion, assumes 2D | Legged | +| | | holonomic particle | | ++-------------------+---------------------------------------+------------------------------+---------------------+ +| `SmacPlanner`_ | Steve Macenski | A SE2 Hybrid-A* | **Ackermann**, | +| | | implementation using either | Differential, | +| | | Dubin or Reeds-shepp motion | Omnidirectional, | +| | | models with smoother and | Legged | +| | | multi-resolution query. | | +| | | Cars, car-like, and | | +| | | ackermann vehicles. | | ++-------------------+---------------------------------------+------------------------------+---------------------+ +| `SmacPlanner2D`_ | Steve Macenski | A 2D A* implementation | Differential, | +| | | Using either 4 or 8 | Omnidirectional, | +| | | connected neighborhoods | Legged | +| | | with smoother and | | +| | | multi-resolution query | | ++-------------------+---------------------------------------+------------------------------+---------------------+ .. _NavFn Planner: https://github.com/ros-planning/navigation2/tree/main/nav2_navfn_planner +.. _SmacPlanner: https://github.com/ros-planning/navigation2/tree/main/smac_planner +.. _SmacPlanner2D: https://github.com/ros-planning/navigation2/tree/main/smac_planner Recoveries ==========