Merge branch 'AUTO-610_enforce_global_inversion' into AUTO-612_mppi_9…

…_sync

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -88,7 +88,7 @@ class PathHandler
    */
   nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
 
-  bool planUpToInversion(const geometry_msgs::msg::PoseStamped & robot_pose);
+  bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
 
 protected:
   /**
@@ -129,22 +129,23 @@ class PathHandler
     * @brief Prune global path to only interesting portions
     * @param end Final path iterator
     */
-  void pruneGlobalPlan(const PathIterator end);
+  void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
 
   std::string name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   ParametersHandler * parameters_handler_;
 
+  nav_msgs::msg::Path global_plan_up_to_inversion_;
   nav_msgs::msg::Path global_plan_;
-  PathIterator global_plan_end_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 
   double max_robot_pose_search_dist_{0};
   double prune_distance_{0};
   double transform_tolerance_{0};
 
   bool enforce_inversion_{false};
+  bool inversion_remaining_{false};
   double inversion_xy_tolerance_{0};
   double inversion_yaw_tolerance{0};
 };

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -629,6 +629,20 @@ inline PathIterator findFirstPathInversion(nav_msgs::msg::Path & path)
   return path.poses.end();
 }
 
+inline bool removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
+{
+  nav_msgs::msg::Path cropped_path = path;
+  PathIterator first_inversion = findFirstPathInversion(cropped_path);
+  cropped_path.poses.erase(first_inversion, cropped_path.poses.end());
+
+  if (path.poses.size() == cropped_path.poses.size()) {
+    return false;
+  } else {
+    path = cropped_path;
+    return true;
+  }
+}
+
 }  // namespace mppi::utils
 
 #endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_

nav2_mppi_controller/src/path_handler.cpp

@@ -48,12 +48,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
 {
   using nav2_util::geometry_utils::euclidean_distance;
 
-  auto begin = global_plan_.poses.begin();
+  auto begin = global_plan_up_to_inversion_.poses.begin();
 
   // Don't search further away than the prune distance
   auto closest_pose_upper_bound =
     nav2_util::geometry_utils::first_after_integrated_distance(
-    global_plan_.poses.begin(), global_plan_end_,
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
     std::min(max_robot_pose_search_dist_, prune_distance_));
 
   // Find closest point to the robot
@@ -69,7 +69,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
 
   auto pose_above_prune_distance =
     nav2_util::geometry_utils::first_after_integrated_distance(
-    closest_point, global_plan_end_, prune_distance_);
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
 
   unsigned int mx, my;
   // Find the furthest relevent pose on the path to consider within costmap
@@ -100,12 +100,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
 geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
   const geometry_msgs::msg::PoseStamped & pose)
 {
-  if (global_plan_.poses.empty()) {
+  if (global_plan_up_to_inversion_.poses.empty()) {
     throw nav2_core::InvalidPath("Received plan with zero length");
   }
 
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
+  if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) {
     throw nav2_core::ControllerTFError(
             "Unable to transform robot pose into global plan's frame");
   }
@@ -120,15 +120,18 @@ nav_msgs::msg::Path PathHandler::transformPath(
   geometry_msgs::msg::PoseStamped global_pose =
     transformToGlobalPlanFrame(robot_pose);
 
-  if (enforce_inversion_) {
-    planUpToInversion(global_pose);
-  } else {
-    global_plan_end_ = global_plan_.poses.end();
-  }
 
   auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
 
-  pruneGlobalPlan(lower_bound);
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if (enforce_inversion_ && inversion_remaining_) {
+    if (isWithinInversionTolerances(global_pose)) {
+      prunePlan(global_plan_, utils::findFirstPathInversion(global_plan_));
+      global_plan_up_to_inversion_ = global_plan_;
+      inversion_remaining_ = utils::removePosesAfterFirstInversion(global_plan_);
+    }
+  }
 
   if (transformed_plan.poses.empty()) {
     throw nav2_core::InvalidPath("Resulting plan has 0 poses in it.");
@@ -168,42 +171,29 @@ double PathHandler::getMaxCostmapDist()
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;
-  std::cout << "setPath" << std::endl;
-  global_plan_end_ = global_plan_.poses.end();
+  global_plan_up_to_inversion_ = global_plan_;
+  inversion_remaining_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
 }
 
 nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
 
-void PathHandler::pruneGlobalPlan(const PathIterator end)
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
 {
-  global_plan_.poses.erase(global_plan_.poses.begin(), end);
+  plan.poses.erase(plan.poses.begin(), end);
 }
 
-bool PathHandler::planUpToInversion(const geometry_msgs::msg::PoseStamped & robot_pose)
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
 {
-  // Update plan end to use
-  global_plan_end_ = utils::findFirstPathInversion(global_plan_);
-
-  if (global_plan_end_ != global_plan_.poses.end()) {
-    // Keep full path if we are within tolerance of the inversion pose
-    double distance = std::hypot(
-      robot_pose.pose.position.x - global_plan_end_->pose.position.x,
-      robot_pose.pose.position.y - global_plan_end_->pose.position.y);
-
-    double angle_distance = angles::shortest_angular_distance(
-      tf2::getYaw(robot_pose.pose.orientation),
-      tf2::getYaw(robot_pose.pose.orientation));
-
-    if (distance < inversion_xy_tolerance_ && fabs(angle_distance) < inversion_yaw_tolerance) {
-      // Prune up to the current validated inversion (avoid oscillation)
-      pruneGlobalPlan(global_plan_end_);
-      // Refind plan end to use
-      global_plan_end_ = utils::findFirstPathInversion(global_plan_);
-    }
-    return true;
-  }
+  // Keep full path if we are within tolerance of the inversion pose
+  double distance = std::hypot(
+    robot_pose.pose.position.x - global_plan_up_to_inversion_.poses.end()->pose.position.x,
+    robot_pose.pose.position.y - global_plan_up_to_inversion_.poses.end()->pose.position.y);
 
-  return false;
+  double angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(global_plan_up_to_inversion_.poses.end()->pose.orientation));
+
+  return distance < inversion_xy_tolerance_ && fabs(angle_distance) < inversion_yaw_tolerance;
 }
 
 }  // namespace mppi

nav2_mppi_controller/test/path_handler_test.cpp

@@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler
   PathHandlerWrapper()
   : PathHandler() {}
 
-  void pruneGlobalPlanWrapper(const PathIterator end)
+  void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end)
   {
-    return pruneGlobalPlan(end);
+    return prunePlan(path, end);
   }
 
   double getMaxCostmapDistWrapper()
@@ -82,7 +82,7 @@ TEST(PathHandlerTests, GetAndPrunePath)
   EXPECT_EQ(path.poses.size(), rtn_path.poses.size());
 
   PathIterator it = rtn_path.poses.begin() + 5;
-  handler.pruneGlobalPlanWrapper(it);
+  handler.pruneGlobalPlanWrapper(path, it);
   auto rtn2_path = handler.getPath();
   EXPECT_EQ(rtn2_path.poses.size(), 6u);
 }
@@ -133,7 +133,7 @@ TEST(PathHandlerTests, TestBounds)
     handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
   auto & path_in = handler.getPath();
   EXPECT_EQ(closest - path_in.poses.begin(), 25);
-  handler.pruneGlobalPlanWrapper(closest);
+  handler.pruneGlobalPlanWrapper(path, closest);
   auto & path_pruned = handler.getPath();
   EXPECT_EQ(path_pruned.poses.size(), 75u);
 }