From 5707ed7d6b2f63205e4d1393791ee271193bf8c0 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 8 Jul 2025 03:20:12 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=94=81=E8=BD=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- r1.sh | 19 ++ .../rm_serial_driver/script/pub_OnlyAim.py | 79 ++++++ .../rm_serial_driver/script/pub_self.py | 101 ++++++++ .../script/receive_and_pub.py | 241 ++++++++++++++++++ src/rm_simpal_move/script/pub_goal.py | 184 ------------- 5 files changed, 440 insertions(+), 184 deletions(-) create mode 100644 r1.sh create mode 100644 src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py create mode 100644 src/rm_driver/rm_serial_driver/script/pub_self.py create mode 100644 src/rm_driver/rm_serial_driver/script/receive_and_pub.py delete mode 100644 src/rm_simpal_move/script/pub_goal.py diff --git a/r1.sh b/r1.sh new file mode 100644 index 0000000..08c20a1 --- /dev/null +++ b/r1.sh @@ -0,0 +1,19 @@ +source install/setup.bash + +commands=( + "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py" + "ros2 launch rm_nav_bringup bringup_real.launch.py \ + world:=RC2025 \ + mode:=nav \ + lio:=fastlio \ + localization:=icp \ + lio_rviz:=false \ + nav_rviz:=true" + "ros2 launch rm_simpal_move simple_move.launch.py" + "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.56, y: 3.960, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once" +) + +for cmd in "${commands[@]}"; do + gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash" + sleep 0.5 +done \ No newline at end of file diff --git a/src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py b/src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py new file mode 100644 index 0000000..af70745 --- /dev/null +++ b/src/rm_driver/rm_serial_driver/script/pub_OnlyAim.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(' 0: + data = self.receive_serial.read(self.receive_serial.in_waiting) + buffer.extend(data) + + # 查找完整的数据包 + while len(buffer) >= 22: # 最小包长度:包头(1) + 数据(16) + 校验(1) + 包尾(1) = 19 + # 查找包头 + start_idx = buffer.find(0xFF) + if start_idx == -1: + break + + # 移除包头前的数据 + if start_idx > 0: + buffer = buffer[start_idx:] + + # 检查包长度 + if len(buffer) < 22: + break + + # 查找包尾 + if buffer[21] == 0xFE: + # 校验数据 + checksum = 0 + for i in range(1, 20): + checksum ^= buffer[i] + + if checksum == buffer[20]: + # 解析数据 + x = struct.unpack('