Fix goal pose stamp

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -92,9 +92,10 @@ class PathHandler
 
   /**
    * @brief Get the global goal pose transformed to the local frame
+   * @param stamp Time to get the goal pose at
    * @return Transformed goal pose
    */
-  geometry_msgs::msg::PoseStamped getTransformedGoal();
+  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
 
 protected:
   /**

nav2_mppi_controller/src/controller.cpp

@@ -84,7 +84,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 #endif
 
   std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
-  geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().pose;
+  geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
 
   nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
 

nav2_mppi_controller/src/path_handler.cpp

@@ -186,10 +186,12 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
   plan.poses.erase(plan.poses.begin(), end);
 }
 
-geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal()
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
 {
   auto goal = global_plan_.poses.back();
-  goal.header = global_plan_.header;
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
   if (goal.header.frame_id.empty()) {
     throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
   }