Add symmetric goal for goal checker and mppi goal angle critic

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -75,6 +75,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
 protected:
   double xy_goal_tolerance_, yaw_goal_tolerance_, path_length_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

@@ -41,10 +41,7 @@
 #include "angles/angles.h"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.hpp"
-#pragma GCC diagnostic pop
 
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
@@ -58,6 +55,7 @@ SimpleGoalChecker::SimpleGoalChecker()
   path_length_tolerance_(1.0),
   stateful_(true),
   check_xy_(true),
+  symmetric_yaw_tolerance_(false),
   xy_goal_tolerance_sq_(0.0625)
 {
 }
@@ -75,6 +73,8 @@ void SimpleGoalChecker::initialize(
   path_length_tolerance_ = node->declare_or_get_parameter(
     plugin_name + ".path_length_tolerance", 1.0);
   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_;
 
@@ -110,10 +110,23 @@ 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 {
+    double dyaw = angles::shortest_angular_distance(query_yaw, goal_yaw);
+    return fabs(dyaw) <= yaw_goal_tolerance_;
+  }
 }
 
 bool SimpleGoalChecker::getTolerances(
@@ -161,6 +174,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

@@ -233,7 +233,8 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
     {rclcpp::Parameter("test2.xy_goal_tolerance", 200.0),
       rclcpp::Parameter("test2.yaw_goal_tolerance", 200.0),
       rclcpp::Parameter("test2.path_length_tolerance", 200.0),
-      rclcpp::Parameter("test2.stateful", true)});
+      rclcpp::Parameter("test2.stateful", true),
+      rclcpp::Parameter("test2.symmetric_yaw_tolerance", true)});
 
   rclcpp::spin_until_future_complete(
     x->get_node_base_interface(),
@@ -243,6 +244,7 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
   EXPECT_EQ(x->get_parameter("test2.yaw_goal_tolerance").as_double(), 200.0);
   EXPECT_EQ(x->get_parameter("test2.path_length_tolerance").as_double(), 200.0);
   EXPECT_EQ(x->get_parameter("test2.stateful").as_bool(), true);
+  EXPECT_EQ(x->get_parameter("test2.symmetric_yaw_tolerance").as_bool(), true);
 
   // Test the dynamic parameters impacted the tolerances
   EXPECT_TRUE(sgc.getTolerances(pose_tol, vel_tol));
@@ -404,6 +406,26 @@ TEST(StoppedGoalChecker, is_reached)
     transformed_global_plan));
   EXPECT_FALSE(pgc.isGoalReached(first_pose.pose, last_pose.pose, velocity,
     transformed_global_plan));
+
+  current_pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(0.25 + M_PI);
+  transformed_global_plan.poses.clear();
+  EXPECT_FALSE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
+  EXPECT_FALSE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
+  EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
+
+  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("test2.symmetric_yaw_tolerance", true),
+      rclcpp::Parameter("test.symmetric_yaw_tolerance", true)});
+  rclcpp::spin_until_future_complete(
+    x->get_node_base_interface(),
+    results);
+  EXPECT_TRUE(sgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
+  EXPECT_TRUE(gc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
+  EXPECT_TRUE(pgc.isGoalReached(current_pose, goal_pose, velocity, transformed_global_plan));
 }
 
 int main(int argc, char ** argv)

nav2_mppi_controller/README.md

@@ -85,6 +85,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 if either is valid and wish the controller to select the easiest one itself. |
 
 #### 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,7 +13,7 @@
 // limitations under the License.
 
 #include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
-
+#include "angles/angles.h"
 namespace mppi::critics
 {
 
@@ -24,12 +24,13 @@ 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)
@@ -42,12 +43,20 @@ void GoalAngleCritic::score(CriticData & data)
 
   double goal_yaw = tf2::getYaw(goal.orientation);
 
+  auto angular_distances = utils::shortest_angular_distance(data.trajectories.yaws,
+    goal_yaw).abs().eval();
+
+  if (symmetric_yaw_tolerance_) {
+    double symmetric_goal_yaw = angles::normalize_angle(goal_yaw + M_PI);
+    auto symmetric_distances = utils::shortest_angular_distance(data.trajectories.yaws,
+      symmetric_goal_yaw).abs().eval();
+    angular_distances = angular_distances.min(symmetric_distances);
+  }
+
   if (power_ > 1u) {
-    data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
-      rowwise().mean()) * weight_).pow(power_).eval();
+    data.costs += ((angular_distances.rowwise().mean()) * weight_).pow(power_).eval();
   } else {
-    data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
-      rowwise().mean()) * weight_).eval();
+    data.costs += ((angular_distances.rowwise().mean()) * weight_).eval();
   }
 }
 

nav2_mppi_controller/test/critics_tests.cpp

@@ -269,6 +269,66 @@ TEST(CriticTests, GoalAngleCritic)
   EXPECT_NEAR(costs(0), 9.42, 0.02);  // (3.14 - 0.0) * 3.0 weight
 }
 
+TEST(CriticTests, GoalAngleCriticSymmetric)
+{
+  auto node = std::make_shared<nav2::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  generated_trajectories.reset(1000, 30);
+  models::Path path;
+  geometry_msgs::msg::Pose goal;
+  path.reset(10);
+  Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000);
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, goal, costs, model_dt,
+    false, nullptr, nullptr, std::nullopt, std::nullopt};
+  data.motion_model = std::make_shared<DiffDriveMotionModel>();
+
+  // Make sure initializes correctly
+  auto getParam = param_handler.getParamGetter("critic");
+  bool symmetric_yaw_tolerance = true;
+  getParam(symmetric_yaw_tolerance, "symmetric_yaw_tolerance", true);
+  GoalAngleCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // provide state poses and path too far from `threshold_to_consider` to consider
+  state.pose.pose.position.x = 9.7;
+  path.x(9) = 10.0;
+  path.y(9) = 0.0;
+  path.yaws(9) = 3.14;
+  goal.position.x = 10.0;
+  goal.position.y = 0.0;
+  goal.orientation.x = 0.0;
+  goal.orientation.y = 0.0;
+  goal.orientation.z = 1.0;
+  goal.orientation.w = 0.0;
+  state.local_path_length = std::abs(state.pose.pose.position.x - goal.position.x);
+
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  EXPECT_NEAR(costs(0), 0, 0.02);  // Should be zero cost due to symmetry
+
+  path.yaws(9) = 0.0;
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  EXPECT_NEAR(costs(0), 0, 0.02);  // (0.0 - 0.0) * 3.0 weight
+
+  path.yaws(9) = 1.57;
+  critic.score(data);
+  EXPECT_GT(costs.sum(), 0);
+  EXPECT_NEAR(costs(0), 4.71, 0.02);  // (1.57 - 0.0) * 3.0 weight
+}
+
 TEST(CriticTests, GoalCritic)
 {
   // Standard preamble