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.

<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