Smac State Lattice Planner

Paths generated by the Smac State Lattice

This planner implements a generic state lattice global path planner using precomputed, offline minimum control sets. The package also contains a python utility for computing minimum control sets based on minimum curvature constraints, in-place rotations, and omnidirectional moves. Though, any other minimum control set may be used as long as it follows the file format specification in the package's README file.

<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: bool Default: true: Whether to allow traversing/search in unknown space.

`<name>.tolerance`

Type: double Default: 0.25: 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 in distance-to-goal.

`<name>.max_iterations`

Type: int Default: 1000000: Maximum number of search iterations before failing to limit compute time, disabled by -1.

`<name>.max_on_approach_iterations`

Type: int Default: 1000: 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.

`<name>.terminal_checking_interval`

Type: int Default: 5000: Number of iterations between checking if the goal has been cancelled or planner timed out

`<name>.max_planning_time`

Type: double Default: 5.0: Maximum planning time in seconds.

`<name>.analytic_expansion_ratio`

Type: double Default: 3.5: SE2 node will attempt to complete an analytic expansion with frequency proportional to this value and the minimum heuristic. Negative values convert to infinite.

`<name>.analytic_expansion_max_length`

Type: double Default: 3.0: 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>.analytic_expansion_max_cost`

Type: double Default: 200.0: For Hybrid/Lattice nodes: The maximum single cost for any part of an analytic expansion to contain and be considered valid (except when necessary on approach to goal). This allows for removing of potential shortcutting into higher cost spaces than you might otherwise desire

`<name>.analytic_expansion_max_cost_override`

Type: bool Default: false: For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required). If expansion is within 2*pi*min_r of the goal, then it will override the max cost if false.

`<name>.reverse_penalty`

Type: double Default: 2.0: Heuristic penalty to apply to SE2 node if searching in reverse direction. Only used in allow_reverse_expansion = true.

`<name>.change_penalty`

Type: double Default: 0.05: Heuristic penalty to apply to SE2 node if changing direction (e.g. left to right) in search.

`<name>.non_straight_penalty`

Type: double Default: 1.05: Heuristic penalty to apply to SE2 node if searching in non-straight direction.

`<name>.cost_penalty`

Type: double Default: 2.0: Heuristic penalty to apply to SE2 node for cost at pose. Allows State Lattice to be cost aware.

`<name>.rotation_penalty`

Type: double Default: 5.0: 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: double Default: 0.015: 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: string Default: "": The filepath to the state lattice minimum control set graph, this will default to a 16 bin, 0.5m turning radius control set located in test/ for basic testing and evaluation (opposed to Hybrid-A*'s default of 0.5m).

`<name>.lookup_table_size`

Type: double Default: 20.0: Size of the dubin/reeds-sheep distance window to cache, in meters.

`<name>.cache_obstacle_heuristic`

Type: bool Default: false: Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.

`<name>.allow_reverse_expansion`

Type: bool Default: 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).

`<name>.debug_visualizations`

Type: bool Default: false: Whether to publish expansions on the /expansions topic as an array of poses and the path's footprints on the /planned_footprints topic.

Warning

Heavy to compute and to display, for debug only as it degrades the performance. Lattice expansions show end-points of primitives, not intermediary steps as seen in dense path outputs.

`<name>.smooth_path`

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

`<name>.smoother.max_iterations`

Type: int Default: 1000: The maximum number of iterations the smoother has to smooth the path, to bound potential computation.

`<name>.smoother.w_smooth`

Type: double Default: 0.3: Weight for smoother to apply to smooth out the data points

`<name>.smoother.w_data`

Type: double Default: 0.2: Weight for smoother to apply to retain original data information

`<name>.smoother.tolerance`

Type: double Default: 1e-10: Parameter tolerance change amount to terminate smoothing session

`<name>.smoother.do_refinement`

Type: bool Default: true: 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.

`<name>.smoother.refinement_num`

Type: int Default: 2: Number of times to recursively attempt to smooth, must be >= 1.

`<name>.goal_heading_mode`

Type: string Default: "DEFAULT": Goal heading mode enum string to plan goal with multiple orientation. Options are "DEFAULT", "BIDIRECTIONAL" and "ALL_DIRECTION". With default mode, the planner will plan the goal with the orientation of the goal pose as was sent by the user. With the "BIDIRECTIONAL" mode, the planner will plan the goal with the orientation of the goal pose and with orientation 180 degree offset from the goal pose orientation. For "ALL_DIRECTION" mode, the planner will plan the goal with the orientation of the goal pose and all the possible orientation based on the angle quantization bins. For both the "BIDIRECTIONAL" and "ALL_DIRECTION" mode, the planner returns the path with the minimum cost.

`<name>.coarse_search_resolution`

Type: int Default: 1: Number of goal heading bins to skip during the coarse search phase of analytic expansion goal-finding. When a goal is found, a fine search is performed to determine the exact path during full-resolution. This parameter is only used when the goal heading mode is set to "ALL_DIRECTION" and It helps to reduce search time of analytic expansions. g the coarse search phase of analytic expansion goal-finding. When a goal is found, a fine search is performed to determine the exact path during full-resolution. This parameter is only used when the goal heading mode is set to "ALL_DIRECTION" and it helps to reduce search time of analytic expansions. It is recommended to only do coarser search if the number of bins are > 24. Otherwise, leave as 1(fine search).

`<name>.downsample_obstacle_heuristic`

Type: bool Default: true: Advanced feature: This allows a user to disable downsampling of the obstacle heuristic's costmap representation to search at the costmap's full-resolution. This will come at increased up-front costs while searching for the 2D approximate route to the goal in exchange for less search iterations and a slightly more smooth path. With smooth_path on, this increased smoothness is noticeable but not massively different. When combined with all of the advanced features however, it can contribute to a better overall plan in exchange for some compute time. This scales with map size and complexity of the path plan requested. For simpler maps / paths, this may actually improve performance due to low up-front search times and lower iterations.

`<name>.use_quadratic_cost_penalty`

Type: bool Default: false: Advanced feature: This allows a user to specify a quadratic traversal and heuristic cost computation (e.g. cost * cost) rather than linear. This will speed up the planner since the optimal channel for feasible search is deeper and prunes search branches more aggressively. This will also create overall much smoother paths since search will not attempt to refine itself to stay in the center of wide aisleways or open spaces to reduce low finite costs. However, the smoothness and less sensitivity to cost also makes it come somewhat closer to obstacles. Broadly speaking the change and non-straight penalties can be disabled when this feature is in use. The cost penalty and inflation layer parameters may need to be adjusted when enabling this parameter to create optimal performance.

`allow_parameter_qos_overrides`

Type: bool Default: true: Whether to allow QoS profiles to be overwritten with parameterized values.

Example

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerLattice"
      allow_unknown: true                 # Allow traveling in unknown space
      tolerance: 0.25                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      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
      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
      analytic_expansion_max_cost: 200.0  # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: false  #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      downsample_obstacle_heuristic: true   # Downsample the obstacle map dynamic programming distance expansion heuristic to speed up search at the cost of some path quality
      use_quadratic_cost_penalty: false   # Use quadratic cost penalty for traversal and heuristic cost computations rather than linear
      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 subsequent 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).
      coarse_search_resolution: 1         # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode.
      goal_heading_mode: "DEFAULT"        # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
      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
        refinement_num: 2