79 lines
2.6 KiB
Python
79 lines
2.6 KiB
Python
#!/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() |