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>.tolerance

Type

Default

double

0.25

Description

If an exact path cannot be found, the tolerance (as measured by the heuristic cost-to-goal) that would be acceptable to diverge from the requested pose.

<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 once a visited node is within the goal tolerances to continue to try to find an exact match before returning the best path solution within tolerances. Negative values convert to infinite.

<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 shortcutting of search with its penalty functions far out from the goal itself (e.g. so we don’t reverse half-way across open maps or cut through high cost zones). 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>.retrospective_penalty

Type

Default

double

0.015

Description

Heuristic penalty to apply to SE2 node penalty. Causes Hybrid-A* 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>.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>.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 due to the extra “wobbling” it can have due to the very small primitive lengths but may cause the path to get slightly closer to some obstacles.

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)
      tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      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_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
      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.
      retrospective_penalty: 0.015
      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.
      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: 1.0e-10
        do_refinement: true