merged symmetric yaw tolerance from humble

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -74,6 +74,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
 protected:
   double xy_goal_tolerance_, yaw_goal_tolerance_;
   bool stateful_, check_xy_;
+  bool symmetric_yaw_tolerance_;
   // Cached squared xy_goal_tolerance_
   double xy_goal_tolerance_sq_;
   // Dynamic parameters handler

nav2_controller/plugins/simple_goal_checker.cpp

@@ -57,6 +57,7 @@ SimpleGoalChecker::SimpleGoalChecker()
   yaw_goal_tolerance_(0.25),
   stateful_(true),
   check_xy_(true),
+  symmetric_yaw_tolerance_(false),
   xy_goal_tolerance_sq_(0.0625)
 {
 }
@@ -72,6 +73,8 @@ void SimpleGoalChecker::initialize(
   xy_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".xy_goal_tolerance", 0.25);
   yaw_goal_tolerance_ = node->declare_or_get_parameter(plugin_name + ".yaw_goal_tolerance", 0.25);
   stateful_ = node->declare_or_get_parameter(plugin_name + ".stateful", true);
+  symmetric_yaw_tolerance_ = node->declare_or_get_parameter(
+    plugin_name + ".symmetric_yaw_tolerance", false);
 
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 
@@ -101,10 +104,26 @@ bool SimpleGoalChecker::isGoalReached(
       check_xy_ = false;
     }
   }
-  double dyaw = angles::shortest_angular_distance(
-    tf2::getYaw(query_pose.orientation),
-    tf2::getYaw(goal_pose.orientation));
-  return fabs(dyaw) <= yaw_goal_tolerance_;
+
+  double query_yaw = tf2::getYaw(query_pose.orientation);
+  double goal_yaw = tf2::getYaw(goal_pose.orientation);
+
+  if (symmetric_yaw_tolerance_) {
+    // For symmetric robots: accept either goal orientation or goal + 180°
+    double dyaw_forward = angles::shortest_angular_distance(query_yaw, goal_yaw);
+    double dyaw_backward = angles::shortest_angular_distance(
+      query_yaw,
+      angles::normalize_angle(goal_yaw + M_PI));
+
+    bool forward_match = fabs(dyaw_forward) <= yaw_goal_tolerance_;
+    bool backward_match = fabs(dyaw_backward) <= yaw_goal_tolerance_;
+
+    return forward_match || backward_match;
+  } else {
+    // Standard behavior: only accept the specified goal orientation
+    double dyaw = angles::shortest_angular_distance(query_yaw, goal_yaw);
+    return fabs(dyaw) <= yaw_goal_tolerance_;
+  }
 }
 
 bool SimpleGoalChecker::getTolerances(
@@ -150,6 +169,8 @@ SimpleGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == plugin_name_ + ".stateful") {
         stateful_ = parameter.as_bool();
+      } else if (param_name == plugin_name_ + ".symmetric_yaw_tolerance") {
+        symmetric_yaw_tolerance_ = parameter.as_bool();
       }
     }
   }

nav2_controller/plugins/test/goal_checker.cpp

@@ -340,6 +340,67 @@ TEST(StoppedGoalChecker, is_reached)
   EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity));
 }
 
