Fix pose timestamp when using transformPoseInTargetFrame (#5499)

* Fix pose timestamp when using transformPoseInTargetFrame

Signed-off-by: mini-1235 <[email protected]>

* Fix frame

Signed-off-by: mini-1235 <[email protected]>

* Update nav2_route/src/goal_intent_extractor.cpp

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: mini-1235 <[email protected]>

---------

Signed-off-by: mini-1235 <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: mini-1235

nav2_controller/src/controller_server.cpp

@@ -799,6 +799,7 @@ bool ControllerServer::isGoalReached()
   geometry_msgs::msg::Twist velocity = getThresholdedTwist(odom_sub_->getRawTwist());
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
+  end_pose_.header.stamp = pose.header.stamp;
   nav2_util::transformPoseInTargetFrame(
     end_pose_, transformed_end_pose, *costmap_ros_->getTfBuffer(),
     costmap_ros_->getGlobalFrameID(), costmap_ros_->getTransformTolerance());

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -290,6 +290,7 @@ DWBLocalPlanner::prepareGlobalPlan(
   }
 
   goal_pose.header.frame_id = global_plan_.header.frame_id;
+  goal_pose.header.stamp = pose.header.stamp;
   goal_pose.pose = global_plan_.poses.back().pose;
   nav2_util::transformPoseInTargetFrame(
     goal_pose, goal_pose, *tf_,
@@ -535,10 +536,17 @@ DWBLocalPlanner::transformGlobalPlan(
   // Helper function for the transform below. Converts a pose2D from global
   // frame to local
   auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
-      geometry_msgs::msg::PoseStamped transformed_pose;
-      nav2_util::transformPoseInTargetFrame(
-        global_plan_pose, transformed_pose, *tf_,
-        transformed_plan.header.frame_id, transform_tolerance_);
+      geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
+      stamped_pose.header.frame_id = global_plan_.header.frame_id;
+      stamped_pose.header.stamp = robot_pose.header.stamp;
+      stamped_pose.pose = global_plan_pose.pose;
+      if (!nav2_util::transformPoseInTargetFrame(
+          stamped_pose, transformed_pose, *tf_,
+          transformed_plan.header.frame_id, transform_tolerance_))
+      {
+        throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame");
+      }
+      transformed_pose.pose.position.z = 0.0;
       return transformed_pose;
     };
 

nav2_route/src/goal_intent_extractor.cpp

@@ -175,6 +175,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
       node_pose.pose.position.x = node_data.coords.x;
       node_pose.pose.position.y = node_data.coords.y;
       node_pose.header.frame_id = node_data.coords.frame_id;
+      node_pose.header.stamp = start_pose.header.stamp;
       candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
     }
 
@@ -202,6 +203,7 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
       node_pose.pose.position.x = node_data.coords.x;
       node_pose.pose.position.y = node_data.coords.y;
       node_pose.header.frame_id = node_data.coords.frame_id;
+      node_pose.header.stamp = goal_pose.header.stamp;
       candidate_nodes.push_back(transformPose(node_pose, costmap_frame_id));
     }