# D.7  Smac Planner

Smac Planner plugin မှာ three A\* base planning algorithms များဖြစ်တဲ့ 2D A\*, Hybrid-A\* နဲ့ State Lattice path planners တို့ကို ပါဝင် ပါတယ်။ အတိအကျပြောရမယ်ဆိုရင်တော့ အောက်ကကောင်တွေပါ။

* Smac 2D Planner
* Smac Hybrid-A\* Planner
* Smac State Lattice Planner

Nav2\_smac\_planner package မှာ robot platforms အမျိုးမျိုးအတွက် အဆင့်မြင့်ထားတဲ့ A\*-based search algorithm ပါဝင်ပါတယ်။ template node types တွေကိုအသုံးပြုထားပြီး နောက်ထပ် planners အသစ်တွေ develop လုပ်လို့လည်းရပါတယ်။

circular differential-drive robots  နဲ့ omni-directional drive robots တွေအတွက်ဆိုရင် SmacPlanner2D planner အသုံးပြုရမှာဖြစ်ပါတယ်။

ကားပုံစံ ackermann နဲ့ legged vehicles robot တွေအတွက်ဆိုရင်တော့ SmacPlannerHybrid plugin ကို support လုပ်ပေးပါတယ်။&#x20;

Non-circular တွေ၊ arbitrary shaped robot တွေနဲ့ စိတ်ကြိုက် model vehicles များကိိုအသုံးပြုမယ်ဆိုရင်တော့ SmacPlannerLattice plugin ကိုသုံးနိုင်ပါတယ်။ ဒီကောင်သည် omni, diff, ackermann, legged, custom အမျိုးအစားတွေအားလုံးအတွက် support ပေးတယ်လို့ဆိုပါတယ်။

အပေါ်မှာ ဖေါ်ပြခဲ့တဲ့ plugin ၃ထဲက နောက်ဆုံး plugin နှစ်ခုကတော့ kinematically feasible ဖြစ်ပါတယ်။&#x20;

အောက်မှာ planner သုံးခု နဲ့ ၇၅ မီတာအကို အကြမ်းအားဖြင့် plan လုပ်ရာမှာ ကြာချိန်အသီးသီးကို ဖေါ်ပြထားပါတယ်။

* Hybrid-A\* computed the path in 144 ms
* State Lattice computed the path in 113 ms
* 2D A\* computed the path in 243 ms
* For reference: NavFn compute the path in 146 ms, including some nasty path discontinuity artifacts

တချို့ environment များအတွက် 100ms အောက်ကို ပုံမှန် planning လုပ်ပြီး၊ တခါတရံ 200 ms နဲ့ ထိကြာပါတယ်။

<figure><img src="https://2903086714-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2Fr9x7fOEsshA97WfbNvfL%2Fuploads%2FlzBrhpOM5dWe2pocriOB%2F3planners.png?alt=media&#x26;token=305f4339-d819-4c1f-be37-e1d0fd3466db" alt=""><figcaption><p>Left to Right [ 2D A* planner, Hybird-A* planner, State Lattice planner ] source from nav2 docs</p></figcaption></figure>

#### Smac 2D Planner

<table><thead><tr><th width="250">Param Names</th><th width="310">Description</th><th>Type</th><th>Default</th></tr></thead><tbody><tr><td>&#x3C;name>.tolerance</td><td>Tolerance in meters between requested goal pose and end of path.</td><td>double</td><td>0.125</td></tr><tr><td>&#x3C;name>.downsample_costmap</td><td>Whether to downsample costmap to another resolution for search.</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name>.downsampling_factor</td><td>Multiplier factor to downsample costmap by (e.g. if 5cm costmap at 2 downsample_factor, 10cm output).</td><td>int</td><td>1</td></tr><tr><td>&#x3C;name><br>.allow_unknown</td><td>Whether to allow traversing/search in unknown space.</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name>.max_iterations</td><td>Maximum number of search iterations before failing to limit compute time, disabled by -1.</td><td>int</td><td>1000000</td></tr><tr><td>&#x3C;name>.max_on_approach_iterations</td><td>Maximum number of iterations after the search is within tolerance before returning approximate path with best heuristic if exact path is not found.</td><td>int</td><td>1000</td></tr><tr><td>&#x3C;name><br>.max_planning_time</td><td>Maximum planning time in seconds.</td><td>double</td><td>2</td></tr></tbody></table>

နမူနာ

