fix MPPI goal critic inversion (#5088) (#5105)

brayanpa · web-flow · commit 6f57e8248a2d · 2025-05-19T11:38:08.000-07:00
* fix MPPI goal critic inversion (#5088) Signed-off-by: brayanpa <brayanspallares@gmail.com> * Support path inversion in all critics (#5088) Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix code style issues and formatting Signed-off-by: brayanpa <brayanspallares@gmail.com> * Remove trailing whitespaces Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix goalAngleCritic tests Signed-off-by: brayanpa <brayanspallares@gmail.com> * Normalize code formatting Signed-off-by: brayanpa <brayanspallares@gmail.com> * Add getLastPathPose test Signed-off-by: brayanpa <brayanspallares@gmail.com> * Abstract getCriticGoal in nav2_mppi critics Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix whitespace issues Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix getCriticGoal test Signed-off-by: brayanpa <brayanspallares@gmail.com> --------- Signed-off-by: brayanpa <brayanspallares@gmail.com> Signed-off-by: Brayan Pallares <brayanspallares@gmail.com>
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -73,7 +73,7 @@ This process is then repeated a number of times and returns a converged solution
  | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping.   |
  | prune_distance             | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to.                            |
  | transform_tolerance        | double | Default: 0.1. Time tolerance for data transformations with TF.                                              |
- | enforce_path_inversion        | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.      |
+ | enforce_path_inversion        | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. In addition, these cusping points will also be treated by the critics as local goals that the robot will attempt to reach. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested.      |
  | inversion_xy_tolerance        | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion.      |
  | inversion_yaw_tolerance        | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg.  |
 
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp
@@ -144,6 +144,7 @@ class CostCritic : public CriticFunction
   std::string inflation_layer_name_;
 
   unsigned int power_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp
@@ -46,6 +46,7 @@ class GoalAngleCritic : public CriticFunction
   float threshold_to_consider_{0};
   unsigned int power_{0};
   float weight_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp
@@ -46,6 +46,7 @@ class GoalCritic : public CriticFunction
   unsigned int power_{0};
   float weight_{0};
   float threshold_to_consider_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
@@ -97,6 +97,7 @@ class ObstaclesCritic : public CriticFunction
 
   unsigned int power_{0};
   float repulsion_weight_, critical_weight_{0};
+  bool enforce_path_inversion_{false};
   std::string inflation_layer_name_;
 };
 
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
@@ -52,6 +52,7 @@ class PathAlignCritic : public CriticFunction
   bool use_path_orientations_{false};
   unsigned int power_{0};
   float weight_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
@@ -81,6 +81,7 @@ class PathAngleCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp
@@ -53,6 +53,7 @@ class PathFollowCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp
@@ -45,6 +45,7 @@ class PreferForwardCritic : public CriticFunction
   unsigned int power_{0};
   float weight_{0};
   float threshold_to_consider_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp
@@ -44,6 +44,7 @@ class TwirlingCritic : public CriticFunction
 protected:
   unsigned int power_{0};
   float weight_{0};
+  bool enforce_path_inversion_{false};
 };
 
 }  // namespace mppi::critics
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -218,6 +218,49 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path)
   return result;
 }
 
