#!/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()