RC2025启动！

src/rm_driver/rm_serial_driver/script/pub_aim.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import serial
+import struct
+from rm_msgs.msg import DataAim  # 假设使用DataAim消息类型，您可以根据实际消息类型调整
+class AimDataSerial(Node):
+    def __init__(self):
+        super().__init__('aim_data_serial')
+        
+        # 串口配置
+        self.serial_port = '/dev/ttyUSB0'  # 根据实际串口设备调整
+        self.baud_rate = 115200
+        
+        try:
+            self.serial_conn = serial.Serial(
+                port=self.serial_port,
+                baudrate=self.baud_rate,
+                timeout=1
+            )
+            self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
+        except Exception as e:
+            self.get_logger().error(f'Failed to open serial port: {e}')
+            return
+        
+        # 订阅话题
+        self.subscription = self.create_subscription(
+            DataAim,  # 根据实际消息类型调整
+            '/chassis/data_aim',
+            self.aim_callback,
+            10
+        )
+        
+        self.get_logger().info('Aim data serial node started')
+    
+    def aim_callback(self, msg):
+        try:
+            # 提取yaw和distance数据
+            # 根据实际消息结构调整，这里假设使用Point32的x和y字段
+            yaw = msg.yaw
+            distance = msg.distance
+            
+            self.get_logger().info(f'Received - yaw: {yaw}, distance: {distance}')
+            
+            # 构建发送数据包
+            # 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
+            packet = bytearray()
+            packet.append(0xFF)  # 包头
+            packet.extend(struct.pack('<f', yaw))      # yaw (小端序float)
+            packet.extend(struct.pack('<f', distance)) # distance (小端序float)
+            packet.append(0xFE)  # 包尾
+            
+            # 发送数据
+            self.serial_conn.write(packet)
+            self.get_logger().debug(f'Sent packet: {packet.hex()}')
+            
+        except Exception as e:
+            self.get_logger().error(f'Error in aim_callback: {e}')
+    
+    def __del__(self):
+        if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
+            self.serial_conn.close()
+            self.get_logger().info('Serial port closed')
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    try:
+        node = AimDataSerial()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if rclpy.ok():
+            rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()