diff --git a/configuration/packages/smac/configuring-smac-hybrid.rst b/configuration/packages/smac/configuring-smac-hybrid.rst index fc4042e9d4..5e26408054 100644 --- a/configuration/packages/smac/configuring-smac-hybrid.rst +++ b/configuration/packages/smac/configuring-smac-hybrid.rst @@ -378,6 +378,28 @@ Parameters Description Number of times to recursively attempt to smooth, must be ``>= 1``. +:````.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. + +:````.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: diff --git a/configuration/packages/smac/configuring-smac-lattice.rst b/configuration/packages/smac/configuring-smac-lattice.rst index 29c366f7ae..6a8b64b585 100644 --- a/configuration/packages/smac/configuring-smac-lattice.rst +++ b/configuration/packages/smac/configuring-smac-lattice.rst @@ -328,6 +328,28 @@ Parameters Description Number of times to recursively attempt to smooth, must be ``>= 1``. +:````.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. + +:````.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 diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 8ad570b0f2..c164371804 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -329,6 +329,33 @@ MPPI controller re-implemented using Eigen library and performance improved by 4 In the `PR #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 `_ 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 `_ adds new parameters ``acceleration_limit``, ``deceleration_limit``, ``minimum_speed`` for the `DriveOnHeading` and `BackUp` Behaviors. The default values are as follows: diff --git a/migration/images/smacHybrid_with_all_direction_goal_heading_mode.gif b/migration/images/smacHybrid_with_all_direction_goal_heading_mode.gif new file mode 100644 index 0000000000..ec3c67543e Binary files /dev/null and b/migration/images/smacHybrid_with_all_direction_goal_heading_mode.gif differ diff --git a/migration/images/smacHybrid_with_bidirectional_goal_heading_mode.gif b/migration/images/smacHybrid_with_bidirectional_goal_heading_mode.gif new file mode 100644 index 0000000000..5b6042590f Binary files /dev/null and b/migration/images/smacHybrid_with_bidirectional_goal_heading_mode.gif differ diff --git a/migration/images/smacHybrid_with_default_goal_heading_mode.gif b/migration/images/smacHybrid_with_default_goal_heading_mode.gif new file mode 100644 index 0000000000..1634f883a9 Binary files /dev/null and b/migration/images/smacHybrid_with_default_goal_heading_mode.gif differ