[Nav2] Enable Goal Orientation Flexibility in Smac Planners

Hey Nav2 Community! :waving_hand:

We’re excited to share a new feature in the Smac Planners that makes dealing with goal orientations way more flexible. This came out of this feature request — where the idea was to let the planner consider multiple orientations for the same goal, allowing more flexibility for applications where the goal orientation isn’t as important while still retaining kinematic feasibility!. :bullseye:

This results in better planning speed and less hassle.

Description of contribution

What’s New?

This is part of PR #4127, and it adds two new more parameters to the Smac planners (SmacHybrid and SmacLattice):

  • goal_heading_mode
    • "DEFAULT" – stick with the orientation you give it (classic behavior)
    • "BIDIRECTIONAL" – try your orientation and its 180° flip
    • "ALL_DIRECTION" – explore all quantized headings and pick the best path cost-wise
  • coarse_search_resolution
    • This parameter allows users to skip goal heading bins during coarse search, reducing compute time for "ALL_DIRECTION" mode. It speeds up planning significantly when many angle bins are in use.

Visuals!

Default
smacHybrid_with_default_goal_heading_mode

Bidirectional
smacHybrid_with_bidirectional_goal_heading_mode

All direction
smacHybrid_with_all_direction_goal_heading_mode

There’s still more room to improve — especially around optimizing the distance heuristic when planning with multiple goal headings. So expect even more speed-ups in future updates.

Happy Navigating! :rocket:

3 Likes