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

<name>.use_final_approach_orientation:

Type	Default
bool	false

Description: If true, the last pose of the path generated by the planner will have its orientation set to the approach orientation, i.e. the orientation of the vector connecting the last two points of the path

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