Skip to content
Snippets Groups Projects
Commit cfa3ccea authored by Emil Harlan's avatar Emil Harlan
Browse files

a

parent 4232c18e
No related branches found
No related tags found
No related merge requests found
......@@ -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))
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment