Smac Hybrid-A* Planner

Paths generated by the Smac Hybrid-A*

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

Parameters

<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_planning_time:
 
Type Default
double 5.0
Description

Maximum planning time in seconds.

<name>.analytic_expansion_ratio:
 
Type Default
double 3.5
Description

Planner will attempt to complete an analytic expansions in a 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>.motion_model_for_search:
 
Type Default
string “DUBIN”
Description

Motion model enum string to search with. For Hybrid-A* node, default is “DUBIN”. Options for SE2 are DUBIN or REEDS_SHEPP.

<name>.angle_quantization_bins:
 
Type Default
int 72
Description

Number of angular bins to use for SE2 search. This can be any even number, but a good baseline is 64 or 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.0
Description

Heuristic penalty to apply to SE2 node if changing direction (e.g. left to right) in search. Disabled by default after change to guarantee admissibility of the Hybrid-A* planner.

<name>.non_straight_penalty:
 
Type Default
double 1.20
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 Hybrid-A* to be cost aware.

<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

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/SmacPlannerHybrid"
      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: 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
      motion_model_for_search: "DUBIN"    # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72         # Number of angle bins for search
      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
      minimum_turning_radius: 0.40        # minimum turning radius in m of path / vehicle
      reverse_penalty: 2.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.2           # 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.
      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.

      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10