[Smac Planner] Enable goal orientation non-specificity

configuration/packages/smac/configuring-smac-hybrid.rst

@@ -378,6 +378,17 @@ Parameters
   Description
     Number of times to recursively attempt to smooth, must be ``>= 1``.
 
+:``<name>``.goal_heading_mode:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  string            "DEFAULT"   
+  ============== ===========================
+
+  Description
+    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.
+
 Example
 *******
 .. code-block:: yaml
@@ -414,6 +425,7 @@ Example
         use_quadratic_cost_penalty: False
         downsample_obstacle_heuristic: True
         allow_primitive_interpolation: False
+        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:

configuration/packages/smac/configuring-smac-lattice.rst

@@ -328,6 +328,19 @@ Parameters
   Description
     Number of times to recursively attempt to smooth, must be ``>= 1``.
 
+:``<name>``.goal_heading_mode:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  string            "DEFAULT"   
+  ============== ===========================
+
+  Description
+    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.
+
+
+
 Example
 *******
 .. code-block:: yaml
@@ -358,6 +371,7 @@ Example
         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).
+        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

migration/Iron.rst

@@ -282,3 +282,29 @@ New interface for ``GlobalPlanner::createPlan``:
 
 This is implemented for all the planners in the stack, you can check them for the example use of ``cancel_checker`` function (simply check ``cancel_checker()``).
 Smac and Theta* planners have a new parameter ``terminal_checking_interval`` which is the frequency of the cancel or timeout checking in terms of number of iterations.
+
+Enable goal orientation non-specificity
+*******************************************
+`PR #4148 <https://github.com/ros-planning/navigation2/pull/4127>`_  introduces a new parameter(goal_heading_mode) in the smac planner, specifically the smac planner hybrid and smac planner lattice that allows for the
+planner to plan to a goal with multiple orientations and return the best path in just one planning call. This is useful for robots that can approach a goal from multiple orientations and the user does not want to plan to each orientation separately.
+
+Here is an Example of the smacHybrid planner with the default goal_heading_mode to see the difference in the planned path.
+
+.. image:: images/smacHybrid_with_default_goal_heading_mode.gif
+    :width: 700px
+    :alt: Navigation2 with smacHybrid planner with default goal_heading_mode
+    :align: center
+
+Here is an Example of the smacHybrid planner with the bidirectional goal_heading_mode to see the difference in the planned path.
+
+.. image:: images/smacHybrid_with_bidirectional_goal_heading_mode.gif
+    :width: 700px
+    :alt: Navigation2 with smacHybrid planner with bidirectional goal_heading_mode
+    :align: center
+
+Here is an Example of the smacHybrid planner with the all_directions goal_heading_mode to see the difference in the planned path.
+
+.. image:: images/smacHybrid_with_all_direction_goal_heading_mode.gif
+    :width: 700px
+    :alt: Navigation2 with smacHybrid planner with all_direction goal_heading_mode
+    :align: center