r1-7-12

提交一下
R1
2025-07-12 22:13:39 +08:00 · 2025-07-11 00:48:19 +08:00 · 2025-07-10 13:21:45 +08:00
59 changed files with 522 additions and 740 deletions
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@ -0,0 +1,44 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/robofish/RC2025/install/teb_local_planner/include/**",
+        "/home/robofish/RC2025/install/teb_msgs/include/**",
+        "/home/robofish/RC2025/install/ros2_livox_simulation/include/**",
+        "/home/robofish/RC2025/install/rm_msgs/include/**",
+        "/home/robofish/RC2025/install/pointcloud_to_laserscan/include/**",
+        "/home/robofish/RC2025/install/icp_registration/include/**",
+        "/home/robofish/RC2025/install/fast_lio/include/**",
+        "/home/robofish/RC2025/install/livox_ros_driver2/include/**",
+        "/home/robofish/RC2025/install/linefit_ground_segmentation/include/**",
+        "/home/robofish/RC2025/install/imu_complementary_filter/include/**",
+        "/home/robofish/RC2025/install/fake_vel_transform/include/**",
+        "/home/robofish/RC2025/install/costmap_converter_msgs/include/**",
+        "/opt/ros/humble/include/**",
+        "/home/robofish/RC2025/src/rm_driver/rm_serial_driver/include/**",
+        "/home/robofish/RC2025/src/rm_localization/FAST_LIO/include/**",
+        "/home/robofish/RC2025/src/rm_localization/icp_registration/include/**",
+        "/home/robofish/RC2025/src/rm_localization/point_lio/include/**",
+        "/home/robofish/RC2025/src/rm_navigation/costmap_converter/costmap_converter/include/**",
+        "/home/robofish/RC2025/src/rm_navigation/fake_vel_transform/include/**",
+        "/home/robofish/RC2025/src/rm_navigation/teb_local_planner/teb_local_planner/include/**",
+        "/home/robofish/RC2025/src/rm_perception/imu_complementary_filter/include/**",
+        "/home/robofish/RC2025/src/rm_perception/linefit_ground_segementation_ros2/linefit_ground_segmentation/include/**",
+        "/home/robofish/RC2025/src/rm_perception/pointcloud_to_laserscan/include/**",
+        "/home/robofish/RC2025/src/rm_simpal_move/include/**",
+        "/home/robofish/RC2025/src/rm_simulation/livox_laser_simulation_RO2/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -0,0 +1,21 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/RC2025/src/rm_msg",
+    "python.autoComplete.extraPaths": [
+        "/home/robofish/RC2025/install/teb_msgs/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/rm_msgs/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/fast_lio/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/livox_ros_driver2/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/costmap_converter_msgs/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/robofish/RC2025/install/teb_msgs/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/rm_msgs/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/fast_lio/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/livox_ros_driver2/local/lib/python3.10/dist-packages",
+        "/home/robofish/RC2025/install/costmap_converter_msgs/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}
--- a/README.md
+++ b/README.md
@ -1,221 +0,0 @@
-# 🎯 RC2025 自动定位瞄准代码
-
-> 基于ROS2的机器人自动定位与瞄准系统，支持激光雷达建图和导航功能
-
-[![ROS2](https://img.shields.io/badge/ROS2-Humble-blue.svg)](https://docs.ros.org/en/humble/)
-[![Ubuntu](https://img.shields.io/badge/Ubuntu-22.04-orange.svg)](https://ubuntu.com/)
-[![License](https://img.shields.io/badge/License-MIT-green.svg)](LICENSE)
-
-## 📋 目录
-
- [系统要求](#系统要求)
- [环境配置](#环境配置)
- [快速开始](#快速开始)
- [重要参数配置](#重要参数配置)
- [故障排除](#故障排除)
- [贡献指南](#贡献指南)
-
-## 🔧 系统要求
-
-| 组件 | 版本/型号 |
-|------|-----------|
-| 操作系统 | Ubuntu 22.04 LTS |
-| ROS版本 | ROS2 Humble |
-| 激光雷达 | Livox MID360 |
-| 处理器 | x86_64 (推荐) |
-| 内存 | 8GB+ (推荐) |
-
-## 🚀 环境配置
-
-### 1. 安装 Livox SDK2
-
-```bash
-# 安装依赖
-sudo apt update
-sudo apt install cmake build-essential
-
-# 克隆并编译 Livox SDK2
-git clone https://github.com/Livox-SDK/Livox-SDK2.git
-cd ./Livox-SDK2/
-mkdir build && cd build
-cmake .. && make -j$(nproc)
-sudo make install
-```
-
-### 2. 安装串口驱动
-
-```bash
-pip install pyserial
-```
-
-### 3. 安装ROS2依赖
-
-```bash
-# 进入工作区
-cd /Users/lvzucheng/Documents/R/RC2025
-
-# 安装依赖包
-rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
-```
-
-## 🎯 快速开始
-
-### 1. 编译项目
-
-```bash
-. build.sh
-```
-
-### 2. 🗺️ 建图模式
-
-用于创建环境地图和点云数据：
-
-```bash
-. mapping.sh
-```
-
-#### 建图前配置
-
-1. **修改地图保存文件名**
-   ```bash
-   # 编辑 mapping.sh
-   nano mapping.sh
-   # 将 'RC2025' 改为您的项目名
-   ```
-
-2. **同步修改点云文件配置**
-   ```bash
-   # 编辑 FAST-LIO 配置文件
-   nano src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml
-   # 确保 pcd 文件名与 mapping.sh 中一致
-   ```
-
-#### 建图操作步骤
-
-1. **启动建图程序**
-   ```bash
-   ./mapping.sh
-   ```
-
-2. **控制机器人移动**
-   - 使用遥控器或键盘控制机器人
-   - 确保覆盖所有需要建图的区域
-
-3. **保存点云文件**
-   ```bash
-   ros2 service call /map_save std_srvs/srv/Trigger
-   ```
-
-4. **保存地图文件**
-   - 在RViz中使用地图保存功能
-   - 确保地图名称保持一致
-
-#### 建图效果展示
-
-<details>
-<summary>点击查看建图效果图</summary>
-
-![建图效果1](doc/img/07b3c725_11812035.png)
-![建图效果2](doc/img/5032aa1d_11812035.png)
-![建图效果3](doc/img/bea7dae2_11812035.jpeg)
-
-</details>
-
-### 3. 🧭 导航模式
-
-使用已建立的地图进行导航：
-
-```bash
-chmod +x nav.sh
-./nav.sh
-```
-
-#### 导航操作说明
-
-1. **启动导航程序**
-2. **在RViz中设置初始位置**
-3. **设置目标点进行导航**
-4. **监控导航状态**
-
-## ⚙️ 重要参数配置
-
-### 📍 激光雷达安装位置
-
-#### 位置参数配置
-```yaml
-# 文件：src/rm_nav_bringup/config/reality/measurement_params_real.yaml
-translation:
-  x: 0.0  # 前后位置 (m)
-  y: 0.0  # 左右位置 (m)
-  z: 0.0  # 上下位置 (m)
-```
-> ⚠️ **注意**：不要修改 `rpy` 参数
-
-#### 姿态参数配置
-```json
-// 文件：src/rm_nav_bringup/config/reality/MID360_config.json
-{
-  "yaw": 0.0,    // 偏航角
-  "pitch": 0.0,  // 俯仰角
-  "roll": 0.0    // 翻滚角
-}
-```
-> ⚠️ **注意**：不要修改 `xyz` 参数
-
-### 🌍 地面点云分割
-
-```yaml
-# 文件：src/rm_nav_bringup/config/reality/segmentation_real.yaml
-sensor_height: 0.3        # 激光雷达距离地面的高度 (m)
-max_dist_to_line: 0.05   # 地面点云分割的最低高度 (m)
-```
-
-### 🎯 目标点设定
-
-```bash
-# 文件：nav.sh
-# 篮筐目标点坐标
-TARGET_X=1.0  # X坐标
-TARGET_Y=0.0  # Y坐标
-```
-
-## 🔧 故障排除
-
-### 常见问题
-
-<details>
-<summary>激光雷达无法连接</summary>
-
-1. 检查网络连接
-2. 确认IP地址配置
-3. 检查防火墙设置
-4. 验证SDK安装
-
-</details>
-
-<details>
-<summary>建图效果不佳</summary>
-
-1. 检查激光雷达安装位置
-2. 调整地面分割参数
-3. 确保移动速度适中
-4. 检查环境光照条件
-
-</details>
-
-<details>
-<summary>导航精度不够</summary>
-
-1. 重新标定雷达参数
-2. 优化地图质量
-3. 调整导航参数
-4. 检查里程计数据
-
-</details>
-
-## 📝 使用技巧
-
- **建图时**：保持稳定的移动速度，避免急转急停
- **导航时**：确保地图与实际环境一致
- **调试时**：使用RViz可视化工具监控状态
- **维护时**：定期更新地图数据
--- a/mapping.sh
+++ b/mapping.sh
@ -2,11 +2,11 @@ source install/setup.bash

 commands=(
    "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2026 \
+    world:=test \
    mode:=mapping \
    lio:=fastlio \
    localization:=icp \
-    lio_rviz:=false \
+    lio_rviz:=true \
    nav_rviz:=true"
 )

--- a/nav.sh
+++ b/nav.sh
@ -1,7 +1,7 @@
+# 备场代码
 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 \
    mode:=nav \
@ -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"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map"
+    
 )

 for cmd in "${commands[@]}"; do
--- a/nav1.sh
+++ b/nav1.sh
@ -1,16 +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 \
+    world:=map1 \
    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"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map1"
+    
 )

 for cmd in "${commands[@]}"; do
--- a/nav2.sh
+++ b/nav2.sh
@ -0,0 +1,22 @@
+# 备场代码
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+    world:=map2 \
+    mode:=nav \
+    lio:=fastlio \
+    localization:=icp \
+    lio_rviz:=false \
+    nav_rviz:=true"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
+    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map2"
+    
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done
--- a/src/rm_driver/livox_ros_driver2/.gitignore
+++ b/src/rm_driver/livox_ros_driver2/.gitignore
@ -1,10 +1,7 @@
-devel/
+                                    @~!@
 build/
 install/
 log/

 .vscode/
-__pycache__/
-.catkin_workspace
-*.gv
-*.pdf
+
--- a/src/rm_driver/livox_ros_driver2/README.md
+++ b/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"
--- a/src/rm_driver/livox_ros_driver2/src/config/MID360_config.json
+++ b/src/rm_driver/livox_ros_driver2/src/config/MID360_config.json
@ -25,7 +25,7 @@
  },
  "lidar_configs" : [
    {
-      "ip" : "192.168.1.137",
+      "ip" : "192.168.1.176",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
--- a/src/rm_driver/rm_serial_driver/script/R2_Serial.py
+++ b/src/rm_driver/rm_serial_driver/script/R2_Serial.py
@ -0,0 +1,119 @@
+import rclpy
+from rclpy.node import Node
+import serial
+import struct
+from rm_msgs.msg import DataAim
+
+class R2SerialNode(Node):
+    def __init__(self):
+        super().__init__('r2_serial_node')
+        
+        # 声明参数
+        self.declare_parameter('yaw_port', '/dev/ttyUSB0')
+        self.declare_parameter('distance_port', '/dev/ttyUSB1')
+        self.declare_parameter('baud_rate', 115200)
+        
+        # 获取参数
+        self.yaw_port = self.get_parameter('yaw_port').get_parameter_value().string_value
+        self.distance_port = self.get_parameter('distance_port').get_parameter_value().string_value
+        self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
+        
+        # 初始化串口
+        try:
+            self.yaw_serial = serial.Serial(self.yaw_port, self.baud_rate, timeout=1)
+            self.get_logger().info(f"Yaw serial port {self.yaw_port} opened successfully")
+        except Exception as e:
+            self.get_logger().error(f"Failed to open yaw serial port: {e}")
+            self.yaw_serial = None
+            
+        try:
+            self.distance_serial = serial.Serial(self.distance_port, self.baud_rate, timeout=1)
+            self.get_logger().info(f"Distance serial port {self.distance_port} opened successfully")
+        except Exception as e:
+            self.get_logger().error(f"Failed to open distance serial port: {e}")
+            self.distance_serial = None
+        
+        # 数据存储
+        self.yaw1 = 0.0
+        self.yaw2 = 0.0
+        self.distance1 = 0.0
+        self.distance2 = 0.0
+        
+        # 订阅话题
+        self.sub1 = self.create_subscription(
+            DataAim, 
+            '/chassis/data_aim', 
+            self.data_aim_callback, 
+            10
+        )
+        self.sub2 = self.create_subscription(
+            DataAim, 
+            '/data_aim_r2', 
+            self.data_aim_r2_callback, 
+            10
+        )
+        
+        self.get_logger().info("R2 Serial Node initialized")
+    
+    def data_aim_callback(self, msg):
+        """处理/data_aim话题的回调"""
+        self.yaw1 = msg.yaw
+        self.distance1 = msg.distance
+        self.get_logger().debug(f"Received data_aim: yaw={self.yaw1}, distance={self.distance1}")
+        self.send_data()
+    
+    def data_aim_r2_callback(self, msg):
+        """处理/data_aim_r2话题的回调"""
+        self.yaw2 = msg.yaw
+        self.distance2 = msg.distance
+        self.get_logger().debug(f"Received data_aim_r2: yaw={self.yaw2}, distance={self.distance2}")
+        self.send_data()
+    
+    def send_data(self):
+        """发送数据到串口"""
+        # 打包所有数据：包头FF + yaw1 + distance1 + yaw2 + distance2 + 包尾FE
+        try:
+            # 包头FF + 四个float32值 + 包尾FE
+            all_data = struct.pack('<ffff', self.yaw1, self.distance1, self.yaw2, self.distance2)  # 小端序
+            data_packet = b'\xFF' + all_data + b'\xFE'
+            
+            # 通过yaw串口发送
+            if self.yaw_serial and self.yaw_serial.is_open:
+                self.yaw_serial.write(data_packet)
+                print(f"Sent yaw data: {self.yaw1}, {self.yaw2}")
+            
+            # 通过distance串口发送相同数据
+            if self.distance_serial and self.distance_serial.is_open:
+                self.distance_serial.write(data_packet)
+                print(f"Sent distance data: {self.distance1}, {self.distance2}")
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to send data: {e}")
+            
+    def __del__(self):
+        """析构函数，关闭串口"""
+        if hasattr(self, 'yaw_serial') and self.yaw_serial and self.yaw_serial.is_open:
+            self.yaw_serial.close()
+            self.get_logger().info("Yaw serial port closed")
+        
+        if hasattr(self, 'distance_serial') and self.distance_serial and self.distance_serial.is_open:
+            self.distance_serial.close()
+            self.get_logger().info("Distance serial port closed")
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    try:
+        node = R2SerialNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    except Exception as e:
+        print(f"Error in main: {e}")
+    finally:
+        if 'node' in locals():
+            node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
--- a/src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py
+++ b/src/rm_driver/rm_serial_driver/script/pub_OnlyAim.py
@ -1,79 +0,0 @@
-#!/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()
--- a/src/rm_driver/rm_serial_driver/script/pub_aim.py
+++ b/src/rm_driver/rm_serial_driver/script/pub_aim.py
@ -1,79 +0,0 @@
-#!/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()
--- a/src/rm_driver/rm_serial_driver/script/pub_self.py
+++ b/src/rm_driver/rm_serial_driver/script/pub_self.py
@ -1,101 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-import tf2_ros
-from geometry_msgs.msg import TransformStamped
-from tf2_geometry_msgs import do_transform_pose
-from geometry_msgs.msg import PoseStamped
-import math
-
-class SelfPositionSerial(Node):
-    def __init__(self):
-        super().__init__('self_position_serial')
-        
-        # 串口配置
-        self.serial_port = '/dev/ttyUSB1'  # 根据实际串口设备调整
-        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
-        
-        # TF监听器
-        self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
-        
-        # 定时器，定期发送位置信息
-        self.timer = self.create_timer(0.1, self.send_position)  # 10Hz
-        
-        self.get_logger().info('Self position serial node started')
-    
-    def send_position(self):
-        try:
-            # 获取base_link在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
-            
-            # 计算yaw角（从四元数）
-            quat = transform.transform.rotation
-            yaw = math.atan2(2.0 * (quat.w * quat.z + quat.x * quat.y),
-                           1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z))
-            
-            self.get_logger().debug(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, yaw: {yaw:.3f}')
-            
-            # 构建发送数据包
-            # 格式: 包头(0xFF) + x(4字节) + y(4字节) + z(4字节) + yaw(4字节) + 校验和(1字节) + 包尾(0xFE)
-            packet = bytearray()
-            packet.append(0xFF)  # 包头
-            packet.extend(struct.pack('<f', x))    # x坐标
-            packet.extend(struct.pack('<f', y))    # y坐标
-            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)  # 包尾
-            
-            # 发送数据
-            self.serial_conn.write(packet)
-            self.get_logger().debug(f'Sent position packet: {packet.hex()}')
-            
-        except Exception as e:
-            self.get_logger().warn(f'Failed to get transform or send data: {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 = SelfPositionSerial()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        if rclpy.ok():
-            rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
--- a/src/rm_driver/rm_serial_driver/script/receive.py
+++ b/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()
--- a/src/rm_driver/rm_serial_driver/script/receive_and_pub.py
+++ b/src/rm_driver/rm_serial_driver/script/receive_and_pub.py
@ -1,241 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-import serial
-import struct
-import tf2_ros
-from geometry_msgs.msg import TransformStamped
-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/ttyUSB2'  # 接收串口
-        self.send_port = '/dev/ttyUSB3'     # 发送串口
-        self.baud_rate = 115200
-        
-        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.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port}')
-        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}
-        
-        # 启动接收线程
-        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.1, 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:
-                if self.receive_serial.in_waiting > 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('<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().debug(f'Received position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
-                            else:
-                                self.get_logger().warn('Checksum error')
-                            
-                            # 移除已处理的数据包
-                            buffer = buffer[22:]
-                        else:
-                            # 包尾不匹配，移除包头继续查找
-                            buffer = buffer[1:]
-                            
-            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 = self.received_position['yaw']
-            r2_distance = math.sqrt(
-                self.received_position['x']**2 + 
-                self.received_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']:
-                original_yaw = self.aim_data['yaw']
-                original_distance = self.aim_data['distance']
-            else:
-                original_yaw = 0.0
-                original_distance = 0.0
-            
-            # 获取R2目标数据
-            r2_yaw, r2_distance = self.get_r2_target_data()
-            
-            # 构建发送数据包
-            # 格式: 包头(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', 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
-            
-            # 计算校验和
-            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})')
-            
-        except Exception as e:
-            self.get_logger().error(f'Error sending target data: {e}')
-    
-    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()
-        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()
--- a/src/rm_driver/rm_serial_driver/script/slect.py
+++ b/src/rm_driver/rm_serial_driver/script/slect.py
@ -0,0 +1,65 @@
+import rclpy
+from rclpy.node import Node
+from rm_msgs.msg import MoveGoal
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+import sys
+
+# 预设多组参数
+MAP_CONFIGS = {
+    "map":  {"split_x": 7.155,   "goal1": (0.65, 3.95),  "goal2": (13.66, 3.73)},
+    "map1": {"split_x": 7.085,   "goal1": (0.54, -3.28),     "goal2": (13.63, -3.37)},
+    "map2": {"split_x": 7.075,   "goal1": (0.57, -3.34),    "goal2": (13.58, -3.35)},
+}
+
+class AimSelector(Node):
+    def __init__(self, config):
+        super().__init__('aim_selector')
+        self.split_x = config["split_x"]
+        self.goal1_x, self.goal1_y = config["goal1"]
+        self.goal2_x, self.goal2_y = config["goal2"]
+        self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.timer = self.create_timer(0.05, self.timer_callback)
+
+    def timer_callback(self):
+        try:
+            trans: TransformStamped = self.tf_buffer.lookup_transform(
+                'map', 'base_link', rclpy.time.Time())
+            x = trans.transform.translation.x
+            self.get_logger().info(f'base_link x in map: {x:.2f}')
+            msg = MoveGoal()
+            if x < self.split_x:
+                msg.x = self.goal1_x
+                msg.y = self.goal1_y
+            else:
+                msg.x = self.goal2_x
+                msg.y = self.goal2_y
+            msg.angle = 0.0
+            msg.max_speed = 0.0
+            msg.tolerance = 0.1
+            msg.rotor = False
+            self.publisher.publish(msg)
+        except Exception as e:
+            self.get_logger().warn(f'Failed to get transform: {e}')
+
+def main(args=None):
+    # 读取命令行参数
+    if len(sys.argv) < 2:
+        print("用法: python slect.py [map|map1|map2]")
+        sys.exit(1)
+    map_key = sys.argv[1]
+    if map_key not in MAP_CONFIGS:
+        print(f"未知参数: {map_key}")
+        sys.exit(1)
+    config = MAP_CONFIGS[map_key]
+
+    rclpy.init(args=args)
+    node = AimSelector(config)
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
--- a/src/rm_msg/README.md
+++ b/src/rm_msg/README.md
@ -3,4 +3,7 @@ Some ROS 2 custom messages for Robotaster

 Usage
 Modify or add files in the /msg directory as needed
-colcon build
+colcon build
+KERNELS=="3-2:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="underpan"
+KERNELS=="3-1:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="upper"
+KERNELS=="3-3:1.0", SUBSYSTEMS=="USB", MODE:="0666",SYMLINK+="r2"
--- a/src/rm_msg/msg/DataAim.msg
+++ b/src/rm_msg/msg/DataAim.msg
@ -1,2 +1,2 @@
 float32 yaw
-float32 distance
+float32 distance
--- a/src/rm_nav_bringup/PCD/1/rc_map1.data
+++ b/src/rm_nav_bringup/PCD/1/rc_map1.data
--- a/src/rm_nav_bringup/PCD/1/rc_map1.pcd
+++ b/src/rm_nav_bringup/PCD/1/rc_map1.pcd
--- a/src/rm_nav_bringup/PCD/1/rc_map1.pgm
+++ b/src/rm_nav_bringup/PCD/1/rc_map1.pgm
--- a/src/rm_nav_bringup/PCD/1/rc_map1.posegraph
+++ b/src/rm_nav_bringup/PCD/1/rc_map1.posegraph
--- a/src/rm_nav_bringup/PCD/1/rc_map1.yaml
+++ b/src/rm_nav_bringup/PCD/1/rc_map1.yaml
@ -0,0 +1,7 @@
+image: rc_map1.pgm
+mode: trinary
+resolution: 0.05
+origin: [-6.19, -7.3, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
--- a/src/rm_nav_bringup/PCD/2/rc_map1.data
+++ b/src/rm_nav_bringup/PCD/2/rc_map1.data
--- a/src/rm_nav_bringup/PCD/2/rc_map1.pcd
+++ b/src/rm_nav_bringup/PCD/2/rc_map1.pcd
--- a/src/rm_nav_bringup/PCD/2/rc_map1.pgm
+++ b/src/rm_nav_bringup/PCD/2/rc_map1.pgm
--- a/src/rm_nav_bringup/PCD/2/rc_map1.posegraph
+++ b/src/rm_nav_bringup/PCD/2/rc_map1.posegraph
--- a/src/rm_nav_bringup/PCD/2/rc_map1.yaml
+++ b/src/rm_nav_bringup/PCD/2/rc_map1.yaml
@ -0,0 +1,7 @@
+image: rc_map1.pgm
+mode: trinary
+resolution: 0.05
+origin: [-7.9, -8.67, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
--- a/src/rm_nav_bringup/PCD/RC2025.pcd
+++ b/src/rm_nav_bringup/PCD/RC2025.pcd
--- a/src/rm_nav_bringup/PCD/map1.pcd
+++ b/src/rm_nav_bringup/PCD/map1.pcd
--- a/src/rm_nav_bringup/PCD/map2.pcd
+++ b/src/rm_nav_bringup/PCD/map2.pcd
--- a/src/rm_nav_bringup/PCD/rc_map1.pcd
+++ b/src/rm_nav_bringup/PCD/rc_map1.pcd
--- a/src/rm_nav_bringup/PCD/test.pcd
+++ b/src/rm_nav_bringup/PCD/test.pcd
--- a/src/rm_nav_bringup/PCD/test2.pcd
+++ b/src/rm_nav_bringup/PCD/test2.pcd
--- a/src/rm_nav_bringup/config/reality/MID360_config.json
+++ b/src/rm_nav_bringup/config/reality/MID360_config.json
@ -25,7 +25,7 @@
  },
  "lidar_configs" : [
    {
-      "ip" : "192.168.1.176",
+      "ip" : "192.168.1.137",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
--- a/src/rm_nav_bringup/config/reality/fastlio_mid360_real.yaml
+++ b/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/test2.pcd"

        common:
            lid_topic:  "/livox/lidar"
--- a/src/rm_nav_bringup/config/reality/icp_registration_real.yaml
+++ b/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: ""
+    pcd_path: "map2"
    map_frame_id: "map"
    odom_frame_id: "odom"
    range_odom_frame_id: "lidar_odom"
--- a/src/rm_nav_bringup/config/reality/measurement_params_real.yaml
+++ b/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.246 -0.135 0.397\""
  rpy: "\"0.0  0.0  0.0\""
--- a/src/rm_nav_bringup/map/map1.data
+++ b/src/rm_nav_bringup/map/map1.data
--- a/src/rm_nav_bringup/map/map1.pgm
+++ b/src/rm_nav_bringup/map/map1.pgm
--- a/src/rm_nav_bringup/map/map1.posegraph
+++ b/src/rm_nav_bringup/map/map1.posegraph
--- a/src/rm_nav_bringup/map/map1.yaml
+++ b/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
--- a/src/rm_nav_bringup/map/map2.data
+++ b/src/rm_nav_bringup/map/map2.data
--- a/src/rm_nav_bringup/map/map2.pgm
+++ b/src/rm_nav_bringup/map/map2.pgm
--- a/src/rm_nav_bringup/map/map2.posegraph
+++ b/src/rm_nav_bringup/map/map2.posegraph
--- a/src/rm_nav_bringup/map/map2.yaml
+++ b/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
--- a/src/rm_nav_bringup/map/rc_map1.data
+++ b/src/rm_nav_bringup/map/rc_map1.data
--- a/src/rm_nav_bringup/map/rc_map1.pgm
+++ b/src/rm_nav_bringup/map/rc_map1.pgm
--- a/src/rm_nav_bringup/map/rc_map1.posegraph
+++ b/src/rm_nav_bringup/map/rc_map1.posegraph
--- a/src/rm_nav_bringup/map/rc_map1.yaml
+++ b/src/rm_nav_bringup/map/rc_map1.yaml
@ -0,0 +1,7 @@
+image: rc_map1.pgm
+mode: trinary
+resolution: 0.05
+origin: [-7.9, -8.67, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
--- a/src/rm_nav_bringup/map/test.data
+++ b/src/rm_nav_bringup/map/test.data
--- a/src/rm_nav_bringup/map/test.pgm
+++ b/src/rm_nav_bringup/map/test.pgm
--- a/src/rm_nav_bringup/map/test.posegraph
+++ b/src/rm_nav_bringup/map/test.posegraph
--- a/src/rm_nav_bringup/map/test.yaml
+++ b/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
--- a/src/rm_nav_bringup/map/test2.data
+++ b/src/rm_nav_bringup/map/test2.data
--- a/src/rm_nav_bringup/map/test2.pgm
+++ b/src/rm_nav_bringup/map/test2.pgm
--- a/src/rm_nav_bringup/map/test2.posegraph
+++ b/src/rm_nav_bringup/map/test2.posegraph
--- a/src/rm_nav_bringup/map/test2.yaml
+++ b/src/rm_nav_bringup/map/test2.yaml
@ -0,0 +1,7 @@
+image: test2.pgm
+mode: trinary
+resolution: 0.05
+origin: [-5.88, -6.46, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25
Author	SHA1	Message	Date
Robofish	2f3e6caa8d	r1-7-12	2025-07-12 22:13:39 +08:00
Robofish	ccb4a91e64	提交一下	2025-07-11 00:48:19 +08:00
Robofish	dcef535b33	R1	2025-07-10 13:21:45 +08:00