From cfa3cceaf38b2318bda99274961d69108ff90d40 Mon Sep 17 00:00:00 2001
From: Emil Harlan <eharlan@uni-bremen.de>
Date: Fri, 2 Jun 2023 14:05:40 +0200
Subject: [PATCH] a
---
robomaster_pi/pup_position.py | 25 ++++++++++++++++++++++++-
1 file changed, 24 insertions(+), 1 deletion(-)
diff --git a/robomaster_pi/pup_position.py b/robomaster_pi/pup_position.py
index 5684fa9..0ba98a2 100644
--- a/robomaster_pi/pup_position.py
+++ b/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))
--
GitLab