+TEST(SimpleGoalChecker, symmetric_yaw_tolerance_disabled)
+{
+  auto x = std::make_shared<TestLifecycleNode>("goal_checker_symmetric");
+
+  SimpleGoalChecker gc;
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  gc.initialize(x, "symmetric_test", costmap);
+
+  // Test that with symmetric_yaw_tolerance disabled (default),
+  // robot at 0 rad and goal at PI rad is NOT reached
+  // (only exact goal orientation or close to it is accepted)
+  checkMacro(gc, 0, 0, 0, 0, 0, 3.14159, 0, 0, 0, false);
+}
+
+TEST(SimpleGoalChecker, symmetric_yaw_tolerance_enabled)
+{
+  auto x = std::make_shared<TestLifecycleNode>("goal_checker_symmetric_enabled");
+
+  SimpleGoalChecker gc;
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  gc.initialize(x, "symmetric_test2", costmap);
+
+  // Enable symmetric_yaw_tolerance
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    x->get_node_base_interface(), x->get_node_topics_interface(),
+    x->get_node_graph_interface(),
+    x->get_node_services_interface());
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("symmetric_test2.symmetric_yaw_tolerance", true),
+      rclcpp::Parameter("symmetric_test2.yaw_goal_tolerance", 0.3)});
+
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(x->get_parameter("symmetric_test2.symmetric_yaw_tolerance").as_bool(), true);
+
+  // Test that with symmetric_yaw_tolerance enabled,
+  // robot at 0 rad and goal at PI rad IS reached
+  // (because robot can face backward, which is PI rad)
+  checkMacro(gc, 0, 0, 0, 0, 0, 3.14159, 0, 0, 0, true);
+
+  // Test that forward orientation still works
+  checkMacro(gc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
+
+  // Test that intermediate angles work with tolerance
+  checkMacro(gc, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, true);
+  checkMacro(gc, 0, 0, -0.1, 0, 0, 0, 0, 0, 0, true);
+
+  // Test that intermediate angles work for backward (PI) orientation
+  checkMacro(gc, 0, 0, 3.14159 + 0.1, 0, 0, 3.14159, 0, 0, 0, true);
+  checkMacro(gc, 0, 0, 3.14159 - 0.1, 0, 0, 3.14159, 0, 0, 0, true);
+
+  // Test that angles outside tolerance are not reached
+  checkMacro(gc, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, false);
+  checkMacro(gc, 0, 0, 1.5, 0, 0, 3.14159, 0, 0, 0, false);
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_mppi_controller/README.md

@@ -95,6 +95,7 @@ This process is then repeated a number of times and returns a converged solution
  | cost_weight                      | double | Default 3.0. Weight to apply to critic term.                                                                |
  | cost_power                       | int    | Default 1. Power order to apply to term.                                                                    |
  | threshold_to_consider            | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered.               |
+ | symmetric_yaw_tolerance          | bool   | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty.|
 
 #### Goal Critic
  | Parameter            | Type   | Definition                                                                                                  |

nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp

@@ -43,6 +43,7 @@ class GoalAngleCritic : public CriticFunction
   void score(CriticData & data) override;
 
 protected:
+  bool symmetric_yaw_tolerance_{false};
   float threshold_to_consider_{0};
   unsigned int power_{0};
   float weight_{0};

nav2_mppi_controller/src/critics/goal_angle_critic.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
+#include "angles/angles.h"
 
 namespace mppi::critics
 {
@@ -24,12 +25,14 @@ void GoalAngleCritic::initialize()
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.0f);
   getParam(threshold_to_consider_, "threshold_to_consider", 0.5f);
+  getParam(symmetric_yaw_tolerance_, "symmetric_yaw_tolerance", false);
 
   RCLCPP_INFO(
     logger_,
-    "GoalAngleCritic instantiated with %d power, %f weight, and %f "
-    "angular threshold.",
-    power_, weight_, threshold_to_consider_);
+    "GoalAngleCritic instantiated with %d power, %f weight, %f "
+    "angular threshold, and symmetric_yaw_tolerance %s.",
+    power_, weight_, threshold_to_consider_,
+    symmetric_yaw_tolerance_ ? "enabled" : "disabled");
 }
 
 void GoalAngleCritic::score(CriticData & data)
@@ -41,13 +44,22 @@ void GoalAngleCritic::score(CriticData & data)
   geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path);
 
   double goal_yaw = tf2::getYaw(goal.orientation);
