Smac Planner

The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins:

• SmacPlannerHybrid: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models).
• SmacPlannerLattice: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models.
• SmacPlanner2D: a highly optimized fully reconfigurable grid-based A* implementation supporting 8-connected neighborhood models.

It also introduces the following basic building blocks:

• CostmapDownsampler: A library to take in a costmap object and downsample it to another resolution.
• AStar: A generic and highly optimized A* template library used by the planning plugins to search. Additional template for planning also could be made available using it.
• CollisionChecker: Collision check based on a robot’s radius or footprint.
• Smoother: A simple path smoother to smooth out 2D, Hybrid-A*, and State Lattice paths.

We have users reporting using this on:

• Delivery robots
• Industrial robots
• Vertical farming
• Solar farms

See its Configuration Guide Page for additional parameter descriptions.

Introduction

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. It was built by Steve Macenski while at Samsung Research. We support circular differential-drive and omni-directional drive robots using the SmacPlanner2D planner which implements a cost-aware A* planner. We support legged, cars, car-like, and ackermann vehicles using the SmacPlannerHybrid plugin which implements a Hybrid-A* planner. We support non-circular, arbitrary shaped, any model vehicles using the SmacPlannerLattice plugin which implements a State Lattice planner. It contains control sets and generators for ackermann, legged, differential drive and omnidirectional vehicles, but you may provide your own for another robot type or to have different planning behaviors. The last two plugins are also useful for curvature constrained or kinematically feasible planning, like when planning robot at high speeds to make sure they don’t flip over or otherwise skid out of control. It is also applicable to non-round robots (such as large rectangular or arbitrary shaped robots of differential/omnidirectional drivetrains) that need pose-based collision checking.

The SmacPlannerHybrid implements the Hybrid-A* planner as proposed in Practical Search Techniques in Path Planning for Autonomous Driving, with modifications to the heuristic, traversal functions to increase path quality without needing expensive optimization-based smoothing.

The SmacPlannerLattice implements the State Lattice planner. While we do not implement it precisely the same way as Optimal, Smooth, Nonholonomic MobileRobot Motion Planning in State Lattices (with control sets found using Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces), it is sufficiently similar it may be used as a good reference. Additional optimizations for on-approach analytic expansions and improved heuristic functions were used, largely matching those of Hybrid-A* to allow them to share these optimized implementations to drive search towards the goal, faster.

In summary…

The SmacPlannerHybrid is designed to work with:

• Ackermann, car, and car-like robots
• High speed or curvature constrained robots (as to not flip over, skid, or dump load at high speeds)
• Arbitrary shaped, non-circular differential or omnidirectional robots requiring kinematically feasible planning with SE2 collision checking
• Legged robots

The SmacPlannerLattice is designed to work with:

• Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain
• Flexibility to use other robot model types or with provided non-circular differential, ackermann, and omni support

The SmacPlanner2D is designed to work with:

• Circular, differential or omnidirectional robots
• Relatively small robots with respect to environment size (e.g. RC car in a hallway or large robot in a convention center) that can be approximated by circular footprint.

Features

We further improve on Hybrid-A* in the following ways:

• Remove need for upsampling by searching with 10x smaller motion primitives (same as their upsampling ratio).
• Multi-resolution search allowing planning to occur at a coarser resolution for wider spaces (O(N^2) faster).
• Cost-aware penalty functions in search resulting in far smoother plans (further reducing requirement to smooth).
• Gradient-descent, basic but fast smoother
• Faster planning than original paper by highly optimizing the template A* algorithm.
• Faster planning via custom precomputed heuristic, motion primitive, and other functions.
• Automatically adjusted search motion model sizes by motion model, costmap resolution, and bin sizing.
• Closest path on approach within tolerance if exact path cannot be found or in invalid space.
• Multi-model hybrid searching including Dubin and Reeds-Shepp models. More models may be trivially added.
• High unit and integration test coverage, doxygen documentation.
• Uses modern C++14 language features and individual components are easily reusable.
• Speed optimizations: no data structure graph lookups in main loop, near-zero copy main loop, dynamically generated graph and dynamic programming-based obstacle heuristic, optional recomputation of heuristics for subsequent planning requests of the same goal, etc.
• Templated Nodes and A* implementation to support additional robot extensions.
• Selective re-evaluation of the obstacle heuristic per goal/map or each iteration, which can speed up subsequent replanning 20x or more.

