a

robomaster_pi/pup_position.py

-import rospy
-import tf2_ros
+import rclpy
+from rclpy.node import Node
 from geometry_msgs.msg import PoseStamped
+from tf2_ros import TransformListener, Buffer
+from tf2_geometry_msgs import do_transform_pose
+from tf2_ros import TransformStamped
 
-def get_transform(listener, source_frame, target_frame):
-    try:
-        transform = listener.lookup_transform(target_frame, source_frame, rospy.Time())
-        return transform
-    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
-        return None
 
-def get_base_link_pose():
-    tf_buffer = tf2_ros.Buffer()
-    tf_listener = tf2_ros.TransformListener(tf_buffer)
+class RobotPosePublisher(Node):
+    def __init__(self):
+        super().__init__('robot_pose_publisher')
+        self.publisher_ = self.create_publisher(PoseStamped, 'robot_pose', 10)
+        self.tf_buffer_ = Buffer()
+        self.tf_listener_ = TransformListener(self.tf_buffer_, self)
+        self.timer_ = self.create_timer(1.0, self.publish_robot_pose)
 
-    # Warte auf die Transformation
-    tf_listener.wait_for_transform("map", "base_link", rospy.Time(), rospy.Duration(1.0))
+    def publish_robot_pose(self):
+        try:
+            transform = self.tf_buffer_.lookup_transform('map', 'base_link', rclpy.Time())
+            pose_stamped = PoseStamped()
+            pose_stamped.header = transform.header
+            pose_stamped.pose = transform.transform
+            self.publisher_.publish(pose_stamped)
+        except Exception as e:
+            self.get_logger().error('Failed to lookup transform: %s' % str(e))
 
-    # Transformierung von base_link nach map
-    transform = get_transform(tf_buffer, "base_link", "map")
 
-    if transform is not None:
-        # Pose von base_link in map-Koordinaten
-        pose = transform.transform
-        return pose
-    else:
-        return None
+def main(args=None):
+    rclpy.init(args=args)
+    robot_pose_publisher = RobotPosePublisher()
+    rclpy.spin(robot_pose_publisher)
+    rclpy.shutdown()
 
-def main():
-    rospy.init_node('robot_pose_publisher')
-    pub = rospy.Publisher('robot_pose', PoseStamped, queue_size=10)
-    rate = rospy.Rate(10)  # 10 Hz
-
-    while not rospy.is_shutdown():
-        pose = get_base_link_pose()
-
-        if pose is not None:
-            # Erstelle ein PoseStamped-Objekt und fülle es mit den Pose-Daten
-            msg = PoseStamped()
-            msg.header.stamp = rospy.Time.now()
-            msg.header.frame_id = "map"
-            msg.pose = pose
-
-            # Veröffentliche die Nachricht
-            pub.publish(msg)
-
-        rate.sleep()
 
 if __name__ == '__main__':
-    try:
-        main()
-    except rospy.ROSInterruptException:
-        pass
+    main()