Smac State Lattice Planner

Paths generated by the Smac State Lattice

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

Note: State Lattice does not have the costmap downsampler due to the minimum control sets being tied with map resolutions on generation. The minimum turning radius is also not a parameter in State Lattice since this was specified at the minimum control set pre-computation phase. See the Smac Planner package to generate custom control sets for your vehicle or use one of our pre-generated examples.

The image above you can see the reverse expansion enabled, such that the robot can back into a tight requested spot close to an obstacle.

Parameters

<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_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 expansion with frequency proportional to this value and the minimum heuristic.

<name>.analytic_expansion_max_length:
 
Type Default
double 3.0
Description

If the length is too far, reject this expansion. This prevents unsafe shortcutting of paths into higher cost areas far out from the goal itself, let search to the work of getting close before the analytic expansion brings it home. This should never be smaller than 4-5x the minimum turning radius being used, or planning times will begin to spike.

<name>.reverse_penalty:
 
Type Default
double 2.0
Description

Heuristic penalty to apply to SE2 node if searching in reverse direction. Only used in allow_reverse_expansion = true.

<name>.change_penalty:
 
Type Default
double 0.05
Description

Heuristic penalty to apply to SE2 node if changing direction (e.g. left to right) in search.

<name>.non_straight_penalty:
 
Type Default
double 1.05
Description

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

<name>.cost_penalty:
 
Type Default
double 2.0
Description

Heuristic penalty to apply to SE2 node for cost at pose. Allows State Lattice to be cost aware.

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

The filepath to the state lattice minimum control set graph, this will default to a 16 bin, 0.4m turning radius control set located in test/ for basic testing and evaluation.

<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>.allow_reverse_expansion:
 
Type Default
bool false
Description

If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot’s orientation (to reverse).

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

The maximum number of iterations the smoother has to smooth the path, to bound potential computation.

<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/SmacPlannerLattice"
      allow_unknown: true                 # 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_planning_time: 5.0              # Max time in s for planner to plan, smooth
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      reverse_penalty: 2.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.05                # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.05          # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.0                   # 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: ""                # The filepath to the state lattice graph
      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: false     # 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.
      allow_reverse_expansion: false      # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10