1. import rclpy
    2. from rclpy.node import Node
    3. import serial 
    4. import struct
    5. import time
    6. import threading
    8. class MyNode(Node):
    9.     def __init__(self,node_name):
    10.         super().__init__(node_name)
    11.         self.ser = serial.Serial("/dev/ttyS3",115200,timeout = 5)
    12.         self.ser.flushInput()
    14.         self.i = 0
    15.         # 需要发送的头 类型和数据长度
    16.         self.head = struct.pack('<b',0x7b)
    17.         self.instruction = struct.pack('<b',0x00)
    18.         self.length = struct.pack('<b',3)
    19.         self.tail = struct.pack('<b',0x7a)
    21.         self.t1 = threading.Thread(target=self.recv_speed)
    22.         self.t2 = threading.Thread(target=self.send_speed,args=(-1.145,-4.786,9.295))
    23.         self.t1.setDaemon(True)
    24.         self.t1.setDaemon(True)
    25.         self.t1.start()
    26.         self.t2.start()
    28.     def send_speed(self,x,y,z):
    29.         while True:
    30.             data = struct.pack('<f',x)+struct.pack('<f',y)+struct.pack('<f',z)
    31.             check_num = 0x00+3+x+y+z
    32.             check = struct.pack('<f',check_num)
    33.             # 需要发送的数据
    34.             data = self.head+self.instruction+self.length+data+check+self.tail
    35.             # 发送串口数据
    36.             self.ser.write(data)
    37.             time.sleep(0.06)
    39.     def recv_speed(self):
    40.         print('start')
    41.         print(f'is_open:{self.ser.is_open}')
    42.         # while True:
    43.         #     print(f'i:{self.i}')
    44.         #     self.i += 1
    45.         #     time.sleep(0.06)
    46.         while True:
    47.             self.i += 1
    48.             num = self.ser.inWaiting()
    49.             # print(f'i:{self.i} start num:{num}')
    50.             # num = self.ser.in_waiting()
    51.             # if num<=0 or num != 48:
    52.             #     continue
    54.             if num<=0 or num<48:
    55.                 continue
    56.             if num>48:
    57.                 # 清除
    58.                 self.ser.read(num)
    59.                 continue
    61.             print(f'num:{num}')
    62.             data = self.ser.read(num)
    63.             # 解析数据
    64.             self.parse_speed(data)
    65.             print('end')
    66.             time.sleep(0.06)
    68.     def parse_speed(self,data):
    69.         # 帧头
    70.         head = struct.unpack('<B',data[:1])[0]
    71.         if head!=122:
    72.             return
    73.         # 命令
    74.         ins = struct.unpack('<B',data[1:2])[0]
    75.         if ins!=1:
    76.             return
    77.         # 数据长度
    78.         length = struct.unpack('<B',data[2:3])[0]
    79.         if length!=10:
    80.             return
    81.         # 数据
    82.         want_data = struct.unpack('<ffffffffff',data[3:43])
    83.         # 校验位
    84.         check = struct.unpack('<f',data[43:47])[0]
    85.         # 帧尾
    86.         tail = struct.unpack('<B',data[47:])[0]
    87.         # self.get_logger().info(f'i:{self.i},帧头:{head},命令:{ins},数据长度:{length},数据:{want_data},校验位:{check},帧尾:{tail}')
    88.         print(f'i:{self.i},帧头:{head},命令:{ins},数据长度:{length},数据:{want_data},校验位:{check},帧尾:{tail}')
    89.         print(f'i:{self.i} end')
    91. def main():
    92.     # 初始化rclpy
    93.     rclpy.init()
    94.     # 参数:节点名称
    95.     node = MyNode('serial_node')
    97.     # 节点代码执行完之后需要销毁节点
    98.     node.destroy_node()
    99.     # 关闭rclpy
    100.     rclpy.shutdown()
    102. if __name__ == '__main__':
    103.     main()