From 0ad2c20d333d0908bd467312ec725fb4ae8b7936 Mon Sep 17 00:00:00 2001 From: Maurice Date: Thu, 1 Jan 2026 23:54:18 +0700 Subject: [PATCH 1/6] Add documentation for symmetric_yaw_tolerance Signed-off-by: Maurice Co-authored-by: Sandeep Dutta --- configuration/packages/configuring-mppic.rst | 11 +++++++++++ .../nav2_controller-plugins/simple_goal_checker.rst | 11 +++++++++++ .../nav2_controller-plugins/stopped_goal_checker.rst | 11 +++++++++++ migration/Kilted.rst | 5 +++++ tuning/index.rst | 10 ++++++++++ 5 files changed, 48 insertions(+) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 26b10217f0..8b837a3b5b 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -482,6 +482,17 @@ This critic incentivizes navigating to achieve the angle of the goal posewhen in Description Minimal distance (m) between robot and goal above which angle goal cost considered. +:symmetric_yaw_tolerance: + + ============== =========================== + Type Default + -------------- --------------------------- + bool false + ============== =========================== + + Description + Enable symmetric goal orientation acceptance. When enabled, the critic prefers trajectories that approach the goal at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. When enabled, the critic uses the minimum distance to either goal orientation, reducing the cost penalty for approaching from the backward direction. See :ref:`tuning` for detailed information. + Goal Critic ----------- diff --git a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst index c5bca8abd0..dd1301e09d 100644 --- a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst @@ -53,3 +53,14 @@ Parameters Description Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes. + +:````.symmetric_yaw_tolerance: + + ==== ======= + Type Default + ---- ------- + bool false + ==== ======= + + Description + Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. See :ref:`tuning` for detailed information. diff --git a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst index 3e56d5dfb3..73865ea230 100644 --- a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst @@ -75,3 +75,14 @@ Parameters Description Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes. + +:````.symmetric_yaw_tolerance: + + ==== ======= + Type Default + ---- ------- + bool false + ==== ======= + + Description + Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. See :ref:`tuning` for detailed information. diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 8bf06d7c06..d0f2b7db31 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -732,3 +732,8 @@ MPPI - ``enforce_path_inversion`` - ``inversion_xy_tolerance`` - ``inversion_yaw_tolerance`` + +Symmetric Yaw Tolerance for Goal Checking and Navigation +-------------------------------------------------------- + +`PR #5833 `_ introduces the symmetric yaw tolerance feature for goal checking and navigation, allowing symmetric robots to reach goals without unnecessary 180° rotations. diff --git a/tuning/index.rst b/tuning/index.rst index 854a5b3b55..c25b3dec5d 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -136,12 +136,22 @@ Costmap2D has a number of plugins that you can use (including the availability f - ``PluginContainerLayer``: Combines the costmap layers specified within this plugin, resulting in an internal costmap that is a product of the costmap layers specified under this layer. This would allow different isolated combinations of costmap layers within the same parent costmap, such as applying a different inflation layers to static layers and obstacle layers In addition, costmap filters: + - ``KeepoutFilter``: Marks keepout, higher weighted, or lower weighted zones in the costmap - ``SpeedFilter``: Reduces or increases robot speeds based on position - ``BinaryFilter``: Enables or disables a binary topic when in particular zones Note: When the costmap filters can be paired with the ``VectorObject`` server to use vectorized zones rather than map rastered zones sharing the same software. +Symmetric Yaw Tolerance +======================= + +For robots that can drive equally well forward and backward, you can set `symmetric_yaw_tolerance: true` to allow the robot to reach goals without unnecessary 180° rotations. This is especially useful for symmetric robots (e.g., differential drive robots with sensors on both ends). + +- ``SimpleGoalChecker``: Accepts the goal as reached if the robot is within tolerance of either the goal orientation or the goal orientation + 180°. +- ``GoalAngleCritic (MPPI Controller)``: Scores trajectories based on the minimum angular distance to either the goal orientation or the flipped orientation. + +We also **highly recommend** watching this ROSCon 2023 talk for more context: https://vimeo.com/879000809 Nav2 Launch Options =================== From d6e5bd04840b0e57ed16652f3f391f430ae984dd Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Mon, 5 Jan 2026 23:51:03 +0800 Subject: [PATCH 2/6] Update configuration/packages/configuring-mppic.rst Co-authored-by: Steve Macenski Signed-off-by: Maurice Alexander Purnawan --- configuration/packages/configuring-mppic.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/configuring-mppic.rst b/configuration/packages/configuring-mppic.rst index 8b837a3b5b..bcd4771cd2 100644 --- a/configuration/packages/configuring-mppic.rst +++ b/configuration/packages/configuring-mppic.rst @@ -491,7 +491,7 @@ This critic incentivizes navigating to achieve the angle of the goal posewhen in ============== =========================== Description - Enable symmetric goal orientation acceptance. When enabled, the critic prefers trajectories that approach the goal at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. When enabled, the critic uses the minimum distance to either goal orientation, reducing the cost penalty for approaching from the backward direction. See :ref:`tuning` for detailed information. + Enable symmetric goal orientation acceptance. When enabled, the critic prefers trajectories that approach the goal at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in (i.e. MPPI decides). When enabled, the critic uses the minimum distance to either goal orientation, reducing the cost penalty for approaching from the backward direction. See :ref:`tuning` for detailed information. Goal Critic ----------- From 2416413e836effcbf159b6304fb840e048d796b1 Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Mon, 5 Jan 2026 23:51:17 +0800 Subject: [PATCH 3/6] Update configuration/packages/nav2_controller-plugins/simple_goal_checker.rst Co-authored-by: Steve Macenski Signed-off-by: Maurice Alexander Purnawan --- .../packages/nav2_controller-plugins/simple_goal_checker.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst index dd1301e09d..6838ca384a 100644 --- a/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/simple_goal_checker.rst @@ -63,4 +63,4 @@ Parameters ==== ======= Description - Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. See :ref:`tuning` for detailed information. + Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in (i.e. controller algorithm decides). See :ref:`tuning` for detailed information. From e1d402453f5fc582cf784f72277b15bc33d578a6 Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Mon, 5 Jan 2026 23:51:30 +0800 Subject: [PATCH 4/6] Update tuning/index.rst Co-authored-by: Steve Macenski Signed-off-by: Maurice Alexander Purnawan --- tuning/index.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/tuning/index.rst b/tuning/index.rst index c25b3dec5d..dbb8feec85 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -151,7 +151,6 @@ For robots that can drive equally well forward and backward, you can set `symmet - ``SimpleGoalChecker``: Accepts the goal as reached if the robot is within tolerance of either the goal orientation or the goal orientation + 180°. - ``GoalAngleCritic (MPPI Controller)``: Scores trajectories based on the minimum angular distance to either the goal orientation or the flipped orientation. -We also **highly recommend** watching this ROSCon 2023 talk for more context: https://vimeo.com/879000809 Nav2 Launch Options =================== From 5912309a28b2a24c162f364e527d130a6b8577ed Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Mon, 5 Jan 2026 23:51:42 +0800 Subject: [PATCH 5/6] Update tuning/index.rst Co-authored-by: Steve Macenski Signed-off-by: Maurice Alexander Purnawan --- tuning/index.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tuning/index.rst b/tuning/index.rst index dbb8feec85..4266358d6f 100644 --- a/tuning/index.rst +++ b/tuning/index.rst @@ -146,7 +146,9 @@ Note: When the costmap filters can be paired with the ``VectorObject`` server to Symmetric Yaw Tolerance ======================= -For robots that can drive equally well forward and backward, you can set `symmetric_yaw_tolerance: true` to allow the robot to reach goals without unnecessary 180° rotations. This is especially useful for symmetric robots (e.g., differential drive robots with sensors on both ends). +For robots that can drive equally well forward and backward, you can set `symmetric_yaw_tolerance: true` to allow the robot to reach goals without unnecessary 180° rotations. This is especially useful for symmetric robots (e.g., differential drive robots with sensors on both ends) whereas you do not care which orientation it ends in for the controller algorithm to decide. + +If you wish to control which orientation it ends in using a symmetric robot, check out `Bidirectional navigation with Nav2 `_ from ROSCon 2023 for setting up and tuning a bidirectional robot using Nav2. It highlights how to tune controllers, planners, and various other settings to obtain fully unbiased navigation with a platform with no traditional 'front'. - ``SimpleGoalChecker``: Accepts the goal as reached if the robot is within tolerance of either the goal orientation or the goal orientation + 180°. - ``GoalAngleCritic (MPPI Controller)``: Scores trajectories based on the minimum angular distance to either the goal orientation or the flipped orientation. From 60391bd2ff776b3cade34345c918157df0fadb73 Mon Sep 17 00:00:00 2001 From: Maurice Alexander Purnawan Date: Mon, 5 Jan 2026 23:51:53 +0800 Subject: [PATCH 6/6] Update configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst Co-authored-by: Steve Macenski Signed-off-by: Maurice Alexander Purnawan --- .../packages/nav2_controller-plugins/stopped_goal_checker.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst index 73865ea230..fdd0d08296 100644 --- a/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst +++ b/configuration/packages/nav2_controller-plugins/stopped_goal_checker.rst @@ -85,4 +85,4 @@ Parameters ==== ======= Description - Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in. See :ref:`tuning` for detailed information. + Enable symmetric goal orientation acceptance. When enabled, the robot accepts the goal as reached when oriented at either the goal orientation or the goal orientation + 180°. This is useful for symmetric robots (e.g., differential drives with sensors on both ends) that can navigate equally well in forward and backward directions and does not care which direction it ends in (i.e. controller algorithm decides). See :ref:`tuning` for detailed information.