SW1AG7/360
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上传文件至 /

Browse Source
This commit is contained in:
SW1AG7
2025-07-16 18:37:38 +08:00
commit 18c43d3b59
5 changed files with 3515 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

716
README.md Normal file
View File

@@ -0,0 +1,716 @@
# 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>

135
autostart_core.sh Normal file
View File

@@ -0,0 +1,135 @@
#!/bin/bash
# FAST-LIO 开机自启动脚本 (核心版本)
# 只启动必要的核心程序，适用于开机自启动
# 不依赖图形界面，使用后台运行方式
set -e
# 配置
WORKSPACE_DIR="/home/jetson/mid360test"
LOG_DIR="$WORKSPACE_DIR/logs"
STARTUP_LOG="$LOG_DIR/autostart_$(date +%Y%m%d_%H%M%S).log"
# 创建日志目录
mkdir -p "$LOG_DIR"
# 日志函数
log() {
echo "[$(date '+%Y-%m-%d %H:%M:%S')] $1" | tee -a "$STARTUP_LOG"
}
log "🚀 FAST-LIO 自启动开始"
# 切换到工作目录
cd "$WORKSPACE_DIR" || {
log "❌ 无法切换到工作目录: $WORKSPACE_DIR"
exit 1
}
# 清理旧进程
log "🧹 清理旧进程..."
pkill -f "livox_ros_driver2" 2>/dev/null || true
pkill -f "fastlio_mapping" 2>/dev/null || true
pkill -f "distance_calculator" 2>/dev/null || true
sleep 3
# 设置环境
log "⚙️ 设置ROS2环境..."
source /opt/ros/humble/setup.bash 2>/dev/null || {
log "❌ 无法加载ROS2环境"
exit 1
}
source "$WORKSPACE_DIR/install/setup.bash" 2>/dev/null || {
log "❌ 无法加载工作空间环境"
exit 1
}
# 等待网络稳定
log "🌐 等待网络连接稳定..."
sleep 5
# 验证网络连接
if ping -c 1 192.168.1.194 >/dev/null 2>&1; then
log "✅ 雷达网络连接正常"
else
log "⚠️ 雷达网络连接异常，继续启动"
fi
# 启动雷达驱动 (后台运行)
log "📡 启动雷达驱动..."
nohup ros2 launch livox_ros_driver2 msg_MID360_launch.py \
> "$LOG_DIR/livox_driver.log" 2>&1 &
LIVOX_PID=$!
log "📡 雷达驱动已启动 (PID: $LIVOX_PID)"
# 等待雷达驱动稳定
sleep 8
# 启动FAST-LIO (后台运行)
log "🎯 启动FAST-LIO..."
nohup ros2 launch fast_lio mapping.launch.py \
> "$LOG_DIR/fastlio.log" 2>&1 &
FASTLIO_PID=$!
log "🎯 FAST-LIO已启动 (PID: $FASTLIO_PID)"
# 等待FAST-LIO稳定
sleep 5
# 启动距离计算器 (后台运行)
if [ -f "$WORKSPACE_DIR/distance_calculator.py" ]; then
log "📏 启动距离计算器..."
nohup python3 "$WORKSPACE_DIR/distance_calculator.py" \
> "$LOG_DIR/distance_calculator.log" 2>&1 &
CALC_PID=$!
log "📏 距离计算器已启动 (PID: $CALC_PID)"
else
log "⚠️ 距离计算器文件不存在"
fi
# 保存PID文件，便于后续管理
echo "$LIVOX_PID" > "$LOG_DIR/livox_driver.pid"
echo "$FASTLIO_PID" > "$LOG_DIR/fastlio.pid"
[ -n "$CALC_PID" ] && echo "$CALC_PID" > "$LOG_DIR/distance_calculator.pid"
log "✅ 所有核心程序启动完成"
log "📊 进程状态:"
log " - 雷达驱动: PID $LIVOX_PID"
log " - FAST-LIO: PID $FASTLIO_PID"
[ -n "$CALC_PID" ] && log " - 距离计算器: PID $CALC_PID"
log "📁 日志文件位置:"
log " - 启动日志: $STARTUP_LOG"
log " - 雷达驱动: $LOG_DIR/livox_driver.log"
log " - FAST-LIO: $LOG_DIR/fastlio.log"
[ -n "$CALC_PID" ] && log " - 距离计算器: $LOG_DIR/distance_calculator.log"
log "💡 提示:"
log " - 查看实时状态: tail -f $LOG_DIR/*.log"
log " - 停止系统: ./stop_system.sh 或 pkill -f 'ros2|python3.*distance_calculator'"
log " - 系统需要5-10秒完全稳定后才开始UART传输"
log "🎉 自启动脚本执行完成"
# 可选：监控进程状态
sleep 10
log "🔍 检查进程状态..."
check_process() {
local pid=$1
local name=$2
if kill -0 "$pid" 2>/dev/null; then
log "✅ $name 运行正常 (PID: $pid)"
return 0
else
log "❌ $name 进程异常 (PID: $pid)"
return 1
fi
}
check_process "$LIVOX_PID" "雷达驱动"
check_process "$FASTLIO_PID" "FAST-LIO"
[ -n "$CALC_PID" ] && check_process "$CALC_PID" "距离计算器"
log "🏁 初始化完成，系统正在运行中..."

