commit 18c43d3b591500b669df7a3053c161350bdc3442 Author: SW1AG7 <1423255457@qq.com> Date: Wed Jul 16 18:37:38 2025 +0800 上传文件至 / diff --git a/README.md b/README.md new file mode 100644 index 0000000..e550bc5 --- /dev/null +++ b/README.md @@ -0,0 +1,716 @@ +# FAST-LIO 雷达定位系统 + +
+ +![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 定位解决方案 + +
+ +## 📋 目录 + +- [项目概述](#项目概述) +- [功能特性](#功能特性) +- [系统要求](#系统要求) +- [安装指南](#安装指南) +- [快速开始](#快速开始) +- [详细配置](#详细配置) +- [使用说明](#使用说明) +- [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 + +--- + +
+ +**⭐ 如果这个项目对您有帮助，请给我们一个星标！** + +Made with ❤️ by the FAST-LIO Community + +
\ No newline at end of file diff --git a/autostart_core.sh b/autostart_core.sh new file mode 100644 index 0000000..58c76ab --- /dev/null +++ b/autostart_core.sh @@ -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 "🏁 初始化完成，系统正在运行中..." diff --git a/distance_calculator.py b/distance_calculator.py new file mode 100644 index 0000000..da94fa0 --- /dev/null +++ b/distance_calculator.py @@ -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(' 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() diff --git a/laserMapping.cpp b/laserMapping.cpp new file mode 100644 index 0000000..64e2be4 --- /dev/null +++ b/laserMapping.cpp @@ -0,0 +1,1218 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. +// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + +// Modifier: Livox dev@livoxtech.com + +// Copyright 2013, Ji Zhang, Carnegie Mellon University +// Further contributions copyright (c) 2016, Southwest Research Institute +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from this +// software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "IMU_Processing.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "preprocess.h" +#include + +#define INIT_TIME (0.1) +#define LASER_POINT_COV (0.001) +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) + +/*** Time Log Variables ***/ +double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; +double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; +double match_time = 0, solve_time = 0, solve_const_H_time = 0; +int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; +bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true; +/**************************/ + +float res_last[100000] = {0.0}; +float DET_RANGE = 300.0f; +const float MOV_THRESHOLD = 1.5f; +double time_diff_lidar_to_imu = 0.0; + +mutex mtx_buffer; +condition_variable sig_buffer; + +string root_dir = ROOT_DIR; +string map_file_path, lid_topic, imu_topic; + +double res_mean_last = 0.05, total_residual = 0.0; +double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; +double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; +double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; +double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0; +int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0; +int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = -1, pcd_index = 0; +bool point_selected_surf[100000] = {0}; +bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited; +bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; +bool is_first_lidar = true; + +vector> pointSearchInd_surf; +vector cub_needrm; +vector Nearest_Points; +vector extrinT(3, 0.0); +vector extrinR(9, 0.0); +deque time_buffer; +deque lidar_buffer; +deque imu_buffer; + +PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr _featsArray; + +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; + +KD_TREE ikdtree; + +V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); +V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); +V3D euler_cur; +V3D position_last(Zero3d); +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); + +/*** EKF inputs and output ***/ +MeasureGroup Measures; +esekfom::esekf kf; +state_ikfom state_point; +vect3 pos_lid; + +nav_msgs::msg::Path path; +nav_msgs::msg::Odometry odomAftMapped; +geometry_msgs::msg::Quaternion geoQuat; +geometry_msgs::msg::PoseStamped msg_body_pose; + +shared_ptr p_pre(new Preprocess()); +shared_ptr p_imu(new ImuProcess()); + +void SigHandle(int sig) +{ + flg_exit = true; + std::cout << "catch sig %d" << sig << std::endl; + sig_buffer.notify_all(); + rclcpp::shutdown(); +} + +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang(Log(state_point.rot.toRotationMatrix())); + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle + fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega + fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc + fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g + fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a + fprintf(fp, "\r\n"); + fflush(fp); +} + +void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + + +void pointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +template +void pointBodyToWorld(const Matrix &pi, Matrix &po) +{ + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po[0] = p_global(0); + po[1] = p_global(1); + po[2] = p_global(2); +} + +void RGBpointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po) +{ + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I); + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + po->intensity = pi->intensity; +} + +void points_cache_collect() +{ + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + // for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); +} + +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; +void lasermap_fov_segment() +{ + cub_needrm.clear(); + kdtree_delete_counter = 0; + kdtree_delete_time = 0.0; + pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); + V3D pos_LiD = pos_lid; + if (!Localmap_Initialized){ + for (int i = 0; i < 3; i++){ + LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; + LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; + } + Localmap_Initialized = true; + return; + } + float dist_to_map_edge[3][2]; + bool need_move = false; + for (int i = 0; i < 3; i++){ + dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; + } + if (!need_move) return; + BoxPointType New_LocalMap_Points, tmp_boxpoints; + New_LocalMap_Points = LocalMap_Points; + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1))); + for (int i = 0; i < 3; i++){ + tmp_boxpoints = LocalMap_Points; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] -= mov_dist; + New_LocalMap_Points.vertex_min[i] -= mov_dist; + tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; + cub_needrm.push_back(tmp_boxpoints); + } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] += mov_dist; + New_LocalMap_Points.vertex_min[i] += mov_dist; + tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; + cub_needrm.push_back(tmp_boxpoints); + } + } + LocalMap_Points = New_LocalMap_Points; + + points_cache_collect(); + double delete_begin = omp_get_wtime(); + if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); + kdtree_delete_time = omp_get_wtime() - delete_begin; +} + +void standard_pcl_cbk(const sensor_msgs::msg::PointCloud2::UniquePtr msg) +{ + mtx_buffer.lock(); + scan_count ++; + double cur_time = get_time_sec(msg->header.stamp); + double preprocess_start_time = omp_get_wtime(); + if (!is_first_lidar && cur_time < last_timestamp_lidar) + { + std::cerr << "lidar loop back, clear buffer" << std::endl; + lidar_buffer.clear(); + } + if (is_first_lidar) + { + is_first_lidar = false; + } + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(cur_time); + last_timestamp_lidar = cur_time; + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +double timediff_lidar_wrt_imu = 0.0; +bool timediff_set_flg = false; +void livox_pcl_cbk(const livox_ros_driver2::msg::CustomMsg::UniquePtr msg) +{ + mtx_buffer.lock(); + double cur_time = get_time_sec(msg->header.stamp); + double preprocess_start_time = omp_get_wtime(); + scan_count ++; + if (!is_first_lidar && cur_time < last_timestamp_lidar) + { + std::cerr << "lidar loop back, clear buffer" << std::endl; + lidar_buffer.clear(); + } + if(is_first_lidar) + { + is_first_lidar = false; + } + last_timestamp_lidar = cur_time; + + if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() ) + { + printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar); + } + + if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty()) + { + timediff_set_flg = true; + timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; + printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); + } + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(last_timestamp_lidar); + + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void imu_cbk(const sensor_msgs::msg::Imu::UniquePtr msg_in) +{ + publish_count ++; + // cout<<"IMU got at: "<header.stamp.toSec()<header.stamp = get_ros_time(get_time_sec(msg_in->header.stamp) - time_diff_lidar_to_imu); + if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en) + { + msg->header.stamp = \ + rclcpp::Time(timediff_lidar_wrt_imu + get_time_sec(msg_in->header.stamp)); + } + + double timestamp = get_time_sec(msg->header.stamp); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + std::cerr << "lidar loop back, clear buffer" << std::endl; + imu_buffer.clear(); + } + + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +double lidar_mean_scantime = 0.0; +int scan_num = 0; +bool sync_packages(MeasureGroup &meas) +{ + if (lidar_buffer.empty() || imu_buffer.empty()) { + return false; + } + + /*** push a lidar scan ***/ + if(!lidar_pushed) + { + meas.lidar = lidar_buffer.front(); + meas.lidar_beg_time = time_buffer.front(); + if (meas.lidar->points.size() <= 1) // time too little + { + lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; + std::cerr << "Too few input point cloud!\n"; + } + else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) + { + lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; + } + else + { + scan_num ++; + lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); + lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num; + } + + meas.lidar_end_time = lidar_end_time; + + lidar_pushed = true; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + + /*** push imu data, and pop from imu buffer ***/ + double imu_time = get_time_sec(imu_buffer.front()->header.stamp); + meas.imu.clear(); + while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) + { + imu_time = get_time_sec(imu_buffer.front()->header.stamp); + if(imu_time > lidar_end_time) break; + meas.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); + } + + lidar_buffer.pop_front(); + time_buffer.pop_front(); + lidar_pushed = false; + return true; +} + +int process_increments = 0; +void map_incremental() +{ + PointVector PointToAdd; + PointVector PointNoNeedDownsample; + PointToAdd.reserve(feats_down_size); + PointNoNeedDownsample.reserve(feats_down_size); + for (int i = 0; i < feats_down_size; i++) + { + /* transform to world frame */ + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + /* decide if need add to map */ + if (!Nearest_Points[i].empty() && flg_EKF_inited) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + float dist = calc_dist(feats_down_world->points[i],mid_point); + if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){ + PointNoNeedDownsample.push_back(feats_down_world->points[i]); + continue; + } + for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++) + { + if (points_near.size() < NUM_MATCH_POINTS) break; + if (calc_dist(points_near[readd_i], mid_point) < dist) + { + need_add = false; + break; + } + } + if (need_add) PointToAdd.push_back(feats_down_world->points[i]); + } + else + { + PointToAdd.push_back(feats_down_world->points[i]); + } + } + + double st_time = omp_get_wtime(); + add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); + add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); + kdtree_incremental_time = omp_get_wtime() - st_time; +} + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI()); +PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); +void publish_frame_world(rclcpp::Publisher::SharedPtr pubLaserCloudFull) +{ + if(scan_pub_en) + { + PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], \ + &laserCloudWorld->points[i]); + } + + sensor_msgs::msg::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); + // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFull->publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + /* + if (pcd_save_en) + { + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&feats_undistort->points[i], \ + &laserCloudWorld->points[i]); + } + *pcl_wait_save += *laserCloudWorld; + + static int scan_wait_num = 0; + scan_wait_num ++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index ++; + string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << all_points_dir << endl; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + pcl_wait_save->clear(); + scan_wait_num = 0; + } + } + */ +} + +void publish_frame_body(rclcpp::Publisher::SharedPtr pubLaserCloudFull_body) +{ + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyLidarToIMU(&feats_undistort->points[i], \ + &laserCloudIMUBody->points[i]); + } + + sensor_msgs::msg::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); + laserCloudmsg.header.frame_id = "body"; + pubLaserCloudFull_body->publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; +} + +void publish_effect_world(rclcpp::Publisher::SharedPtr pubLaserCloudEffect) +{ + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(effct_feat_num, 1)); + for (int i = 0; i < effct_feat_num; i++) + { + RGBpointBodyToWorld(&laserCloudOri->points[i], \ + &laserCloudWorld->points[i]); + } + sensor_msgs::msg::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = get_ros_time(lidar_end_time); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect->publish(laserCloudFullRes3); +} + +void publish_map(rclcpp::Publisher::SharedPtr pubLaserCloudMap) +{ + PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], \ + &laserCloudWorld->points[i]); + } + *pcl_wait_pub += *laserCloudWorld; + + sensor_msgs::msg::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*pcl_wait_pub, laserCloudmsg); + // laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.stamp = get_ros_time(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudMap->publish(laserCloudmsg); + + // sensor_msgs::msg::PointCloud2 laserCloudMap; + // pcl::toROSMsg(*featsFromMap, laserCloudMap); + // laserCloudMap.header.stamp = get_ros_time(lidar_end_time); + // laserCloudMap.header.frame_id = "camera_init"; + // pubLaserCloudMap->publish(laserCloudMap); +} + +void save_to_pcd() +{ + pcl::PCDWriter pcd_writer; + pcd_writer.writeBinary(map_file_path, *pcl_wait_pub); +} + +template +void set_posestamp(T & out) +{ + out.pose.position.x = state_point.pos(0); + out.pose.position.y = state_point.pos(1); + out.pose.position.z = state_point.pos(2); + out.pose.orientation.x = geoQuat.x; + out.pose.orientation.y = geoQuat.y; + out.pose.orientation.z = geoQuat.z; + out.pose.orientation.w = geoQuat.w; + +} + +void publish_odometry(const rclcpp::Publisher::SharedPtr pubOdomAftMapped, std::unique_ptr & tf_br) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "body"; + odomAftMapped.header.stamp = get_ros_time(lidar_end_time); + set_posestamp(odomAftMapped.pose); + + // 添加速度数据设置 - 修复速度为0的问题 + odomAftMapped.twist.twist.linear.x = state_point.vel(0); + odomAftMapped.twist.twist.linear.y = state_point.vel(1); + odomAftMapped.twist.twist.linear.z = state_point.vel(2); + odomAftMapped.twist.twist.angular.x = 0.0; // 角速度暂时设为0 + odomAftMapped.twist.twist.angular.y = 0.0; + odomAftMapped.twist.twist.angular.z = 0.0; + + // 设置速度协方差矩阵 + for(int i = 0; i < 36; i++) { + odomAftMapped.twist.covariance[i] = 0.0; + } + odomAftMapped.twist.covariance[0] = 0.1; // x线速度方差 + odomAftMapped.twist.covariance[7] = 0.1; // y线速度方差 + odomAftMapped.twist.covariance[14] = 0.1; // z线速度方差 + odomAftMapped.twist.covariance[21] = 0.1; // x角速度方差 + odomAftMapped.twist.covariance[28] = 0.1; // y角速度方差 + odomAftMapped.twist.covariance[35] = 0.1; // z角速度方差 + + pubOdomAftMapped->publish(odomAftMapped); + auto P = kf.get_P(); + for (int i = 0; i < 6; i ++) + { + int k = i < 3 ? i + 3 : i - 3; + odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); + odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); + odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); + odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); + odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); + odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); + } + + geometry_msgs::msg::TransformStamped trans; + trans.header.frame_id = "camera_init"; + trans.child_frame_id = "body"; + trans.header.stamp = get_ros_time(lidar_end_time); + trans.transform.translation.x = odomAftMapped.pose.pose.position.x; + trans.transform.translation.y = odomAftMapped.pose.pose.position.y; + trans.transform.translation.z = odomAftMapped.pose.pose.position.z; + trans.transform.rotation.w = odomAftMapped.pose.pose.orientation.w; + trans.transform.rotation.x = odomAftMapped.pose.pose.orientation.x; + trans.transform.rotation.y = odomAftMapped.pose.pose.orientation.y; + trans.transform.rotation.z = odomAftMapped.pose.pose.orientation.z; + tf_br->sendTransform(trans); +} + +void publish_path(rclcpp::Publisher::SharedPtr pubPath) +{ + set_posestamp(msg_body_pose); + msg_body_pose.header.stamp = get_ros_time(lidar_end_time); // ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.frame_id = "camera_init"; + + /*** if path is too large, the rvis will crash ***/ + static int jjj = 0; + jjj++; + if (jjj % 10 == 0) + { + path.poses.push_back(msg_body_pose); + pubPath->publish(path); + } +} + +void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_data) +{ + double match_start = omp_get_wtime(); + laserCloudOri->clear(); + corr_normvect->clear(); + total_residual = 0.0; + + /** closest surface search and residual computation **/ + #ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); + #pragma omp parallel for + #endif + for (int i = 0; i < feats_down_size; i++) + { + PointType &point_body = feats_down_body->points[i]; + PointType &point_world = feats_down_world->points[i]; + + /* transform to world frame */ + V3D p_body(point_body.x, point_body.y, point_body.z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + point_world.x = p_global(0); + point_world.y = p_global(1); + point_world.z = p_global(2); + point_world.intensity = point_body.intensity; + + vector pointSearchSqDis(NUM_MATCH_POINTS); + + auto &points_near = Nearest_Points[i]; + + if (ekfom_data.converge) + { + /** Find the closest surfaces in the map **/ + ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis); + point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true; + } + + if (!point_selected_surf[i]) continue; + + VF(4) pabcd; + point_selected_surf[i] = false; + if (esti_plane(pabcd, points_near, 0.1f)) + { + float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); + float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); + + if (s > 0.9) + { + point_selected_surf[i] = true; + normvec->points[i].x = pabcd(0); + normvec->points[i].y = pabcd(1); + normvec->points[i].z = pabcd(2); + normvec->points[i].intensity = pd2; + res_last[i] = abs(pd2); + } + } + } + + effct_feat_num = 0; + + for (int i = 0; i < feats_down_size; i++) + { + if (point_selected_surf[i]) + { + laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; + corr_normvect->points[effct_feat_num] = normvec->points[i]; + total_residual += res_last[i]; + effct_feat_num ++; + } + } + + if (effct_feat_num < 1) + { + ekfom_data.valid = false; + std::cerr << "No Effective Points!" << std::endl; + // ROS_WARN("No Effective Points! \n"); + return; + } + + res_mean_last = total_residual / effct_feat_num; + match_time += omp_get_wtime() - match_start; + double solve_start_ = omp_get_wtime(); + + /*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/ + ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23 + ekfom_data.h.resize(effct_feat_num); + + for (int i = 0; i < effct_feat_num; i++) + { + const PointType &laser_p = laserCloudOri->points[i]; + V3D point_this_be(laser_p.x, laser_p.y, laser_p.z); + M3D point_be_crossmat; + point_be_crossmat << SKEW_SYM_MATRX(point_this_be); + V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I; + M3D point_crossmat; + point_crossmat<points[i]; + V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); + + /*** calculate the Measuremnt Jacobian matrix H ***/ + V3D C(s.rot.conjugate() *norm_vec); + V3D A(point_crossmat * C); + if (extrinsic_est_en) + { + V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec); + ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + + /*** Measuremnt: distance to the closest surface/corner ***/ + ekfom_data.h(i) = -norm_p.intensity; + } + solve_time += omp_get_wtime() - solve_start_; +} + +class LaserMappingNode : public rclcpp::Node +{ +public: + LaserMappingNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) : Node("laser_mapping", options) + { + this->declare_parameter("publish.path_en", true); + this->declare_parameter("publish.effect_map_en", false); + this->declare_parameter("publish.map_en", false); + this->declare_parameter("publish.scan_publish_en", true); + this->declare_parameter("publish.dense_publish_en", true); + this->declare_parameter("publish.scan_bodyframe_pub_en", true); + this->declare_parameter("max_iteration", 4); + this->declare_parameter("map_file_path", ""); + this->declare_parameter("common.lid_topic", "/livox/lidar"); + this->declare_parameter("common.imu_topic", "/livox/imu"); + this->declare_parameter("common.time_sync_en", false); + this->declare_parameter("common.time_offset_lidar_to_imu", 0.0); + this->declare_parameter("filter_size_corner", 0.5); + this->declare_parameter("filter_size_surf", 0.5); + this->declare_parameter("filter_size_map", 0.5); + this->declare_parameter("cube_side_length", 200.); + this->declare_parameter("mapping.det_range", 300.); + this->declare_parameter("mapping.fov_degree", 180.); + this->declare_parameter("mapping.gyr_cov", 0.1); + this->declare_parameter("mapping.acc_cov", 0.1); + this->declare_parameter("mapping.b_gyr_cov", 0.0001); + this->declare_parameter("mapping.b_acc_cov", 0.0001); + this->declare_parameter("preprocess.blind", 0.01); + this->declare_parameter("preprocess.lidar_type", AVIA); + this->declare_parameter("preprocess.scan_line", 16); + this->declare_parameter("preprocess.timestamp_unit", US); + this->declare_parameter("preprocess.scan_rate", 10); + this->declare_parameter("point_filter_num", 2); + this->declare_parameter("feature_extract_enable", false); + this->declare_parameter("runtime_pos_log_enable", false); + this->declare_parameter("mapping.extrinsic_est_en", true); + this->declare_parameter("pcd_save.pcd_save_en", false); + this->declare_parameter("pcd_save.interval", -1); + this->declare_parameter>("mapping.extrinsic_T", vector()); + this->declare_parameter>("mapping.extrinsic_R", vector()); + + this->get_parameter_or("publish.path_en", path_en, true); + this->get_parameter_or("publish.effect_map_en", effect_pub_en, false); + this->get_parameter_or("publish.map_en", map_pub_en, false); + this->get_parameter_or("publish.scan_publish_en", scan_pub_en, true); + this->get_parameter_or("publish.dense_publish_en", dense_pub_en, true); + this->get_parameter_or("publish.scan_bodyframe_pub_en", scan_body_pub_en, true); + this->get_parameter_or("max_iteration", NUM_MAX_ITERATIONS, 4); + this->get_parameter_or("map_file_path", map_file_path, ""); + this->get_parameter_or("common.lid_topic", lid_topic, "/livox/lidar"); + this->get_parameter_or("common.imu_topic", imu_topic,"/livox/imu"); + this->get_parameter_or("common.time_sync_en", time_sync_en, false); + this->get_parameter_or("common.time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); + this->get_parameter_or("filter_size_corner",filter_size_corner_min,0.5); + this->get_parameter_or("filter_size_surf",filter_size_surf_min,0.5); + this->get_parameter_or("filter_size_map",filter_size_map_min,0.5); + this->get_parameter_or("cube_side_length",cube_len,200.f); + this->get_parameter_or("mapping.det_range",DET_RANGE,300.f); + this->get_parameter_or("mapping.fov_degree",fov_deg,180.f); + this->get_parameter_or("mapping.gyr_cov",gyr_cov,0.1); + this->get_parameter_or("mapping.acc_cov",acc_cov,0.1); + this->get_parameter_or("mapping.b_gyr_cov",b_gyr_cov,0.0001); + this->get_parameter_or("mapping.b_acc_cov",b_acc_cov,0.0001); + this->get_parameter_or("preprocess.blind", p_pre->blind, 0.01); + this->get_parameter_or("preprocess.lidar_type", p_pre->lidar_type, AVIA); + this->get_parameter_or("preprocess.scan_line", p_pre->N_SCANS, 16); + this->get_parameter_or("preprocess.timestamp_unit", p_pre->time_unit, US); + this->get_parameter_or("preprocess.scan_rate", p_pre->SCAN_RATE, 10); + this->get_parameter_or("point_filter_num", p_pre->point_filter_num, 2); + this->get_parameter_or("feature_extract_enable", p_pre->feature_enabled, false); + this->get_parameter_or("runtime_pos_log_enable", runtime_pos_log, 0); + this->get_parameter_or("mapping.extrinsic_est_en", extrinsic_est_en, true); + this->get_parameter_or("pcd_save.pcd_save_en", pcd_save_en, false); + this->get_parameter_or("pcd_save.interval", pcd_save_interval, -1); + this->get_parameter_or>("mapping.extrinsic_T", extrinT, vector()); + this->get_parameter_or>("mapping.extrinsic_R", extrinR, vector()); + + RCLCPP_INFO(this->get_logger(), "p_pre->lidar_type %d", p_pre->lidar_type); + + path.header.stamp = this->get_clock()->now(); + path.header.frame_id ="camera_init"; + + // /*** variables definition ***/ + // int effect_feat_num = 0, frame_num = 0; + // double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0; + // bool flg_EKF_converged, EKF_stop_flg = 0; + + FOV_DEG = (fov_deg + 10.0) > 179.9 ? 179.9 : (fov_deg + 10.0); + HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); + + _featsArray.reset(new PointCloudXYZI()); + + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); + + Lidar_T_wrt_IMU<set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); + p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); + p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); + + fill(epsi, epsi+23, 0.001); + kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); + + /*** debug record ***/ + // FILE *fp; + string pos_log_dir = root_dir + "/Log/pos_log.txt"; + fp = fopen(pos_log_dir.c_str(),"w"); + + // ofstream fout_pre, fout_out, fout_dbg; + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); + fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); + if (fout_pre && fout_out) + cout << "~~~~"<lidar_type == AVIA) + { + sub_pcl_livox_ = this->create_subscription(lid_topic, 20, livox_pcl_cbk); + } + else + { + sub_pcl_pc_ = this->create_subscription(lid_topic, rclcpp::SensorDataQoS(), standard_pcl_cbk); + } + sub_imu_ = this->create_subscription(imu_topic, 10, imu_cbk); + pubLaserCloudFull_ = this->create_publisher("/cloud_registered", 20); + pubLaserCloudFull_body_ = this->create_publisher("/cloud_registered_body", 20); + pubLaserCloudEffect_ = this->create_publisher("/cloud_effected", 20); + pubLaserCloudMap_ = this->create_publisher("/Laser_map", 20); + pubOdomAftMapped_ = this->create_publisher("/Odometry", 20); + pubPath_ = this->create_publisher("/path", 20); + tf_broadcaster_ = std::make_unique(*this); + + //------------------------------------------------------------------------------------------------------ + auto period_ms = std::chrono::milliseconds(static_cast(1000.0 / 100.0)); + timer_ = rclcpp::create_timer(this, this->get_clock(), period_ms, std::bind(&LaserMappingNode::timer_callback, this)); + + auto map_period_ms = std::chrono::milliseconds(static_cast(1000.0)); + map_pub_timer_ = rclcpp::create_timer(this, this->get_clock(), map_period_ms, std::bind(&LaserMappingNode::map_publish_callback, this)); + + map_save_srv_ = this->create_service("map_save", std::bind(&LaserMappingNode::map_save_callback, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Node init finished."); + } + + ~LaserMappingNode() + { + fout_out.close(); + fout_pre.close(); + fclose(fp); + } + +private: + void timer_callback() + { + if(sync_packages(Measures)) + { + if (flg_first_scan) + { + first_lidar_time = Measures.lidar_beg_time; + p_imu->first_lidar_time = first_lidar_time; + flg_first_scan = false; + return; + } + + double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; + + match_time = 0; + kdtree_search_time = 0.0; + solve_time = 0; + solve_const_H_time = 0; + svd_time = 0; + t0 = omp_get_wtime(); + + p_imu->Process(Measures, kf, feats_undistort); + state_point = kf.get_x(); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + + if (feats_undistort->empty() || (feats_undistort == NULL)) + { + RCLCPP_WARN(this->get_logger(), "No point, skip this scan!\n"); + return; + } + + flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \ + false : true; + /*** Segment the map in lidar FOV ***/ + lasermap_fov_segment(); + + /*** downsample the feature points in a scan ***/ + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + t1 = omp_get_wtime(); + feats_down_size = feats_down_body->points.size(); + /*** initialize the map kdtree ***/ + if(ikdtree.Root_Node == nullptr) + { + RCLCPP_INFO(this->get_logger(), "Initialize the map kdtree"); + if(feats_down_size > 5) + { + ikdtree.set_downsample_param(filter_size_map_min); + feats_down_world->resize(feats_down_size); + for(int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + ikdtree.Build(feats_down_world->points); + } + return; + } + int featsFromMapNum = ikdtree.validnum(); + kdtree_size_st = ikdtree.size(); + + // cout<<"[ mapping ]: In num: "<points.size()<<" downsamp "<get_logger(), "No point, skip this scan!\n"); + return; + } + + normvec->resize(feats_down_size); + feats_down_world->resize(feats_down_size); + + V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_pre<clear(); + featsFromMap->points = ikdtree.PCL_Storage; + } + + pointSearchInd_surf.resize(feats_down_size); + Nearest_Points.resize(feats_down_size); + int rematch_num = 0; + bool nearest_search_en = true; // + + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ + double t_update_start = omp_get_wtime(); + double solve_H_time = 0; + kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); + state_point = kf.get_x(); + euler_cur = SO3ToEuler(state_point.rot); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + geoQuat.x = state_point.rot.coeffs()[0]; + geoQuat.y = state_point.rot.coeffs()[1]; + geoQuat.z = state_point.rot.coeffs()[2]; + geoQuat.w = state_point.rot.coeffs()[3]; + + double t_update_end = omp_get_wtime(); + + /******* Publish odometry *******/ + publish_odometry(pubOdomAftMapped_, tf_broadcaster_); + + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + map_incremental(); + t5 = omp_get_wtime(); + + /******* Publish points *******/ + if (path_en) publish_path(pubPath_); + if (scan_pub_en) publish_frame_world(pubLaserCloudFull_); + if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body_); + if (effect_pub_en) publish_effect_world(pubLaserCloudEffect_); + // if (map_pub_en) publish_map(pubLaserCloudMap_); + + /*** Debug variables ***/ + if (runtime_pos_log) + { + frame_num ++; + kdtree_size_end = ikdtree.size(); + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; + aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num; + aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num; + aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num; + aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num; + T1[time_log_counter] = Measures.lidar_beg_time; + s_plot[time_log_counter] = t5 - t0; + s_plot2[time_log_counter] = feats_undistort->points.size(); + s_plot3[time_log_counter] = kdtree_incremental_time; + s_plot4[time_log_counter] = kdtree_search_time; + s_plot5[time_log_counter] = kdtree_delete_counter; + s_plot6[time_log_counter] = kdtree_delete_time; + s_plot7[time_log_counter] = kdtree_size_st; + s_plot8[time_log_counter] = kdtree_size_end; + s_plot9[time_log_counter] = aver_time_consu; + s_plot10[time_log_counter] = add_point_size; + time_log_counter ++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time); + ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<points.size()<get_logger(), "Saving map to %s...", map_file_path.c_str()); + if (pcd_save_en) + { + save_to_pcd(); + res->success = true; + res->message = "Map saved."; + } + else + { + res->success = false; + res->message = "Map save disabled."; + } + } + +private: + rclcpp::Publisher::SharedPtr pubLaserCloudFull_; + rclcpp::Publisher::SharedPtr pubLaserCloudFull_body_; + rclcpp::Publisher::SharedPtr pubLaserCloudEffect_; + rclcpp::Publisher::SharedPtr pubLaserCloudMap_; + rclcpp::Publisher::SharedPtr pubOdomAftMapped_; + rclcpp::Publisher::SharedPtr pubPath_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_pcl_pc_; + rclcpp::Subscription::SharedPtr sub_pcl_livox_; + + std::unique_ptr tf_broadcaster_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr map_pub_timer_; + rclcpp::Service::SharedPtr map_save_srv_; + + bool effect_pub_en = false, map_pub_en = false; + int effect_feat_num = 0, frame_num = 0; + double deltaT, deltaR, aver_time_consu = 0, aver_time_icp = 0, aver_time_match = 0, aver_time_incre = 0, aver_time_solve = 0, aver_time_const_H_time = 0; + bool flg_EKF_converged, EKF_stop_flg = 0; + double epsi[23] = {0.001}; + + FILE *fp; + ofstream fout_pre, fout_out, fout_dbg; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + signal(SIGINT, SigHandle); + + rclcpp::spin(std::make_shared()); + + if (rclcpp::ok()) + rclcpp::shutdown(); + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. pcd save will largely influence the real-time performences **/ + if (pcl_wait_save->size() > 0 && pcd_save_en) + { + string file_name = string("scans.pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << file_name< t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; + FILE *fp2; + string log_dir = root_dir + "/Log/fast_lio_time_log.csv"; + fp2 = fopen(log_dir.c_str(),"w"); + fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n"); + for (int i = 0;i + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() : feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + SCAN_RATE = 10; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid = 6.25; + limit_midmin = 6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit / 180 * M_PI); + jump_down_limit = cos(jump_down_limit / 180 * M_PI); + cos160 = cos(cos160 / 180 * M_PI); + smallp_intersect = cos(smallp_intersect / 180 * M_PI); +} + +Preprocess::~Preprocess() +{ +} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + feature_enabled = feat_en; + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +void Preprocess::process(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg, PointCloudXYZI::Ptr& pcl_out) +{ + avia_handler(msg); + *pcl_out = pl_surf; +} + +void Preprocess::process(const sensor_msgs::msg::PointCloud2::UniquePtr &msg, PointCloudXYZI::Ptr& pcl_out) +{ + switch (time_unit) + { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case MID360: + mid360_handler(msg); + break; + + default: + default_handler(msg); + break; + } + *pcl_out = pl_surf; +} + +void Preprocess::avia_handler(const livox_ros_driver2::msg::CustomMsg::UniquePtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + double t1 = omp_get_wtime(); + int plsize = msg->point_num; + // cout<<"plsie: "<points[i].line < N_SCANS) && + ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = + msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points + + bool is_new = false; + if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || + (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) + { + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + } + static int count = 0; + static double time = 0.0; + count++; + double t0 = omp_get_wtime(); + for (int j = 0; j < N_SCANS; j++) + { + if (pl_buff[j].size() <= 5) + continue; + pcl::PointCloud& pl = pl_buff[j]; + plsize = pl.size(); + vector& types = typess[j]; + types.clear(); + types.resize(plsize); + plsize--; + for (uint i = 0; i < plsize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = sqrt(vx * vx + vy * vy + vz * vz); + } + types[plsize].range = sqrt(pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y); + give_feature(pl, types); + // pl_surf += pl; + } + time += omp_get_wtime() - t0; + printf("Feature extraction time: %lf \n", time / count); + } + else + { + for (uint i = 1; i < plsize; i++) + { + if ((msg->points[i].line < N_SCANS) && + ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + valid_num++; + if (valid_num % point_filter_num == 0) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / + float(1000000); // use curvature as time of each laser points, curvature unit: ms + + if (((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) + && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) + { + pl_surf.push_back(pl_full[i]); + } + } + } + } + } +} + +void Preprocess::oust64_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (uint i = 0; i < plsize; i++) + { + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < (blind * blind)) + continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) + yaw_angle -= 360.0; + if (yaw_angle <= -180.0) + yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; + if (pl_orig.points[i].ring < N_SCANS) + { + pl_buff[pl_orig.points[i].ring].push_back(added_pt); + } + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI& pl = pl_buff[j]; + int linesize = pl.size(); + vector& types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + double time_stamp = rclcpp::Time(msg->header.stamp).seconds(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) + continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) + continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::velodyne_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) + return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS, true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].time > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) + continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI& pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) + continue; + vector& types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + } +} + +void Preprocess::mid360_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) + return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS, true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].line; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].line == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + + for (uint i = 0; i < plsize; ++i) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].reflectivity; + added_pt.curvature = 0.; + + int layer = pl_orig.points[i].line; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + // compute offset time + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) + { + pl_surf.push_back(std::move(added_pt)); + } + } +} + +void Preprocess::default_handler(const sensor_msgs::msg::PointCloud2::UniquePtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) + return; + pl_surf.reserve(plsize); + + for(uint i = 0; i < plsize; ++i) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = 0.; + + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind * blind)) + { + pl_surf.push_back(std::move(added_pt)); + } + } +} + +void Preprocess::give_feature(pcl::PointCloud& pl, vector& types) +{ + int plsize = pl.size(); + int plsize2; + if (plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while (types[head].range < blind) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; + uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for (uint i = head; i < plsize2; i++) + { + if (types[i].range < blind) + { + continue; + } + + i2 = i; + + plane_type = plane_judge(pl, types, i, i_nex, curr_direct); + + if (plane_type == 1) + { + for (uint j = i; j <= i_nex; j++) + { + if (j != i && j != i_nex) + { + types[j].ftype = Real_Plane; + } + else + { + types[j].ftype = Poss_Plane; + } + } + + // if(last_state==1 && fabs(last_direct.sum())>0.5) + if (last_state == 1 && last_direct.norm() > 0.1) + { + double mod = last_direct.transpose() * curr_direct; + if (mod > -0.707 && mod < 0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + // else if(plane_type == 0) + // { + // if(last_state == 1) + // { + // uint i_nex_tem; + // uint j; + // for(j=last_i+1; j<=last_i_nex; j++) + // { + // uint i_nex_tem2 = i_nex_tem; + // Eigen::Vector3d curr_direct2; + + // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + // if(ttem != 1) + // { + // i_nex_tem = i_nex_tem2; + // break; + // } + // curr_direct = curr_direct2; + // } + + // if(j == last_i+1) + // { + // last_state = 0; + // } + // else + // { + // for(uint k=last_i_nex; k<=i_nex_tem; k++) + // { + // if(k != i_nex_tem) + // { + // types[k].ftype = Real_Plane; + // } + // else + // { + // types[k].ftype = Poss_Plane; + // } + // } + // i = i_nex_tem-1; + // i_nex = i_nex_tem; + // i2 = j-1; + // last_state = 1; + // } + + // } + // } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for (uint i = head + 3; i < plsize2; i++) + { + if (types[i].range < blind || types[i].ftype >= Real_Plane) + { + continue; + } + + if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16) + { + continue; + } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for (int j = 0; j < 2; j++) + { + int m = -1; + if (j == 1) + { + m = 1; + } + + if (types[i + m].range < blind) + { + if (types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if (types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if (types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && + types[i].dista > 4 * types[i - 1].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && + types[i - 1].dista > 4 * types[i].dista) + { + if (types[i].intersect > cos160) + { + if (edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf) + { + if (edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor) + { + if (edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor) + { + if (types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } + + plsize2 = plsize - 1; + double ratio; + for (uint i = head + 1; i < plsize2; i++) + { + if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind) + { + continue; + } + + if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8) + { + continue; + } + + if (types[i].ftype == Nor) + { + if (types[i - 1].dista > types[i].dista) + { + ratio = types[i - 1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i - 1].dista; + } + + if (types[i].intersect < smallp_intersect && ratio < smallp_ratio) + { + if (types[i - 1].ftype == Nor) + { + types[i - 1].ftype = Real_Plane; + } + if (types[i + 1].ftype == Nor) + { + types[i + 1].ftype = Real_Plane; + } + types[i].ftype = Real_Plane; + } + } + } + + int last_surface = -1; + for (uint j = head; j < plsize; j++) + { + if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane) + { + if (last_surface == -1) + { + last_surface = j; + } + + if (j == uint(last_surface + point_filter_num - 1)) + { + PointType ap; + ap.x = pl[j].x; + ap.y = pl[j].y; + ap.z = pl[j].z; + ap.intensity = pl[j].intensity; + ap.curvature = pl[j].curvature; + pl_surf.push_back(ap); + + last_surface = -1; + } + } + else + { + if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane) + { + pl_corn.push_back(pl[j]); + } + if (last_surface != -1) + { + PointType ap; + for (uint k = last_surface; k < j; k++) + { + ap.x += pl[k].x; + ap.y += pl[k].y; + ap.z += pl[k].z; + ap.intensity += pl[k].intensity; + ap.curvature += pl[k].curvature; + } + ap.x /= (j - last_surface); + ap.y /= (j - last_surface); + ap.z /= (j - last_surface); + ap.intensity /= (j - last_surface); + ap.curvature /= (j - last_surface); + pl_surf.push_back(ap); + } + last_surface = -1; + } + } +} + +void Preprocess::pub_func(PointCloudXYZI& pl, const rclcpp::Time& ct) +{ + pl.height = 1; + pl.width = pl.size(); + sensor_msgs::msg::PointCloud2 output; + pcl::toROSMsg(pl, output); + output.header.frame_id = "livox"; + output.header.stamp = ct; +} + +int Preprocess::plane_judge(const PointCloudXYZI& pl, vector& types, uint i_cur, uint& i_nex, + Eigen::Vector3d& curr_direct) +{ + double group_dis = disA * types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++) + { + if (types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + disarr.push_back(types[i_nex].dista); + } + + for (;;) + { + if ((i_cur >= pl.size()) || (i_nex >= pl.size())) + break; + + if (types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx * vx + vy * vy + vz * vz; + if (two_dis >= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for (uint j = i_cur + 1; j < i_nex; j++) + { + if ((j >= pl.size()) || (i_cur >= pl.size())) + break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1] * vz - vy * v1[2]; + v2[1] = v1[2] * vx - v1[0] * vz; + v2[2] = v1[0] * vy - vx * v1[1]; + + double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]; + if (lw > leng_wid) + { + leng_wid = lw; + } + } + + if ((two_dis * two_dis / leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for (uint j = 0; j < disarrsize - 1; j++) + { + for (uint k = j + 1; k < disarrsize; k++) + { + if (disarr[j] < disarr[k]) + { + leng_wid = disarr[j]; + disarr[j] = disarr[k]; + disarr[k] = leng_wid; + } + } + } + + if (disarr[disarr.size() - 2] < 1e-16) + { + curr_direct.setZero(); + return 0; + } + + if (lidar_type == AVIA) + { + double dismax_mid = disarr[0] / disarr[disarrsize / 2]; + double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2]; + + if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize - 2]; + if (dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI& pl, vector& types, uint i, Surround nor_dir) +{ + if (nor_dir == 0) + { + if (types[i - 1].range < blind || types[i - 2].range < blind) + { + return false; + } + } + else if (nor_dir == 1) + { + if (types[i + 1].range < blind || types[i + 2].range < blind) + { + return false; + } + } + double d1 = types[i + nor_dir - 1].dista; + double d2 = types[i + 3 * nor_dir - 2].dista; + double d; + + if (d1 < d2) + { + d = d1; + d1 = d2; + d2 = d; + } + + d1 = sqrt(d1); + d2 = sqrt(d2); + + if (d1 > edgea * d2 || (d1 - d2) > edgeb) + { + return false; + } + + return true; +}