From 895ab039a2a0cdaf6246c59b002a8932d44843cd Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Fri, 12 May 2023 09:39:28 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_odom.py | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/robomaster_pi/conn_odom.py b/robomaster_pi/conn_odom.py
index 8e3e67a..559b2d6 100644
--- a/robomaster_pi/conn_odom.py
+++ b/robomaster_pi/conn_odom.py
@@ -23,7 +23,8 @@ class OdometryPublisher(Node):
ep_chassis = ep_robot.chassis
ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
- ep_chassis.sub_imu(freq=50, callback=self.sub_imu_info_handler)
+ ep_chassis.sub_attitude(freq=50, callback=sub_attitude_info_handler)
+ #ep_chassis.sub_imu(freq=50, callback=self.sub_imu_info_handler)
def sub_imu_info_handler(self, imu_info):
@@ -36,6 +37,9 @@ class OdometryPublisher(Node):
if(self.zWinkel + (gyro_z /50)* (180 / math.pi) <= -165):
self.zWinkel = self.zWinkel + (gyro_z /50)* (180 / math.pi) + 360
+ def sub_attitude_info_handler(self, attitude_info):
+ self.yaw, pitch, roll = attitude_info
+ print("chassis attitude: yaw:{0}, pitch:{1}, roll:{2} ".format(self.yaw, pitch, roll))
@@ -44,14 +48,14 @@ class OdometryPublisher(Node):
def sub_position_handler(self, position_info):
self.x, self.y, self.z = position_info
- print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.zWinkel))
+ print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.yaw))
self.publish_odometry()
self.publish_transform()
def publish_odometry(self):
- q = self.quaternion_from_euler(0, 0, -self.zWinkel / (180 / math.pi))
+ q = self.quaternion_from_euler(0, 0, -self.yaw / (180 / math.pi))
msg = Odometry()
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.pose.position.x = self.x
@@ -69,7 +73,7 @@ class OdometryPublisher(Node):
self.transform_stamped_.transform.translation.x = self.x
self.transform_stamped_.transform.translation.y = -self.y
self.transform_stamped_.transform.translation.z = 0.0
- q = self.quaternion_from_euler(0.0, 0.0, -self.zWinkel / (180 / math.pi))
+ q = self.quaternion_from_euler(0.0, 0.0, -self.yaw / (180 / math.pi))
self.transform_stamped_.transform.rotation.x = q[0]
self.transform_stamped_.transform.rotation.y = q[1]
--
GitLab