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>.rotation_penalty:

Type	Default
double	5.0

Description: Penalty to apply for rotations in place, if minimum control set contains in-place rotations. This should always be set sufficiently high to weight against in-place rotations unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where the plan requests the robot to rotate in place to short-cut an otherwise smooth forward-moving path for marginal path distance savings.

<name>.retrospective_penalty:

Type	Default
double	0.015

Description: Heuristic penalty to apply to SE2 node penalty. Causes State Lattice to prefer later maneuvers before earlier ones along the path. Saves search time since earlier (shorter) branches are not expanded until it is necessary. Must be >= 0.0 and <= 1.0. Must be 0.0 to be fully admissible.

<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>.smooth_path:

Type	Default
bool	true

Description: If true, does simple and fast smoothing post-processing to the path from search

<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

<name>.smoother.do_refinement:

Type	Default
bool	true

Description: Performs extra refinement smoothing runs. Essentially, this recursively calls the smoother using the output from the last smoothing cycle to further smooth the path for macro-trends. This typically improves quality especially in the Hybrid-A* planner but can be helpful on the state lattice planner to reduce the “blocky” movements in State Lattice caused by the limited number of headings.

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.
      rotation_penalty: 5.0               # Penalty to apply to in-place rotations, if minimum control set contains them
      retrospective_penalty: 0.015
      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).
      smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10
        do_refinement: true