allow to use latest time to get transforms

Author: pepisg

nav2_controller/src/controller_server.cpp

@@ -794,7 +794,7 @@ bool ControllerServer::isGoalReached()
 bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!costmap_ros_->getRobotPose(current_pose)) {
+  if (!costmap_ros_->getRobotPose(current_pose, true)) {
     return false;
   }
   pose = current_pose;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -192,7 +192,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    * @param global_pose Will be set to the pose of the robot in the global frame of the costmap
    * @return True if the pose was set successfully, false otherwise
    */
-  bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose);
+  bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose, bool use_latest_time = false);
 
   /**
    * @brief Transform the input_pose in the global frame of the costmap

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -692,11 +692,17 @@ Costmap2DROS::resetLayers()
 }
 
 bool
-Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
+Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose, bool use_latest_time)
 {
-  return nav2_util::getCurrentPose(
-    global_pose, *tf_buffer_,
-    global_frame_, robot_base_frame_, transform_tolerance_);
+  if (use_latest_time) {
+    return nav2_util::getCurrentPose(
+      global_pose, *tf_buffer_,
+      global_frame_, robot_base_frame_, transform_tolerance_, now());
+  } else {
+    return nav2_util::getCurrentPose(
+      global_pose, *tf_buffer_,
+      global_frame_, robot_base_frame_, transform_tolerance_);
+  }
 }
 
 bool