Update crazyflie.py

crazyflie_py/crazyflie_py/crazyflie.py

@@ -26,6 +26,7 @@
 import rclpy.node
 import rowan
 from std_srvs.srv import Empty
+from tf2_ros import TransformException, Buffer, TransformListener 
 
 
 def arrayToGeometryPoint(a):
@@ -112,6 +113,9 @@ def __init__(self, node, cfname, paramTypeDict):
         self.prefix = prefix
         self.node = node
 
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self.node)
+
         # self.tf = tf
 
         self.emergencyService = node.create_client(Empty, prefix + '/emergency')
@@ -491,7 +495,7 @@ def arm(self, arm=True):
         req.arm = arm
         self.armService.call_async(req)
 
-    # def position(self):
+    # def position(self): # crazyswarm (ros)
     #     """Returns the last true position measurement from motion capture.
 
     #     If at least one position measurement for this robot has been received
@@ -508,6 +512,34 @@ def arm(self, arm=True):
     #     position, quaternion = self.tf.lookupTransform(
     #       '/world', '/cf' + str(self.id), rospy.Time(0))
     #     return np.array(position)
+
+    def position(self):    # crazyswarm2 (ros2) instead of def position(self)
+    	"""
+    	Returns the last true position measurement from motion capture using TF2.
+    	"""
+    	# Use the prefix already defined in __init__ (e.g., '/cf1')
+    	# TF2 in ROS 2 generally prefers frame names without the leading slash.
+    	target_frame = self.prefix.replace('/', '') 
+    	source_frame = 'world'
+
+    	try:
+        	# lookup_transform(target_frame, source_frame, time)
+        	t = self.tf_buffer.lookup_transform(
+            	source_frame,
+            	target_frame,
+            	rclpy.time.Time(), # Get the latest available transform
+            	timeout=rclpy.duration.Duration(seconds=1.0)
+        	)
+
+        	# Extract translation data
+        	pos = t.transform.translation
+        	return np.array([pos.x, pos.y, pos.z])
+
+    	except TransformException as ex:
+        	# We must use self.node to access the logger
+        	self.node.get_logger().error(f'Could not transform {source_frame} to {target_frame}: {ex}')
+        	# Return zeros so the script doesn't crash, but you'll know there's an error
+        	return np.array([0.0, 0.0, 0.0])
 
     def getParam(self, name):
         """