From 8f158b52489ea7ae8d9a2a46e7ecf202951d97f1 Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Wed, 17 May 2023 10:34:40 +0200
Subject: [PATCH] a
---
robomaster_pi/Odom.py | 34 ++++++++++++++++++++++++++++++++++
setup.py | 2 ++
2 files changed, 36 insertions(+)
create mode 100644 robomaster_pi/Odom.py
diff --git a/robomaster_pi/Odom.py b/robomaster_pi/Odom.py
new file mode 100644
index 0000000..290dc49
--- /dev/null
+++ b/robomaster_pi/Odom.py
@@ -0,0 +1,34 @@
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+import robomaster
+from robomaster import robot
+
+class OdometryPublisher(Node):
+ def __init__(self):
+ super().__init__('odometry_publisher')
+ self.publisher_ = self.create_publisher(Odometry, 'encoder_odometry', 10)
+ self.msg = Odometry()
+ ep_robot = robot.Robot()
+ ep_robot.initialize(conn_type="rndis")
+
+ ep_chassis = ep_robot.chassis
+ ep_chassis.sub_velocity(freq=50, callback=self.sub_sub_velocity_handler)
+
+ def sub_velocity_handler(self, velocity_info):
+ a, b, c, x, y, z = velocity_info
+ 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.publisher_.publish(self.msg)
+
+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 b7fbce0..57605e3 100644
--- a/setup.py
+++ b/setup.py
@@ -20,6 +20,8 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
+ "IMU = robomaster_pi.IMU:main",
+ "Odom = robomaster_pi.Odom:main",
"conn_odom = robomaster_pi.conn_odom:main",
"conn_vel = robomaster_pi.conn_vel:main",
],
--
GitLab