377
distance_calculator.py Normal file
View File

@@ -0,0 +1,377 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
FAST-LIO Distance Calculator + STM32 Communication
Real-time calculation of radar movement distance and position,
and send data to STM32 via UART
"""
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
import math
import time
from datetime import datetime
import serial
import struct
import os
import sys
class DistanceCalculator(Node):
def __init__(self):
super().__init__('distance_calculator')
# Subscribe to odometry topic
self.subscription = self.create_subscription(
Odometry, '/Odometry', self.odom_callback, 10)
# Initialize variables
self.last_position = None
self.total_distance = 0.0
self.start_time = time.time()
self.last_log_time = time.time()
self.position_history = []
self.last_sent_time = time.time()
# 添加静态检测和噪声过滤
self.static_threshold = 0.002 # 静态检测阈值（米）
self.static_count = 0
self.static_position_buffer = []
self.max_static_buffer_size = 10
self.velocity_threshold = 0.001 # 速度阈值（米/秒）
self.last_position_filtered = None
# 系统启动稳定机制
self.startup_delay = 5.0 # 启动后5秒延迟发送
self.system_stable = False
self.data_valid_count = 0
self.min_valid_data_count = 10 # 需要10个连续有效数据才开始发送
# Initialize UART communication
self.uart_port = '/dev/ttyTHS1' # Jetson Orin NX UART1
self.baudrate = 115200
self.ser = None
try:
self.ser = serial.Serial(
port=self.uart_port,
baudrate=self.baudrate,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0.1
)
self.get_logger().info(f"UART port opened: {self.uart_port}, Baudrate: {self.baudrate}")
except Exception as e:
self.get_logger().error(f"Failed to open UART port: {str(e)}")
self.get_logger().info("Data will not be sent to STM32")
# Create log file
self.log_filename = f"trajectory_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.txt"
self.get_logger().info("Distance calculator started")
self.get_logger().info(f"Trajectory data will be saved to: {self.log_filename}")
self.get_logger().info("Waiting for radar positioning data...")
# Write log file header
with open(self.log_filename, 'w', encoding='utf-8') as f:
f.write("Timestamp,Runtime(s),X(m),Y(m),Z(m),Total Distance(m),Velocity X(m/s),Velocity Y(m/s),Velocity Z(m/s)\n")
def send_to_stm32(self, x, y, z, vx, vy, vz):
"""Send data to STM32 using anonymous protocol"""
if self.ser is None or not self.ser.is_open:
return False
try:
# Protocol format: [0xAA][0xFF][0x01][Data Length][Data][Sum Check][Add Check]
# Scale factors
POS_SCALE_FACTOR = 1000 # 1 mm precision
VEL_SCALE_FACTOR = 1000 # 1 mm/s precision
# 数据有效性检查
# 检查是否为有效数值
if not all(math.isfinite(val) for val in [x, y, z, vx, vy, vz]):
self.get_logger().warning("Invalid data detected (NaN or Inf), skipping transmission")
return False
# 位置范围限制（±30米）
MAX_POS = 30.0
MIN_POS = -30.0
x = max(MIN_POS, min(MAX_POS, x))
y = max(MIN_POS, min(MAX_POS, y))
z = max(MIN_POS, min(MAX_POS, z))
# 速度范围限制（±30 m/s）
MAX_VEL = 30.0
MIN_VEL = -30.0
vx = max(MIN_VEL, min(MAX_VEL, vx))
vy = max(MIN_VEL, min(MAX_VEL, vy))
vz = max(MIN_VEL, min(MAX_VEL, vz))
# Convert to integer values
x_int = int(x * POS_SCALE_FACTOR)
y_int = int(y * POS_SCALE_FACTOR)
z_int = int(z * POS_SCALE_FACTOR)
vx_int = int(vx * VEL_SCALE_FACTOR)
vy_int = int(vy * VEL_SCALE_FACTOR)
vz_int = int(vz * VEL_SCALE_FACTOR)
# 短整型范围检查（-32768 到 32767）
SHORT_MAX = 32767
SHORT_MIN = -32768
# 进一步限制确保在短整型范围内
x_int = max(SHORT_MIN, min(SHORT_MAX, x_int))
y_int = max(SHORT_MIN, min(SHORT_MAX, y_int))
z_int = max(SHORT_MIN, min(SHORT_MAX, z_int))
vx_int = max(SHORT_MIN, min(SHORT_MAX, vx_int))
vy_int = max(SHORT_MIN, min(SHORT_MAX, vy_int))
vz_int = max(SHORT_MIN, min(SHORT_MAX, vz_int))
# Pack data: 3 int16 (x,y,z) + 3 int16 (vx,vy,vz)
data_content = struct.pack('<hhhhhh',
x_int, y_int, z_int,
vx_int, vy_int, vz_int)
LEN = len(data_content) # Data length = 6 * 2 = 12 bytes
# Build frame
frame = bytearray()
frame.append(0xAA) # Header
frame.append(0xFF) # Target address
frame.append(0x01) # Function code
frame.append(LEN) # Data length
frame.extend(data_content)
# Calculate checksums
sum_check = 0
add_check = 0
for byte in frame:
sum_check = (sum_check + byte) & 0xFF
add_check = (add_check + sum_check) & 0xFF
# Add checksums
frame.append(sum_check)
frame.append(add_check)
# Send data
self.ser.write(frame)
return True
except Exception as e:
self.get_logger().error(f"Error sending data: {str(e)}")
return False
def filter_static_noise(self, current_pos, vx, vy, vz):
"""过滤静态噪声，当设备静止时减少位置抖动"""
# 计算速度大小
velocity_magnitude = math.sqrt(vx**2 + vy**2 + vz**2)
# 如果速度很小，认为是静止状态
if velocity_magnitude < self.velocity_threshold:
# 添加到静态位置缓冲区
self.static_position_buffer.append((current_pos.x, current_pos.y, current_pos.z))
# 保持缓冲区大小
if len(self.static_position_buffer) > self.max_static_buffer_size:
self.static_position_buffer.pop(0)
# 如果有足够的静态数据，使用平均值
if len(self.static_position_buffer) >= 5:
avg_x = sum(pos[0] for pos in self.static_position_buffer) / len(self.static_position_buffer)
avg_y = sum(pos[1] for pos in self.static_position_buffer) / len(self.static_position_buffer)
avg_z = sum(pos[2] for pos in self.static_position_buffer) / len(self.static_position_buffer)
# 创建过滤后的位置
class FilteredPos:
def __init__(self, x, y, z):
self.x = x
self.y = y
self.z = z
return FilteredPos(avg_x, avg_y, avg_z), True
else:
# 运动状态，清空缓冲区
self.static_position_buffer.clear()
return current_pos, False
def odom_callback(self, msg):
current_time = time.time()
current_pos = msg.pose.pose.position
# Calculate runtime
elapsed_time = current_time - self.start_time
# Get linear velocity components
linear_vel = msg.twist.twist.linear
vx = linear_vel.x
vy = linear_vel.y
vz = linear_vel.z
# Calculate total velocity magnitude
velocity = math.sqrt(vx**2 + vy**2 + vz**2)
# 应用静态噪声过滤
filtered_pos, is_static = self.filter_static_noise(current_pos, vx, vy, vz)
if is_static:
self.get_logger().debug(f"Static mode: using filtered position")
# 使用过滤后的位置
current_pos = filtered_pos
if self.last_position is not None:
# Calculate distance between points
dx = current_pos.x - self.last_position.x
dy = current_pos.y - self.last_position.y
dz = current_pos.z - self.last_position.z
distance = math.sqrt(dx*dx + dy*dy + dz*dz)
# Update total distance
self.total_distance += distance
# Display info every 2 seconds
if current_time - self.last_log_time > 2.0:
self.display_status(current_pos, elapsed_time, vx, vy, vz, velocity)
self.last_log_time = current_time
# 检查系统稳定性
if not self.system_stable:
# 检查启动延迟
if elapsed_time < self.startup_delay:
self.get_logger().debug(f"System warming up... {self.startup_delay - elapsed_time:.1f}s remaining")
return
# 检查数据有效性
data_is_valid = (
abs(current_pos.x) < 10.0 and abs(current_pos.y) < 10.0 and abs(current_pos.z) < 10.0 and
abs(vx) < 5.0 and abs(vy) < 5.0 and abs(vz) < 5.0 and
all(math.isfinite(val) for val in [current_pos.x, current_pos.y, current_pos.z, vx, vy, vz])
)
if data_is_valid:
self.data_valid_count += 1
if self.data_valid_count >= self.min_valid_data_count:
self.system_stable = True
self.get_logger().info("System stabilized, starting UART transmission")
else:
self.data_valid_count = 0 # 重置计数
self.get_logger().debug("Waiting for stable data...")
# Send data to STM32 every 50ms (20Hz) - 优化发送频率
if self.system_stable and current_time - self.last_sent_time > 0.05:
if self.ser and self.ser.is_open:
success = self.send_to_stm32(
current_pos.x,
current_pos.y,
current_pos.z,
vx,
vy,
vz
)
if success:
self.get_logger().info(f"Sent to STM32: X={current_pos.x:.3f}, Y={current_pos.y:.3f}, Z={current_pos.z:.3f}, Vx={vx:.3f}, Vy={vy:.3f}, Vz={vz:.3f}")
self.last_sent_time = current_time
# Save position history
self.position_history.append({
'time': current_time,
'position': current_pos,
'velocity': (vx, vy, vz),
'distance': self.total_distance
})
# Write to log file
self.log_to_file(elapsed_time, current_pos, vx, vy, vz)
# Update last position
self.last_position = current_pos
def display_status(self, position, elapsed_time, vx, vy, vz, velocity):
"""Display current status"""
self.get_logger().info("=" * 60)
self.get_logger().info(f"Runtime: {elapsed_time:.1f} sec")
self.get_logger().info(f"Position: X={position.x:.3f}m, Y={position.y:.3f}m, Z={position.z:.3f}m")
self.get_logger().info(f"Total distance: {self.total_distance:.3f} m")
self.get_logger().info(f"Velocity X: {vx:.3f} m/s")
self.get_logger().info(f"Velocity Y: {vy:.3f} m/s")
self.get_logger().info(f"Velocity Z: {vz:.3f} m/s")
self.get_logger().info(f"Total speed: {velocity:.3f} m/s")
self.get_logger().info(f"Avg speed: {self.total_distance/elapsed_time:.3f} m/s")
# Add UART status
if self.ser and self.ser.is_open:
self.get_logger().info("UART communication: Connected")
else:
self.get_logger().warning("UART communication: Disconnected")
def log_to_file(self, elapsed_time, position, vx, vy, vz):
"""Log data to file"""
timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
with open(self.log_filename, 'a', encoding='utf-8') as f:
f.write(f"{timestamp},{elapsed_time:.3f},{position.x:.6f},{position.y:.6f},{position.z:.6f},{self.total_distance:.6f},{vx:.6f},{vy:.6f},{vz:.6f}\n")
def print_summary(self):
"""Print summary information"""
if len(self.position_history) > 0:
elapsed_time = time.time() - self.start_time
print("\n" + "="*80)
print("FAST-LIO Trajectory Report")
print("="*80)
print(f"Total runtime: {elapsed_time:.1f} sec ({elapsed_time/60:.1f} min)")
print(f"Total distance: {self.total_distance:.3f} m")
print(f"Avg speed: {self.total_distance/elapsed_time:.3f} m/s")
print(f"Data points: {len(self.position_history)}")
print(f"Trajectory saved to: {self.log_filename}")
# Add UART status
if self.ser and self.ser.is_open:
print(f"UART port: {self.uart_port} @ {self.baudrate} bps")
print(f"Protocol: Anonymous (Header 0xAA, Target 0xFF, ID 0x01)")
print(f"Data format: X/Y/Z/Vx/Vy/Vz (all int16)")
else:
print("UART communication: Disconnected")
if len(self.position_history) >= 2:
start_pos = self.position_history[0]['position']
end_pos = self.position_history[-1]['position']
# Calculate straight-line distance
dx = end_pos.x - start_pos.x
dy = end_pos.y - start_pos.y
dz = end_pos.z - start_pos.z
straight_distance = math.sqrt(dx*dx + dy*dy + dz*dz)
print(f"\nStart position: ({start_pos.x:.3f}, {start_pos.y:.3f}, {start_pos.z:.3f})")
print(f"End position: ({end_pos.x:.3f}, {end_pos.y:.3f}, {end_pos.z:.3f})")
print(f"Straight distance: {straight_distance:.3f} m")
print(f"Path efficiency: {(straight_distance/self.total_distance)*100:.1f}%")
print("="*80)
# Close UART
if self.ser and self.ser.is_open:
self.ser.close()
print("UART connection closed")
def main():
print("Starting FAST-LIO distance calculator...")
print("Tip: Press Ctrl+C to stop and view report")
print(f"Initializing UART: /dev/ttyTHS1 @ 115200 bps")
rclpy.init()
calculator = DistanceCalculator()
try:
rclpy.spin(calculator)
except KeyboardInterrupt:
print("\nStopping distance calculator...")
finally:
calculator.print_summary()
calculator.destroy_node()
rclpy.shutdown()
print("Program finished")
if __name__ == '__main__':
main()

1218
laserMapping.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1069
preprocess.cpp Normal file
View File

File diff suppressed because it is too large Load Diff