```yaml
planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.125                      # tolerance for planning if unable to reach exact pose, in meters
      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_on_approach_iterations: 1000      # maximum number of iterations to attempt to reach goal once in tolerance
      max_planning_time: 2.0                # max time in s for planner to plan, smooth
      cost_travel_multiplier: 2.0           # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10
```

#### Smac Hybrid-A\* Planner

<table><thead><tr><th width="241">Param Names</th><th width="305">Description</th><th>Type</th><th>Default</th></tr></thead><tbody><tr><td>&#x3C;name>.downsample_costmap</td><td>Whether to downsample costmap to another resolution for search.</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name>.downsampling_factor</td><td>Multiplier factor to downsample costmap by (e.g. if 5cm costmap at 2 downsample_factor, 10cm output).</td><td>int</td><td>1</td></tr><tr><td>&#x3C;name>.allow_unknown</td><td>Whether to allow traversing/search in unknown space.</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.tolerance</td><td>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.</td><td>double</td><td>0.25</td></tr><tr><td>&#x3C;name><br>.max_iterations</td><td>Maximum number of search iterations before failing to limit compute time, disabled by -1.</td><td>int</td><td>1000000</td></tr><tr><td>&#x3C;name><br>.max_on_approach_iterations</td><td>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.</td><td>int</td><td>1000</td></tr><tr><td>&#x3C;name><br>.max_planning_time</td><td>Maximum planning time in seconds.</td><td>double</td><td>5.0</td></tr><tr><td>&#x3C;name><br>.analytic_expansion_ratio</td><td>Planner will attempt to complete an analytic expansions in a frequency proportional to this value and the minimum heuristic.</td><td>double</td><td>3.5</td></tr><tr><td>&#x3C;name><br>.analytic_expansion_max_length</td><td>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.</td><td>double</td><td>3.0</td></tr><tr><td>&#x3C;name><br>.motion_model_for_search</td><td>Motion model enum string to search with. For Hybrid-A* node, default is “DUBIN”. Options for SE2 are DUBIN or REEDS_SHEPP.</td><td>string</td><td>“DUBIN”</td></tr><tr><td>&#x3C;name><br>.angle_quantization_bins</td><td>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).</td><td>int</td><td>72</td></tr><tr><td>&#x3C;name><br>.minimum_turning_radius</td><td>Minimum turning radius in meters of vehicle. Also used in the smoother to compute maximum curvature.</td><td>double</td><td>0.4</td></tr><tr><td>&#x3C;name><br>.reverse_penalty</td><td>Heuristic penalty to apply to SE2 node if searching in reverse direction. Only used in REEDS_SHEPP motion model.</td><td>double</td><td>2.0</td></tr><tr><td>&#x3C;name><br>.change_penalty</td><td>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.</td><td>double</td><td>0</td></tr><tr><td>&#x3C;name><br>.non_straight_penalty</td><td>Heuristic penalty to apply to SE2 node if searching in non-straight direction.</td><td>double</td><td>1.2</td></tr><tr><td>&#x3C;name><br>.cost_penalty</td><td>Heuristic penalty to apply to SE2 node for cost at pose. Allows Hybrid-A* to be cost aware.</td><td>double</td><td>2</td></tr><tr><td>&#x3C;name><br>.retrospective_penalty</td><td>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 &#x3C;= 1.0. Must be 0.0 to be fully admissible.</td><td>double</td><td>0.015</td></tr><tr><td>&#x3C;name><br>.lookup_table_size</td><td>Size of the dubin/reeds-sheep distance window to cache, in meters.</td><td>double</td><td>20.0</td></tr><tr><td>&#x3C;name><br>.viz_expansions</td><td>Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name><br>.cache_obstacle_heuristic</td><td>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.</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name><br>.smooth_path</td><td>If true, does simple and fast smoothing post-processing to the path from search</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.smoother.max_iterations</td><td>The maximum number of iterations the smoother has to smooth the path, to bound potential computation.</td><td>int</td><td>1000</td></tr><tr><td>&#x3C;name><br>.smoother.w_smooth</td><td>Weight for smoother to apply to smooth out the data points</td><td>double</td><td>0.3</td></tr><tr><td>&#x3C;name><br>.smoother.w_data</td><td>Weight for smoother to apply to retain original data information</td><td>double</td><td>0.2</td></tr><tr><td>&#x3C;name><br>.smoother.tolerance</td><td>Parameter tolerance change amount to terminate smoothing session</td><td>double</td><td>1E-10</td></tr><tr><td>&#x3C;name><br>.smoother.do_refinement</td><td>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.</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.smoother.refinement_num</td><td>Number of times to recursively attempt to smooth, must be >= 1.</td><td>int</td><td>2</td></tr></tbody></table>