+/**
+ * @brief Get the last pose from a path
+ * @param path Reference to the path
+ * @return geometry_msgs::msg::Pose Last pose in the path
+ */
+inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
+{
+  const unsigned int path_last_idx = path.x.size() - 1;
+
+  auto last_orientation = path.yaws(path_last_idx);
+
+  tf2::Quaternion pose_orientation;
+  pose_orientation.setRPY(0.0, 0.0, last_orientation);
+
+  geometry_msgs::msg::Pose pathPose;
+  pathPose.position.x = path.x(path_last_idx);
+  pathPose.position.y = path.y(path_last_idx);
+  pathPose.orientation.x = pose_orientation.x();
+  pathPose.orientation.y = pose_orientation.y();
+  pathPose.orientation.z = pose_orientation.z();
+  pathPose.orientation.w = pose_orientation.w();
+
+  return pathPose;
+}
+
+/**
+ * @brief Get the target pose to be evaluated by the critic
+ * @param data Data to use
+ * @param enforce_path_inversion True to return the cusp point (last pose of the path)
+ * instead of the original goal
+ * @return geometry_msgs::msg::Pose Target pose for the critic
+ */
+inline geometry_msgs::msg::Pose getCriticGoal(
+  const CriticData & data,
+  bool enforce_path_inversion)
+{
+  if (enforce_path_inversion) {
+    return getLastPathPose(data.path);
+  } else {
+    return data.goal;
+  }
+}
+
 /**
  * @brief Check if the robot pose is within the Goal Checker's tolerances to goal
  * @param global_checker Pointer to the goal checker
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -22,6 +22,9 @@ namespace mppi::critics
 
 void CostCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
   getParam(power_, "cost_power", 1);
@@ -132,6 +135,8 @@ void CostCritic::score(CriticData & data)
     return;
   }
 
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
   // Setup cost information for various parts of the critic
   is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
   auto * costmap = collision_checker_.getCostmap();
@@ -148,7 +153,7 @@ void CostCritic::score(CriticData & data)
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) {
     near_goal = true;
   }
 
diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp
@@ -19,11 +19,12 @@ namespace mppi::critics
 
 void GoalAngleCritic::initialize()
 {
-  auto getParam = parameters_handler_->getParamGetter(name_);
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
 
+  auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.0f);
-
   getParam(threshold_to_consider_, "threshold_to_consider", 0.5f);
 
   RCLCPP_INFO(
@@ -35,14 +36,19 @@ void GoalAngleCritic::initialize()
 
 void GoalAngleCritic::score(CriticData & data)
 {
-  if (!enabled_ || !utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (!utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
 
-  const auto goal_idx = data.path.x.size() - 1;
-  const float goal_yaw = data.path.yaws(goal_idx);
+  double goal_yaw = tf2::getYaw(goal.orientation);
 
   if(power_ > 1u) {
     data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()).
diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -21,6 +21,9 @@ namespace mppi::critics
 
 void GoalCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
 
   getParam(power_, "cost_power", 1);
@@ -34,14 +37,20 @@ void GoalCritic::initialize()
 
 void GoalCritic::score(CriticData & data)
 {
-  if (!enabled_ || !utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (!utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
 
-  const auto & goal_x = data.goal.position.x;
-  const auto & goal_y = data.goal.position.y;
+  auto goal_x = goal.position.x;
+  auto goal_y = goal.position.y;
 
   const auto delta_x = data.trajectories.x - goal_x;
   const auto delta_y = data.trajectories.y - goal_y;
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -23,6 +23,9 @@ namespace mppi::critics
 
 void ObstaclesCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion1", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
   getParam(power_, "cost_power", 1);
@@ -127,9 +130,11 @@ void ObstaclesCritic::score(CriticData & data)
     possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
   }
 
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, goal)) {
     near_goal = true;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -19,6 +19,9 @@ namespace mppi::critics
 
 void PathAlignCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 10.0f);
@@ -39,9 +42,15 @@ void PathAlignCritic::initialize()
 
 void PathAlignCritic::score(CriticData & data)
 {
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
   // Don't apply close to goal, let the goal critics take over
-  if (!enabled_ || utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -23,6 +23,7 @@ namespace mppi::critics
 void PathAngleCritic::initialize()
 {
   auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
   float vx_min;
   getParentParam(vx_min, "vx_min", -0.35);
   if (fabs(vx_min) < 1e-6f) {  // zero
@@ -61,8 +62,14 @@ void PathAngleCritic::initialize()
 
 void PathAngleCritic::score(CriticData & data)
 {
-  if (!enabled_ ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -21,6 +21,9 @@ namespace mppi::critics
 
 void PathFollowCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
 
   getParam(
@@ -33,8 +36,15 @@ void PathFollowCritic::initialize()
 
 void PathFollowCritic::score(CriticData & data)
 {
-  if (!enabled_ || data.path.x.size() < 2 ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (data.path.x.size() < 2 ||
+    utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
@@ -21,6 +21,9 @@ namespace mppi::critics
 
 void PreferForwardCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0f);
@@ -34,8 +37,14 @@ void PreferForwardCritic::initialize()
 
 void PreferForwardCritic::score(CriticData & data)
 {
-  if (!enabled_ || utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, goal))
   {
     return;
   }
diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp
@@ -21,6 +21,9 @@ namespace mppi::critics
 
 void TwirlingCritic::initialize()
 {
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  getParentParam(enforce_path_inversion_, "enforce_path_inversion", false);
+
   auto getParam = parameters_handler_->getParamGetter(name_);
 
   getParam(power_, "cost_power", 1);
@@ -32,8 +35,14 @@ void TwirlingCritic::initialize()
 
 void TwirlingCritic::score(CriticData & data)
 {
-  if (!enabled_ ||
-    utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal))
+  if (!enabled_) {
+    return;
+  }
+
+  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
+
+  if (utils::withinPositionGoalTolerance(
+      data.goal_checker, data.state.pose.pose, goal))
   {
     return;
   }
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
