feat: add getRobotPose method to BasicNavigator using TF2

nav2_simple_commander/nav2_simple_commander/robot_navigator.py

@@ -42,6 +42,7 @@
 from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
 from rclpy.task import Future
 from rclpy.type_support import GetResultServiceResponse
+import tf2_ros
 
 
 # Task Result enum for the result of the task being executed
@@ -252,6 +253,10 @@ def __init__(self, node_name: str = 'basic_navigator', namespace: str = '') -> N
             GetCostmap, 'local_costmap/get_costmap'
         )
 
+        # TF2 buffer and listener for pose transformations
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+
     def destroyNode(self) -> None:
         self.destroy_node()
 
@@ -1182,6 +1187,29 @@ def lifecycleShutdown(self) -> None:
                 future.result()
         return
 
+    def getRobotPose(
+            self, base_frame: str = 'base_link', map_frame: str = 'map') -> Optional[PoseStamped]:
+        """Get the current robot pose from TF2 transformation."""
+        try:
+            # Get the transformation from map_frame to base_frame
+            transform = self.tf_buffer.lookup_transform(
+                map_frame, base_frame, self.get_clock().now(), timeout=rclpyDuration(seconds=0.2)
+            )
+
+            # Create a PoseStamped from the transform
+            pose_stamped = PoseStamped()
+            pose_stamped.header = transform.header
+            pose_stamped.pose.position.x = transform.transform.translation.x
+            pose_stamped.pose.position.y = transform.transform.translation.y
+            pose_stamped.pose.position.z = transform.transform.translation.z
+            pose_stamped.pose.orientation = transform.transform.rotation
+
+            return pose_stamped
+
+        except Exception as e:
+            self.warn(f'Failed to get robot pose: {str(e)}')
+            return None
+
     def _waitForNodeToActivate(self, node_name: str) -> None:
         # Waits for the node within the tester namespace to become active
         self.debug(f'Waiting for {node_name} to become active..')