a

robomaster_pi/connection.py

@@ -88,8 +88,6 @@ def main():
     rclpy.init()
     node = vel_from_ros_node(ep_chassis)
     rclpy.spin(node)
-    node1 = Robomaster_odom(ep_chassis)
-    rclpy.spin(node1)
     rclpy.shutdown()
 
     ep_robot.close()

robomaster_pi/robot_ros_odom.py

+import robomaster
+from robomaster import robot
 import math
 import time
-import socket
-import sys
-from geometry_msgs.msg import TransformStamped
-
 import numpy as np
 import rclpy
 from rclpy.node import Node
 from tf2_ros import TransformBroadcaster
+from geometry_msgs.msg import TransformStamped
+from geometry_msgs.msg import Twist
+from tf2_ros import TransformBroadcaster
 
 
 from rclpy.node import Node
+x, y, z = 0, 0, 0
 
-host = "192.168.42.2"
-port = 40923
-address = (host, int(port))
-s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-s.connect(address)
-s.send(("command;").encode('utf-8'))
-abfangen = s.recv(1024)
 
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
@@ -45,33 +40,37 @@ def quaternion_from_euler(ai, aj, ak):
 
 class Robomaster_odom(Node):
     def __init__(self):
+        self.ep_chassis.sub_position(freq=10, callback=sub_position_handler)
+    
+
+    def sub_position_handler(position_info):
+        super().__init__("robomaster_odom")
+        tf_broadcaster = TransformBroadcaster()
+        t = TransformStamped()
+        t.header.stampep_chassis = time.get_clock().now().to_msg()
+        t.header.frame_id = 'odom'
+        t.child_frame_id = "base_footprint"
+        t.transform.translation.x = position_info.x
+        t.transform.translation.y = -position_info.y
+        t.transform.translation.z = 0.0
+        q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180)
+        t.transform.rotation.x = q[0]
+        t.transform.rotation.y = q[1]
+        t.transform.rotation.z = q[2]
+        t.transform.rotation.w = q[3]
+        tf_broadcaster.sendTransform(t)
+        print("ok")
 
-        while True:
-            super().__init__("robomaster_odom")
-            self.tf_broadcaster = TransformBroadcaster(self)
-            s.send(("chassis position ?;").encode('utf-8'))
-            buf = s.recv(1024)
-            xyz = buf.decode('utf-8')
-            self.get_logger().info(xyz)
-            xyzlist = xyz.split(" ")
-            t = TransformStamped()
-            t.header.stamp = self.get_clock().now().to_msg()
-            t.header.frame_id = 'odom'
-            t.child_frame_id = "base_footprint"
-            t.transform.translation.x = float(xyzlist[0])
-            t.transform.translation.y = -float(xyzlist[1])
-            t.transform.translation.z = 0.0
-            q = quaternion_from_euler(0, 0, -float(xyzlist[2])* math.pi/180)
-            t.transform.rotation.x = q[0]
-            t.transform.rotation.y = q[1]
-            t.transform.rotation.z = q[2]
-            t.transform.rotation.w = q[3]
-            self.tf_broadcaster.sendTransform(t)
-            time.sleep(0.5)
 
 def main():
+    ep_robot = robot.Robot()
+    ep_robot.initialize(conn_type="rndis")
+
+    ep_chassis = ep_robot.chassis
+
     rclpy.init()
-    node = Robomaster_odom()
-    
-    rclpy.spin(node)
-    rclpy.shutdown()
\ No newline at end of file
+    node1 = Robomaster_odom(ep_chassis)
+    rclpy.spin(node1)
+    rclpy.shutdown()
+
+    ep_robot.close()
\ No newline at end of file