diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 3b3d2feac..4d5eec845 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/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): """