a

robomaster_pi/robot_ros_odom.py

+import math
+import time
+from geometry_msgs.msg import TransformStamped
+
+import numpy as np
+import serial
+import rclpy
+from rclpy.node import Node
+from tf2_ros import TransformBroadcaster
+
+
+from rclpy.node import Node
+
+ser = serial.Serial()
+ser.port = '/dev/ttyS0'
+ser.baudrate = 115200
+ser.bytesize = serial.EIGHTBITS
+ser.stopbits = serial.STOPBITS_ONE
+ser.parity = serial.PARITY_NONE
+ser.timeout = 0.2
+ser.open()
+ser.write(("command;").encode('utf-8'))
+ser.read_all()
+
+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
+
+class Robomaster_odom(Node):
+    def __init__(self):
+
+        while True:
+            super().__init__("robomaster_odom")
+            self.tf_broadcaster = TransformBroadcaster(self)
+
+            ser.write(("chassis position ?;").encode('utf-8'))
+            buf = ser.read_all(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_link"
+            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.1)
+
+def main():
+    rclpy.init()
+    node = Robomaster_odom()
+    
+    rclpy.spin(node)
+    rclpy.shutdown()
\ No newline at end of file