[MPPI] Fix transformed path oscillations (ros-navigation#3443) (ros-n…

…avigation#3453) * use path distance instead of euclidean for upper bound search * rename to pruned_plan_end * rename to pruned_plan_end * fix tests * do not use prune_distance to limit the search for the closest pose --------- Co-authored-by: Guillaume Doisy <[email protected]> (cherry picked from commit 34f18d9) Co-authored-by: Guillaume Doisy <[email protected]> (cherry picked from commit 7d04c8e)
alyquantillion · Mar 8, 2023 · 3a5cb47 · 3a5cb47
1 parent 0b575e9
commit 3a5cb47
Showing 1 changed file with 8 additions and 8 deletions.
diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp
@@ -44,8 +44,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
   using nav2_util::geometry_utils::euclidean_distance;
 
   auto begin = global_plan_.poses.begin();
-  auto end = global_plan_.poses.end();
 
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
   auto closest_pose_upper_bound =
     nav2_util::geometry_utils::first_after_integrated_distance(
     global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_);
@@ -61,17 +61,17 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
   transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
   transformed_plan.header.stamp = global_pose.header.stamp;
 
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_.poses.end(), prune_distance_);
+
   unsigned int mx, my;
   // Find the furthest relevent pose on the path to consider within costmap
   // bounds
   // Transforming it to the costmap frame in the same loop
-  for (auto global_plan_pose = closest_point; global_plan_pose != end; ++global_plan_pose) {
-    // Distance relative to robot pose check
-    auto distance = euclidean_distance(global_pose, *global_plan_pose);
-    if (distance >= prune_distance_) {
-      return {transformed_plan, closest_point};
-    }
-
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
     // Transform from global plan frame to costmap frame
     geometry_msgs::msg::PoseStamped costmap_plan_pose;
     global_plan_pose->header.stamp = global_pose.header.stamp;