Fix using different frame for global and local costmap

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -114,24 +114,14 @@ class PathHandler
   geometry_msgs::msg::PoseStamped
   transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
 
-  /**
-    * @brief Transform a plan to the costmap reference frame
-    * @param begin Start of path to transform
-    * @param end End of path to transform
-    * @param stamp Timestamp to use for transformation
-    * @return output path in costmap reference frame
-    */
-  nav_msgs::msg::Path
-  transformPlanPosesToCostmapFrame(
-    PathIterator begin, PathIterator end,
-    const builtin_interfaces::msg::Time & stamp);
-
   /**
     * @brief Get global plan within window of the local costmap size
     * @param global_pose Robot pose
-    * @return Range of path iterators belonging to the path within costmap window
+    * @return plan transformed in the costmap frame and iterator to the first pose of the global
+    * plan (for pruning)
     */
-  PathRange getGlobalPlanConsideringBounds(const geometry_msgs::msg::PoseStamped & global_pose);
+  std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
+    const geometry_msgs::msg::PoseStamped & global_pose);
 
   /**
     * @brief Prune global path to only interesting portions

nav2_mppi_controller/src/path_handler.cpp

@@ -37,10 +37,12 @@ void PathHandler::initialize(
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
 }
 
-PathRange PathHandler::getGlobalPlanConsideringBounds(
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
   const geometry_msgs::msg::PoseStamped & global_pose)
 {
   using nav2_util::geometry_utils::euclidean_distance;
+
   auto begin = global_plan_.poses.begin();
   auto end = global_plan_.poses.end();
 
@@ -55,19 +57,38 @@ PathRange PathHandler::getGlobalPlanConsideringBounds(
       return euclidean_distance(global_pose, ps);
     });
 
-  // Find the furthest relevent point on the path to consider within costmap
-  // bounds
-  const auto * costmap = costmap_->getCostmap();
+  nav_msgs::msg::Path transformedPlan;
+  transformedPlan.header.frame_id = costmap_->getGlobalFrameID();
+  transformedPlan.header.stamp = global_pose.header.stamp;
+
   unsigned int mx, my;
-  auto last_point =
-    std::find_if(
-    closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) {
-      auto distance = euclidean_distance(global_pose, global_plan_pose);
-      return distance >= prune_distance_ || !costmap->worldToMap(
-        global_plan_pose.pose.position.x, global_plan_pose.pose.position.y, 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 {transformedPlan, closest_point};
+    }
+
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformedPlan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformedPlan.poses.push_back(costmap_plan_pose);
+  }
 
-  return {closest_point, last_point};
+  return {transformedPlan, closest_point};
 }
 
 geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
@@ -92,12 +113,7 @@ nav_msgs::msg::Path PathHandler::transformPath(
   // Find relevent bounds of path to use
   geometry_msgs::msg::PoseStamped global_pose =
     transformToGlobalPlanFrame(robot_pose);
-  auto [lower_bound, upper_bound] = getGlobalPlanConsideringBounds(global_pose);
-
-  // Transform these bounds into the local costmap frame and prune older points
-  const auto & stamp = global_pose.header.stamp;
-  nav_msgs::msg::Path transformed_plan =
-    transformPlanPosesToCostmapFrame(lower_bound, upper_bound, stamp);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
 
   pruneGlobalPlan(lower_bound);
 
@@ -136,31 +152,6 @@ double PathHandler::getMaxCostmapDist()
          costmap->getResolution() / 2.0;
 }
 
-nav_msgs::msg::Path PathHandler::transformPlanPosesToCostmapFrame(
-  PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-{
-  std::string frame = costmap_->getGlobalFrameID();
-  auto transformToFrame = [&](const auto & global_plan_pose) {
-      geometry_msgs::msg::PoseStamped from_pose;
-      geometry_msgs::msg::PoseStamped to_pose;
-
-      from_pose.header.frame_id = global_plan_.header.frame_id;
-      from_pose.header.stamp = stamp;
-      from_pose.pose = global_plan_pose.pose;
-
-      transformPose(frame, from_pose, to_pose);
-      return to_pose;
-    };
-
-  nav_msgs::msg::Path plan;
-  plan.header.frame_id = frame;
-  plan.header.stamp = stamp;
-
-  std::transform(begin, end, std::back_inserter(plan.poses), transformToFrame);
-
-  return plan;
-}
-
 void PathHandler::setPath(const nav_msgs::msg::Path & plan)
 {
   global_plan_ = plan;

nav2_mppi_controller/test/path_handler_test.cpp

@@ -47,9 +47,10 @@ class PathHandlerWrapper : public PathHandler
     return getMaxCostmapDist();
   }
 
-  PathRange getGlobalPlanConsideringBoundsWrapper(const geometry_msgs::msg::PoseStamped & pose)
+  std::pair<nav_msgs::msg::Path, PathIterator>
+  getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose)
   {
-    return getGlobalPlanConsideringBounds(pose);
+    return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
   }
 
   bool transformPoseWrapper(
@@ -59,12 +60,6 @@ class PathHandlerWrapper : public PathHandler
     return transformPose(frame, in_pose, out_pose);
   }
 
-  nav_msgs::msg::Path transformPlanPosesToCostmapFrameWrapper(
-    PathIterator begin, PathIterator end, const builtin_interfaces::msg::Time & stamp)
-  {
-    return transformPlanPosesToCostmapFrame(begin, end, stamp);
-  }
-
   geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
     const geometry_msgs::msg::PoseStamped & pose)
   {
@@ -118,10 +113,10 @@ TEST(PathHandlerTests, TestBounds)
   robot_pose.pose.position.x = 25.0;
 
   handler.setPath(path);
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
+  auto [transformed_plan, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
   auto & path_in = handler.getPath();
   EXPECT_EQ(closest - path_in.poses.begin(), 25);
-  EXPECT_EQ(furthest - path_in.poses.begin(), 25);
   handler.pruneGlobalPlanWrapper(closest);
   auto & path_pruned = handler.getPath();
   EXPECT_EQ(path_pruned.poses.size(), 75u);
@@ -159,10 +154,8 @@ TEST(PathHandlerTests, TestTransforms)
   handler.setPath(path);
   EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose));
 
-  auto [closest, furthest] = handler.getGlobalPlanConsideringBoundsWrapper(robot_pose);
-
-  builtin_interfaces::msg::Time stamp;
-  auto path_out = handler.transformPlanPosesToCostmapFrameWrapper(closest, furthest, stamp);
+  auto [path_out, closest] =
+    handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
 
   // Put it all together
   auto final_path = handler.transformPath(robot_pose);