a

robomaster_pi/pup_position.py

@@ -13,6 +13,23 @@ class RobotPosePublisher(Node):
         self.tf_listener_ = TransformListener(self.tf_buffer_, self)
         self.timer_ = self.create_timer(1.0, self.publish_robot_pose)
 
+    def quaternion_to_z_angle_deg(rotation):
+    x = rotation.x
+    y = rotation.y
+    z = rotation.z
+    w = rotation.w
+
+    # Umrechnung in Roll, Pitch, Yaw
+    roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
+    pitch = math.asin(2 * (w * y - z * x))
+    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
+
+    # Umrechnung von Bogenmaß in Grad
+    yaw_deg = math.degrees(yaw)
+
+    return yaw_deg
+
+
     def publish_robot_pose(self):
         try:
             transform = self.tf_buffer_.lookup_transform('map', 'base_link', Time())
@@ -21,7 +38,13 @@ class RobotPosePublisher(Node):
             pose_stamped.pose.position = Point(x=transform.transform.translation.x,
                                                y=transform.transform.translation.y,
                                                z=transform.transform.translation.z)
-            pose_stamped.pose.orientation = transform.transform.rotation
+            self.yaw = self.quaternion_to_z_angle_deg(transform.transform.rotation)
+
+            pose_stamped.pose.orientation = Quaternion (
+                                               x = 0.0,
+                                               y = 0.0,
+                                               z = self.yaw,
+                                               w = 1.0)
             self.publisher_.publish(pose_stamped)
         except Exception as e:
             self.get_logger().error('Failed to lookup transform: %s' % str(e))