Use integral path distance for MPPI Critics (#5495)

tonynajjar · web-flow · commit c0da22a1c8bc · 2025-09-04T12:11:58.000-07:00
* Implement integral distance for critics Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * . Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * more linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix tests Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset path length Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix sign Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix return value Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix return value" This reverts commit 8b21e89. Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove extra variable Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add doxygen Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * rename getCriticGoalPathDistance Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * delete withinPositionGoalTolerance Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use only one path distance Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * cleanup Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reorder Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * format Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix tests Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
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,7 +144,6 @@ 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,7 +46,6 @@ 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,7 +46,6 @@ 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,7 +97,6 @@ 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,7 +52,6 @@ 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,7 +81,6 @@ 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,7 +53,6 @@ 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,7 +45,6 @@ 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,7 +44,6 @@ 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/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp
@@ -40,6 +40,7 @@ struct State
 
   geometry_msgs::msg::PoseStamped pose;
   geometry_msgs::msg::Twist speed;
+  float local_path_length;
 
   /**
     * @brief Reset state data
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp
@@ -33,6 +33,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_mppi_controller/models/optimizer_settings.hpp"
 #include "nav2_mppi_controller/motion_models.hpp"
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -243,81 +243,6 @@ inline geometry_msgs::msg::Pose getLastPathPose(const models::Path & path)
   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
- * @param robot Pose of robot
- * @param goal Goal pose
- * @return bool If robot is within goal checker tolerances to the goal
- */
-inline bool withinPositionGoalTolerance(
-  nav2_core::GoalChecker * goal_checker,
-  const geometry_msgs::msg::Pose & robot,
-  const geometry_msgs::msg::Pose & goal)
-{
-  if (goal_checker) {
-    geometry_msgs::msg::Pose pose_tolerance;
-    geometry_msgs::msg::Twist velocity_tolerance;
-    goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
-
-    const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
-
-    auto dx = robot.position.x - goal.position.x;
-    auto dy = robot.position.y - goal.position.y;
-
-    auto dist_sq = dx * dx + dy * dy;
-
-    if (dist_sq < pose_tolerance_sq) {
-      return true;
-    }
-  }
-
-  return false;
-}
-
-/**
- * @brief Check if the robot pose is within tolerance to the goal
- * @param pose_tolerance Pose tolerance to use
- * @param robot Pose of robot
- * @param goal Goal pose
- * @return bool If robot is within tolerance to the goal
- */
-inline bool withinPositionGoalTolerance(
-  float pose_tolerance,
-  const geometry_msgs::msg::Pose & robot,
-  const geometry_msgs::msg::Pose & goal)
-{
-  const double & dist_sq =
-    std::pow(goal.position.x - robot.position.x, 2) +
-    std::pow(goal.position.y - robot.position.y, 2);
-
-  const float pose_tolerance_sq = pose_tolerance * pose_tolerance;
-
-  if (dist_sq < pose_tolerance_sq) {
-    return true;
-  }
-
-  return false;
-}
-
 /**
   * @brief normalize
   * Normalizes the angle to be -M_PIF circle to +M_PIF circle
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -23,8 +23,6 @@ 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);
@@ -135,8 +133,6 @@ 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();
@@ -153,7 +149,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, goal)) {
+  if (data.state.local_path_length < near_goal_distance_) {
     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
@@ -20,8 +20,6 @@ namespace mppi::critics
 void GoalAngleCritic::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", 3.0f);
@@ -36,17 +34,11 @@ void GoalAngleCritic::initialize()
 
 void GoalAngleCritic::score(CriticData & data)
 {
-  if (!enabled_) {
+  if (!enabled_ || data.state.local_path_length > threshold_to_consider_) {
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (!utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
-    return;
-  }
+  geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path);
 
   double goal_yaw = tf2::getYaw(goal.orientation);
 
diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp
@@ -22,10 +22,7 @@ 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);
   getParam(weight_, "cost_weight", 5.0f);
   getParam(threshold_to_consider_, "threshold_to_consider", 1.4f);
@@ -37,17 +34,11 @@ void GoalCritic::initialize()
 
 void GoalCritic::score(CriticData & data)
 {
-  if (!enabled_) {
+  if (!enabled_ || data.state.local_path_length > threshold_to_consider_) {
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (!utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
-    return;
-  }
+  geometry_msgs::msg::Pose goal = utils::getLastPathPose(data.path);
 
   auto goal_x = goal.position.x;
   auto goal_y = goal.position.y;
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -24,8 +24,6 @@ namespace mppi::critics
 void ObstaclesCritic::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);
@@ -130,11 +128,9 @@ 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, goal)) {
+  if (data.state.local_path_length < near_goal_distance_) {
     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
@@ -20,12 +20,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);
-
   getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f);
   getParam(offset_from_furthest_, "offset_from_furthest", 20);
   getParam(trajectory_point_step_, "trajectory_point_step", 4);
@@ -42,16 +39,7 @@ 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 (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -23,7 +23,6 @@ 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
@@ -62,15 +61,7 @@ void PathAngleCritic::initialize()
 
 void PathAngleCritic::score(CriticData & data)
 {
-  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))
-  {
+  if (!enabled_ || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -22,10 +22,7 @@ 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(
     threshold_to_consider_,
     "threshold_to_consider", 1.4f);
@@ -40,12 +37,7 @@ void PathFollowCritic::score(CriticData & data)
     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))
-  {
+  if (data.path.x.size() < 2 || data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
@@ -22,8 +22,6 @@ 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);
@@ -41,11 +39,7 @@ void PreferForwardCritic::score(CriticData & data)
     return;
   }
 
-  geometry_msgs::msg::Pose goal = utils::getCriticGoal(data, enforce_path_inversion_);
-
-  if (utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, goal))
-  {
+  if (data.state.local_path_length < threshold_to_consider_) {
     return;
   }
 
diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
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/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
