From 0db5c095c67dd0420468c3843fd6285d6be0acca Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Tue, 9 May 2023 09:41:46 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_odom.py | 79 +++++++++++++++++---------------------
1 file changed, 36 insertions(+), 43 deletions(-)
diff --git a/robomaster_pi/conn_odom.py b/robomaster_pi/conn_odom.py
index 958270d..7d85a93 100644
--- a/robomaster_pi/conn_odom.py
+++ b/robomaster_pi/conn_odom.py
@@ -1,48 +1,32 @@
-import asyncio
-import math
-import numpy as np
import rclpy
-from nav_msgs.msg import Odometry
from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import math
+import numpy as np
import robomaster
from robomaster import robot
-
class OdometryPublisher(Node):
-
+
def __init__(self):
super().__init__('odometry_publisher')
self.publisher_ = self.create_publisher(Odometry, 'odometry', 10)
+ ep_robot = robot.Robot()
+ ep_robot.initialize(conn_type="rndis")
+ ep_chassis = ep_robot.chassis
+ ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
self.x = 0
self.y = 0
self.z = 0
- 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 sub_position_handler(self, position_info):
+ self.x, self.y, self.z = position_info
+ self.publish_odometry()
+ print("ok")
def publish_odometry(self):
- print("publishing odometry")
- q = self.quaternion_from_euler(0, 0, -self.z * math.pi / 180)
+ print("ja")
+ q = self.quaternion_from_euler(0, 0, -self.z* math.pi/180)
msg = Odometry()
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.pose.position.x = self.x
@@ -54,27 +38,36 @@ class OdometryPublisher(Node):
msg.pose.pose.orientation.w = 0
self.publisher_.publish(msg)
- async def sub_position_handler(self, position_info):
- self.x = position_info.x
- self.y = position_info.y
- self.z = position_info.z
- print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.z))
- self.publish_odometry()
+ 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
- async def start(self):
- ep_robot = robot.Robot()
- ep_robot.initialize(conn_type="rndis")
- ep_chassis = ep_robot.chassis
- await ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
+ 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()
- asyncio.run(node.start())
+ rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
-
if __name__ == '__main__':
main()
\ No newline at end of file
--
GitLab