From 3d35e68da2e903ed3d7328fa7d6cac7136cf6f04 Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Mon, 22 May 2023 10:19:30 +0200
Subject: [PATCH] a
---
robomaster_pi/Odom.py | 46 +++++++++++++++++++++++++++++++++++++++++++
1 file changed, 46 insertions(+)
diff --git a/robomaster_pi/Odom.py b/robomaster_pi/Odom.py
index 4cb8995..90119d1 100644
--- a/robomaster_pi/Odom.py
+++ b/robomaster_pi/Odom.py
@@ -2,7 +2,9 @@ import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
import robomaster
+import math
from robomaster import robot
+import numpy as np
class OdometryPublisher(Node):
def __init__(self):
@@ -14,15 +16,59 @@ class OdometryPublisher(Node):
ep_chassis = ep_robot.chassis
ep_chassis.sub_velocity(freq=50, callback=self.sub_velocity_handler)
+ ep_chassis.sub_attitude(freq=50, callback=self.sub_attitude_info_handler)
+ ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
+
+
+ def sub_attitude_info_handler(self, attitude_info):
+ self.yaw, pitch, roll = attitude_info
+
+
+ def sub_position_handler(self, position_info):
+ self.x, self.y, self.z = position_info
+ self.publish_odometry()
+ self.publish_transform()
def sub_velocity_handler(self, velocity_info):
a, b, c, x, y, z = velocity_info
+ q = self.quaternion_from_euler(0, 0, -self.yaw / (180 / math.pi))
self.msg.header.stamp = self.get_clock().now().to_msg()
self.msg.twist.twist.linear.x = x
self.msg.twist.twist.linear.y = y
+ self.msg.twist.twist.linear.z = 0.0
+ self.msg.pose.pose.position.x = self.x
+ self.msg.pose.pose.position.y = -self.y
+ self.msg.pose.pose.position.z = 0.0
+ self.msg.pose.pose.orientation.x = 0.0
+ self.msg.pose.pose.orientation.y = 0.0
+ self.msg.pose.pose.orientation.z = q[2]
+ self.msg.pose.pose.orientation.w = 0.0
self.publisher_.publish(self.msg)
+ def quaternion_from_euler(self, ai, aj, ak):
+ ai /= 2.0
+ aj /= 2.0
+ ak /= 2.0
+ ci = math.cos(ai)
+ si = math.sin(ai)
+ cj = math.cos(aj)
+ sj = math.sin(aj)
+ ck = math.cos(ak)
+ sk = math.sin(ak)
+ cc = ci*ck
+ cs = ci*sk
+ sc = si*ck
+ ss = si*sk
+
+ q = np.empty((4, ))
+ q[0] = cj*sc - sj*cs
+ q[1] = cj*ss + sj*cc
+ q[2] = cj*cs - sj*sc
+ q[3] = cj*cc + sj*ss
+
+ return q
+
def main(args=None):
rclpy.init(args=args)
node = OdometryPublisher()
--
GitLab