提交一下

nav.sh

@@ -3,18 +3,18 @@ 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:=RC2026 \
+    world:=map2 \
     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.596, y: 4.01, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    # "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.596, y: 4.01, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
     #"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 13.58, y: -3.35, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
-    "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.54, y: -3.30, 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/siyuanshu.py"
-    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
+    # "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.54, y: -3.30, 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/receive_and_pub.py"
+    # "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
 )
 
 for cmd in "${commands[@]}"; do

src/rm_driver/livox_ros_driver2/README.md

@@ -327,3 +327,7 @@ Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
   export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
   source ~/.bashrc
   ```
+  
+KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="underpan"
+KERNELS=="3-1:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="upper"
+KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="r2"

src/rm_driver/rm_serial_driver/script/R2_Serial.py

@@ -0,0 +1,130 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import tf2_ros
+from tf2_ros import TransformException
+from geometry_msgs.msg import TransformStamped
+import math
+
+class ImuAndTfMonitor(Node):
+    def __init__(self):
+        super().__init__('imu_and_tf_monitor')
+        
+        # 初始化TF缓冲区和监听器
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        
+        # 订阅IMU话题
+        self.imu_subscription = self.create_subscription(
+            Imu,
+            '/livox/imu',
+            self.imu_callback,
+            10
+        )
+        
+        # 创建定时器用于查询TF变换
+        self.tf_timer = self.create_timer(0.1, self.tf_timer_callback)
+        
+        self.get_logger().info('IMU and TF Monitor started successfully.')
+        self.get_logger().info('Monitoring /livox/imu topic and TF transforms:')
+        self.get_logger().info('- base_link -> map')
+        self.get_logger().info('- r2_base_link -> map')
+
+    def imu_callback(self, msg: Imu):
+        """处理IMU数据回调"""
+        # 将四元数转换为欧拉角
+        orientation = msg.orientation
+        roll, pitch, yaw = self.quaternion_to_euler(
+            orientation.x, orientation.y, orientation.z, orientation.w
+        )
+        
+        self.get_logger().info(
+            f'[IMU Data] Frame: {msg.header.frame_id}\n'
+            f'  Orientation (quaternion): x={orientation.x:.4f}, y={orientation.y:.4f}, '
+            f'z={orientation.z:.4f}, w={orientation.w:.4f}\n'
+            f'  Orientation (euler): roll={math.degrees(roll):.2f}°, '
+            f'pitch={math.degrees(pitch):.2f}°, yaw={math.degrees(yaw):.2f}°\n'
+            f'  Angular velocity: x={msg.angular_velocity.x:.4f}, '
+            f'y={msg.angular_velocity.y:.4f}, z={msg.angular_velocity.z:.4f} rad/s\n'
+            f'  Linear acceleration: x={msg.linear_acceleration.x:.4f}, '
+            f'y={msg.linear_acceleration.y:.4f}, z={msg.linear_acceleration.z:.4f} m/s²'
+        )
+
+    def tf_timer_callback(self):
+        """定时器回调，查询TF变换"""
+        # 查询 base_link -> map 的变换
+        self.query_transform('map', 'base_link', 'base_link -> map')
+        
+        # 查询 r2_base_link -> map 的变换
+        self.query_transform('map', 'r2_base_link', 'r2_base_link -> map')
+
+    def query_transform(self, target_frame: str, source_frame: str, description: str):
+        """查询指定的坐标变换"""
+        try:
+            # 获取最新可用的变换
+            transform: TransformStamped = self.tf_buffer.lookup_transform(
+                target_frame,
+                source_frame,
+                rclpy.time.Time(),
+                timeout=rclpy.duration.Duration(seconds=0.1)
+            )
+            
+            # 提取位置和姿态信息
+            translation = transform.transform.translation
+            rotation = transform.transform.rotation
+            
+            # 将四元数转换为欧拉角
+            roll, pitch, yaw = self.quaternion_to_euler(
+                rotation.x, rotation.y, rotation.z, rotation.w
+            )
+            
+            self.get_logger().info(
+                f'[TF Transform] {description}:\n'
+                f'  Position: x={translation.x:.4f}, y={translation.y:.4f}, z={translation.z:.4f}\n'
+                f'  Orientation (quaternion): x={rotation.x:.4f}, y={rotation.y:.4f}, '
+                f'z={rotation.z:.4f}, w={rotation.w:.4f}\n'
+                f'  Orientation (euler): roll={math.degrees(roll):.2f}°, '
+                f'pitch={math.degrees(pitch):.2f}°, yaw={math.degrees(yaw):.2f}°\n'
+                f'  Timestamp: {transform.header.stamp.sec}.{transform.header.stamp.nanosec:09d}'
+            )
+            
+        except TransformException as ex:
+            self.get_logger().warn(f'Failed to get transform {description}: {ex}')
+
+    def quaternion_to_euler(self, x, y, z, w):
+        """将四元数转换为欧拉角 (roll, pitch, yaw)"""
+        # Roll (x-axis rotation)
+        sinr_cosp = 2 * (w * x + y * z)
+        cosr_cosp = 1 - 2 * (x * x + y * y)
+        roll = math.atan2(sinr_cosp, cosr_cosp)
+
+        # Pitch (y-axis rotation)
+        sinp = 2 * (w * y - z * x)
+        if abs(sinp) >= 1:
+            pitch = math.copysign(math.pi / 2, sinp)  # 使用90度如果超出范围
+        else:
+            pitch = math.asin(sinp)
+
+        # Yaw (z-axis rotation)
+        siny_cosp = 2 * (w * z + x * y)
+        cosy_cosp = 1 - 2 * (y * y + z * z)
+        yaw = math.atan2(siny_cosp, cosy_cosp)
+
+        return roll, pitch, yaw
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ImuAndTfMonitor()
+    
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info('Shutting down IMU and TF Monitor...')
+    except Exception as e:
+        node.get_logger().error(f'An error occurred: {e}')
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_driver/rm_serial_driver/script/r1_serial.py

@@ -1,323 +0,0 @@
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-import tf2_ros
-from geometry_msgs.msg import TransformStamped
-from sensor_msgs.msg import Imu
-from rm_msgs.msg import DataAim
-import math
-import threading
-from rclpy.executors import MultiThreadedExecutor
-
-class ReceiveAndPubNode(Node):
-    def __init__(self):
-        super().__init__('receive_and_pub')
-        
-        # 串口配置
-        self.receive_port = '/dev/r2'  # 接收串口
-        self.send_port = '/dev/underpan'
-        self.send_port_upper = '/dev/upper'     # 发送串口
-        self.baud_rate = 115200
-        self.imu_angular_z = 0.0
-
-        # 角度平滑滤波器参数
-        self.smooth_alpha = 0.7  # 平滑系数，0-1之间，越小越平滑
-        self.filtered_original_yaw = 0.0
-        self.filtered_r2_yaw = 0.0
-        self.filter_initialized = False
-
-        self.imu_subscription = self.create_subscription(
-            Imu,
-            '/livox/imu',
-            self.imu_callback,
-            10
-        )
-        try:
-            # 接收串口
-            self.receive_serial = serial.Serial(
-                port=self.receive_port,
-                baudrate=self.baud_rate,
-                timeout=0.1
-            )
-            # 发送串口
-            self.send_serial = serial.Serial(
-                port=self.send_port,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.send_serial_upper = serial.Serial(
-                port=self.send_port_upper,
-                baudrate=self.baud_rate,
-                timeout=1
-            )
-            self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
-        except Exception as e:
-            self.get_logger().error(f'Failed to open serial ports: {e}')
-            return
-        
-        # TF广播器和监听器
-        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
-        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.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
-        self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
-        
-        # R1->base_link坐标监听，结果存储在self.transform_position
-        self.transform_position = {'x': 0.0, 'y': 0.0}
-        self.r1_tf_timer = self.create_timer(0.05, self._update_r1_position)
-        
-        # 启动接收线程
-        self.receive_thread = threading.Thread(target=self.receive_position_thread)
-        self.receive_thread.daemon = True
-        self.receive_thread.start()
-        
-        # 定时发送数据
-        self.send_timer = self.create_timer(0.005, self.send_target_data)  # 10Hz
-        
-        self.get_logger().info('Receive and pub node started')
-
-    def angle_smooth_filter(self, new_angle, filtered_angle):
-        """角度平滑滤波器，处理角度跳跃问题"""
-        if not self.filter_initialized:
-            return new_angle
-        
-        # 处理角度跳跃问题（-π到π的边界）
-        diff = new_angle - filtered_angle
-        if diff > math.pi:
-            diff -= 2 * math.pi
-        elif diff < -math.pi:
-            diff += 2 * math.pi
-        
-        # 指数移动平均滤波
-        return filtered_angle + self.smooth_alpha * diff
-
-    def imu_callback(self, msg):
-        """接收IMU数据，提取angular.z"""
-        try:
-            # self.imu_angular_z = msg.angular_velocity.z
-            self.imu_angular_z = 0.0 
-        except Exception as e:
-            self.get_logger().error(f'Error in imu callback: {e}')
-
-    def receive_position_thread(self):
-        buffer = bytearray()
-        while rclpy.ok():
-            try:
-                if self.receive_serial.in_waiting > 0:
-                    data = self.receive_serial.read(self.receive_serial.in_waiting)
-                    buffer.extend(data)
-                    
-                    # 查找完整的数据包
-                    while len(buffer) >= 18:  # 包头(1) + 4个float(16) + 包尾(1) = 18字节
-                        # 查找包头
-                        start_idx = buffer.find(0xFF)
-                        if start_idx == -1:
-                            break
-                        
-                        # 移除包头前的数据
-                        if start_idx > 0:
-                            buffer = buffer[start_idx:]
-                        
-                        # 检查包长度
-                        if len(buffer) < 18:
-                            break
-                        
-                        # 查找包尾
-                        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]
-                                yaw = struct.unpack('<f', buffer[13:17])[0]
-                                
-                                self.received_position = {
-                                    'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
-                                }
-                                
-                                # 发布tf变换
-                                self.publish_r2_transform(x, y, z, yaw)
-                                
-                                # 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[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}')
-        
-    def publish_r2_transform(self, x, y, z, yaw):
-        """发布R2到map的tf变换"""
-        try:
-            t = TransformStamped()
-            t.header.stamp = self.get_clock().now().to_msg()
-            t.header.frame_id = 'map'
-            t.child_frame_id = 'R2'
-            
-            t.transform.translation.x = x
-            t.transform.translation.y = y
-            t.transform.translation.z = z
-            
-            # 将yaw角转换为四元数
-            t.transform.rotation.x = 0.0
-            t.transform.rotation.y = 0.0
-            t.transform.rotation.z = math.sin(yaw / 2.0)
-            t.transform.rotation.w = math.cos(yaw / 2.0)
-            
-            self.tf_broadcaster.sendTransform(t)
-            
-        except Exception as e:
-            self.get_logger().error(f'Error publishing tf: {e}')
-    
-    def aim_callback(self, msg):
-        """接收瞄准数据"""
-        try:
-            self.aim_data = {
-                'yaw': msg.yaw,
-                'distance': msg.distance,
-                'valid': True
-            }
-            self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
-        except Exception as e:
-            self.get_logger().error(f'Error in aim callback: {e}')
-    
-    def get_r2_target_data(self):
-        """获取R2的瞄准数据"""
-        try:
-            if not self.received_position['valid']:
-                return 0.0, 0.0
-            
-            # 这里可以根据R2的位置计算目标数据
-            # 简化处理：返回R2的yaw角和到原点的距离
-            r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
-            if r2_yaw > 1.57:
-                r2_yaw = r2_yaw - 3.14
-            if r2_yaw < -1.57:
-                r2_yaw = r2_yaw + 3.14
-            r2_distance = math.sqrt(
-                (self.transform_position['x'])**2 + 
-                self.transform_position['y']**2
-            )
-            
-            return r2_yaw, r2_distance
-            
-        except Exception as e:
-            self.get_logger().error(f'Error getting R2 target data: {e}')
-            return 0.0, 0.0
-    
-    def send_target_data(self):
-        """发送目标数据"""
-        try:
-            # 获取原始目标数据
-            if self.aim_data['valid']:
-                raw_original_yaw = self.aim_data['yaw']
-                original_distance = self.aim_data['distance']
-            else:
-                raw_original_yaw = 0.0
-                original_distance = 0.0
-
-            # 获取R2目标数据
-            raw_r2_yaw, r2_distance = self.get_r2_target_data()
-
-            # 初始化滤波器
-            if not self.filter_initialized:
-                self.filtered_original_yaw = raw_original_yaw
-                self.filtered_r2_yaw = raw_r2_yaw
-                self.filter_initialized = True
-
-            # 应用平滑滤波
-            self.filtered_original_yaw = self.angle_smooth_filter(raw_original_yaw, self.filtered_original_yaw)
-            self.filtered_r2_yaw = self.angle_smooth_filter(raw_r2_yaw, self.filtered_r2_yaw)
-
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', self.filtered_original_yaw))      # 滤波后的原始yaw
-            # packet.extend(struct.pack('<f', original_distance)) # 原始distance
-            packet.extend(struct.pack('<f', self.filtered_r2_yaw))            # 滤波后的R2 yaw
-            # packet.extend(struct.pack('<f', r2_distance))       # R2 distance
-
-            # 添加IMU angular.z 到 underpan 的第三个数据
-            packet.extend(struct.pack('<f', self.imu_angular_z))  # 新增：IMU angular.z
-
-            packet.append(0xFE)  # 包尾
-
-            packet_upper = bytearray()
-            packet_upper.append(0xFF)  # 包头
-            # packet.extend(struct.pack('<f', original_yaw))      # 原始yaw
-            packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
-            # packet.extend(struct.pack('<f', r2_yaw))            # R2 yaw
-            packet_upper.extend(struct.pack('<f', r2_distance))       # R2 distance
-
-            # 计算校验和
-            checksum = 0
-
-            packet_upper.append(0xFE)  # 包尾
-
-            # 发送数据
-            self.send_serial.write(packet)
-            self.send_serial_upper.write(packet_upper)
-            self.get_logger().info(f'Sent filtered angles - original({self.filtered_original_yaw:.3f}→{raw_original_yaw:.3f}) R2({self.filtered_r2_yaw:.3f}→{raw_r2_yaw:.3f}) dist({original_distance:.3f},{r2_distance:.3f}) IMU_z({self.imu_angular_z:.3f})')
-
-        except Exception as e:
-            self.get_logger().error(f'Error sending target data: {e}')
-    
-    def _update_r1_position(self):
-        try:
-            trans = self.tf_buffer.lookup_transform(
-                'R2',          # 目标坐标系（修正）
-                'base_link',   # 源坐标系（修正）
-                rclpy.time.Time()
-            )
-            self.transform_position['x'] = trans.transform.translation.x
-            self.transform_position['y'] = trans.transform.translation.y
-        except Exception:
-            pass
-
-    def __del__(self):
-        if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
-            self.receive_serial.close()
-        if hasattr(self, 'send_serial') and self.send_serial.is_open:
-            self.send_serial.close()
-        if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
-            self.send_serial_upper.close()
-        self.get_logger().info('Serial ports closed')
-
-def main(args=None):
-    rclpy.init(args=args)
-    
-    try:
-        node = ReceiveAndPubNode()
-        executor = MultiThreadedExecutor()
-        executor.add_node(node)
-        executor.spin()
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

src/rm_driver/rm_serial_driver/script/receive.py

@@ -0,0 +1,169 @@
+import serial
+import struct
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PointStamped, TransformStamped
+from rm_msgs.msg import DataAim
+import tf2_ros
+import tf2_geometry_msgs
+import math
+
+# 串口参数
+SERIAL_PORT = '/dev/ttyACM0'  # 根据实际情况修改
+BAUDRATE = 115200
+
+# 数据包格式
+PACKET_FORMAT = '<BfffB'  # 1字节int8头，3个float，1字节int8尾
+PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
+HEADER = 0xAA
+TAIL = 0xBB
+
+class SerialReceiver(Node):
+    def __init__(self):
+        super().__init__('serial_receiver')
+        
+        # 创建发布者
+        self.point_publisher = self.create_publisher(PointStamped, 'r2_map_point', 10)
+        self.data_aim_publisher = self.create_publisher(DataAim, 'data_aim_r2', 10)
+        
+        # 创建TF监听器和广播器
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
+        
+        # 串口初始化
+        self.ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
+        self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
+        
+        # 创建定时器用于读取串口数据
+        self.timer = self.create_timer(0.01, self.read_serial_data)  # 10ms
+        
+        self.buffer = bytearray()
+        
+    def read_serial_data(self):
+        try:
+            data = self.ser.read(1)
+            if data:
+                self.buffer += data
+                # 保证缓冲区长度不超过一个包
+                if len(self.buffer) > PACKET_SIZE:
+                    self.buffer = self.buffer[-PACKET_SIZE:]
+                # 检查是否有完整包
+                if len(self.buffer) >= PACKET_SIZE:
+                    # 检查包头和包尾
+                    if self.buffer[0] == HEADER and self.buffer[PACKET_SIZE-1] == TAIL:
+                        # 解析数据
+                        _, x, y, z, _ = struct.unpack(PACKET_FORMAT, self.buffer[:PACKET_SIZE])
+                        self.get_logger().info(f"接收到数据: x={x:.3f}, y={y:.3f}, z={z:.3f}")
+                        
+                        # 发布map坐标点
+                        self.publish_map_point(x, y, z)
+                        
+                        # 发布R2的TF变换
+                        self.publish_r2_tf(x, y, z)
+                        
+                        # 计算相对于base_link的角度和距离
+                        self.calculate_and_publish_data_aim(x, y, z)
+                        
+                        self.buffer = self.buffer[PACKET_SIZE:]  # 移除已处理数据
+                    else:
+                        # 移除第一个字节，继续查找包头
+                        self.buffer = self.buffer[1:]
+        except Exception as e:
+            self.get_logger().error(f"读取串口数据错误: {e}")
+    
+    def publish_map_point(self, x, y, z):
+        """发布地图坐标点"""
+        point_msg = PointStamped()
+        point_msg.header.frame_id = 'map'
+        point_msg.header.stamp = self.get_clock().now().to_msg()
+        point_msg.point.x = x
+        point_msg.point.y = y
+        point_msg.point.z = z
+        
+        self.point_publisher.publish(point_msg)
+    
+    def publish_r2_tf(self, x, y, z):
+        """发布R2的TF变换"""
+        transform = TransformStamped()
+        
+        # 设置header
+        transform.header.stamp = self.get_clock().now().to_msg()
+        transform.header.frame_id = 'map'
+        transform.child_frame_id = 'r2_robot'
+        
+        # 设置位置
+        transform.transform.translation.x = x
+        transform.transform.translation.y = y
+        transform.transform.translation.z = z
+        
+        # 设置旋转（假设R2面向x轴正方向，无旋转）
+        transform.transform.rotation.x = 0.0
+        transform.transform.rotation.y = 0.0
+        transform.transform.rotation.z = 0.0
+        transform.transform.rotation.w = 1.0
+        
+        # 发布TF变换
+        self.tf_broadcaster.sendTransform(transform)
+    
+    def calculate_and_publish_data_aim(self, x, y, z):
+        """计算相对于base_link的角度和距离，并发布DataAim消息"""
+        try:
+            # 创建map坐标系下的点
+            point_stamped = PointStamped()
+            point_stamped.header.frame_id = 'map'
+            point_stamped.header.stamp = self.get_clock().now().to_msg()
+            point_stamped.point.x = x
+            point_stamped.point.y = y
+            point_stamped.point.z = z
+            
+            # 获取从map到base_link的变换
+            transform = self.tf_buffer.lookup_transform(
+                'base_link', 'map', rclpy.time.Time())
+            
+            # 将点变换到base_link坐标系
+            point_base_link = tf2_geometry_msgs.do_transform_point(point_stamped, transform)
+            
+            # 计算距离
+            distance = math.sqrt(
+                point_base_link.point.x**2 + 
+                point_base_link.point.y**2 + 
+                point_base_link.point.z**2
+            )
+            
+            # 计算yaw角度（绕z轴旋转角度）
+            yaw = math.atan2(point_base_link.point.y, point_base_link.point.x)
+            
+            # 发布DataAim消息
+            data_aim_msg = DataAim()
+            data_aim_msg.yaw = yaw
+            data_aim_msg.distance = distance
+            
+            self.data_aim_publisher.publish(data_aim_msg)
+            
+            self.get_logger().info(f"发布DataAim: yaw={yaw:.3f} rad, distance={distance:.3f} m")
+            
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+            self.get_logger().warn(f"TF变换失败: {e}")
+    
+    def destroy_node(self):
+        """节点销毁时关闭串口"""
+        if hasattr(self, 'ser') and self.ser.is_open:
+            self.ser.close()
+        super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    receiver = SerialReceiver()
+    
+    try:
+        rclpy.spin(receiver)
+    except KeyboardInterrupt:
+        print("接收中断，退出程序。")
+    finally:
+        receiver.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_driver/rm_serial_driver/script/receive1.py

@@ -0,0 +1,169 @@
+import serial
+import struct
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PointStamped, TransformStamped
+from rm_msgs.msg import DataAim
+import tf2_ros
+import tf2_geometry_msgs
+import math
+
+# 串口参数
+SERIAL_PORT = '/dev/ttyACM0'  # 根据实际情况修改
+BAUDRATE = 115200
+
+# 数据包格式
+PACKET_FORMAT = '<BfffB'  # 1字节int8头，3个float，1字节int8尾
+PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
+HEADER = 0xAA
+TAIL = 0xBB
+
+class SerialReceiver(Node):
+    def __init__(self):
+        super().__init__('serial_receiver')
+        
+        # 创建发布者
+        self.point_publisher = self.create_publisher(PointStamped, 'r2_map_point', 10)
+        self.data_aim_publisher = self.create_publisher(DataAim, 'data_aim_r2', 10)
+        
+        # 创建TF监听器和广播器
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
+        
+        # 串口初始化
+        self.ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
+        self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
+        
+        # 创建定时器用于读取串口数据
+        self.timer = self.create_timer(0.01, self.read_serial_data)  # 10ms
+        
+        self.buffer = bytearray()
+        
+    def read_serial_data(self):
+        try:
+            data = self.ser.read(1)
+            if data:
+                self.buffer += data
+                # 保证缓冲区长度不超过一个包
+                if len(self.buffer) > PACKET_SIZE:
+                    self.buffer = self.buffer[-PACKET_SIZE:]
+                # 检查是否有完整包
+                if len(self.buffer) >= PACKET_SIZE:
+                    # 检查包头和包尾
+                    if self.buffer[0] == HEADER and self.buffer[PACKET_SIZE-1] == TAIL:
+                        # 解析数据
+                        _, x, y, z, _ = struct.unpack(PACKET_FORMAT, self.buffer[:PACKET_SIZE])
+                        self.get_logger().info(f"接收到数据: x={x:.3f}, y={y:.3f}, z={z:.3f}")
+                        
+                        # 发布map坐标点
+                        self.publish_map_point(x, y, z)
+                        
+                        # 发布R2的TF变换
+                        self.publish_r2_tf(x, y, z)
+                        
+                        # 计算相对于base_link的角度和距离
+                        self.calculate_and_publish_data_aim(x, y, z)
+                        
+                        self.buffer = self.buffer[PACKET_SIZE:]  # 移除已处理数据
+                    else:
+                        # 移除第一个字节，继续查找包头
+                        self.buffer = self.buffer[1:]
+        except Exception as e:
+            self.get_logger().error(f"读取串口数据错误: {e}")
+    
+    def publish_map_point(self, x, y, z):
+        """发布地图坐标点"""
+        point_msg = PointStamped()
+        point_msg.header.frame_id = 'map'
+        point_msg.header.stamp = self.get_clock().now().to_msg()
+        point_msg.point.x = x
+        point_msg.point.y = y
+        point_msg.point.z = z
+        
+        self.point_publisher.publish(point_msg)
+    
+    def publish_r2_tf(self, x, y, z):
+        """发布R2的TF变换"""
+        transform = TransformStamped()
+        
+        # 设置header
+        transform.header.stamp = self.get_clock().now().to_msg()
+        transform.header.frame_id = 'map'
+        transform.child_frame_id = 'r2_robot'
+        
+        # 设置位置
+        transform.transform.translation.x = x
+        transform.transform.translation.y = y
+        transform.transform.translation.z = z
+        
+        # 设置旋转（假设R2面向x轴正方向，无旋转）
+        transform.transform.rotation.x = 0.0
+        transform.transform.rotation.y = 0.0
+        transform.transform.rotation.z = 0.0
+        transform.transform.rotation.w = 1.0
+        
+        # 发布TF变换
+        self.tf_broadcaster.sendTransform(transform)
+    
+    def calculate_and_publish_data_aim(self, x, y, z):
+        """计算相对于base_link的角度和距离，并发布DataAim消息"""
+        try:
+            # 创建map坐标系下的点
+            point_stamped = PointStamped()
+            point_stamped.header.frame_id = 'map'
+            point_stamped.header.stamp = self.get_clock().now().to_msg()
+            point_stamped.point.x = x
+            point_stamped.point.y = y
+            point_stamped.point.z = z
+            
+            # 获取从map到base_link的变换
+            transform = self.tf_buffer.lookup_transform(
+                'base_link', 'map', rclpy.time.Time())
+            
+            # 将点变换到base_link坐标系
+            point_base_link = tf2_geometry_msgs.do_transform_point(point_stamped, transform)
+            
+            # 计算距离
+            distance = math.sqrt(
+                point_base_link.point.x**2 + 
+                point_base_link.point.y**2 + 
+                point_base_link.point.z**2
+            )
+            
+            # 计算yaw角度（绕z轴旋转角度）
+            yaw = math.atan2(point_base_link.point.y, point_base_link.point.x)
+            
+            # 发布DataAim消息
+            data_aim_msg = DataAim()
+            data_aim_msg.yaw = yaw
+            data_aim_msg.distance = distance
+            
+            self.data_aim_publisher.publish(data_aim_msg)
+            
+            self.get_logger().info(f"发布DataAim: yaw={yaw:.3f} rad, distance={distance:.3f} m")
+            
+        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+            self.get_logger().warn(f"TF变换失败: {e}")
+    
+    def destroy_node(self):
+        """节点销毁时关闭串口"""
+        if hasattr(self, 'ser') and self.ser.is_open:
+            self.ser.close()
+        super().destroy_node()
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    receiver = SerialReceiver()
+    
+    try:
+        rclpy.spin(receiver)
+    except KeyboardInterrupt:
+        print("接收中断，退出程序。")
+    finally:
+        receiver.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rm_nav_bringup/config/reality/icp_registration_real.yaml

@@ -3,7 +3,7 @@
     use_sim_time: false 
     rough_leaf_size: 0.4
     refine_leaf_size: 0.1
-    pcd_path: "RC2026"
+    pcd_path: "map2"
     map_frame_id: "map"
     odom_frame_id: "odom"
     range_odom_frame_id: "lidar_odom"

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

@@ -1,4 +1,4 @@
-image: test.pgm
+image: map2.pgm
 mode: trinary
 resolution: 0.05
 origin: [-7.08, -15.4, 0]