From f35f1f8bf3d30af48e1dc69dc91556db2adf1318 Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Mon, 8 May 2023 11:29:40 +0200
Subject: [PATCH] a
---
robomaster_pi/conn_odom.py | 69 ++++++++++++++++++++++++++++++++++++++
setup.py | 1 +
2 files changed, 70 insertions(+)
create mode 100644 robomaster_pi/conn_odom.py
diff --git a/robomaster_pi/conn_odom.py b/robomaster_pi/conn_odom.py
new file mode 100644
index 0000000..89526d7
--- /dev/null
+++ b/robomaster_pi/conn_odom.py
@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import math
+import numpy as np
+import robomaster
+from robomaster import robot
+
+def quaternion_from_euler(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 publish_odometry(self, position_info):
+ x, y, z = position_info
+ q = quaternion_from_euler(0, 0, -z* math.pi/180)
+ msg = Odometry()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ msg.pose.pose.position.x = x
+ msg.pose.pose.position.y = y
+ msg.pose.pose.position.z = 0
+ msg.pose.pose.orientation.x = 0
+ msg.pose.pose.orientation.y = 0
+ msg.pose.pose.orientation.z = q[2]
+ msg.pose.pose.orientation.w = 0
+ self.publisher_.publish(msg)
+
+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=publish_odometry)
+
+
+
+
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = OdometryPublisher()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/setup.py b/setup.py
index c8a5574..0f00793 100644
--- a/setup.py
+++ b/setup.py
@@ -20,6 +20,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
+ "connection = robomaster_pi.conn_odom:main",
"connection = robomaster_pi.connection:main",
"robot_ros_vel = robomaster_pi.robot_ros_vel:main",
"robot_ros_odom = robomaster_pi.robot_ros_odom:main",
--
GitLab