a

robomaster_pi/pup_position.py

+import rospy
+import tf2_ros
+from geometry_msgs.msg import PoseStamped
+
+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)
+
+    # Warte auf die Transformation
+    tf_listener.wait_for_transform("map", "base_link", rospy.Time(), rospy.Duration(1.0))
+
+    # 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():
+    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

setup.py

@@ -24,6 +24,7 @@ setup(
             "Odom = robomaster_pi.Odom:main",
             "conn_odom = robomaster_pi.conn_odom:main",
             "conn_vel = robomaster_pi.conn_vel:main",
+            "pup_position = robomaster_pi.pup_position:main",
         ],
     },
 )