Smac Planner

Source code and README with design, explanations, and metrics can be found on Github.

The Smac Planner plugin implements a 2D A* and Hybrid-A* path planners. It is important to know that as of June 2021, the planner received a major update improving path quality and run-times by 2-3x. An example of the Hybrid-A* planner can be seen below, planning a 60 meter path in 73ms. Usual planning time is below 100ms for most environments, occasionally up to 200ms.

Path generated by the Hybrid-A* planner

<name> is the corresponding planner plugin ID selected for this type.

Parameters

<name>.tolerance:
 
Type Default
double 0.5
Description

Tolerance in meters between requested goal pose and end of path.

<name>.downsample_costmap:
 
Type Default
bool False
Description

Whether to downsample costmap to another resolution for search.

<name>.downsampling_factor:
 
Type Default
int 1
Description

Multiplier factor to downsample costmap by (e.g. if 5cm costmap at 2 downsample_factor, 10cm output).

<name>.allow_unknown:
 
Type Default
bool True
Description

Whether to allow traversing/search in unknown space.

<name>.max_iterations:
 
Type Default
int 1000000
Description

Maximum number of search iterations before failing to limit compute time, disabled by -1.

<name>.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. 2D only.

<name>.max_planning_time:
 
Type Default
double 5.0
Description

Maximum planning time in seconds.

<name>.analytic_expansion_ratio:
 
Type Default
double 3.5
Description

SE2 node will attempt to complete an analytic expansions proportional to this value and the minimum heuristic.

<name>.motion_model_for_search:
 
Type Default
string “DUBIN”
Description

Motion model enum string to search with. For Hybrid-A* 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. For state lattice, use STATE_LATTICE.

<name>.cost_travel_multiplier:
 
Type Default
double 2.0
Description

For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-FREE cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.

<name>.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).

<name>.minimum_turning_radius:
 
Type Default
double 0.4
Description

Minimum turning radius in meters of vehicle. Also used in the smoother to compute maximum curvature.

<name>.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.

<name>.change_penalty:
 
Type Default
double 0.15
Description

Heuristic penalty to apply to SE2 node if changing direction in search.

<name>.non_straight_penalty:
 
Type Default
double 1.50
Description

Heuristic penalty to apply to SE2 node if searching in non-straight direction.

<name>.cost_penalty:
 
Type Default
double 1.7
Description

Heuristic penalty to apply to SE2 node for cost at pose. Allows Hybrid-A* to be cost aware.

<name>.lattice_filepath:
 
Type Default
string “”
Description

The filepath to the state lattice graph

<name>.lookup_table_size:
 
Type Default
double 20.0
Description

Size of the dubin/reeds-sheep distance window to cache, in meters.

<name>.cache_obstacle_heuristic:
 
Type Default
bool false
Description

Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.

<name>.smoother.max_iterations:
 
Type Default
int 1000
Description

Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.

<name>.smoother.w_smooth:
 
Type Default
double 0.3
Description

Weight for smoother to apply to smooth out the data points

<name>.smoother.w_data:
 
Type Default
double 0.2
Description

Weight for smoother to apply to retain original data information

<name>.smoother.tolerance:
 
Type Default
double 1e-10
Description

Parameter tolerance change amount to terminate smoothing session

Example

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_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: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      motion_model_for_search: "DUBIN"    # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally
      cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      angle_quantization_bins: 64         # For Hybrid/Lattice nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
      minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: 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.15                # For Hybrid/Lattice nodes: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.50          # For Hybrid/Lattice nodes: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.7                   # For Hybrid/Lattice nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      lattice_filepath: ""                # For Lattice node: the filepath to the state lattice graph
      lookup_table_size: 20               # For Hybrid/Lattice nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: True      # For Hybrid/Lattice nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10