From 34f18d9222e33a2fddb2e45653cd5c0ef7b3bb11 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 7 Mar 2023 18:06:08 +0000 Subject: [PATCH] [MPPI] Fix transformed path oscillations (#3443) * 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 --- nav2_mppi_controller/src/path_handler.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 83ec415e17..aa872321d6 100644 --- 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;