[Smac Planner] Enable goal orientation non-specificity

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

@@ -378,6 +378,28 @@ 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.
+
+:``<name>``.coarse_search_resolution:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  string            "4"   
+  ============== ===========================
+
+  Description
+    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.
+
 Example
 *******
 .. code-block:: yaml
@@ -414,6 +436,8 @@ Example
         use_quadratic_cost_penalty: False
         downsample_obstacle_heuristic: True
         allow_primitive_interpolation: False
+        coarse_search_resolution: 4         # 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:

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

@@ -328,6 +328,28 @@ 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.
+
+:``<name>``.coarse_search_resolution:
+
+  ============== ===========================
+  Type           Default                    
+  -------------- ---------------------------
+  string            "1"   
+  ============== ===========================
+
+  Description
+    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).
+
 Example
 *******
 .. code-block:: yaml
@@ -358,6 +380,8 @@ 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 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

migration/Iron.rst

@@ -363,4 +363,4 @@ MPPI Controller: Addition of acceleration constraints
 
 RegulatedPurePursuit Controller [RPP]: new parameter ``use_cancel_deceleration``
 ********************************************************************************
-`PR #4441 <https://github.com/ros-navigation/navigation2/pull/4441>`_ adds a new parameter use_cancel_deceleration for the regulated pure pursuit controllers. This parameter enables the controller to use a constant deceleration to stop the robot gracefully instead of stopping immediately when a goal is canceled.
+`PR #4441 <https://github.com/ros-navigation/navigation2/pull/4441>`_ adds a new parameter use_cancel_deceleration for the regulated pure pursuit controllers. This parameter enables the controller to use a constant deceleration to stop the robot gracefully instead of stopping immediately when a goal is canceled.

migration/Jazzy.rst

@@ -254,6 +254,33 @@ MPPI controller re-implemented using Eigen library and performance improved by 4
 In the `PR #4621 <https://github.com/ros-navigation/navigation2/pull/4621>`_ MPPI controller is fully reimplemented using Eigen as it is well supported hpc library and suits better for our use case of two dimensional batches of trajectories. GPU support for rolling out trajectories could also be possible in future using Eigen.
 MPPI Optimizer's performance is improved by 40-50%. Now MPPI Controller can also be run on ARM processors which do not support SIMD Instructions extensively. 
 
+Enable goal orientation non-specificity
+***************************************
+`PR #4148 <https://github.com/ros-planning/navigation2/pull/4127>`_  introduces two new parameters(goal_heading_mode, coarse_search_resolution) 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.
+In addition to this, the coarse_search_resolution parameter is added to the smac planner lattice to allow for a faster search for the best path.
+
+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
+
 DriveOnHeading and BackUp behaviors: Addition of acceleration constraints
 *************************************************************************
 `PR #4810 <https://github.com/ros-navigation/navigation2/pull/4810>`_ adds new parameters ``acceleration_limit``, ``deceleration_limit``, ``minimum_speed`` for the `DriveOnHeading` and `BackUp` Behaviors. The default values are as follows:
@@ -291,4 +318,4 @@ The parameter ``closed_loop`` was introduced to the Rotation Shim Controller to
 
 Default value:
 
-- true
+- true