+  auto min_distance = ((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
+    rowwise().mean()).eval();
+  if (symmetric_yaw_tolerance_) {
+    // For symmetric robots: use minimum distance to either goal orientation or goal + 180°
+    const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI);
 
+    auto distance_to_flipped = utils::shortest_angular_distance(data.trajectories.yaws,
+                                                             goal_yaw_flipped).abs();
+
+    // Use the minimum distance
+    min_distance = min_distance.cwiseMin(distance_to_flipped).rowwise().mean().eval();
+  }
   if (power_ > 1u) {
-    data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
-      rowwise().mean()) * weight_).pow(power_).eval();
+    data.costs += (min_distance * weight_).pow(power_).eval();
   } else {
-    data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
-      rowwise().mean()) * weight_).eval();
+    data.costs += (min_distance * weight_).eval();
   }
 }
 

nav2_mppi_controller/test/critics_tests.cpp

@@ -269,6 +269,178 @@ TEST(CriticTests, GoalAngleCritic)
   EXPECT_NEAR(costs(0), 9.42, 0.02);  // (3.14 - 0.0) * 3.0 weight
 }
 
+TEST(CriticTests, GoalAngleCriticSymmetricYawTolerance)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", "dummy_costmap");
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  // Set parameters for critic with symmetric disabled
+  node->declare_parameter("critic_sym_disabled.cost_power", 1);
+  node->declare_parameter("critic_sym_disabled.cost_weight", 3.0);
+  node->declare_parameter("critic_sym_disabled.threshold_to_consider", 0.5);
+  node->declare_parameter("critic_sym_disabled.symmetric_yaw_tolerance", false);
+
+  // Set parameters for critic with symmetric enabled
+  node->declare_parameter("critic_sym_enabled.cost_power", 1);
+  node->declare_parameter("critic_sym_enabled.cost_weight", 3.0);
+  node->declare_parameter("critic_sym_enabled.threshold_to_consider", 0.5);
+  node->declare_parameter("critic_sym_enabled.symmetric_yaw_tolerance", true);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+
+  // Setup goal angle critic with symmetric_yaw_tolerance disabled (default)
+  GoalAngleCritic critic;
+  critic.on_configure(node, "mppi", "critic_sym_disabled", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic_sym_disabled");
+
+  // Setup goal angle critic with symmetric_yaw_tolerance enabled
+  GoalAngleCritic critic_symmetric;
+  critic_symmetric.on_configure(node, "mppi", "critic_sym_enabled", costmap_ros, &param_handler);
+  EXPECT_EQ(critic_symmetric.getName(), "critic_sym_enabled");
+
+  // Setup state and path: robot at origin, goal at (10, 0) with yaw of PI
+  state.pose.pose.position.x = 9.7;  // Within threshold_to_consider (0.5) of goal at 10.0
+  path.reset(10);
+  path.x(9) = 10.0;
+  path.y(9) = 0.0;
+  path.yaws(9) = 3.14159;  // Goal at PI
+
+  // Test 1: Trajectory at 0 rad (forward) with symmetric disabled
+  // Should have high cost since it's not aligned with goal at PI
+  generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data1.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic.score(data1);
+  float cost_forward_without_symmetric = costs(0);
+  EXPECT_GT(cost_forward_without_symmetric, 9.4);  // Should have significant cost
+
+  // Test 2: Trajectory at PI rad (backward) with symmetric disabled
+  // Should have zero/low cost since backward matches goal orientation PI
+  generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data2.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic.score(data2);
+  float cost_backward_without_symmetric = costs(0);
+  EXPECT_LT(cost_backward_without_symmetric, 0.0001);  // Should be nearly zero
+
+  // Test 3: Trajectory at 0 rad (forward) with symmetric ENABLED
+  // Should have LOWER cost than Test 1 because with symmetric enabled,
+  // 0 rad is aligned with goal + PI (backward), which equals goal orientation
+  generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data3.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic_symmetric.score(data3);
+  float cost_forward_with_symmetric = costs(0);
+  EXPECT_LT(cost_forward_with_symmetric, 0.0001);  // Should be nearly zero
+  // Should be lower than without symmetric
+  EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric);
+}
+
+TEST(CriticTests, GoalAngleCriticSymmetricYawTolerancePow)
+{
+  // Standard preamble
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", "dummy_costmap");
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  // Set parameters for critic with symmetric disabled
+  node->declare_parameter("critic_sym_disabled.cost_power", 2);
+  node->declare_parameter("critic_sym_disabled.cost_weight", 3.0);
+  node->declare_parameter("critic_sym_disabled.threshold_to_consider", 0.5);
+  node->declare_parameter("critic_sym_disabled.symmetric_yaw_tolerance", false);
+
+  // Set parameters for critic with symmetric enabled
+  node->declare_parameter("critic_sym_enabled.cost_power", 2);
+  node->declare_parameter("critic_sym_enabled.cost_weight", 3.0);
+  node->declare_parameter("critic_sym_enabled.threshold_to_consider", 0.5);
+  node->declare_parameter("critic_sym_enabled.symmetric_yaw_tolerance", true);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+
+  // Setup goal angle critic with symmetric_yaw_tolerance disabled (default)
+  GoalAngleCritic critic;
+  critic.on_configure(node, "mppi", "critic_sym_disabled", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic_sym_disabled");
+
+  // Setup goal angle critic with symmetric_yaw_tolerance enabled
+  GoalAngleCritic critic_symmetric;
+  critic_symmetric.on_configure(node, "mppi", "critic_sym_enabled", costmap_ros, &param_handler);
+  EXPECT_EQ(critic_symmetric.getName(), "critic_sym_enabled");
+
+  // Setup state and path: robot at origin, goal at (10, 0) with yaw of PI
+  state.pose.pose.position.x = 9.7;  // Within threshold_to_consider (0.5) of goal at 10.0
+  path.reset(10);
+  path.x(9) = 10.0;
+  path.y(9) = 0.0;
+  path.yaws(9) = 3.14159;  // Goal at PI
+
+  // Test 1: Trajectory at 0 rad (forward) with symmetric disabled
+  // Should have high cost since it's not aligned with goal at PI
+  generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data1 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data1.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic.score(data1);
+  float cost_forward_without_symmetric = costs(0);
+  EXPECT_GT(cost_forward_without_symmetric, 88.8);  // Should have significant cost
+
+  // Test 2: Trajectory at PI rad (backward) with symmetric disabled
+  // Should have zero/low cost since backward matches goal orientation PI
+  generated_trajectories.yaws = Eigen::ArrayXXf::Constant(1000, 30, 3.14159f);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data2 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data2.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic.score(data2);
+  float cost_backward_without_symmetric = costs(0);
+  EXPECT_LT(cost_backward_without_symmetric, 0.0001);  // Should be nearly zero
+
+  // Test 3: Trajectory at 0 rad (forward) with symmetric ENABLED
+  // Should have LOWER cost than Test 1 because with symmetric enabled,
+  // 0 rad is aligned with goal + PI (backward), which equals goal orientation
+  generated_trajectories.yaws = Eigen::ArrayXXf::Zero(1000, 30);
+  costs = Eigen::ArrayXf::Zero(1000);
+  CriticData data3 = {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr,
+    nullptr, std::nullopt, std::nullopt};
+  data3.motion_model = std::make_shared<DiffDriveMotionModel>();
+  critic_symmetric.score(data3);
+  float cost_forward_with_symmetric = costs(0);
+  EXPECT_LT(cost_forward_with_symmetric, 1e-10);  // Should be nearly zero
+  // Should be lower than without symmetric
+  EXPECT_LT(cost_forward_with_symmetric, cost_forward_without_symmetric);
+}
+
 TEST(CriticTests, GoalCritic)
 {
   // Standard preamble