r2分支

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/RC2025/src/rm_msg"
+}

mapping.sh

@@ -2,11 +2,11 @@ source install/setup.bash
 
 commands=(
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2026 \
+    world:=1 \
     mode:=mapping \
     lio:=fastlio \
     localization:=icp \
-    lio_rviz:=false \
+    lio_rviz:=true \
     nav_rviz:=true"
 )
 

nav.sh

@@ -10,7 +10,10 @@ commands=(
     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.60, y: 3.995, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    # "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.61, y: 3.96, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    # "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
+    # "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive_and_pub.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
 )
 
 for cmd in "${commands[@]}"; do

src/rm_driver/rm_serial_driver/script/R2_Serial.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import serial
+import struct
+from rm_msgs.msg import DataAim  
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+import tf2_geometry_msgs
+
+class AimDataSerial(Node):
+    def __init__(self):
+        super().__init__('aim_data_serial')
+        
+        # 串口配置
+        self.serial_port_r2 = '/dev/r2'  # 原有串口，用于发送瞄准数据
+        self.serial_port_r1 = '/dev/r1'  # 新增串口，用于发送坐标数据
+        self.baud_rate = 115200
+        
+        # 初始化r2串口（瞄准数据）
+        try:
+            self.serial_conn_r2 = serial.Serial(
+                port=self.serial_port_r2,
+                baudrate=self.baud_rate,
+                timeout=1
+            )
+            self.get_logger().info(f'Serial port {self.serial_port_r2} opened successfully')
+        except Exception as e:
+            self.get_logger().error(f'Failed to open serial port r2: {e}')
+            self.serial_conn_r2 = None
+        
+        # 初始化r1串口（坐标数据）
+        try:
+            self.serial_conn_r1 = serial.Serial(
+                port=self.serial_port_r1,
+                baudrate=self.baud_rate,
+                timeout=1
+            )
+            self.get_logger().info(f'Serial port {self.serial_port_r1} opened successfully')
+        except Exception as e:
+            self.get_logger().error(f'Failed to open serial port r1: {e}')
+            self.serial_conn_r1 = None
+        
+        # TF2监听器，用于获取baselink在map上的坐标
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        
+        # 订阅话题
+        self.subscription = self.create_subscription(
+            DataAim,
+            '/chassis/data_aim',
+            self.aim_callback,
+            10
+        )
+        
+        # 定时器，定期获取和发送坐标数据
+        self.timer = self.create_timer(0.1, self.position_timer_callback)  # 10Hz
+        
+        self.get_logger().info('Aim data serial node started')
+    
+    def aim_callback(self, msg):
+        if not self.serial_conn_r2:
+            return
+            
+        try:
+            # 提取yaw和distance数据
+            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)  # 包尾
+            
+            # 发送数据到r2串口
+            self.serial_conn_r2.write(packet)
+            self.get_logger().info(f'Sent aim packet to r2: {packet.hex()}')
+            
+        except Exception as e:
+            self.get_logger().error(f'Error in aim_callback: {e}')
+    
+    def position_timer_callback(self):
+        if not self.serial_conn_r1:
+            return
+            
+        try:
+            # 获取baselink相对于map的变换
+            transform = self.tf_buffer.lookup_transform(
+                'map',  # 目标坐标系
+                'base_link',  # 源坐标系
+                rclpy.time.Time()  # 最新时间
+            )
+            
+            # 提取位置坐标
+            x = transform.transform.translation.x
+            y = transform.transform.translation.y
+            z = transform.transform.translation.z
+            
+            self.get_logger().info(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}')
+            
+            # 构建坐标数据包
+            # 格式: 包头(0xAA) + x(4字节float) + y(4字节float) + z(4字节float) + 包尾(0xBB)
+            packet = bytearray()
+            packet.append(0xAA)  # 包头
+            packet.extend(struct.pack('<f', x))  # x坐标 (小端序float)
+            packet.extend(struct.pack('<f', y))  # y坐标 (小端序float)
+            packet.extend(struct.pack('<f', z))  # z坐标 (小端序float)
+            packet.append(0xBB)  # 包尾
+            
+            # 发送数据到r1串口
+            self.serial_conn_r1.write(packet)
+            self.get_logger().info(f'Sent position packet to r1: {packet.hex()}')
+            
+        except tf2_ros.TransformException as e:
+            # TF变换异常，可能是坐标系不存在或时间不同步
+            self.get_logger().warn(f'Transform exception: {e}')
+        except Exception as e:
+            self.get_logger().error(f'Error in position_timer_callback: {e}')
+    
+    def __del__(self):
+        if hasattr(self, 'serial_conn_r2') and self.serial_conn_r2 and self.serial_conn_r2.is_open:
+            self.serial_conn_r2.close()
+            self.get_logger().info('Serial port r2 closed')
+        if hasattr(self, 'serial_conn_r1') and self.serial_conn_r1 and self.serial_conn_r1.is_open:
+            self.serial_conn_r1.close()
+            self.get_logger().info('Serial port r1 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()

src/rm_driver/rm_serial_driver/script/pub_aim.py

@@ -4,13 +4,13 @@ import rclpy
 from rclpy.node import Node
 import serial
 import struct
-from rm_msgs.msg import DataAim  # 假设使用DataAim消息类型，您可以根据实际消息类型调整
+from rm_msgs.msg import DataAim  
 class AimDataSerial(Node):
     def __init__(self):
         super().__init__('aim_data_serial')
         
         # 串口配置
-        self.serial_port = '/dev/ttyUSB0'  # 根据实际串口设备调整
+        self.serial_port = '/dev/r2'  # 根据实际串口设备调整
         self.baud_rate = 115200
         
         try:

src/rm_driver/rm_serial_driver/script/pub_self.py

@@ -15,7 +15,7 @@ class SelfPositionSerial(Node):
         super().__init__('self_position_serial')
         
         # 串口配置
-        self.serial_port = '/dev/ttyUSB1'  # 根据实际串口设备调整
+        self.serial_port = '/dev/r1'  # 根据实际串口设备调整
         self.baud_rate = 115200
         
         try:
@@ -34,7 +34,7 @@ class SelfPositionSerial(Node):
         self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
         
         # 定时器，定期发送位置信息
-        self.timer = self.create_timer(0.1, self.send_position)  # 10Hz
+        self.timer = self.create_timer(0.1, self.send_position)  # 100Hz
         
         self.get_logger().info('Self position serial node started')
     
@@ -65,12 +65,7 @@ class SelfPositionSerial(Node):
             packet.extend(struct.pack('<f', z))    # z坐标
             packet.extend(struct.pack('<f', yaw))  # yaw角
             
-            # 计算校验和（数据部分的异或校验）
-            checksum = 0
-            for i in range(1, len(packet)):
-                checksum ^= packet[i]
-            packet.append(checksum)
-            
+
             packet.append(0xFE)  # 包尾
             
             # 发送数据

src/rm_driver/rm_serial_driver/script/receive_and_pub.py

@@ -16,8 +16,8 @@ class ReceiveAndPubNode(Node):
         super().__init__('receive_and_pub')
         
         # 串口配置
-        self.receive_port = '/dev/ttyUSB2'  # 接收串口
-        self.send_port = '/dev/ttyUSB3'     # 发送串口
+        self.receive_port = '/dev/r1'  # 接收串口
+        self.send_port = '/dev/r2'     # 发送串口
         self.baud_rate = 115200
         
         try:
@@ -61,12 +61,11 @@ class ReceiveAndPubNode(Node):
         self.receive_thread.start()
         
         # 定时发送数据
-        self.send_timer = self.create_timer(0.1, self.send_target_data)  # 10Hz
+        self.send_timer = self.create_timer(0.01, self.send_target_data)  # 10Hz
         
         self.get_logger().info('Receive and pub node started')
     
     def receive_position_thread(self):
-        """接收位置信息的线程"""
         buffer = bytearray()
         while rclpy.ok():
             try:
@@ -75,7 +74,7 @@ class ReceiveAndPubNode(Node):
                     buffer.extend(data)
                     
                     # 查找完整的数据包
-                    while len(buffer) >= 22:  # 最小包长度：包头(1) + 数据(16) + 校验(1) + 包尾(1) = 19
+                    while len(buffer) >= 18:  # 包头(1) + 4个float(16) + 包尾(1) = 18字节
                         # 查找包头
                         start_idx = buffer.find(0xFF)
                         if start_idx == -1:
@@ -86,18 +85,13 @@ class ReceiveAndPubNode(Node):
                             buffer = buffer[start_idx:]
                         
                         # 检查包长度
-                        if len(buffer) < 22:
+                        if len(buffer) < 18:
                             break
                         
                         # 查找包尾
-                        if buffer[21] == 0xFE:
-                            # 校验数据
-                            checksum = 0
-                            for i in range(1, 20):
-                                checksum ^= buffer[i]
-                            
-                            if checksum == buffer[20]:
-                                # 解析数据
+                        if buffer[17] == 0xFE:
+                            # 解析数据
+                            try:
                                 x = struct.unpack('<f', buffer[1:5])[0]
                                 y = struct.unpack('<f', buffer[5:9])[0]
                                 z = struct.unpack('<f', buffer[9:13])[0]
@@ -110,18 +104,23 @@ class ReceiveAndPubNode(Node):
                                 # 发布tf变换
                                 self.publish_r2_transform(x, y, z, yaw)
                                 
-                                self.get_logger().debug(f'Received position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
-                            else:
-                                self.get_logger().warn('Checksum error')
+                                # self.get_logger().info(f'📍 Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
+                                
+                            except struct.error as e:
+                                self.get_logger().warn(f'⚠️ Data parsing error: {e}')
                             
                             # 移除已处理的数据包
-                            buffer = buffer[22:]
+                            buffer = buffer[18:]
                         else:
                             # 包尾不匹配，移除包头继续查找
                             buffer = buffer[1:]
+                else:
+                    # 避免忙等待
+                    import time
+                    time.sleep(0.001)
                             
             except Exception as e:
-                self.get_logger().error(f'Error in receive thread: {e}')
+                self.get_logger().error(f'❌ Error in receive thread: {e}')
     
     def publish_r2_transform(self, x, y, z, yaw):
         """发布R2到map的tf变换"""
@@ -129,7 +128,7 @@ class ReceiveAndPubNode(Node):
             t = TransformStamped()
             t.header.stamp = self.get_clock().now().to_msg()
             t.header.frame_id = 'map'
-            t.child_frame_id = 'R2'
+            t.child_frame_id = 'R1'
             
             t.transform.translation.x = x
             t.transform.translation.y = y
@@ -198,20 +197,20 @@ class ReceiveAndPubNode(Node):
             packet.append(0xFF)  # 包头
             packet.extend(struct.pack('<f', original_yaw))      # 原始yaw
             packet.extend(struct.pack('<f', original_distance)) # 原始distance
-            packet.extend(struct.pack('<f', r2_yaw))            # R2 yaw
-            packet.extend(struct.pack('<f', r2_distance))       # R2 distance
+            # packet.extend(struct.pack('<f', r2_yaw))            # R2 yaw
+            # packet.extend(struct.pack('<f', r2_distance))       # R2 distance
             
             # 计算校验和
-            checksum = 0
-            for i in range(1, len(packet)):
-                checksum ^= packet[i]
-            packet.append(checksum)
+            # checksum = 0
+            # for i in range(1, len(packet)):
+            #     checksum ^= packet[i]
+            # packet.append(checksum)
             
             packet.append(0xFE)  # 包尾
             
             # 发送数据
             self.send_serial.write(packet)
-            self.get_logger().debug(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
+            self.get_logger().info(f'Sent target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.3f})')
             
         except Exception as e:
             self.get_logger().error(f'Error sending target data: {e}')

src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml

@@ -7,7 +7,7 @@
         filter_size_map: 0.5
         cube_side_length: 1000.0
         runtime_pos_log_enable: false
-        map_file_path: "src/rm_nav_bringup/PCD/RC2025.pcd"
+        map_file_path: "src/rm_nav_bringup/PCD/1.pcd"
 
         common:
             lid_topic:  "/livox/lidar"

src/rm_nav_bringup/config/reality/measurement_params_real.yaml

@@ -1,3 +1,3 @@
 base_link2livox_frame:
-  xyz: "\"0.037 -0.354 0.41\""
+  xyz: "\"0.019 -0.33 0.41\""
   rpy: "\"0.0  0.0  0.0\""

src/rm_nav_bringup/map/map1.yaml

@@ -0,0 +1,7 @@
+image: map1.pgm
+mode: trinary
+resolution: 0.05
+origin: [-9.04, -13.8, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/rm_nav_bringup/map/map2.yaml

@@ -0,0 +1,7 @@
+image: map2.pgm
+mode: trinary
+resolution: 0.05
+origin: [-7.08, -15.4, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

src/rm_nav_bringup/map/test.yaml

@@ -0,0 +1,7 @@
+image: test.pgm
+mode: trinary
+resolution: 0.05
+origin: [-9.04, -13.8, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25