# FAST-LIO 雷达定位系统

<div align="center">

![FAST-LIO](https://img.shields.io/badge/FAST--LIO-v2.0-blue.svg)
![ROS2](https://img.shields.io/badge/ROS2-Humble-green.svg)
![Livox](https://img.shields.io/badge/Livox-MID360-orange.svg)
![Platform](https://img.shields.io/badge/Platform-Jetson-red.svg)

**高精度实时激光雷达惯性里程计定位系统**

基于 FAST-LIO 算法和 Livox MID360 雷达的实时 SLAM 定位解决方案

</div>

## 📋 目录

- [项目概述](#项目概述)
- [功能特性](#功能特性)
- [系统要求](#系统要求)
- [安装指南](#安装指南)
- [快速开始](#快速开始)
- [详细配置](#详细配置)
- [使用说明](#使用说明)
- [API文档](#api文档)
- [故障排除](#故障排除)
- [性能优化](#性能优化)
- [开发指南](#开发指南)
- [贡献说明](#贡献说明)
- [许可证](#许可证)

## 🎯 项目概述

FAST-LIO 雷达定位系统是一个基于 **FAST-LIO** 算法和 **Livox MID360** 雷达的高精度实时定位解决方案。该系统融合激光雷达和IMU数据，提供厘米级的位置精度和完整的轨迹跟踪功能。

### 核心技术
- **FAST-LIO**: 快速激光雷达惯性里程计算法
- **Livox MID360**: 中远距离激光雷达传感器
- **ROS2 Humble**: 机器人操作系统框架
- **ikd-Tree**: 增量式KD树数据结构

### 应用场景
- 🏗️ **室内定位导航**
- 🚗 **自动驾驶车辆**
- 🤖 **移动机器人SLAM**
- 📐 **精密测量与建图**
- 🔬 **科研与教育**

## ✨ 功能特性

### 🎯 核心功能
- **实时定位**: 提供 X、Y、Z 三维坐标位置
- **姿态估计**: 完整的 Roll、Pitch、Yaw 角度信息
- **轨迹跟踪**: 完整的运动路径记录
- **距离计算**: 自动累计移动距离统计
- **地图构建**: 实时3D点云地图生成

### 📊 性能指标
- **定位精度**: ±2cm
- **更新频率**: 10Hz
- **延迟**: <100ms
- **检测范围**: 100m
- **角度分辨率**: 0.1°

### 🛠️ 系统特性
- **即插即用**: 简单的配置和启动
- **实时可视化**: RViz2 3D显示界面
- **数据记录**: 轨迹数据自动保存
- **模块化设计**: 易于集成和扩展
- **跨平台支持**: Linux/Ubuntu/Jetson

## 🔧 系统要求

### 硬件要求
- **处理器**: ARM64 或 x86_64 (推荐 4核以上)
- **内存**: 4GB RAM (推荐 8GB)
- **存储**: 10GB 可用空间
- **网络**: 千兆以太网接口
- **雷达**: Livox MID360 或兼容型号

### 软件要求
- **操作系统**: Ubuntu 22.04 LTS
- **ROS版本**: ROS2 Humble
- **编译器**: GCC 9.0+
- **CMake**: 3.16+
- **Python**: 3.8+

### 依赖库
```bash
# 核心依赖
ros-humble-desktop-full
ros-humble-pcl-ros
ros-humble-livox-ros-driver2

# 第三方库
libeigen3-dev
libpcl-dev
libopencv-dev
```

## 🚀 安装指南

### 1. 环境准备

```bash
# 更新系统
sudo apt update && sudo apt upgrade -y

# 安装ROS2 Humble
wget -qO - https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo apt-key add -
sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu jammy main" > /etc/apt/sources.list.d/ros2-latest.list'
sudo apt update
sudo apt install ros-humble-desktop-full -y

# 安装依赖
sudo apt install -y \
    python3-colcon-common-extensions \
    python3-rosdep \
    libeigen3-dev \
    libpcl-dev \
    libopencv-dev \
    ros-humble-pcl-ros \
    ros-humble-rviz2
```

### 2. 工作区创建

```bash
# 创建工作区
mkdir -p ~/mid360test/src
cd ~/mid360test

# 克隆源码
cd src
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
git clone https://github.com/hku-mars/FAST_LIO.git
git clone https://github.com/Livox-SDK/Livox-SDK2.git
```

### 3. 编译安装

```bash
# 初始化rosdep
sudo rosdep init
rosdep update

# 安装依赖
cd ~/mid360test
rosdep install --from-paths src --ignore-src -r -y

# 编译
colcon build

# 设置环境
echo "source ~/mid360test/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
```

### 4. 硬件连接

```bash
# 配置网络 (修改为实际网卡名称)
sudo ip addr add 192.168.1.5/24 dev eth0
sudo ip link set eth0 up

# 测试连接
ping 192.168.1.194
```

## ⚡ 快速开始

### 基本启动流程

```bash
# 1. 启动雷达驱动
cd ~/mid360test
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py
```

```bash
# 2. 启动FAST-LIO定位 (新终端)
cd ~/mid360test
source install/setup.bash
ros2 launch fast_lio mapping.launch.py
```

```bash
# 3. 启动距离计算器 (新终端)
cd ~/mid360test
source install/setup.bash
python3 distance_calculator.py
```

### 验证系统运行

```bash
# 检查话题
ros2 topic list

# 查看定位数据
ros2 topic echo /Odometry --once

# 检查数据频率
ros2 topic hz /Odometry
```

## ⚙️ 详细配置

### 雷达配置文件

编辑 `src/livox_ros_driver2/config/MID360_config.json`:

```json
{
  "lidar_summary_info": {
    "lidar_type": 8
  },
  "MID360": {
    "lidar_net_info": {
      "cmd_data_port": 56100,
      "push_msg_port": 56200,
      "point_data_port": 56300,
      "imu_data_port": 56400,
      "log_data_port": 56500
    },
    "host_net_info": {
      "cmd_data_ip": "192.168.1.5",     // 主机IP
      "cmd_data_port": 56101,
      "push_msg_ip": "192.168.1.5",
      "push_msg_port": 56201,
      "point_data_ip": "192.168.1.5",
      "point_data_port": 56301,
      "imu_data_ip": "192.168.1.5",
      "imu_data_port": 56401,
      "log_data_ip": "",
      "log_data_port": 56501
    }
  },
  "lidar_configs": [
    {
      "ip": "192.168.1.194",           // 雷达IP
      "pcl_data_type": 1,
      "pattern_mode": 0,
      "extrinsic_parameter": {
        "roll": 0.0,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}
```

### FAST-LIO参数配置

编辑 `src/FAST_LIO/config/mid360.yaml`:

```yaml
/**:
  ros__parameters:
    # 基础参数
    feature_extract_enable: false
    point_filter_num: 3
    max_iteration: 3
    filter_size_surf: 0.5
    filter_size_map: 0.5
    cube_side_length: 1000.0
    
    common:
      lid_topic: "/livox/lidar"
      imu_topic: "/livox/imu"
      time_sync_en: false
      time_offset_lidar_to_imu: 0.0
    
    preprocess:
      lidar_type: 1                    # 1: Livox雷达
      scan_line: 4
      blind: 0.5                       # 盲区距离
      timestamp_unit: 3
      scan_rate: 10
    
    mapping:
      acc_cov: 0.1                     # 加速度协方差
      gyr_cov: 0.1                     # 陀螺仪协方差
      b_acc_cov: 0.0001
      b_gyr_cov: 0.0001
      fov_degree: 360.0                # 视场角
      det_range: 100.0                 # 检测范围
      extrinsic_est_en: true           # 外参自动估计
      
    publish:
      path_en: true                    # 发布路径
      scan_publish_en: true            # 发布点云
      dense_publish_en: false
      scan_bodyframe_pub_en: true
      
    pcd_save:
      pcd_save_en: true                # 保存PCD文件
      interval: -1                     # 保存间隔
```

## 📖 使用说明

### 基本操作

#### 1. 启动系统
```bash
# 方式1: 分步启动 (推荐)
ros2 launch livox_ros_driver2 msg_MID360_launch.py    # 终端1
ros2 launch fast_lio mapping.launch.py               # 终端2
python3 distance_calculator.py                       # 终端3

# 方式2: 一键启动
./start_system.sh
```

#### 2. 系统初始化
- 启动后保持雷达静止 **5-10秒**
- 等待系统完成初始化
- 看到 "Node init finished" 提示

#### 3. 开始定位
- 缓慢移动雷达设备
- 观察RViz2中的实时轨迹
- 监控距离计算器输出

#### 4. 停止系统
```bash
# 停止各个进程
Ctrl+C  # 在各个终端中

# 查看最终统计报告
# 距离计算器会自动显示总结
```

### 数据获取

#### 位置信息获取
```bash
# 实时位置 (单次)
ros2 topic echo /Odometry --once

# 持续监控
ros2 topic echo /Odometry

# 位置数据格式
header:
  stamp: {sec: xxx, nanosec: xxx}
  frame_id: camera_init
pose:
  pose:
    position: {x: 1.23, y: -0.45, z: 0.67}    # 位置(米)
    orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}  # 姿态(四元数)
```

#### 轨迹数据获取
```bash
# 轨迹路径
ros2 topic echo /path --once

# 保存轨迹到文件
ros2 bag record /Odometry /path
```

#### 距离计算
距离计算器会自动生成轨迹文件：
```
trajectory_log_YYYYMMDD_HHMMSS.txt
```

文件格式：
```csv
时间戳,运行时间(s),X坐标(m),Y坐标(m),Z坐标(m),累计距离(m),瞬时速度(m/s)
2025-07-01 20:56:28.170,0.142,0.068884,0.051447,0.012272,0.000000,0.000000
```

### 可视化界面

#### RViz2界面说明
- **绿色点云**: 实时构建的环境地图
- **红色轨迹线**: 雷达运动路径
- **坐标轴**: 当前位置和姿态
- **彩色点**: 当前扫描数据

#### 界面操作
- **鼠标左键拖拽**: 旋转视角
- **鼠标右键拖拽**: 平移视角
- **滚轮**: 缩放视角
- **中键拖拽**: 平移视角

### 高级功能

#### 地图保存
```bash
# 地图自动保存在
ls src/FAST_LIO/PCD/

# 手动保存当前地图
ros2 service call /save_map std_srvs/srv/Trigger
```

#### 参数调优
```bash
# 运行时调整参数
ros2 param set /laser_mapping filter_size_surf 0.3
ros2 param set /laser_mapping cube_side_length 500.0
```

## 📡 API文档

### ROS话题

#### 发布话题
| 话题名称 | 消息类型 | 频率 | 描述 |
|---------|---------|------|------|
| `/Odometry` | nav_msgs/Odometry | 10Hz | 位置和姿态信息 |
| `/path` | nav_msgs/Path | 1Hz | 运动轨迹路径 |
| `/cloud_registered` | sensor_msgs/PointCloud2 | 10Hz | 配准后点云 |
| `/Laser_map` | sensor_msgs/PointCloud2 | 1Hz | 全局地图 |

#### 订阅话题
| 话题名称 | 消息类型 | 描述 |
|---------|---------|------|
| `/livox/lidar` | livox_ros_driver2/CustomMsg | 雷达点云数据 |
| `/livox/imu` | sensor_msgs/Imu | IMU惯性数据 |

### 服务接口

```bash
# 保存地图
ros2 service call /save_map std_srvs/srv/Trigger

# 重置定位
ros2 service call /reset_localization std_srvs/srv/Trigger
```

### 参数列表

| 参数名称 | 类型 | 默认值 | 描述 |
|---------|------|--------|------|
| `filter_size_surf` | double | 0.5 | 表面点滤波大小 |
| `filter_size_map` | double | 0.5 | 地图点滤波大小 |
| `cube_side_length` | double | 1000.0 | 局部地图边长 |
| `det_range` | double | 100.0 | 检测范围 |

## 🐛 故障排除

### 常见问题

#### 1. 雷达连接失败
**症状**: 无法ping通雷达IP
```bash
# 检查网络配置
ip addr show
ping 192.168.1.194

# 解决方案
sudo ip addr add 192.168.1.5/24 dev eth0
sudo ip link set eth0 up
```

#### 2. 无定位数据输出
**症状**: `/Odometry`话题无数据
```bash
# 检查数据流
ros2 topic hz /livox/lidar
ros2 topic hz /livox/imu

# 可能原因和解决
# - 系统未初始化: 等待5-10秒
# - 需要运动激励: 缓慢移动雷达
# - 进程冲突: 重启所有进程
```

#### 3. 定位精度差
**症状**: 位置跳跃或漂移
```bash
# 检查环境
# - 确保有足够几何特征
# - 避免玻璃、镜面等反射面
# - 检查雷达安装稳定性

# 参数调优
ros2 param set /laser_mapping filter_size_surf 0.3
```

#### 4. RViz2显示异常
**症状**: 无点云或轨迹显示
```bash
# 检查Fixed Frame设置
# 确保设置为 "camera_init"

# 重启RViz2
pkill rviz2
ros2 run rviz2 rviz2
```

#### 5. 编译错误
```bash
# 常见编译问题
# ikd-Tree未找到
cd src/FAST_LIO/include
git clone https://github.com/hku-mars/ikd-Tree.git

# 依赖缺失
rosdep install --from-paths src --ignore-src -r -y

# 权限问题
sudo chown -R $USER:$USER ~/mid360test
```

### 错误代码

| 错误代码 | 描述 | 解决方案 |
|---------|------|----------|
| `E001` | 雷达连接超时 | 检查网络配置 |
| `E002` | IMU数据异常 | 检查雷达连接 |
| `E003` | 定位初始化失败 | 提供运动激励 |
| `E004` | 内存不足 | 增加swap空间 |

### 诊断工具

```bash
# 系统状态检查脚本
./scripts/system_check.sh

# 网络诊断
./scripts/network_check.sh

# 性能监控
./scripts/performance_monitor.sh
```

## ⚡ 性能优化

### 系统级优化

```bash
# CPU性能优化
echo performance | sudo tee /sys/devices/system/cpu/cpu*/cpufreq/scaling_governor

# 内存优化
echo 1 | sudo tee /proc/sys/vm/swappiness

# 网络优化
sudo sysctl -w net.core.rmem_max=134217728
sudo sysctl -w net.core.wmem_max=134217728
```

### 算法参数优化

```yaml
# 高性能模式 (牺牲精度)
filter_size_surf: 0.8
filter_size_map: 0.8
cube_side_length: 500.0

# 高精度模式 (牺牲性能)
filter_size_surf: 0.2
filter_size_map: 0.2
cube_side_length: 2000.0
```

### 硬件优化建议

- **CPU**: 优先选择高主频多核处理器
- **内存**: 推荐8GB以上DDR4
- **存储**: 使用SSD提升IO性能
- **网络**: 千兆以太网，低延迟网卡

## 🛠️ 开发指南

### 项目结构

```
mid360test/
├── src/
│   ├── livox_ros_driver2/      # 雷达驱动
│   ├── FAST_LIO/               # FAST-LIO算法
│   └── Livox-SDK2/             # Livox SDK
├── install/                    # 安装文件
├── build/                      # 编译文件
├── log/                        # 日志文件
├── distance_calculator.py      # 距离计算器
├── README.md                   # 项目文档
└── scripts/                    # 辅助脚本
    ├── start_system.sh
    ├── system_check.sh
    └── performance_monitor.sh
```

### 自定义开发

#### 添加新的传感器
```cpp
// 在FAST_LIO中添加新传感器支持
// 1. 修改preprocess.h
// 2. 实现数据处理函数
// 3. 更新CMakeLists.txt
```

#### 扩展距离计算器
```python
# distance_calculator.py
class ExtendedCalculator(DistanceCalculator):
    def __init__(self):
        super().__init__()
        # 添加新功能
        
    def custom_analysis(self):
        # 自定义分析功能
        pass
```

#### 集成其他算法
```bash
# 添加新的SLAM算法
cd src
git clone https://github.com/your-slam-algorithm.git

# 修改launch文件
# 更新依赖关系
```

### 测试框架

```bash
# 单元测试
colcon test --packages-select fast_lio

# 集成测试
./scripts/integration_test.sh

# 性能测试
./scripts/benchmark.sh
```

## 🤝 贡献说明

我们欢迎社区贡献！请遵循以下指南：

### 贡献流程

1. **Fork** 项目到您的账户
2. **创建** 功能分支 (`git checkout -b feature/AmazingFeature`)
3. **提交** 更改 (`git commit -m 'Add some AmazingFeature'`)
4. **推送** 分支 (`git push origin feature/AmazingFeature`)
5. **提交** Pull Request

### 代码规范

- 遵循 **Google C++ Style Guide**
- 使用 **4空格缩进**
- 添加适当的 **注释和文档**
- 确保 **代码通过测试**

### Bug报告

提交Bug时请包含：
- 详细的问题描述
- 复现步骤
- 系统环境信息
- 相关日志文件

### 功能请求

提交功能请求时请说明：
- 功能的详细描述
- 使用场景和必要性
- 可能的实现方案

## 📞 技术支持

### 获取帮助

- **GitHub Issues**: [提交问题](https://github.com/your-repo/issues)
- **讨论区**: [GitHub Discussions](https://github.com/your-repo/discussions)
- **技术文档**: [Wiki页面](https://github.com/your-repo/wiki)

### 社区资源

- **FAST-LIO论文**: [Fast Direct LiDAR-Inertial Odometry](https://arxiv.org/abs/2010.08196)
- **Livox官网**: [https://www.livoxtech.com/](https://www.livoxtech.com/)
- **ROS2文档**: [https://docs.ros.org/en/humble/](https://docs.ros.org/en/humble/)

## 📜 许可证

本项目采用 **MIT License** 许可证 - 详见 [LICENSE](LICENSE) 文件。

### 第三方许可证

- **FAST-LIO**: GPLv2 License
- **Livox SDK**: MIT License
- **ROS2**: Apache 2.0 License

## 🙏 致谢

感谢以下项目和贡献者：

- [FAST-LIO](https://github.com/hku-mars/FAST_LIO) - HKU-Mars实验室
- [Livox-SDK](https://github.com/Livox-SDK) - Livox Technology
- [ROS2](https://github.com/ros2) - Open Source Robotics Foundation

---

<div align="center">

**⭐ 如果这个项目对您有帮助，请给我们一个星标！**

Made with ❤️ by the FAST-LIO Community

</div>