AUTO-607 use path distance instead of euclidean distance for pruning path

nav2_mppi_controller/src/path_handler.cpp

@@ -44,11 +44,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
  using nav2_util::geometry_utils::euclidean_distance;
 
  auto begin = global_plan_.poses.begin();
- auto end = global_plan_.poses.end();
 
+ // 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_.poses.end(), max_robot_pose_search_dist_);
+ global_plan_.poses.begin(), global_plan_end_,
+ std::min(max_robot_pose_search_dist_, prune_distance_));
 
  // Find closest point to the robot
  auto closest_point = nav2_util::geometry_utils::min_by(
@@ -61,17 +62,17 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
  transformed_plan.header.stamp = global_pose.header.stamp;
 
+ auto pose_above_prune_distance =
+ 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 != pose_above_prune_distance;
+ ++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;

nav2_mppi_controller/test/path_handler_test.cpp

@@ -109,7 +109,6 @@ TEST(PathHandlerTests, TestBounds)
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
  std::make_unique<tf2_ros::TransformBroadcaster>(node);
  geometry_msgs::msg::TransformStamped t;
- //t.header.stamp = node->get_clock()->now();
  t.header.frame_id = "map";
  t.child_frame_id = "base_link";
  tf_broadcaster_->sendTransform(t);
@@ -157,7 +156,6 @@ TEST(PathHandlerTests, TestTransforms)
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
  std::make_unique<tf2_ros::TransformBroadcaster>(node);
  geometry_msgs::msg::TransformStamped t;
- //t.header.stamp = node->get_clock()->now();
  t.header.frame_id = "map";
  t.child_frame_id = "base_link";
  tf_broadcaster_->sendTransform(t);