နမူနာ

```yaml
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.
      viz_expansions: false               # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      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
```

#### Smac State Lattice Planner

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.

<table><thead><tr><th>Param Names</th><th width="329">Description</th><th>Type</th><th>Default</th></tr></thead><tbody><tr><td>&#x3C;name><br>.allow_unknown</td><td>Whether to allow traversing/search in unknown space.</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.tolerance</td><td>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.</td><td>double</td><td>0.25</td></tr><tr><td>&#x3C;name><br>.max_iterations</td><td>Maximum number of search iterations before failing to limit compute time, disabled by -1.</td><td>int</td><td>1000000</td></tr><tr><td>&#x3C;name><br>.max_on_approach_iterations</td><td>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.</td><td>int</td><td>1000</td></tr><tr><td>&#x3C;name><br>.max_planning_time</td><td>Maximum planning time in seconds.</td><td>double</td><td>5.0</td></tr><tr><td>&#x3C;name><br>.analytic_expansion_ratio</td><td>SE2 node will attempt to complete an analytic expansion with frequency proportional to this value and the minimum heuristic. Negative values convert to infinite.</td><td>double</td><td>3.5</td></tr><tr><td>&#x3C;name><br>.analytic_expansion_max_length</td><td>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.</td><td>double</td><td>3.0</td></tr><tr><td>&#x3C;name><br>.reverse_penalty</td><td>Heuristic penalty to apply to SE2 node if searching in reverse direction. Only used in allow_reverse_expansion = true.</td><td>double</td><td>2.0</td></tr><tr><td>&#x3C;name><br>.change_penalty</td><td>Heuristic penalty to apply to SE2 node if changing direction (e.g. left to right) in search.</td><td>double</td><td>0.05</td></tr><tr><td>&#x3C;name><br>.non_straight_penalty</td><td>Heuristic penalty to apply to SE2 node if searching in non-straight direction.</td><td>double</td><td>1.05</td></tr><tr><td>&#x3C;name><br>.cost_penalty</td><td>Heuristic penalty to apply to SE2 node for cost at pose. Allows State Lattice to be cost aware.</td><td>double</td><td>2.0</td></tr><tr><td>&#x3C;name><br>.rotation_penalty</td><td>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.</td><td>double</td><td>5</td></tr><tr><td>&#x3C;name><br>.retrospective_penalty</td><td>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 &#x3C;= 1.0. Must be 0.0 to be fully admissible.</td><td>double</td><td>0.015</td></tr><tr><td>&#x3C;name><br>.lattice_filepath</td><td>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).</td><td>string</td><td>“”</td></tr><tr><td>&#x3C;name><br>.lookup_table_size</td><td>Size of the dubin/reeds-sheep distance window to cache, in meters.</td><td>double</td><td>20.0</td></tr><tr><td>&#x3C;name><br>.cache_obstacle_heuristic</td><td>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.</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name><br>.allow_reverse_expansion</td><td>If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot’s orientation (to reverse).</td><td>bool</td><td>FALSE</td></tr><tr><td>&#x3C;name><br>.smooth_path</td><td>If true, does simple and fast smoothing post-processing to the path from search</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.smoother.max_iterations</td><td>The maximum number of iterations the smoother has to smooth the path, to bound potential computation.</td><td>int</td><td>1000</td></tr><tr><td>&#x3C;name><br>.smoother.w_smooth</td><td>Weight for smoother to apply to smooth out the data points</td><td>double</td><td>0.3</td></tr><tr><td>&#x3C;name><br>.smoother.w_data</td><td>Weight for smoother to apply to retain original data information</td><td>double</td><td>0.2</td></tr><tr><td>&#x3C;name><br>.smoother.tolerance</td><td>Parameter tolerance change amount to terminate smoothing session</td><td>double</td><td>1.00E-10</td></tr><tr><td>&#x3C;name><br>.smoother.do_refinement</td><td>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.</td><td>bool</td><td>TRUE</td></tr><tr><td>&#x3C;name><br>.smoother.refinement_num</td><td>Number of times to recursively attempt to smooth, must be >= 1.</td><td>int</td><td>2</td></tr></tbody></table>

နမူနာ

```yaml
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
      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
      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: 1.0e-10
        do_refinement: true
        refinement_num: 2
```