Most of these features (multi-resolution, models, smoother, etc) are also available in the SmacPlanner2D and SmacPlannerLattice plugins.

The 2D A* implementation also does not have any of the weird artifacts introduced by the gradient wavefront-based 2D A* implementation in the NavFn Planner. While this 2D A* planner is slightly slower, I believe it’s well worth the increased quality in paths.

Note: In prior releases, a CG smoother largely implementing the original Hybrid-A* paper’s. However, this smoother failed to consistently provide useful results, took too much compute time, and was deprecated. While smoothing a path 95% of the time seems like a “good” solution, we need something far more reliable for practical use. Since we are working with mobile robots and not autonomous cars at 60 mph, we can take some different liberties in smoothing knowing that our local trajectory planners are pretty smart. If you are looking for it, it now lives in the new Smoothing Server as the Cost-aware smoother. This smoother has been replaced by a simpler optimization inspired solution which is faster, far more consistent, and simpler to understand. While this smoother is not cost-aware, we have added cost-aware penalty functions in the planners themselves to push the plans away from high-cost spaces and we do check for validity of smoothed sections to ensure feasibility. It will through terminate when paths become in collision with the environment. If you would like to use this smoother, however, it is available in the smoother server, though it will take some additional compute time.

Metrics

The original Hybrid-A* implementation boasted planning times of 50-300ms for planning across 102,400 cell maps with 72 angular bins. We see much faster results in our evaluations:

• 2-20ms for planning across 147,456 (1.4x larger) cell maps with 72 angular bins.
• 30-200ms for planning across 344,128 (3.3x larger) cell map with 72 angular bins.

An example of the 3 planners can be seen below, planning a roughly 75 m path.

• 2D A* computed the path in 243ms (Panel 1)
• Hybrid-A* computed the path in 144ms (Panel 2)
• State Lattice computed the path in 113ms (Panel 3)
• For reference: NavFn compute the path in 146ms, including some nasty path discontinuity artifacts

Design

The basic design centralizes a templated A* implementation that handles the search of a graph of nodes. The implementation is templated by the nodes, NodeT, which contain the methods needed to compute the hueristics, travel costs, and search neighborhoods. The outcome of this design is then a standard A* implementation that can be used to traverse any type of graph as long as a node template can be created for it.

We provide 3 nodes by default currently. The 2D node template (Node2D) which does 2D grid-search with either 4 or 8-connected neighborhoods. We also provide a Hybrid A* node template (NodeHybrid) which does SE2 (X, Y, theta) search and collision checking on Dubin or Reeds-Shepp motion models. We also provide the Lattice (NodeLattice) node for state lattice planning making use of the wider range of velocity options available to differential and omnidirectional robots. Additional templates could be easily made and included for 3D grid search and non-grid base searching like routing.

In the ROS2 facing plugin, we take in the global goals and pre-process the data to feed into the templated A* used. This includes processing any requests to downsample the costmap to another resolution to speed up search and smoothing the resulting A* path (not available for State Lattice due to the lattices generated are dependent on costmap resolution). For the SmacPlannerHybrid and SmacPlannerLattice plugins, the path is promised to be kinematically feasible due to the kinematically valid models used in branching search. The 2D A* is also promised to be feasible for differential and omni-directional robots.

We isolated the A*, costmap downsampler, smoother, and Node template objects from ROS2 to allow them to be easily testable independently of ROS or the planner. The only place ROS is used is in the planner plugins themselves.

Parameters

See inline description of parameters in the SmacPlanner. This includes comments as specific parameters apply to SmacPlanner2D and SmacPlanner in place.

``` planner_server:

File truncated at 100 lines see the full file