From a91341e76c2bb4b54de137517df567ed2be67b96 Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Mon, 8 May 2023 11:07:20 +0200
Subject: [PATCH] a
---
robomaster_pi/robot_ros_odom.py | 21 +++++++++++++++++++--
1 file changed, 19 insertions(+), 2 deletions(-)
diff --git a/robomaster_pi/robot_ros_odom.py b/robomaster_pi/robot_ros_odom.py
index 7e7dd8f..3fda405 100644
--- a/robomaster_pi/robot_ros_odom.py
+++ b/robomaster_pi/robot_ros_odom.py
@@ -39,7 +39,7 @@ def quaternion_from_euler(ai, aj, ak):
return q
class Robomaster_odom(Node):
-
+
def sub_position_handler(position_info):
print("handler")
tf_broadcaster = TransformBroadcaster()
@@ -60,7 +60,24 @@ class Robomaster_odom(Node):
def __init__(self, ep_chassis):
super().__init__("robomaster_odom")
- ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
+ def sub_position_handler(position_info):
+ print("handler")
+ tf_broadcaster = TransformBroadcaster()
+ t = TransformStamped()
+ t.header.stampep_chassis = time.get_clock().now().to_msg()
+ t.header.frame_id = 'odom'
+ t.child_frame_id = "base_footprint"
+ t.transform.translation.x = position_info.x
+ t.transform.translation.y = -position_info.y
+ t.transform.translation.z = 0.0
+ q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180)
+ t.transform.rotation.x = q[0]
+ t.transform.rotation.y = q[1]
+ t.transform.rotation.z = q[2]
+ t.transform.rotation.w = q[3]
+ tf_broadcaster.sendTransform(t)
+ print("ok")
+ ep_chassis.sub_position(freq=10, callback=sub_position_handler)
print("sub")
--
GitLab