上决策

- 在 optimal_planner.h 中添加缺失的 node_ 成员变量声明
- 修复 teb_local_planner_ros.cpp 中的异常类型（ControllerException -> PlannerException）

这修复了启动时的符号未定义错误：
undefined symbol: _ZN17teb_local_planner17TebOptimalPlannerC1E...

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

.github/copilot-instructions.md

@@ -0,0 +1,6 @@
+---
+description: 当助手已完成当前请求的工作、准备结束回复或不再有明确下一步可执行内容时，必须先调用提问工具 askQuestions 获取进一步指示；不得直接结束对话。 / When the assistant has completed the current task and is about to end the reply or has no clear next action, it must call the askQuestions tool to obtain further instructions instead of ending the conversation.
+applyTo: ‘当检测到回复将以收尾语句结束或无后续动作时 / When a reply is about to end with a closing statement or has no follow-up action’
+---
+当你完成工作时，请调用提问工具（askQuestions）来获取进一步的指示，而不是直接结束对话。
+When you finish the task, call the askQuestions tool to request further instructions instead of ending the conversation.

.gitignore

@@ -122,7 +122,7 @@ devel_isolated/
 CMakeLists.txt.user
 
 srv/_*.py
-*.pcd
+
 *.pyc
 qtcreator-*
 *.user
@@ -156,7 +156,7 @@ build/
 CMakeLists.txt.user
 
 srv/_*.py
-*.pcd
+
 *.pyc
 qtcreator-*
 *.user

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "cmake.sourceDirectory": "/home/robofish/MOVE_AI/src/rm_decision"
+}

CHANGELOG.md

@@ -0,0 +1,142 @@
+# 更新日志
+
+## 2026-03-05 - Sentry 分支重大更新
+
+### 🎯 核心架构重构
+
+#### 1. 决策节点 (rm_decision)
+- ✅ 新增决策节点作为系统核心控制器
+- ✅ 整合视觉自瞄控制（云台）和运动控制（底盘）
+- ✅ 订阅话题：`/data_mcu`, `/data_ref`, `/vision/aim`, `/nav_status`, `/cmd_vel_move`
+- ✅ 发布话题：`/data_ai` (综合控制), `/nav_goal` (导航目标)
+- ✅ 50Hz 控制频率，实时决策
+
+#### 2. 消息系统重构
+- ✅ 新增 `DataAim.msg` - 视觉自瞄控制（仅云台）
+- ✅ 新增 `ChassisMode.msg` - 底盘模式定义
+- ✅ 新增 `GimbalState.msg` - 云台状态数据
+- ✅ 修改 `DataAI.msg` - 综合控制指令（云台+底盘）
+
+#### 3. 视觉系统 ROS2 集成
+- ✅ 创建 `GimbalROS` 适配器，替代直接串口通信
+- ✅ 订阅 `/data_mcu` 获取云台状态
+- ✅ 发布 `/vision/aim` 输出云台控制指令
+- ✅ 修改所有视觉任务文件使用 ROS2 接口
+- ✅ 创建 `rm_vision_bringup` 包，支持标准 ROS2 启动
+
+#### 4. 视觉系统标准化
+- ✅ 添加 `package.xml` 使 rm_vision 成为标准 ROS2 包
+- ✅ 添加 install 规则，可执行文件和配置文件正确安装
+- ✅ 创建 `sentry_vision.launch.py` - Sentry 专用启动文件
+- ✅ 创建 `calibration.launch.py` - 统一标定工具启动
+- ✅ 支持参数化启动不同类型的视觉程序
+
+#### 5. 运动控制增强
+- ✅ `rm_simple_move` 订阅 `/cmd_vel_move` 输出底盘速度
+- ✅ 决策节点整合底盘速度到 `/data_ai` 消息
+
+### 📊 数据流架构
+
+```
+MCU (下位机)
+  ↕ 串口
+Serial Driver
+  ↓ /data_mcu, /data_ref, /gimbal/state
+  ↓
+Vision System ──→ /vision/aim ──→ Decision Node ←── /nav_status
+                                        ↓              ↑
+                                   /nav_goal      /cmd_vel_move
+                                        ↓              ↑
+                                   Simple Move ────────┘
+                                        ↓
+                                   Nav2 Stack
+```
+
+### 🚀 启动方式改进
+
+**之前（复杂）：**
+```bash
+cd build/mr_vision && ./standard_mpc configs/standard4.yaml
+```
+
+**现在（标准化）：**
+```bash
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+```
+
+### 🎯 Sentry 专用支持
+
+- ✅ 默认使用 `sentry.yaml` 配置
+- ✅ 专用启动文件 `sentry_vision.launch.py`
+- ✅ 完整的标定工具链支持
+
+### 📝 文档更新
+
+- ✅ 完善 README.md，添加完整的系统架构图
+- ✅ 添加详细的启动流程说明
+- ✅ 添加话题接口文档
+- ✅ 添加标定工具使用指南
+- ✅ 添加测试命令和调试技巧
+
+### 🔧 技术改进
+
+- ✅ 所有模块统一使用 ROS2 标准接口
+- ✅ 消息类型明确分离（云台/底盘）
+- ✅ 决策节点作为唯一的控制指令输出点
+- ✅ 支持 50Hz 高频控制循环
+- ✅ 线程安全的数据访问（mutex 保护）
+
+### 📦 新增包
+
+- `rm_decision` - 决策节点
+- `rm_vision_bringup` - 视觉系统启动包
+
+### 🐛 修复问题
+
+- ✅ 修复视觉系统 ROS2 类型支持库链接问题
+- ✅ 修复 `capture.cpp` 缺少 node 参数传递
+- ✅ 修复多个视觉任务文件缺少 ROS2 头文件
+- ✅ 修复 launch 文件可执行文件路径错误
+
+### 📋 待完成
+
+- ⏳ 一键启动脚本 `full_system.launch.py`
+- ⏳ 完整的系统测试和验证
+- ⏳ 性能优化和参数调优
+
+---
+
+## 使用说明
+
+### 完整启动流程
+
+```bash
+# 1. 串口驱动
+ros2 launch rm_serial_driver serial_driver.launch.py
+
+# 2. 导航系统
+ros2 launch rm_nav_bringup nav.launch.py
+
+# 3. Sentry 视觉系统
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+
+# 4. 决策节点
+ros2 launch rm_decision decision.launch.py
+```
+
+### 标定工具
+
+```bash
+# 采集标定图像
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture
+
+# 标定相机
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera
+
+# 手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye
+```
+
+---
+
+**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026

COMPLETE_STARTUP_GUIDE.md

@@ -0,0 +1,392 @@
+# 完整启动流程文档
+
+## 系统架构
+
+```
+┌─────────────────┐
+│   rm_vision     │ 视觉节点
+│  (GimbalROS)    │
+└────────┬────────┘
+         │ /gimbal/vision (DataAI)
+         │ 云台控制指令
+         ↓
+┌─────────────────┐
+│ rm_serial_driver│ 串口驱动
+└────────┬────────┘
+         │ 串口
+         ↓
+┌─────────────────┐
+│      MCU        │ 下位机
+└─────────────────┘
+         ↑
+         │ /data_mcu (DataMCU)
+         │ 云台状态反馈
+         │
+┌─────────────────┐
+│  rm_simple_move │ 导航节点
+└─────────────────┘
+         │ /cmd_vel_move (Twist)
+         │ 底盘控制指令
+         ↓
+┌─────────────────┐
+│ rm_serial_driver│
+└─────────────────┘
+```
+
+## 话题映射表
+
+| 话题 | 类型 | 方向 | 说明 |
+|------|------|------|------|
+| `/data_mcu` | `rm_msgs/DataMCU` | MCU → 上位机 | 云台状态（姿态、弹速等） |
+| `/data_ref` | `rm_msgs/DataRef` | MCU → 上位机 | 裁判系统数据 |
+| `/gimbal/vision` | `rm_msgs/DataAI` | 视觉 → MCU | 视觉云台控制指令 |
+| `/cmd_vel_move` | `geometry_msgs/Twist` | 导航 → MCU | 底盘运动控制指令 |
+| `/nav_goal` | `rm_msgs/NavGoal` | 决策 → 导航 | 导航目标点 |
+| `/nav_status` | `rm_msgs/NavStatus` | 导航 → 决策 | 导航状态反馈 |
+| `/current_pose` | `geometry_msgs/PoseStamped` | 导航 → 决策 | 当前位置 |
+
+## 完整启动流程
+
+### 步骤 1: 编译所有包
+
+```bash
+cd ~/MOVE_AI
+
+# 编译消息包
+colcon build --packages-select rm_msgs
+
+# 编译串口驱动
+colcon build --packages-select rm_serial_driver
+
+# 编译导航节点
+colcon build --packages-select rm_simpal_move
+
+# 编译视觉（如果需要）
+colcon build --packages-select mr_vision
+
+# 加载环境
+source install/setup.bash
+```
+
+### 步骤 2: 启动串口驱动（核心节点）
+
+**终端 1：启动 rm_serial_driver**
+
+```bash
+source install/setup.bash
+ros2 run rm_serial_driver rm_serial_driver --ros-args \
+  -p device_name:=/dev/ttyACM0 \
+  -p baud_rate:=115200
+```
+
+**预期输出：**
+```
+========================================
+RM Serial Driver 启动中...
+  设备: /dev/ttyACM0
+  波特率: 115200
+========================================
+话题配置:
+  发布: /data_mcu (MCU状态)
+  发布: /data_ref (裁判系统)
+  订阅: /gimbal/vision (视觉控制)
+  订阅: /cmd_vel_move (导航控制)
+✓ 串口打开成功
+RM Serial Driver 启动完成！
+```
+
+**故障排查：**
+```bash
+# 如果串口打开失败，检查设备
+ls -l /dev/ttyACM*
+ls -l /dev/ttyUSB*
+
+# 添加串口权限
+sudo usermod -a -G dialout $USER
+# 重新登录生效
+
+# 或临时授权
+sudo chmod 666 /dev/ttyACM0
+```
+
+### 步骤 3: 验证串口通信
+
+**终端 2：查看 MCU 数据**
+
+```bash
+source install/setup.bash
+ros2 topic echo /data_mcu
+```
+
+**预期输出：**
+```yaml
+mode: 0
+q0: 1.0
+q1: 0.0
+q2: 0.0
+q3: 0.0
+yaw: 0.0
+yaw_vel: 0.0
+pitch: 0.0
+pitch_vel: 0.0
+bullet_speed: 15.0
+bullet_count: 0
+---
+```
+
+如果能看到数据，说明串口通信正常！
+
+### 步骤 4: 启动导航节点（可选）
+
+**终端 3：启动 rm_simple_move**
+
+```bash
+source install/setup.bash
+ros2 launch rm_simpal_move simple_move.launch.py
+```
+
+**预期输出：**
+```
+[rm_simple_move]: 话题配置:
+[rm_simple_move]:   输入: /nav_goal
+[rm_simple_move]:   输出: /cmd_vel_move (统一输出)
+[rm_simple_move]:   PID模式: 直接计算发布到 /cmd_vel_move
+[rm_simple_move]:   NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move
+[rm_simple_move]:   状态: /nav_status, /current_pose
+[rm_simple_move]: RMSimpleMove 启动
+```
+
+### 步骤 5: 启动视觉节点（如果需要）
+
+**终端 4：启动视觉**
+
+```bash
+source install/setup.bash
+
+# 方式 1: 使用已修复的 standard_mpc
+./build/mr_vision/standard_mpc configs/standard.yaml
+
+# 方式 2: 使用其他视觉节点（需要先修复）
+# ./build/mr_vision/balance_infantry_mpc configs/balance_infantry.yaml
+```
+
+**预期输出：**
+```
+[GimbalROS] Initialized with ROS2 topics
+[GimbalROS]   Subscribe: /data_mcu
+[GimbalROS]   Publish: /gimbal/vision
+[GimbalROS] Waiting for first quaternion...
+[GimbalROS] First q received.
+```
+
+## 测试流程
+
+### 测试 1: 验证话题连接
+
+```bash
+# 查看所有话题
+ros2 topic list
+
+# 应该看到：
+# /data_mcu
+# /data_ref
+# /gimbal/vision
+# /cmd_vel_move
+# /nav_goal
+# /nav_status
+# /current_pose
+
+# 查看话题信息
+ros2 topic info /gimbal/vision
+ros2 topic info /data_mcu
+```
+
+### 测试 2: 测试导航功能
+
+```bash
+# PID 模式导航到 (2, 2)
+ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
+  mode: 0,
+  target_x: 2.0,
+  target_y: 2.0,
+  target_angle: 0.0,
+  max_speed: 1.0,
+  tolerance: 0.2
+}" --once
+
+# 查看导航状态
+ros2 topic echo /nav_status
+
+# 查看底盘控制指令
+ros2 topic echo /cmd_vel_move
+```
+
+### 测试 3: 测试视觉控制（如果视觉节点运行）
+
+```bash
+# 查看视觉发送的云台控制指令
+ros2 topic echo /gimbal/vision
+
+# 应该看到：
+# mode: 0/1/2
+# yaw: ...
+# pitch: ...
+# vx, vy, wz: ...
+```
+
+## 最小启动配置
+
+如果只需要测试串口通信和导航：
+
+```bash
+# 终端 1: 串口驱动
+ros2 run rm_serial_driver rm_serial_driver
+
+# 终端 2: 导航节点
+ros2 launch rm_simpal_move simple_move.launch.py
+
+# 终端 3: 发送导航目标
+ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{mode: 0, target_x: 1.0, target_y: 1.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.2}" --once
+```
+
+## 完整启动配置（包含视觉）
+
+```bash
+# 终端 1: 串口驱动
+ros2 run rm_serial_driver rm_serial_driver
+
+# 终端 2: 导航节点
+ros2 launch rm_simpal_move simple_move.launch.py
+
+# 终端 3: 视觉节点
+./build/mr_vision/standard_mpc configs/standard.yaml
+
+# 终端 4: 监控（可选）
+ros2 topic echo /data_mcu
+```
+
+## 启动脚本
+
+创建一个启动脚本 `start_all.sh`：
+
+```bash
+#!/bin/bash
+
+# 启动所有节点
+cd ~/MOVE_AI
+source install/setup.bash
+
+# 启动串口驱动（后台）
+gnome-terminal -- bash -c "source install/setup.bash && ros2 run rm_serial_driver rm_serial_driver; exec bash"
+
+# 等待串口驱动启动
+sleep 2
+
+# 启动导航节点（后台）
+gnome-terminal -- bash -c "source install/setup.bash && ros2 launch rm_simpal_move simple_move.launch.py; exec bash"
+
+# 等待导航节点启动
+sleep 2
+
+# 启动视觉节点（如果需要）
+# gnome-terminal -- bash -c "source install/setup.bash && ./build/mr_vision/standard_mpc configs/standard.yaml; exec bash"
+
+echo "所有节点已启动！"
+echo "使用 'ros2 topic list' 查看话题"
+echo "使用 'ros2 topic echo /data_mcu' 查看 MCU 数据"
+```
+
+使用方法：
+```bash
+chmod +x start_all.sh
+./start_all.sh
+```
+
+## 关闭流程
+
+```bash
+# 方式 1: 在每个终端按 Ctrl+C
+
+# 方式 2: 使用脚本关闭所有节点
+pkill -f rm_serial_driver
+pkill -f rm_simple_move
+pkill -f standard_mpc
+```
+
+## 常见问题
+
+### Q1: 串口打开失败
+```bash
+# 检查设备
+ls -l /dev/ttyACM*
+
+# 添加权限
+sudo usermod -a -G dialout $USER
+# 重新登录
+
+# 或临时授权
+sudo chmod 666 /dev/ttyACM0
+```
+
+### Q2: 收不到 /data_mcu 数据
+- 检查串口连接是否正常
+- 检查 MCU 是否正常运行
+- 检查波特率是否匹配（默认 115200）
+
+### Q3: 视觉节点等待四元数超时
+- 确保 `rm_serial_driver` 已启动
+- 确保 MCU 正在发送数据
+- 检查 `/data_mcu` 话题是否有数据
+
+### Q4: 导航不动
+- 检查 `/cmd_vel_move` 是否有数据
+- 检查 `rm_serial_driver` 是否订阅了 `/cmd_vel_move`
+- 检查 TF 变换是否正常
+
+## 调试工具
+
+```bash
+# 查看所有节点
+ros2 node list
+
+# 查看所有话题
+ros2 topic list
+
+# 查看话题频率
+ros2 topic hz /data_mcu
+
+# 查看话题信息
+ros2 topic info /gimbal/vision
+
+# 录制数据
+ros2 bag record -a
+
+# 查看 TF 树
+ros2 run tf2_tools view_frames
+```
+
+## 性能监控
+
+```bash
+# 查看节点资源占用
+top -p $(pgrep -f rm_serial_driver)
+
+# 查看话题带宽
+ros2 topic bw /data_mcu
+
+# 查看话题延迟
+ros2 topic delay /data_mcu
+```
+
+## 下一步
+
+1. ✅ 串口驱动正常运行
+2. ✅ 导航节点正常运行
+3. ⏳ 修复剩余的视觉节点文件
+4. ⏳ 测试完整的视觉+导航+串口流程
+
+## 文档
+
+- **使用指南**: `GIMBAL_ROS_GUIDE.md`
+- **迁移状态**: `ROS2_MIGRATION_STATUS.md`
+- **本文档**: `COMPLETE_STARTUP_GUIDE.md`

README.md

@@ -1,11 +1,378 @@
 # MOVE_AI
 
-青岛理工大学QUT
-MOVE战队
-RoboMaster比赛
-所有上位机代码仓库。具体请查看分支
-包含步兵/英雄/无人机，
-哨兵，
-雷达，
-等兵种
-支持导航，自瞄，决策等功能。
+青岛理工大学 QUT MOVE 战队 RoboMaster 比赛上位机代码仓库
+
+支持步兵/英雄/无人机、哨兵、雷达等兵种，集成导航、自瞄、决策等功能。
+
+---
+
+## 📋 目录
+
+- [系统架构](#系统架构)
+- [功能模块](#功能模块)
+- [快速开始](#快速开始)
+- [启动流程](#启动流程)
+- [话题接口](#话题接口)
+- [测试命令](#测试命令)
+
+---
+
+## 🏗️ 系统架构
+
+```
+┌─────────────────────────────────────────────────────────────────┐
+│                          下位机 (MCU)                            │
+│                    (STM32 + 电机 + IMU + 传感器)                 │
+└────────────────────────────┬────────────────────────────────────┘
+                             │ 串口通信
+                             ↓
+┌─────────────────────────────────────────────────────────────────┐
+│                    rm_serial_driver (串口驱动)                   │
+│  ┌──────────────────────────────────────────────────────────┐  │
+│  │ 发布: /data_mcu (MCU状态)                                 │  │
+│  │       /data_ref (裁判系统数据)                            │  │
+│  │       /gimbal/state (云台状态)                            │  │
+│  │ 订阅: /data_ai (综合控制指令)                             │  │
+│  └──────────────────────────────────────────────────────────┘  │
+└───────┬─────────────────────────────────────────────┬───────────┘
+        │                                             │
+        ↓                                             ↓
+┌──────────────────┐                         ┌──────────────────┐
+│   rm_vision      │                         │  rm_decision     │
+│   (视觉系统)      │                         │  (决策节点)       │
+├──────────────────┤                         ├──────────────────┤
+│ 订阅:             │                         │ 订阅:             │
+│ /gimbal/state    │                         │ /data_mcu        │
+│                  │                         │ /data_ref        │
+│ 发布:             │                         │ /vision/aim      │
+│ /vision/aim ─────┼─────────────────────────→ /nav_status      │
+│ (云台控制)        │                         │ /cmd_vel_move    │
+└──────────────────┘                         │                  │
+                                             │ 发布:             │
+┌──────────────────┐                         │ /data_ai ────────┼──┐
+│  rm_simple_move  │                         │ /nav_goal        │  │
+│  (运动控制)       │                         └────────┬─────────┘  │
+├──────────────────┤                                  │            │
+│ 订阅:             │                                  │            │
+│ /nav_goal ←──────┼──────────────────────────────────┘            │
+│                  │                                               │
+│ 发布:             │                                               │
+│ /cmd_vel_move ───┼───────────────────────────────────────────────┘
+│ /nav_status      │
+└────────┬─────────┘
+         │
+         ↓
+┌──────────────────┐
+│   Nav2 Stack     │
+│   (导航系统)      │
+├──────────────────┤
+│ - 路径规划        │
+│ - 避障            │
+│ - 定位            │
+└──────────────────┘
+```
+
+---
+
+## 🔧 功能模块
+
+### 1. **rm_serial_driver** - 串口驱动
+- 与下位机 MCU 进行串口通信
+- 接收 MCU 状态、裁判系统数据、云台状态
+- 发送综合控制指令到 MCU
+
+### 2. **rm_vision** - 视觉系统
+- 自瞄识别与跟踪
+- 能量机关识别
+- 输出云台控制指令（yaw/pitch 角度、速度、加速度）
+
+### 3. **rm_simple_move** - 运动控制
+- **PID 模式**：直接速度控制
+- **NAV 模式**：调用 Nav2 进行路径规划和导航
+- 输出底盘速度指令
+
+### 4. **rm_decision** - 决策节点（核心控制器）
+- 整合视觉自瞄控制（云台）
+- 整合运动控制指令（底盘）
+- 根据裁判系统、导航状态做决策
+- 输出统一的 `/data_ai` 控制指令
+
+### 5. **Nav2** - 导航系统
+- 全局路径规划
+- 局部避障
+- SLAM 定位
+
+---
+
+## 🚀 快速开始
+
+### 环境要求
+- Ubuntu 22.04
+- ROS 2 Humble
+- C++17
+
+### 编译
+```bash
+cd /home/robofish/MOVE_AI
+colcon build --symlink-install
+source install/setup.bash
+```
+
+---
+
+## 📡 启动流程
+
+### 方式一：分步启动（推荐调试）
+
+#### 1️⃣ 启动串口驱动
+```bash
+ros2 launch rm_serial_driver serial_driver.launch.py
+```
+
+#### 2️⃣ 启动导航系统
+```bash
+ros2 launch rm_nav_bringup nav.launch.py
+```
+
+#### 3️⃣ 启动视觉系统
+
+**Sentry 哨兵（推荐）：**
+```bash
+ros2 launch rm_vision_bringup sentry_vision.launch.py
+```
+
+**其他机器人类型：**
+```bash
+# 标准步兵
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=standard_mpc
+
+# 平衡步兵
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=balance_infantry_mpc
+
+# 自瞄调试模式
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_aim_debug_mpc
+
+# 能量机关调试
+ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_buff_debug_mpc
+
+# 自定义配置文件
+ros2 launch rm_vision_bringup vision.launch.py config:=sentry.yaml
+```
+
+#### 4️⃣ 启动决策节点
+```bash
+ros2 launch rm_decision decision.launch.py
+```
+
+### 方式二：一键启动（待实现）
+```bash
+ros2 launch rm_bringup full_system.launch.py
+```
+
+---
+
+## 🎯 标定工具
+
+视觉系统提供了完整的标定工具链：
+
+### 相机标定
+```bash
+# 1. 采集标定图像（按 's' 保存，'q' 退出）
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture
+
+# 2. 标定相机内参
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera
+
+# 3. 手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye
+
+# 4. 机器人-世界手眼标定
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_robotworld_handeye
+
+# 5. 视频分割工具
+ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=split_video
+```
+
+**自定义参数：**
+```bash
+# 指定配置文件和输出目录
+ros2 launch rm_vision_bringup calibration.launch.py \
+  calibration_type:=capture \
+  config:=calibration.yaml \
+  output_folder:=my_calibration_data
+```
+
+---
+
+## 📊 话题接口
+
+### 核心话题
+
+| 话题名称 | 消息类型 | 发布者 | 订阅者 | 说明 |
+|---------|---------|--------|--------|------|
+| `/data_mcu` | `rm_msgs/DataMCU` | serial_driver | decision | MCU 状态数据 |
+| `/data_ref` | `rm_msgs/DataRef` | serial_driver | decision | 裁判系统数据 |
+| `/gimbal/state` | `rm_msgs/GimbalState` | serial_driver | vision | 云台状态（姿态、速度） |
+| `/vision/aim` | `rm_msgs/DataAim` | vision | decision | 视觉自瞄控制（仅云台） |
+| `/nav_goal` | `rm_msgs/NavGoal` | decision | simple_move | 导航目标 |
+| `/nav_status` | `rm_msgs/NavStatus` | simple_move | decision | 导航状态反馈 |
+| `/cmd_vel_move` | `geometry_msgs/Twist` | simple_move | decision | 底盘速度指令 |
+| `/data_ai` | `rm_msgs/DataAI` | decision | serial_driver | 综合控制指令（云台+底盘） |
+
+### 数据流向
+
+```
+视觉控制流:
+MCU → /gimbal/state → Vision → /vision/aim → Decision → /data_ai → MCU
+
+底盘控制流:
+Decision → /nav_goal → SimpleMove → /cmd_vel_move → Decision → /data_ai → MCU
+
+导航反馈流:
+SimpleMove → /nav_status → Decision
+```
+
+---
+
+## 🧪 测试命令
+
+### 查看话题列表
+```bash
+ros2 topic list
+```
+
+### 监控关键话题
+```bash
+# 查看决策输出
+ros2 topic echo /data_ai
+
+# 查看底盘速度
+ros2 topic echo /cmd_vel_move
+
+# 查看视觉自瞄
+ros2 topic echo /vision/aim
+
+# 查看导航状态
+ros2 topic echo /nav_status
+```
+
+### 发送导航目标
+
+#### NAV 模式（路径规划）
+```bash
+ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
+  "{mode: 1, target_x: 4.0, target_y: -4.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.1}"
+```
+
+#### PID 模式（直接控制）
+```bash
+ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \
+  "{mode: 0, target_x: 1.0, target_y: 0.5, target_angle: 0.0, max_speed: 0.5, tolerance: 0.05}"
+```
+
+### 可视化
+```bash
+# 启动 RViz 查看导航
+rviz2 -d $(ros2 pkg prefix rm_bringup)/share/rm_bringup/rviz/nav.rviz
+
+# 查看 TF 树
+ros2 run tf2_tools view_frames
+```
+
+---
+
+## 📝 消息定义
+
+### DataAim（视觉自瞄 - 仅云台）
+```
+uint8 mode              # 0:空闲 1:自瞄 2:小符 3:大符
+float32 yaw             # 偏航角
+float32 yaw_vel         # 偏航角速度
+float32 yaw_acc         # 偏航角加速度
+float32 pitch           # 俯仰角
+float32 pitch_vel       # 俯仰角速度
+float32 pitch_acc       # 俯仰角加速度
+```
+
+### DataAI（综合控制 - 云台+底盘）
+```
+uint8 mode              # 云台模式
+float32 yaw, yaw_vel, yaw_acc
+float32 pitch, pitch_vel, pitch_acc
+float32 vx, vy, wz      # 底盘速度
+uint8 reserved          # 底盘模式
+```
+
+### NavGoal（导航目标）
+```
+uint8 MODE_PID = 0      # PID 直接控制
+uint8 MODE_NAV = 1      # Nav2 导航
+uint8 mode
+float32 target_x, target_y, target_angle
+float32 max_speed, tolerance
+```
+
+### ChassisMode（底盘模式）
+```
+uint8 MODE_MANUAL = 0   # 手动控制
+uint8 MODE_FOLLOW = 1   # 跟随云台
+uint8 MODE_SPIN = 2     # 小陀螺
+uint8 MODE_NAV = 3      # 导航模式
+```
+
+---
+
+## 🐛 调试技巧
+
+### 检查节点状态
+```bash
+ros2 node list
+ros2 node info /rm_decision
+```
+
+### 检查话题连接
+```bash
+ros2 topic info /data_ai
+```
+
+### 查看参数
+```bash
+ros2 param list /rm_decision
+ros2 param get /rm_decision enable_aim
+```
+
+### 日志级别
+```bash
+ros2 run rm_decision decision_node --ros-args --log-level debug
+```
+
+---
+
+## 📂 项目结构
+
+```
+MOVE_AI/
+├── src/
+│   ├── rm_serial_driver/    # 串口驱动
+│   ├── rm_vision/           # 视觉系统
+│   ├── rm_nav/              # 导航相关
+│   │   └── rm_simple_move/  # 运动控制
+│   ├── rm_decision/         # 决策节点
+│   ├── rm_msgs/             # 消息定义
+│   └── rm_bringup/          # 启动文件
+└── README.md
+```
+
+---
+
+## 🤝 贡献
+
+欢迎提交 Issue 和 Pull Request！
+
+## 📄 许可证
+
+本项目采用 MIT 许可证。
+
+---
+
+**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026

mapping.sh

@@ -0,0 +1,19 @@
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_sim.launch.py \
+      world:=2026RMUL \
+      mode:=mapping  \
+      lio:=fastlio \
+      lio_rviz:=True \
+      nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

nav.sh

@@ -0,0 +1,20 @@
+source install/setup.bash
+
+commands=(
+    "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_real.launch.py \
+      world:=wanzheng \
+      mode:=nav \
+      lio:=fastlio \
+      localization:=gicp \
+      lio_rviz:=False \
+      nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

sim.sh

@@ -0,0 +1,20 @@
+source install/setup.bash
+
+commands=(
+    # "ros2 launch rm_serial_driver rm_serial_driver.launch.py"
+    # "ros2 launch rm_decision decision.launch.py"
+    "ros2 launch rm_simpal_move simple_move.launch.py"
+    "ros2 launch rm_nav_bringup bringup_sim.launch.py \
+    world:=2026RMUL \
+    mode:=nav \
+    lio:=fastlio \
+    localization:=gicp \
+    lio_rviz:=False \
+    nav_rviz:=True
+    "
+)
+
+for cmd in "${commands[@]}"; do
+  gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
+  sleep 0.5
+done

src/rm_decision/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_decision)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rm_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# 包含头文件目录
+include_directories(include)
+
+# 创建决策节点库
+add_library(decision_node SHARED
+  src/decision_node.cpp
+)
+
+ament_target_dependencies(decision_node
+  rclcpp
+  rclcpp_components
+  rm_msgs
+  geometry_msgs
+)
+
+# 注册为组件
+rclcpp_components_register_node(decision_node
+  PLUGIN "rm_decision::DecisionNode"
+  EXECUTABLE decision_node_exe
+)
+
+# 安装
+install(TARGETS decision_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY launch test
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# 安装 Python 测试脚本
+install(PROGRAMS
+  test/simple_nav_test.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/rm_decision/README.md

@@ -0,0 +1,107 @@
+# rm_decision — 2026 决策节点
+
+## 概述
+
+RoboMaster 哨兵机器人决策节点，基于有限状态机实现比赛全流程自主决策。节点以 50Hz 运行，综合裁判系统、自瞄视觉、导航反馈等数据，输出统一控制指令给下位机。
+
+## 导航点 (map 坐标系)
+
+| 名称 | X | Y | Angle |
+|------|---|---|-------|
+| 家 (补血点) | -0.098266 | 0.0092684 | -0.074146 |
+| 1号点 | 0.76813 | -4.5578 | -0.23769 |
+| 2号点 | 4.9211 | -4.5856 | -0.27894 |
+| 增益点 | 5.0031 | -2.3273 | -0.20345 |
+
+## 比赛阶段 (game_progress)
+
+| 值 | 含义 |
+|----|------|
+| 0 | 未开始比赛 |
+| 1 | 准备阶段 |
+| 2 | 十五秒裁判系统自检 |
+| 3 | 五秒倒计时 |
+| 4 | 比赛中 |
+| 5 | 比赛结算中 |
+
+## 状态机
+
+```
+IDLE ──(game=4)──> GO_POINT1 ──> GO_POINT2 ──> GO_GAIN ──> HOLD_GAIN
+                                                              │   ▲
+                                                              ▼   │
+                                                           PATROL─┘
+                                                              │
+                                              (hp<200)        │
+                   ┌──────────────────────────────────────────┘
+                   ▼
+              RETREAT_TO_2 ──> RETREAT_TO_1 ──> RETREAT_HOME ──> HEALING
+                                                                    │
+                                                        (hp>=350)   │
+                                                                    ▼
+                                                              GO_POINT1 (重新出发)
+```
+
+### 状态说明
+
+| 状态 | 行为 | 小陀螺 | 云台搜索 | 自瞄 |
+|------|------|--------|---------|------|
+| IDLE | game_progress 0~3，PID 待在家 | 关 | 关 | 关 |
+| GO_POINT1 | PID 导航到 1 号点 | 关 | 关 | 关 |
+| GO_POINT2 | PID 导航到 2 号点 | 关 | 关 | 关 |
+| GO_GAIN | PID 导航到增益点 | 关 | 关 | 关 |
+| HOLD_GAIN | 到达增益点，原地占领；发现目标则原地陀螺开火 | 开 | 有目标时关 | 有目标时开 |
+| PATROL | 无目标时在增益点 ±0.5m 范围随机移动找敌 | 开 | 开 | 关 |
+| RETREAT_TO_2 | 血量 <200，从增益点撤退到 2 号点 | 开 | 关 | 关 |
+| RETREAT_TO_1 | 从 2 号点撤退到 1 号点 | 开 | 关 | 关 |
+| RETREAT_HOME | 从 1 号点撤退回家 | 开 | 关 | 关 |
+| HEALING | 在家补血，等待血量恢复到 ≥350 | 关 | 关 | 关 |
+| MATCH_END | 比赛结束，停止一切动作 | 关 | 关 | 关 |
+
+## 决策规则
+
+1. **赛前待命**：game_progress 为 0/1/2/3 时，PID 控制待在家的位置。
+2. **快速占点**：收到 game_progress=4 后，PID 依次跑 1号点 → 2号点 → 增益点，全程不开小陀螺，以最快速度占领增益点。
+3. **占点防守**：到达增益点后立刻开启底盘小陀螺 + 云台 search 模式。search 到目标后立刻退出 search，转发自瞄数据开火；目标丢失后有 1 秒过渡延时，之后重新进入 search。
+4. **巡逻找敌**：占领增益点时若自瞄无目标，在增益点附近 0.5m 范围内随机移动 + search 扫描；发现目标立刻原地陀螺开火。
+5. **低血量撤退**：血量 <200 时立刻撤退，路线为 增益点 → 2号点 → 1号点 → 家，撤退全程开小陀螺。回血优先级高于自瞄。
+6. **补血重出**：在家补血至 ≥350 后，重新执行出家占点逻辑 (1号点 → 2号点 → 增益点)。
+
+## 话题接口
+
+### 订阅
+
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/data_mcu` | rm_msgs/DataMCU | MCU 状态 (云台角度、弹速等) |
+| `/data_ref` | rm_msgs/DataRef | 裁判系统 (血量、比赛阶段) |
+| `/data_aim` | rm_msgs/DataAim | 视觉自瞄 (目标角度、开火模式) |
+| `/nav_status` | rm_msgs/NavStatus | 导航状态 (到达/运行中/失败) |
+| `/cmd_vel_move` | geometry_msgs/Twist | simple_move 输出的底盘速度 |
+
+### 发布
+
+| 话题 | 类型 | 说明 |
+|------|------|------|
+| `/data_ai` | rm_msgs/DataAI | 综合控制指令 → 串口 → MCU |
+| `/nav_goal` | rm_msgs/NavGoal | 导航目标 → simple_move |
+
+## reserved 标志位 (DataAI.reserved)
+
+| Bit | 宏定义 | 含义 |
+|-----|--------|------|
+| 0 | AI_CHASSIS_ROTOR | 底盘小陀螺 |
+| 1 | AI_GIMBAL_SEARCH | 云台搜索模式 |
+| 2~7 | — | 预留 |
+
+## 参数
+
+| 参数名 | 默认值 | 说明 |
+|--------|--------|------|
+| control_frequency | 50.0 | 决策循环频率 (Hz) |
+| nav_max_speed | 2.0 | 导航最大线速度 (m/s) |
+| nav_tolerance | 0.15 | 导航到达容差 (m) |
+| wp_home | [-0.098266, 0.0092684, -0.074146] | 家坐标 [x, y, angle] |
+| wp_point1 | [0.76813, -4.5578, -0.23769] | 1号点坐标 |
+| wp_point2 | [4.9211, -4.5856, -0.27894] | 2号点坐标 |
+| wp_gain | [5.0031, -2.3273, -0.20345] | 增益点坐标 |

src/rm_decision/include/rm_decision/decision_node.hpp

@@ -0,0 +1,123 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rm_msgs/msg/data_mcu.hpp>
+#include <rm_msgs/msg/data_ref.hpp>
+#include <rm_msgs/msg/data_ai.hpp>
+#include <rm_msgs/msg/data_aim.hpp>
+#include <rm_msgs/msg/nav_goal.hpp>
+#include <rm_msgs/msg/nav_status.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <atomic>
+#include <mutex>
+#include <random>
+#include <chrono>
+
+namespace rm_decision
+{
+
+// 决策状态机
+enum class State : uint8_t
+{
+  IDLE,             // 等待比赛开始 (game_progress 0~3)
+  GO_POINT1,        // 前往1号点
+  GO_POINT2,        // 前往2号点
+  GO_GAIN,          // 前往增益点
+  HOLD_GAIN,        // 占领增益点 (陀螺 + search/自瞄)
+  PATROL,           // 增益点附近巡逻 (无目标时随机移动)
+  RETREAT_TO_2,     // 低血量撤退: 增益点 → 2号点
+  RETREAT_TO_1,     // 低血量撤退: 2号点 → 1号点
+  RETREAT_HOME,     // 低血量撤退: 1号点 → 家
+  HEALING,          // 在家补血
+  MATCH_END,        // 比赛结束
+};
+
+// 导航点坐标
+struct Waypoint {
+  float x, y, angle;
+};
+
+class DecisionNode : public rclcpp::Node
+{
+public:
+  explicit DecisionNode(const rclcpp::NodeOptions &options);
+  ~DecisionNode() override;
+
+private:
+  // 回调函数
+  void data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg);
+  void data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg);
+  void data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg);
+  void nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg);
+  void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  // 定时器回调
+  void control_timer_callback();
+
+  // 决策逻辑
+  void make_decision();
+  void send_control_command();
+
+  // 辅助函数
+  void send_nav_goal(const Waypoint &wp, uint8_t mode, float max_speed, float tolerance);
+  Waypoint random_patrol_point();
+
+  // 订阅者
+  rclcpp::Subscription<rm_msgs::msg::DataMCU>::SharedPtr data_mcu_sub_;
+  rclcpp::Subscription<rm_msgs::msg::DataRef>::SharedPtr data_ref_sub_;
+  rclcpp::Subscription<rm_msgs::msg::DataAim>::SharedPtr data_aim_sub_;
+  rclcpp::Subscription<rm_msgs::msg::NavStatus>::SharedPtr nav_status_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_move_sub_;
+
+  // 发布者
+  rclcpp::Publisher<rm_msgs::msg::DataAI>::SharedPtr data_ai_pub_;
+  rclcpp::Publisher<rm_msgs::msg::NavGoal>::SharedPtr nav_goal_pub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr control_timer_;
+
+  // 状态数据 (mutex保护)
+  std::mutex mutex_;
+  rm_msgs::msg::DataMCU latest_mcu_data_;
+  rm_msgs::msg::DataRef latest_ref_data_;
+  rm_msgs::msg::DataAim latest_aim_data_;
+  rm_msgs::msg::NavStatus latest_nav_status_;
+  geometry_msgs::msg::Twist latest_cmd_vel_;
+
+  // 原子状态标志
+  std::atomic<bool> has_mcu_data_{false};
+  std::atomic<bool> has_aim_data_{false};
+  std::atomic<bool> has_nav_data_{false};
+
+  // 决策状态机
+  State state_{State::IDLE};
+  bool nav_goal_sent_{false};          // 当前状态的导航目标是否已发送
+  bool aim_target_found_{false};       // 自瞄是否发现目标
+  bool enable_rotor_{false};           // 底盘小陀螺
+  bool enable_search_{false};          // 云台搜索模式
+  bool enable_aim_{false};             // 启用自瞄转发
+
+  // 自瞄延时控制
+  rclcpp::Time aim_lost_time_;         // 自瞄丢失目标的时间
+  bool aim_transition_active_{false};  // 自瞄→search过渡中
+  static constexpr double AIM_TRANSITION_DELAY = 1.0;  // 1秒过渡延时
+
+  // 血量阈值
+  static constexpr uint16_t HP_RETREAT_THRESHOLD = 200;
+  static constexpr uint16_t HP_HEALED_THRESHOLD = 350;
+
+  // 导航点
+  Waypoint wp_home_;
+  Waypoint wp_point1_;
+  Waypoint wp_point2_;
+  Waypoint wp_gain_;
+
+  // 巡逻随机数
+  std::mt19937 rng_;
+  std::uniform_real_distribution<float> patrol_dist_{-0.5f, 0.5f};
+
+  // 导航速度参数
+  float nav_max_speed_{2.0f};
+  float nav_tolerance_{0.5f};
+};
+
+}  // namespace rm_decision

src/rm_decision/launch/decision.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='rm_decision',
+            executable='decision_node_exe',
+            name='rm_decision',
+            output='screen',
+            parameters=[{
+                'control_frequency': 50.0,
+                'enable_aim': True,
+                'chassis_mode': 0,
+            }]
+        )
+    ])

src/rm_decision/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rm_decision</name>
+  <version>1.0.0</version>
+  <description>RoboMaster Decision Node - Core Controller</description>
+  <maintainer email="move@qut.edu.cn">MOVE Team</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rm_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_decision/src/decision_node.cpp

@@ -0,0 +1,455 @@
+#include "rm_decision/decision_node.hpp"
+
+namespace rm_decision
+{
+
+DecisionNode::DecisionNode(const rclcpp::NodeOptions &options)
+  : Node("rm_decision", options),
+    rng_(std::chrono::steady_clock::now().time_since_epoch().count())
+{
+  // 声明参数
+  this->declare_parameter("control_frequency", 50.0);
+  this->declare_parameter("nav_max_speed", 2.0);
+  this->declare_parameter("nav_tolerance", 0.15);
+
+  // 导航点参数 (来自 README)
+  this->declare_parameter("wp_home", std::vector<double>{-0.098266, 0.0092684, -0.074146});
+  this->declare_parameter("wp_point1", std::vector<double>{0.76813, -4.5578, -0.23769});
+  this->declare_parameter("wp_point2", std::vector<double>{4.9211, -4.5856, -0.27894});
+  this->declare_parameter("wp_gain", std::vector<double>{5.0031, -2.3273, -0.20345});
+
+  auto control_freq = this->get_parameter("control_frequency").as_double();
+  nav_max_speed_ = this->get_parameter("nav_max_speed").as_double();
+  nav_tolerance_ = this->get_parameter("nav_tolerance").as_double();
+
+  // 加载导航点
+  auto load_wp = [this](const std::string &name) -> Waypoint {
+    auto v = this->get_parameter(name).as_double_array();
+    return Waypoint{static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2])};
+  };
+  wp_home_   = load_wp("wp_home");
+  wp_point1_ = load_wp("wp_point1");
+  wp_point2_ = load_wp("wp_point2");
+  wp_gain_   = load_wp("wp_gain");
+
+  // 创建订阅者
+  data_mcu_sub_ = this->create_subscription<rm_msgs::msg::DataMCU>(
+    "/data_mcu", 10, std::bind(&DecisionNode::data_mcu_callback, this, std::placeholders::_1));
+  data_ref_sub_ = this->create_subscription<rm_msgs::msg::DataRef>(
+    "/data_ref", 10, std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1));
+  data_aim_sub_ = this->create_subscription<rm_msgs::msg::DataAim>(
+    "/data_aim", 10, std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1));
+  nav_status_sub_ = this->create_subscription<rm_msgs::msg::NavStatus>(
+    "/nav_status", 10, std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1));
+  cmd_vel_move_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    "/cmd_vel_move", 10, std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1));
+
+  // 创建发布者
+  data_ai_pub_ = this->create_publisher<rm_msgs::msg::DataAI>("/data_ai", 10);
+  nav_goal_pub_ = this->create_publisher<rm_msgs::msg::NavGoal>("/nav_goal", 10);
+
+  // 创建控制定时器
+  auto period = std::chrono::duration<double>(1.0 / control_freq);
+  control_timer_ = this->create_wall_timer(
+    std::chrono::duration_cast<std::chrono::milliseconds>(period),
+    std::bind(&DecisionNode::control_timer_callback, this));
+
+  aim_lost_time_ = this->now();
+
+  RCLCPP_INFO(this->get_logger(), "Decision Node 启动 | 频率: %.0fHz", control_freq);
+  RCLCPP_INFO(this->get_logger(), "  家: (%.3f, %.3f)", wp_home_.x, wp_home_.y);
+  RCLCPP_INFO(this->get_logger(), "  1号: (%.3f, %.3f)", wp_point1_.x, wp_point1_.y);
+  RCLCPP_INFO(this->get_logger(), "  2号: (%.3f, %.3f)", wp_point2_.x, wp_point2_.y);
+  RCLCPP_INFO(this->get_logger(), "  增益: (%.3f, %.3f)", wp_gain_.x, wp_gain_.y);
+}
+
+DecisionNode::~DecisionNode()
+{
+  RCLCPP_INFO(this->get_logger(), "Decision Node 关闭");
+}
+
+// ==================== 回调函数 ====================
+
+void DecisionNode::data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_mcu_data_ = *msg;
+  has_mcu_data_ = true;
+}
+
+void DecisionNode::data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_ref_data_ = *msg;
+}
+
+void DecisionNode::data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_aim_data_ = *msg;
+  has_aim_data_ = true;
+}
+
+void DecisionNode::nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_nav_status_ = *msg;
+  has_nav_data_ = true;
+}
+
+void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  latest_cmd_vel_ = *msg;
+}
+
+// ==================== 辅助函数 ====================
+
+void DecisionNode::send_nav_goal(const Waypoint &wp, uint8_t mode, float max_speed, float tolerance)
+{
+  auto msg = rm_msgs::msg::NavGoal();
+  msg.control_mode = mode;
+  msg.x = wp.x;
+  msg.y = wp.y;
+  msg.angle = wp.angle;
+  msg.max_speed = max_speed;
+  msg.tolerance = tolerance;
+  nav_goal_pub_->publish(msg);
+}
+
+Waypoint DecisionNode::random_patrol_point()
+{
+  return Waypoint{
+    wp_gain_.x + patrol_dist_(rng_),
+    wp_gain_.y + patrol_dist_(rng_),
+    wp_gain_.angle
+  };
+}
+
+// ==================== 定时器 ====================
+
+void DecisionNode::control_timer_callback()
+{
+  if (!has_mcu_data_) return;
+  make_decision();
+  send_control_command();
+}
+
+// ==================== 决策逻辑 ====================
+
+void DecisionNode::make_decision()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  uint8_t game_progress = latest_ref_data_.game_progress;
+  uint16_t hp = latest_ref_data_.remain_hp;
+  uint8_t nav_status = has_nav_data_ ? latest_nav_status_.status : 0;
+
+  // 自瞄目标检测: mode >= 1 表示发现目标
+  bool aim_has_target = has_aim_data_ && (latest_aim_data_.mode >= 1);
+
+  // 自瞄过渡延时逻辑: 丢失目标后延时1s再切回search
+  if (aim_has_target) {
+    aim_target_found_ = true;
+    aim_transition_active_ = false;
+  } else if (aim_target_found_ && !aim_transition_active_) {
+    // 刚丢失目标，开始过渡计时
+    aim_transition_active_ = true;
+    aim_lost_time_ = this->now();
+  } else if (aim_transition_active_) {
+    double elapsed = (this->now() - aim_lost_time_).seconds();
+    if (elapsed >= AIM_TRANSITION_DELAY) {
+      aim_target_found_ = false;
+      aim_transition_active_ = false;
+    }
+  }
+
+  State prev_state = state_;
+
+  // ===== 比赛结束 =====
+  if (game_progress == 5) {
+    state_ = State::MATCH_END;
+    enable_rotor_ = false;
+    enable_search_ = false;
+    enable_aim_ = false;
+    nav_goal_sent_ = false;
+    return;
+  }
+
+  // ===== 比赛未开始 (0,1,2,3): PID待在家 =====
+  if (game_progress < 4) {
+    if (state_ != State::IDLE) {
+      state_ = State::IDLE;
+      nav_goal_sent_ = false;
+    }
+    enable_rotor_ = false;
+    enable_search_ = false;
+    enable_aim_ = false;
+    if (!nav_goal_sent_) {
+      send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+      nav_goal_sent_ = true;
+      RCLCPP_INFO(this->get_logger(), "[IDLE] PID待在家");
+    }
+    return;
+  }
+
+  // ===== 以下为 game_progress == 4 (比赛中) =====
+
+  // --- 回血优先级最高 (优先于自瞄) ---
+  // 在非撤退/补血状态下，血量低于阈值触发撤退
+  bool in_retreat = (state_ == State::RETREAT_TO_2 || state_ == State::RETREAT_TO_1 ||
+                     state_ == State::RETREAT_HOME || state_ == State::HEALING);
+
+  if (!in_retreat && hp > 0 && hp < HP_RETREAT_THRESHOLD) {
+    // 根据当前位置决定撤退起点
+    if (state_ == State::HOLD_GAIN || state_ == State::PATROL) {
+      state_ = State::RETREAT_TO_2;
+    } else if (state_ == State::GO_GAIN) {
+      state_ = State::RETREAT_TO_2;
+    } else if (state_ == State::GO_POINT2) {
+      state_ = State::RETREAT_TO_1;
+    } else {
+      state_ = State::RETREAT_HOME;
+    }
+    nav_goal_sent_ = false;
+    RCLCPP_WARN(this->get_logger(), "[RETREAT] 血量 %u < %u, 开始撤退!", hp, HP_RETREAT_THRESHOLD);
+  }
+
+  // ===== 状态机执行 =====
+  switch (state_) {
+    case State::IDLE: {
+      // 比赛刚开始(4)，从家出发
+      state_ = State::GO_POINT1;
+      nav_goal_sent_ = false;
+      enable_rotor_ = false;  // 出家过程不开小陀螺
+      enable_search_ = false;
+      enable_aim_ = false;
+      RCLCPP_INFO(this->get_logger(), "[GO_POINT1] 比赛开始，出发前往1号点");
+      break;
+    }
+
+    case State::GO_POINT1: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point1_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {  // 到达1号点
+        state_ = State::GO_POINT2;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_POINT2] 到达1号点，前往2号点");
+      }
+      break;
+    }
+
+    case State::GO_POINT2: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point2_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {
+        state_ = State::GO_GAIN;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_GAIN] 到达2号点，前往增益点");
+      }
+      break;
+    }
+
+    case State::GO_GAIN: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_gain_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (nav_status == 2) {
+        state_ = State::HOLD_GAIN;
+        nav_goal_sent_ = false;
+        enable_rotor_ = true;   // 到达增益点立刻开启小陀螺
+        enable_search_ = true;  // 开启云台search
+        RCLCPP_INFO(this->get_logger(), "[HOLD_GAIN] 到达增益点，开启陀螺+搜索");
+      }
+      break;
+    }
+
+    case State::HOLD_GAIN: {
+      enable_rotor_ = true;
+      if (aim_target_found_) {
+        // 发现目标: 原地陀螺 + 自瞄开火
+        enable_search_ = false;
+        enable_aim_ = true;
+        // 停止导航，原地不动
+        if (nav_goal_sent_) {
+          nav_goal_sent_ = false;
+        }
+      } else {
+        // 没有目标: 进入巡逻模式
+        enable_search_ = true;
+        enable_aim_ = false;
+        state_ = State::PATROL;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
+                             "[PATROL] 无目标，增益点附近巡逻");
+      }
+      break;
+    }
+
+    case State::PATROL: {
+      enable_rotor_ = true;
+      enable_search_ = true;
+      if (aim_target_found_) {
+        // 发现目标: 立刻停下，原地陀螺开火
+        state_ = State::HOLD_GAIN;
+        nav_goal_sent_ = false;
+        enable_aim_ = true;
+        enable_search_ = false;
+        RCLCPP_INFO(this->get_logger(), "[HOLD_GAIN] 巡逻中发现目标，原地开火");
+      } else {
+        // 随机巡逻
+        if (!nav_goal_sent_ || nav_status == 2) {
+          auto patrol_wp = random_patrol_point();
+          send_nav_goal(patrol_wp, 0, 1.0f, 0.2f);
+          nav_goal_sent_ = true;
+        }
+      }
+      break;
+    }
+
+    case State::RETREAT_TO_2: {
+      enable_rotor_ = true;  // 撤退过程开着小陀螺
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point2_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退到2号点");
+      }
+      if (nav_status == 2) {
+        state_ = State::RETREAT_TO_1;
+        nav_goal_sent_ = false;
+      }
+      break;
+    }
+
+    case State::RETREAT_TO_1: {
+      enable_rotor_ = true;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_point1_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退到1号点");
+      }
+      if (nav_status == 2) {
+        state_ = State::RETREAT_HOME;
+        nav_goal_sent_ = false;
+      }
+      break;
+    }
+
+    case State::RETREAT_HOME: {
+      enable_rotor_ = true;
+      enable_search_ = false;
+      enable_aim_ = false;
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+        RCLCPP_INFO(this->get_logger(), "[RETREAT] 撤退回家");
+      }
+      if (nav_status == 2) {
+        state_ = State::HEALING;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[HEALING] 到家，开始补血");
+      }
+      break;
+    }
+
+    case State::HEALING: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      // 待在家等血量恢复
+      if (!nav_goal_sent_) {
+        send_nav_goal(wp_home_, 0, nav_max_speed_, nav_tolerance_);
+        nav_goal_sent_ = true;
+      }
+      if (hp >= HP_HEALED_THRESHOLD) {
+        // 血量恢复，重新出发
+        state_ = State::GO_POINT1;
+        nav_goal_sent_ = false;
+        RCLCPP_INFO(this->get_logger(), "[GO_POINT1] 血量恢复到 %u，重新出发!", hp);
+      }
+      break;
+    }
+
+    case State::MATCH_END: {
+      enable_rotor_ = false;
+      enable_search_ = false;
+      enable_aim_ = false;
+      break;
+    }
+  }
+
+  if (state_ != prev_state) {
+    RCLCPP_INFO(this->get_logger(), "状态切换: %d -> %d",
+                static_cast<int>(prev_state), static_cast<int>(state_));
+  }
+}
+
+// ==================== 发送控制指令 ====================
+
+void DecisionNode::send_control_command()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  auto msg = rm_msgs::msg::DataAI();
+
+  // 云台控制
+  if (enable_aim_ && aim_target_found_ && has_aim_data_) {
+    // 自瞄模式: 转发视觉数据
+    msg.mode = latest_aim_data_.mode;
+    msg.yaw = latest_aim_data_.yaw;
+    msg.yaw_vel = latest_aim_data_.yaw_vel;
+    msg.yaw_acc = latest_aim_data_.yaw_acc;
+    msg.pitch = latest_aim_data_.pitch;
+    msg.pitch_vel = latest_aim_data_.pitch_vel;
+    msg.pitch_acc = latest_aim_data_.pitch_acc;
+  } else {
+    msg.mode = 0;
+  }
+
+  // 底盘速度: 来自 simple_move
+  // 在 HOLD_GAIN 且发现目标时停止移动(原地陀螺)
+  if (state_ == State::HOLD_GAIN && aim_target_found_) {
+    msg.vx = 0.0f;
+    msg.vy = 0.0f;
+    msg.wz = 0.0f;
+  } else if (state_ == State::MATCH_END) {
+    msg.vx = 0.0f;
+    msg.vy = 0.0f;
+    msg.wz = 0.0f;
+  } else {
+    msg.vx = static_cast<float>(latest_cmd_vel_.linear.x);
+    msg.vy = static_cast<float>(latest_cmd_vel_.linear.y);
+    msg.wz = static_cast<float>(latest_cmd_vel_.angular.z);
+  }
+
+  // reserved 标志位
+  uint8_t flags = 0;
+  if (enable_rotor_) flags |= 0x01;  // bit0: 底盘小陀螺
+  if (enable_search_) flags |= 0x02; // bit1: 云台搜索
+  msg.reserved = flags;
+
+  data_ai_pub_->publish(msg);
+}
+
+}  // namespace rm_decision
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(rm_decision::DecisionNode)

src/rm_decision/test/README.md

@@ -0,0 +1,79 @@
+# 简单导航测试脚本
+
+## 快速使用
+
+```bash
+# 直接运行
+python3 src/rm_decision/test/simple_nav_test.py
+
+# 或者通过 ros2 run
+ros2 run rm_decision simple_nav_test.py
+```
+
+## 修改目标点
+
+直接编辑 `simple_nav_test.py` 文件中的 `waypoints` 列表：
+
+```python
+# 格式: [x, y, angle, max_speed, tolerance, name]
+self.waypoints = [
+    [0.0, 0.0, 0.0, 1.0, 0.15, "起点"],
+    [1.0, 0.0, 0.0, 1.5, 0.15, "右"],
+    [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"],
+    [0.0, 1.0, math.pi, 1.5, 0.15, "左上"],
+    [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"],
+]
+```
+
+### 参数说明
+
+- `x, y`: 目标坐标（米）
+- `angle`: 目标角度（弧度）
+  - 0° = 0
+  - 90° = math.pi/2 = 1.57
+  - 180° = math.pi = 3.14
+  - -90° = -math.pi/2 = -1.57
+- `max_speed`: 最大速度（米/秒）
+- `tolerance`: 到达容差（米）
+- `name`: 目标点名称（用于日志显示）
+
+## 示例输出
+
+```
+[simple_nav_test]: ========================================
+[simple_nav_test]: 简单导航测试脚本启动
+[simple_nav_test]: 目标点数量: 5
+[simple_nav_test]: ========================================
+[simple_nav_test]:   [0] 起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]:   [1] 右: (1.00, 0.00, 0°) speed=1.5 tol=0.15
+[simple_nav_test]:   [2] 右上: (1.00, 1.00, 90°) speed=1.5 tol=0.15
+[simple_nav_test]:   [3] 左上: (0.00, 1.00, 180°) speed=1.5 tol=0.15
+[simple_nav_test]:   [4] 回到起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15
+[simple_nav_test]: ========================================
+[simple_nav_test]: 📍 发送目标点 [1/5] 起点: (0.00, 0.00, 0°) speed=1.0
+[simple_nav_test]: 🚗 导航中 [1/5] 起点: 距离=0.50m 状态=1 (5s)
+[simple_nav_test]: ✓ 到达目标点 [1/5] 起点 (用时 8.2秒)
+[simple_nav_test]: 📍 发送目标点 [2/5] 右: (1.00, 0.00, 0°) speed=1.5
+...
+[simple_nav_test]: ✅ 所有目标点完成！
+```
+
+## 优点
+
+- ✅ 无需编译，直接运行
+- ✅ 修改目标点只需编辑 Python 代码
+- ✅ 代码简单易懂
+- ✅ 实时显示导航进度
+- ✅ 自动处理超时和失败
+
+## 与 C++ 版本对比
+
+| 特性 | Python 脚本 | C++ test_nav_node |
+|------|------------|-------------------|
+| 修改目标点 | 编辑 Python 代码 | 编辑 YAML 配置 |
+| 运行方式 | 直接运行 | 需要编译 |
+| 灵活性 | 高（可随时修改） | 中（需重新编译） |
+| 性能 | 足够 | 更高 |
+| 适用场景 | 快速测试 | 生产环境 |
+
+推荐使用 Python 脚本进行快速测试！

src/rm_decision/test/simple_nav_test.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""
+简单的导航测试脚本
+直接在代码中定义目标点，通过 /nav_goal 话题发送给 simple_move
+"""
+
+import rclpy
+from rclpy.node import Node
+from rm_msgs.msg import NavGoal, NavStatus
+import time
+import math
+
+
+class SimpleNavTest(Node):
+    def __init__(self):
+        super().__init__('simple_nav_test')
+
+        # 发布者和订阅者
+        self.nav_goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10)
+        self.nav_status_sub = self.create_subscription(
+            NavStatus, '/nav_status', self.nav_status_callback, 10)
+
+        # 状态
+        self.current_status = None
+        self.current_distance = 0.0
+        self.navigation_started = False  # 标记导航是否真正开始
+
+        # 定义目标点列表 [x, y, angle, max_speed, tolerance, name]
+        self.waypoints = [
+            [0.0, 0.0, 0.0, 0.8, 1.0, "起点"],
+            [0.12, -6.75, 0.0, 0.8, 1.0, "1"],
+            [1.65, -6.76, 0.0, 0.8, 1.0, "2"],
+            [1.15, 0.5, 0.0, 0.8, 1.0, "3"],
+            [2.70, 0.7, 0.0, 0.8, 1.0, "4"],
+            [2.10, -6.72, 0.0, 0.8, 1.0, "5"],
+            [3.56, -6.82, 0.0, 0.8, 1.0, "6"],
+            [3.2, 0.9, 0.0, 0.8, 1.0, "7"],
+            [5.0, 0.56, 0.0, 0.8, 1.0, "8"],
+            [4.96, -5.96, 0.0, 0.8, 1.0, "9"],
+            # [0.0, 0.0, 0.0, 1.0, 0.25, "回到起点"],
+        ]
+
+        self.current_index = 0
+        self.goal_sent = False
+        self.goal_timeout = 120.0  # 超时时间（秒）
+        self.goal_start_time = None
+
+        self.get_logger().info('========================================')
+        self.get_logger().info('简单导航测试脚本启动')
+        self.get_logger().info(f'目标点数量: {len(self.waypoints)}')
+        self.get_logger().info('========================================')
+
+        # 显示所有目标点
+        for i, wp in enumerate(self.waypoints):
+            self.get_logger().info(
+                f'  [{i}] {wp[5]}: ({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) '
+                f'speed={wp[3]:.1f} tol={wp[4]:.2f}')
+
+        self.get_logger().info('========================================')
+
+        # 等待2秒后开始
+        time.sleep(2.0)
+        self.send_next_goal()
+
+        # 创建定时器检查状态
+        self.timer = self.create_timer(0.5, self.check_status)
+
+    def nav_status_callback(self, msg):
+        """接收导航状态"""
+        self.current_status = msg.status
+        self.current_distance = msg.distance
+
+    def send_next_goal(self):
+        """发送下一个目标点"""
+        if self.current_index >= len(self.waypoints):
+            self.get_logger().info('✅ 所有目标点完成！')
+            rclpy.shutdown()
+            return
+
+        wp = self.waypoints[self.current_index]
+
+        # 创建导航目标消息
+        goal_msg = NavGoal()
+        goal_msg.control_mode = 0  # 0: PID模式
+        goal_msg.x = float(wp[0])
+        goal_msg.y = float(wp[1])
+        goal_msg.angle = float(wp[2])
+        goal_msg.max_speed = float(wp[3])
+        goal_msg.tolerance = float(wp[4])
+
+        # 重置状态标志
+        self.navigation_started = False
+
+        # 发布目标
+        self.nav_goal_pub.publish(goal_msg)
+        self.goal_sent = True
+        self.goal_start_time = time.time()
+
+        self.get_logger().info(
+            f'📍 发送目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+            f'({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) speed={wp[3]:.1f}')
+
+    def check_status(self):
+        """检查导航状态"""
+        if not self.goal_sent:
+            return
+
+        if self.current_status is None:
+            return
+
+        wp = self.waypoints[self.current_index]
+        elapsed = time.time() - self.goal_start_time
+
+        # 标记导航已开始（状态为1表示正在导航）
+        if self.current_status == 1:
+            self.navigation_started = True
+
+        # 检查是否到达 (status == 2)，但必须先经过导航中状态
+        if self.current_status == 2:
+            # 如果导航还没开始就到达，说明是误判，等待真正开始
+            if not self.navigation_started and elapsed < 2.0:
+                return
+
+            self.get_logger().info(
+                f'✓ 到达目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
+                f'(用时 {elapsed:.1f}秒)')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)  # 短暂延迟，避免消息丢失
+            self.send_next_goal()
+            return
+
+        # 检查是否失败 (status == 3)
+        if self.current_status == 3:
+            self.get_logger().warn(
+                f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)
+            self.send_next_goal()
+            return
+
+        # 检查超时
+        if elapsed > self.goal_timeout:
+            self.get_logger().warn(
+                f'⏱ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} '
+                f'超时 ({elapsed:.1f}秒)，跳过')
+            self.current_index += 1
+            self.goal_sent = False
+            # time.sleep(0.1)
+            self.send_next_goal()
+            return
+
+        # 每5秒显示一次进度
+        if int(elapsed) % 5 == 0 and int(elapsed * 2) % 2 == 0:
+            self.get_logger().info(
+                f'🚗 导航中 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: '
+                f'距离={self.current_distance:.2f}m 状态={self.current_status} ({elapsed:.0f}s)')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    try:
+        node = SimpleNavTest()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print('\n用户中断')
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rm_msgs/.gitignore

@@ -0,0 +1,52 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+!msg/NavGoal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

src/rm_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_msgs)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/DataMCU.msg"
+        "msg/DataRef.msg"
+        "msg/DataAI.msg"
+        "msg/DataAim.msg"
+        "msg/NavGoal.msg"
+        "msg/NavStatus.msg"
+)
+
+ament_package()
+

src/rm_msgs/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 zucheng Lv
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/rm_msgs/README.md

@@ -0,0 +1,6 @@
+# rm_msg
+Some ROS 2 custom messages for Robotaster
+
+Usage
+Modify or add files in the /msg directory as needed
+colcon build

src/rm_msgs/msg/DataAI.msg

@@ -0,0 +1,13 @@
+# AI 控制数据 (上位机 -> MCU)
+
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角
+float32 yaw_vel     # 偏航角速度
+float32 yaw_acc     # 偏航角加速度
+float32 pitch       # 目标俯仰角
+float32 pitch_vel   # 俯仰角速度
+float32 pitch_acc   # 俯仰角加速度
+float32 vx          # X 方向速度
+float32 vy          # Y 方向速度
+float32 wz          # Z 方向角速度
+uint8 reserved      # 预留字段（用于传递底盘模式等）

src/rm_msgs/msg/DataAim.msg

@@ -0,0 +1,9 @@
+# 视觉自瞄控制数据
+
+uint8 mode          # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
+float32 yaw         # 目标偏航角
+float32 yaw_vel     # 偏航角速度
+float32 yaw_acc     # 偏航角加速度
+float32 pitch       # 目标俯仰角
+float32 pitch_vel   # 俯仰角速度
+float32 pitch_acc   # 俯仰角加速度

src/rm_msgs/msg/DataMCU.msg

@@ -0,0 +1,13 @@
+# MCU 状态数据 (MCU -> 上位机)
+
+uint8 mode              # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+float32 q0              # 四元数 w
+float32 q1              # 四元数 x
+float32 q2              # 四元数 y
+float32 q3              # 四元数 z
+float32 yaw             # 偏航角
+float32 yaw_vel         # 偏航角速度
+float32 pitch           # 俯仰角
+float32 pitch_vel       # 俯仰角速度
+float32 bullet_speed    # 弹速
+uint16 bullet_count     # 子弹累计发送次数

src/rm_msgs/msg/DataRef.msg

@@ -0,0 +1,5 @@
+# 裁判系统数据
+
+uint16 remain_hp            # 剩余血量
+uint8 game_progress         # 比赛进度
+uint16 stage_remain_time    # 比赛剩余时间

src/rm_msgs/msg/NavGoal.msg

@@ -0,0 +1,11 @@
+# 导航目标
+
+uint8 MODE_PID=0
+uint8 MODE_NAV=1
+
+uint8 control_mode      # 控制模式: 0=PID, 1=NAV
+float32 x               # 目标 X 坐标 (map)
+float32 y               # 目标 Y 坐标 (map)
+float32 angle           # 目标角度 (-pi ~ pi)
+float32 max_speed       # 最大线速度 (PID 模式使用)
+float32 tolerance       # 到达容差

src/rm_msgs/msg/NavStatus.msg

@@ -0,0 +1,7 @@
+# 导航状态
+
+uint8 status            # 0: 空闲, 1: 运行中, 2: 到达目标, 3: 失败
+float32 distance        # 距离目标的距离
+float32 current_x       # 当前 X 坐标
+float32 current_y       # 当前 Y 坐标
+float32 current_angle   # 当前角度

src/rm_msgs/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>rm_msgs</name>
+    <version>0.0.1</version>
+    <description>Describe custom messages</description>
+    <maintainer email="1683502971@qq.com">biao</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/rm_nav/.gitignore

@@ -0,0 +1,173 @@
+# ---> C
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+
+# ---> C++
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# ---> ROS
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+# *.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+# ---> ROS2
+install/
+log/
+build/
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+# *.pcd
+*.pyc
+qtcreator-*
+*.user
+
+*~
+
+# Emacs
+.#*
+
+# Colcon custom files
+COLCON_IGNORE
+AMENT_IGNORE
+
+.claude/

src/rm_nav/README.md

@@ -0,0 +1,20 @@
+# MOVE_AI
+
+青岛理工大学QUT
+MOVE战队
+RoboMaster比赛
+纯mid360激光雷达导航项目
+
+
+ros2 launch rm_nav_bringup bringup_real.launch.py \
+    world:=RUML \
+    mode:=nav \
+    lio:=fastlio \
+    lio_rviz:=True \
+    nav_rviz:=True
+
+
+
+    sudo mv /opt/MVS/lib/64/libusb-1.0.so.0 /opt/MVS/lib/64/libusb-1.0.so.0.bak
+[sudo] robofish 的密码： 
+robofish@robofish-ASUS-TUF-Gaming-F15-FX507ZM-FX507ZM:~/MOVE_AI$ sudo mv /opt/MVS/lib/32/libusb-1.0.so.0 /opt/MVS/lib/32/libusb-1.0.so.0.bak

src/rm_nav/build.sh

@@ -0,0 +1 @@
+colcon build --symlink-install

src/rm_nav/rm_driver/livox_ros_driver2/.gitignore

@@ -0,0 +1,10 @@
+devel/
+build/
+install/
+log/
+
+.vscode/
+__pycache__/
+.catkin_workspace
+*.gv
+*.pdf

src/rm_nav/rm_driver/livox_ros_driver2/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 ziknagXie
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/rm_nav/rm_driver/livox_ros_driver2/README.md

@@ -0,0 +1,329 @@
+# Livox ROS Driver 2
+
+Livox ROS Driver 2 is the 2nd-generation driver package used to connect LiDAR products produced by Livox, applicable for ROS (noetic recommended) and ROS2 (foxy or humble recommended).
+
+## ATTENTION
+
+本仓库为深圳北理莫斯科大学北极熊战队内部修改版，非原版 livox_ros_driver2 。
+
+相较于原版，修改了消息发布机制和 ```MID360_config.json``` 的 ip
+
+```ros2 launch msg_MID360_launch.py``` 时会同时发布 ```CustomMsg``` 和 ```PointCloud2``` 两种类型消息。
+
+ **Topic name**           | **Type**                        | **Note**       
+:------------------------:|:-------------------------------:|:--------------:
+ /livox/lidar             | livox_ros_driver2/msg/CustomMsg | mid360 自定义消息类型 
+ /livox/lidar/pointcloud | sensor_msgs/msg/PointCloud2     | ROS2 点云消息格式    
+ /livox/imu               | sensor_msgs/msg/Imu             | mid360 机内 imu            
+
+## 1. Preparation
+
+### 1.1 OS requirements
+
+  * Ubuntu 22.04 for ROS2 Humble;
+
+  **Tips:**
+
+  Colcon is a build tool used in ROS2.
+
+  How to install colcon: [Colcon installation instructions](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html)
+
+### 1.2 Install ROS2 Humble
+
+For ROS2 Humble installation, please refer to:
+[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
+
+Desktop-Full installation is recommend.
+
+## 2. Build & Run Livox ROS Driver 2
+
+### 2.1 Clone Livox ROS Driver 2 source code:
+
+```shell
+git clone https://gitee.com/SMBU-POLARBEAR/livox_ros_driver2_humble.git
+```
+
+  **Note :**
+
+  Be sure to clone the source code in a '[work_space]/src/' folder (as shown above), otherwise compilation errors will occur due to the compilation tool restriction.
+
+### 2.2 Build & install the Livox-SDK2
+
+  **Note :**
+
+  Please follow the guidance of installation in the [Livox-SDK2/README.md](https://github.com/Livox-SDK/Livox-SDK2/blob/master/README.md)
+
+### 2.3 Build the Livox ROS Driver 2:
+
+### For ROS2 Humble:
+```shell
+colcon build --symlink-install
+```
+
+### 2.4 Run Livox ROS Driver 2:
+
+```shell
+source install/setup.sh
+ros2 launch livox_ros_driver2 [launch file]
+```
+
+in which,  
+
+* **[launch file]** : is the ROS2 launch file you want to use; the 'launch_ROS2' folder contains several launch samples for your reference.
+
+A rviz launch example for HAP LiDAR would be:
+
+```shell
+ros2 launch livox_ros_driver2 rviz_HAP_launch.py
+```
+
+## 3. Launch file and livox_ros_driver2 internal parameter configuration instructions
+
+### 3.1 Launch file configuration instructions
+
+Launch files of ROS are in the "ws_livox/src/livox_ros_driver2/launch_ROS1" directory and launch files of ROS2 are in the "ws_livox/src/livox_ros_driver2/launch_ROS2" directory. Different launch files have different configuration parameter values and are used in different scenarios:
+
+| launch file name          | Description                                                  |
+| ------------------------- | ------------------------------------------------------------ |
+| rviz_HAP.launch   | Connect to HAP LiDAR device<br>Publish pointcloud2 format  data<br>Autoload rviz |
+| msg_HAP.launch     | Connect to HAP LiDAR device<br>Publish livox customized pointcloud data|
+| rviz_MID360.launch        | Connect to MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
+| msg_MID360.launch          | Connect to MID360 LiDAR device<br>Publish livox customized pointcloud data |
+| rviz_mixed.launch    | Connect to HAP and MID360 LiDAR device<br>Publish pointcloud2 format data <br>Autoload rviz|
+| msg_mixed.launch      | Connect to HAP and MID360 LiDAR device<br>Publish livox customized pointcloud data |
+
+### 3.2 Livox ros driver 2 internal main parameter configuration instructions
+
+All internal parameters of Livox_ros_driver2 are in the launch file. Below are detailed descriptions of the three commonly used parameters :
+
+| Parameter    | Detailed description                                         | Default |
+| ------------ | ------------------------------------------------------------ | ------- |
+| publish_freq | Set the frequency of point cloud publish <br>Floating-point data type, recommended values 5.0, 10.0, 20.0, 50.0, etc. The maximum publish frequency is 100.0 Hz.| 10.0    |
+| multi_topic  | If the LiDAR device has an independent topic to publish pointcloud data<br>0 -- All LiDAR devices use the same topic to publish pointcloud data<br>1 -- Each LiDAR device has its own topic to publish point cloud data | 0       |
+| xfer_format  | Set pointcloud format<br>0 -- Livox pointcloud2(PointXYZRTLT) pointcloud format<br>1 -- Livox customized pointcloud format<br>2 -- Standard pointcloud2 (pcl :: PointXYZI) pointcloud format in the PCL library (just for ROS) | 0       |
+
+  **Note :**
+
+  Other parameters not mentioned in this table are not suggested to be changed unless fully understood.
+
+&ensp;&ensp;&ensp;&ensp;***Livox_ros_driver2 pointcloud data detailed description :***
+
+1. Livox pointcloud2 (PointXYZRTLT) point cloud format, as follows :
+
+```c
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+float32 intensity       # the value is reflectivity, 0.0~255.0
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
+float64 timestamp       # Timestamp of point
+```
+  **Note :**
+
+  The number of points in the frame may be different, but each point provides a timestamp.
+
+2. Livox customized data package format, as follows :
+
+```c
+std_msgs/Header header     # ROS standard message header
+uint64          timebase   # The time of first point
+uint32          point_num  # Total number of pointclouds
+uint8           lidar_id   # Lidar device id number
+uint8[3]        rsvd       # Reserved use
+CustomPoint[]   points     # Pointcloud data
+```
+
+&ensp;&ensp;&ensp;&ensp;Customized Point Cloud (CustomPoint) format in the above customized data package :
+
+```c
+uint32  offset_time     # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8   reflectivity    # reflectivity, 0~255
+uint8   tag             # livox tag
+uint8   line            # laser number in lidar
+```
+
+3. The standard pointcloud2 (pcl :: PointXYZI) format in the PCL library (only ROS can publish):
+
+&ensp;&ensp;&ensp;&ensp;Please refer to the pcl :: PointXYZI data structure in the point_types.hpp file of the PCL library.
+
+## 4. LiDAR config
+
+LiDAR Configurations (such as ip, port, data type... etc.) can be set via a json-style config file. Config files for single HAP, Mid360 and mixed-LiDARs are in the "config" folder. The parameter naming *'user_config_path'* in launch files indicates such json file path.
+
+1. Follow is a configuration example for HAP LiDAR (located in config/HAP_config.json):
+
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index, please don't revise this value
+  },
+  "HAP": {
+    "device_type" : "HAP",
+    "lidar_ipaddr": "",
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,  # command port
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",  # host ip (it can be revised)
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",  # ip of the LiDAR you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "blind_spot_set" : 50,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+```
+
+The parameter attributes in the above json file are described in the following table :
+
+**LiDAR configuration parameter**
+| Parameter                  | Type    | Description                                                  | Default         |
+| :------------------------- | ------- | ------------------------------------------------------------ | --------------- |
+| ip             | String  | Ip of the LiDAR you want to config | 192.168.1.100 |
+| pcl_data_type             | Int | Choose the resolution of the point cloud data to send<br>1 -- Cartesian coordinate data (32 bits)<br>2 -- Cartesian coordinate data (16 bits) <br>3 --Spherical coordinate data| 1           |
+| pattern_mode                | Int     | Space scan pattern<br>0 -- non-repeating scanning pattern mode<br>1 -- repeating scanning pattern mode <br>2 -- repeating scanning pattern mode (low scanning rate) | 0               |
+| blind_spot_set (Only for HAP LiDAR)                 | Int     | Set blind spot<br>Range from 50 cm to 200 cm               | 50               |
+| extrinsic_parameter |      | Set extrinsic parameter<br> The data types of "roll" "picth" "yaw" are float <br>  The data types of "x" "y" "z" are int<br>               |
+
+For more infomation about the HAP config, please refer to:
+[HAP Config File Description](https://github.com/Livox-SDK/Livox-SDK2/wiki/hap-config-file-description)
+
+2. When connecting multiple LiDARs, add objects corresponding to different LiDARs to the "lidar_configs" array. Examples of mixed-LiDARs config file contents are as follows :
+
+```json
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8  # protocol type index, please don't revise this value
+  },
+  "HAP": {
+    "lidar_net_info" : {  # HAP ports, please don't revise these values
+      "cmd_data_port": 56000,  # HAP command port
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",  # host ip
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "MID360": {
+    "lidar_net_info" : {  # Mid360 ports, please don't revise these values
+      "cmd_data_port": 56100,  # Mid360 command port
+      "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",  # host ip
+      "cmd_data_port": 56101,
+      "push_msg_ip": "192.168.1.5",  # host ip
+      "push_msg_port": 56201,
+      "point_data_ip": "192.168.1.5",  # host ip
+      "point_data_port": 56301,
+      "imu_data_ip" : "192.168.1.5",  # host ip
+      "imu_data_port": 56401,
+      "log_data_ip" : "",
+      "log_data_port": 56501
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",  # ip of the HAP you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "blind_spot_set" : 50,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    },
+    {
+      "ip" : "192.168.1.12",  # ip of the Mid360 you want to config
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+```
+
+## 5. Supported LiDAR list
+
+* HAP
+* Mid360
+* (more types are comming soon...)
+
+## 6. FAQ
+
+### 6.1 launch with "livox_lidar_rviz_HAP.launch" but no point cloud display on the grid?
+
+Please check the "Global Options - Fixed Frame" field in the RViz "Display" pannel. Set the field value to "livox_frame" and check the "PointCloud2" option in the pannel.
+
+### 6.2 launch with command "ros2 launch livox_lidar_rviz_HAP_launch.py" but cannot open shared object file "liblivox_sdk_shared.so" ?
+
+Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
+
+* If you want to add to current terminal:
+
+  ```shell
+  export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
+  ```
+
+* If you want to add to current user:
+
+  ```shell
+  vim ~/.bashrc
+  export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
+  source ~/.bashrc
+  ```

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/allocators.h

@@ -0,0 +1,308 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ALLOCATORS_H_
+#define RAPIDJSON_ALLOCATORS_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Allocator
+
+/*! \class rapidjson::Allocator
+    \brief Concept for allocating, resizing and freeing memory block.
+
+    Note that Malloc() and Realloc() are non-static but Free() is static.
+
+    So if an allocator need to support Free(), it needs to put its pointer in
+    the header of memory block.
+
+\code
+concept Allocator {
+    static const bool kNeedFree;    //!< Whether this allocator needs to call
+Free().
+
+    // Allocate a memory block.
+    // \param size of the memory block in bytes.
+    // \returns pointer to the memory block.
+    void* Malloc(size_t size);
+
+    // Resize a memory block.
+    // \param originalPtr The pointer to current memory block. Null pointer is
+permitted.
+    // \param originalSize The current size in bytes. (Design issue: since some
+allocator may not book-keep this, explicitly pass to it can save memory.)
+    // \param newSize the new size in bytes.
+    void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
+
+    // Free a memory block.
+    // \param pointer to the memory block. Null pointer is permitted.
+    static void Free(void *ptr);
+};
+\endcode
+*/
+
+/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kDefaultChunkCapacity definition.
+
+    User can define this as any \c size that is a power of 2.
+*/
+
+#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// CrtAllocator
+
+//! C-runtime library allocator.
+/*! This class is just wrapper for standard C library memory routines.
+    \note implements Allocator concept
+*/
+class CrtAllocator {
+ public:
+  static const bool kNeedFree = true;
+  void *Malloc(size_t size) {
+    if (size)  //  behavior of malloc(0) is implementation defined.
+      return std::malloc(size);
+    else
+      return NULL;  // standardize to returning NULL.
+  }
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    (void)originalSize;
+    if (newSize == 0) {
+      std::free(originalPtr);
+      return NULL;
+    }
+    return std::realloc(originalPtr, newSize);
+  }
+  static void Free(void *ptr) { std::free(ptr); }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// MemoryPoolAllocator
+
+//! Default memory allocator used by the parser and DOM.
+/*! This allocator allocate memory blocks from pre-allocated memory chunks.
+
+    It does not free memory blocks. And Realloc() only allocate new memory.
+
+    The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
+   default.
+
+    User may also supply a buffer as the first chunk.
+
+    If the user-buffer is full then additional chunks are allocated by
+   BaseAllocator.
+
+    The user-buffer is not deallocated by this allocator.
+
+    \tparam BaseAllocator the allocator type for allocating memory chunks.
+   Default is CrtAllocator. \note implements Allocator concept
+*/
+template <typename BaseAllocator = CrtAllocator>
+class MemoryPoolAllocator {
+ public:
+  static const bool kNeedFree =
+      false;  //!< Tell users that no need to call Free() with this allocator.
+              //!< (concept Allocator)
+
+  //! Constructor with chunkSize.
+  /*! \param chunkSize The size of memory chunk. The default is
+     kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
+     chunks.
+  */
+  MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(0),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {}
+
+  //! Constructor with user-supplied buffer.
+  /*! The user buffer will be used firstly. When it is full, memory pool
+     allocates new chunk with chunk size.
+
+      The user buffer will not be deallocated when this allocator is destructed.
+
+      \param buffer User supplied buffer.
+      \param size Size of the buffer in bytes. It must at least larger than
+     sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
+     is kDefaultChunkSize. \param baseAllocator The allocator for allocating
+     memory chunks.
+  */
+  MemoryPoolAllocator(void *buffer, size_t size,
+                      size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(buffer),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {
+    RAPIDJSON_ASSERT(buffer != 0);
+    RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
+    chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
+    chunkHead_->capacity = size - sizeof(ChunkHeader);
+    chunkHead_->size = 0;
+    chunkHead_->next = 0;
+  }
+
+  //! Destructor.
+  /*! This deallocates all memory chunks, excluding the user-supplied buffer.
+   */
+  ~MemoryPoolAllocator() {
+    Clear();
+    RAPIDJSON_DELETE(ownBaseAllocator_);
+  }
+
+  //! Deallocates all memory chunks, excluding the user-supplied buffer.
+  void Clear() {
+    while (chunkHead_ && chunkHead_ != userBuffer_) {
+      ChunkHeader *next = chunkHead_->next;
+      baseAllocator_->Free(chunkHead_);
+      chunkHead_ = next;
+    }
+    if (chunkHead_ && chunkHead_ == userBuffer_)
+      chunkHead_->size = 0;  // Clear user buffer
+  }
+
+  //! Computes the total capacity of allocated memory chunks.
+  /*! \return total capacity in bytes.
+   */
+  size_t Capacity() const {
+    size_t capacity = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
+      capacity += c->capacity;
+    return capacity;
+  }
+
+  //! Computes the memory blocks allocated.
+  /*! \return total used bytes.
+   */
+  size_t Size() const {
+    size_t size = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
+    return size;
+  }
+
+  //! Allocates a memory block. (concept Allocator)
+  void *Malloc(size_t size) {
+    if (!size) return NULL;
+
+    size = RAPIDJSON_ALIGN(size);
+    if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
+      if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
+        return NULL;
+
+    void *buffer = reinterpret_cast<char *>(chunkHead_) +
+                   RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
+    chunkHead_->size += size;
+    return buffer;
+  }
+
+  //! Resizes a memory block (concept Allocator)
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    if (originalPtr == 0) return Malloc(newSize);
+
+    if (newSize == 0) return NULL;
+
+    originalSize = RAPIDJSON_ALIGN(originalSize);
+    newSize = RAPIDJSON_ALIGN(newSize);
+
+    // Do not shrink if new size is smaller than original
+    if (originalSize >= newSize) return originalPtr;
+
+    // Simply expand it if it is the last allocation and there is sufficient
+    // space
+    if (originalPtr ==
+        reinterpret_cast<char *>(chunkHead_) +
+            RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
+            originalSize) {
+      size_t increment = static_cast<size_t>(newSize - originalSize);
+      if (chunkHead_->size + increment <= chunkHead_->capacity) {
+        chunkHead_->size += increment;
+        return originalPtr;
+      }
+    }
+
+    // Realloc process: allocate and copy memory, do not free original buffer.
+    if (void *newBuffer = Malloc(newSize)) {
+      if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
+      return newBuffer;
+    } else
+      return NULL;
+  }
+
+  //! Frees a memory block (concept Allocator)
+  static void Free(void *ptr) { (void)ptr; }  // Do nothing
+
+ private:
+  //! Copy constructor is not permitted.
+  MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
+  //! Copy assignment operator is not permitted.
+  MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
+
+  //! Creates a new chunk.
+  /*! \param capacity Capacity of the chunk in bytes.
+      \return true if success.
+  */
+  bool AddChunk(size_t capacity) {
+    if (!baseAllocator_)
+      ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
+    if (ChunkHeader *chunk =
+            reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
+                RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
+      chunk->capacity = capacity;
+      chunk->size = 0;
+      chunk->next = chunkHead_;
+      chunkHead_ = chunk;
+      return true;
+    } else
+      return false;
+  }
+
+  static const int kDefaultChunkCapacity =
+      RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY;  //!< Default chunk capacity.
+
+  //! Chunk header for perpending to each chunk.
+  /*! Chunks are stored as a singly linked list.
+   */
+  struct ChunkHeader {
+    size_t capacity;  //!< Capacity of the chunk in bytes (excluding the header
+                      //!< itself).
+    size_t size;      //!< Current size of allocated memory in bytes.
+    ChunkHeader *next;  //!< Next chunk in the linked list.
+  };
+
+  ChunkHeader *chunkHead_;  //!< Head of the chunk linked-list. Only the head
+                            //!< chunk serves allocation.
+  size_t chunk_capacity_;   //!< The minimum capacity of chunk when they are
+                            //!< allocated.
+  void *userBuffer_;        //!< User supplied buffer.
+  BaseAllocator
+      *baseAllocator_;  //!< base allocator for allocating memory chunks.
+  BaseAllocator *ownBaseAllocator_;  //!< base allocator created by this object.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ENCODINGS_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/cursorstreamwrapper.h

@@ -0,0 +1,81 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
+#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
+
+#include "stream.h"
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Cursor stream wrapper for counting line and column number if error exists.
+/*!
+    \tparam InputStream     Any stream that implements Stream Concept
+*/
+template <typename InputStream, typename Encoding = UTF8<>>
+class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  CursorStreamWrapper(InputStream &is)
+      : GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
+
+  // counting line and column number
+  Ch Take() {
+    Ch ch = this->is_.Take();
+    if (ch == '\n') {
+      line_++;
+      col_ = 0;
+    } else {
+      col_++;
+    }
+    return ch;
+  }
+
+  //! Get the error line number, if error exists.
+  size_t GetLine() const { return line_; }
+  //! Get the error column number, if error exists.
+  size_t GetColumn() const { return col_; }
+
+ private:
+  size_t line_;  //!< Current Line
+  size_t col_;   //!< Current Column
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CURSORSTREAMWRAPPER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/encodedstream.h

@@ -0,0 +1,407 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ENCODEDSTREAM_H_
+#define RAPIDJSON_ENCODEDSTREAM_H_
+
+#include "memorystream.h"
+#include "stream.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Input byte stream wrapper with a statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input
+   byte stream. For example, FileReadStream.
+*/
+template <typename Encoding, typename InputByteStream>
+class EncodedInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedInputStream(InputByteStream &is) : is_(is) {
+    current_ = Encoding::TakeBOM(is_);
+  }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = Encoding::Take(is_);
+    return c;
+  }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+
+  InputByteStream &is_;
+  Ch current_;
+};
+
+//! Specialized for UTF8 MemoryStream.
+template <>
+class EncodedInputStream<UTF8<>, MemoryStream> {
+ public:
+  typedef UTF8<>::Ch Ch;
+
+  EncodedInputStream(MemoryStream &is) : is_(is) {
+    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
+  }
+  Ch Peek() const { return is_.Peek(); }
+  Ch Take() { return is_.Take(); }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) {}
+  void Flush() {}
+  Ch *PutBegin() { return 0; }
+  size_t PutEnd(Ch *) { return 0; }
+
+  MemoryStream &is_;
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+};
+
+//! Output byte stream wrapper with statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam OutputByteStream Type of input
+   byte stream. For example, FileWriteStream.
+*/
+template <typename Encoding, typename OutputByteStream>
+class EncodedOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
+    if (putBOM) Encoding::PutBOM(os_);
+  }
+
+  void Put(Ch c) { Encoding::Put(os_, c); }
+  void Flush() { os_.Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedOutputStream(const EncodedOutputStream &);
+  EncodedOutputStream &operator=(const EncodedOutputStream &);
+
+  OutputByteStream &os_;
+};
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+//! Input stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for reading.
+    \tparam InputByteStream type of input byte stream to be wrapped.
+*/
+template <typename CharType, typename InputByteStream>
+class AutoUTFInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param is input stream to be wrapped.
+      \param type UTF encoding type if it is not detected from the stream.
+  */
+  AutoUTFInputStream(InputByteStream &is, UTFType type = kUTF8)
+      : is_(&is), type_(type), hasBOM_(false) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+    DetectType();
+    static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
+    takeFunc_ = f[type_];
+    current_ = takeFunc_(*is_);
+  }
+
+  UTFType GetType() const { return type_; }
+  bool HasBOM() const { return hasBOM_; }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = takeFunc_(*is_);
+    return c;
+  }
+  size_t Tell() const { return is_->Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFInputStream(const AutoUTFInputStream &);
+  AutoUTFInputStream &operator=(const AutoUTFInputStream &);
+
+  // Detect encoding type with BOM or RFC 4627
+  void DetectType() {
+    // BOM (Byte Order Mark):
+    // 00 00 FE FF  UTF-32BE
+    // FF FE 00 00  UTF-32LE
+    // FE FF        UTF-16BE
+    // FF FE        UTF-16LE
+    // EF BB BF     UTF-8
+
+    const unsigned char *c =
+        reinterpret_cast<const unsigned char *>(is_->Peek4());
+    if (!c) return;
+
+    unsigned bom =
+        static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
+    hasBOM_ = false;
+    if (bom == 0xFFFE0000) {
+      type_ = kUTF32BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if (bom == 0x0000FEFF) {
+      type_ = kUTF32LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFFFE) {
+      type_ = kUTF16BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFEFF) {
+      type_ = kUTF16LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
+      type_ = kUTF8;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    }
+
+    // RFC 4627: Section 3
+    // "Since the first two characters of a JSON text will always be ASCII
+    // characters [RFC0020], it is possible to determine whether an octet
+    // stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
+    // at the pattern of nulls in the first four octets."
+    // 00 00 00 xx  UTF-32BE
+    // 00 xx 00 xx  UTF-16BE
+    // xx 00 00 00  UTF-32LE
+    // xx 00 xx 00  UTF-16LE
+    // xx xx xx xx  UTF-8
+
+    if (!hasBOM_) {
+      int pattern =
+          (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
+      switch (pattern) {
+        case 0x08:
+          type_ = kUTF32BE;
+          break;
+        case 0x0A:
+          type_ = kUTF16BE;
+          break;
+        case 0x01:
+          type_ = kUTF32LE;
+          break;
+        case 0x05:
+          type_ = kUTF16LE;
+          break;
+        case 0x0F:
+          type_ = kUTF8;
+          break;
+        default:
+          break;  // Use type defined by user.
+      }
+    }
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+  }
+
+  typedef Ch (*TakeFunc)(InputByteStream &is);
+  InputByteStream *is_;
+  UTFType type_;
+  Ch current_;
+  TakeFunc takeFunc_;
+  bool hasBOM_;
+};
+
+//! Output stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for writing.
+    \tparam OutputByteStream type of output byte stream to be wrapped.
+*/
+template <typename CharType, typename OutputByteStream>
+class AutoUTFOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param os output stream to be wrapped.
+      \param type UTF encoding type.
+      \param putBOM Whether to write BOM at the beginning of the stream.
+  */
+  AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM)
+      : os_(&os), type_(type) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+
+    static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
+    putFunc_ = f[type_];
+
+    if (putBOM) PutBOM();
+  }
+
+  UTFType GetType() const { return type_; }
+
+  void Put(Ch c) { putFunc_(*os_, c); }
+  void Flush() { os_->Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFOutputStream(const AutoUTFOutputStream &);
+  AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
+
+  void PutBOM() {
+    typedef void (*PutBOMFunc)(OutputByteStream &);
+    static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)};
+    f[type_](*os_);
+  }
+
+  typedef void (*PutFunc)(OutputByteStream &, Ch);
+
+  OutputByteStream *os_;
+  UTFType type_;
+  PutFunc putFunc_;
+};
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/encodings.h

@@ -0,0 +1,816 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ENCODINGS_H_
+#define RAPIDJSON_ENCODINGS_H_
+
+#include "rapidjson.h"
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(
+    4244)  // conversion from 'type1' to 'type2', possible loss of data
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+#elif defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(overflow)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Encoding
+
+/*! \class rapidjson::Encoding
+    \brief Concept for encoding of Unicode characters.
+
+\code
+concept Encoding {
+    typename Ch;    //! Type of character. A "character" is actually a code unit
+in unicode's definition.
+
+    enum { supportUnicode = 1 }; // or 0 if not supporting unicode
+
+    //! \brief Encode a Unicode codepoint to an output stream.
+    //! \param os Output stream.
+    //! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
+inclusively. template<typename OutputStream> static void Encode(OutputStream&
+os, unsigned codepoint);
+
+    //! \brief Decode a Unicode codepoint from an input stream.
+    //! \param is Input stream.
+    //! \param codepoint Output of the unicode codepoint.
+    //! \return true if a valid codepoint can be decoded from the stream.
+    template <typename InputStream>
+    static bool Decode(InputStream& is, unsigned* codepoint);
+
+    //! \brief Validate one Unicode codepoint from an encoded stream.
+    //! \param is Input stream to obtain codepoint.
+    //! \param os Output for copying one codepoint.
+    //! \return true if it is valid.
+    //! \note This function just validating and copying the codepoint without
+actually decode it. template <typename InputStream, typename OutputStream>
+    static bool Validate(InputStream& is, OutputStream& os);
+
+    // The following functions are deal with byte streams.
+
+    //! Take a character from input byte stream, skip BOM if exist.
+    template <typename InputByteStream>
+    static CharType TakeBOM(InputByteStream& is);
+
+    //! Take a character from input byte stream.
+    template <typename InputByteStream>
+    static Ch Take(InputByteStream& is);
+
+    //! Put BOM to output byte stream.
+    template <typename OutputByteStream>
+    static void PutBOM(OutputByteStream& os);
+
+    //! Put a character to output byte stream.
+    template <typename OutputByteStream>
+    static void Put(OutputByteStream& os, Ch c);
+};
+\endcode
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF8
+
+//! UTF-8 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-8
+    http://tools.ietf.org/html/rfc3629
+    \tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct UTF8 {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      os.Put(static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+#define RAPIDJSON_COPY() \
+  c = is.Take();         \
+  *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    typename InputStream::Ch c = is.Take();
+    if (!(c & 0x80)) {
+      *codepoint = static_cast<unsigned char>(c);
+      return true;
+    }
+
+    unsigned char type = GetRange(static_cast<unsigned char>(c));
+    if (type >= 32) {
+      *codepoint = 0;
+    } else {
+      *codepoint = (0xFFu >> type) & static_cast<unsigned char>(c);
+    }
+    bool result = true;
+    switch (type) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+#define RAPIDJSON_COPY() os.Put(c = is.Take())
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    Ch c;
+    RAPIDJSON_COPY();
+    if (!(c & 0x80)) return true;
+
+    bool result = true;
+    switch (GetRange(static_cast<unsigned char>(c))) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  static unsigned char GetRange(unsigned char c) {
+    // Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
+    // With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
+    // can test multiple types.
+    static const unsigned char type[] = {
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0x10, 0x10, 0x10, 0x10,
+        0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
+        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
+        0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        8,    8,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    10,   3,    3,    3,
+        3,    3,    3,    3,    3,    3,    3,    3,    3,    4,    3,    3,
+        11,   6,    6,    6,    5,    8,    8,    8,    8,    8,    8,    8,
+        8,    8,    8,    8,
+    };
+    return type[c];
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    typename InputByteStream::Ch c = Take(is);
+    if (static_cast<unsigned char>(c) != 0xEFu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBBu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBFu) return c;
+    c = is.Take();
+    return c;
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF16
+
+//! UTF-16 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-16
+    http://tools.ietf.org/html/rfc2781
+    \tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
+   C++11 may use char16_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF16LE and UTF16BE, which handle endianness.
+*/
+template <typename CharType = wchar_t>
+struct UTF16 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      os.Put(static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      os.Put(static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      PutUnsafe(os,
+                static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    typename InputStream::Ch c = is.Take();
+    if (c < 0xD800 || c > 0xDFFF) {
+      *codepoint = static_cast<unsigned>(c);
+      return true;
+    } else if (c <= 0xDBFF) {
+      *codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
+      c = is.Take();
+      *codepoint |= (static_cast<unsigned>(c) & 0x3FF);
+      *codepoint += 0x10000;
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    typename InputStream::Ch c;
+    os.Put(static_cast<typename OutputStream::Ch>(c = is.Take()));
+    if (c < 0xD800 || c > 0xDFFF)
+      return true;
+    else if (c <= 0xDBFF) {
+      os.Put(c = is.Take());
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+};
+
+//! UTF-16 little endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16LE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+  }
+};
+
+//! UTF-16 big endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16BE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF32
+
+//! UTF-32 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-32
+    \tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
+   C++11 may use char32_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF32LE and UTF32BE, which handle endianness.
+*/
+template <typename CharType = unsigned>
+struct UTF32 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    os.Put(codepoint);
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    PutUnsafe(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c = is.Take();
+    *codepoint = c;
+    return c <= 0x10FFFF;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c;
+    os.Put(c = is.Take());
+    return c <= 0x10FFFF;
+  }
+};
+
+//! UTF-32 little endian enocoding.
+template <typename CharType = unsigned>
+struct UTF32LE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+  }
+};
+
+//! UTF-32 big endian encoding.
+template <typename CharType = unsigned>
+struct UTF32BE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// ASCII
+
+//! ASCII encoding.
+/*! http://en.wikipedia.org/wiki/ASCII
+    \tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct ASCII {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 0 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    os.Put(static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    *codepoint = c;
+    return c <= 0X7F;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    os.Put(static_cast<typename OutputStream::Ch>(c));
+    return c <= 0x7F;
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    uint8_t c = static_cast<uint8_t>(Take(is));
+    return static_cast<Ch>(c);
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    (void)os;
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// AutoUTF
+
+//! Runtime-specified UTF encoding type of a stream.
+enum UTFType {
+  kUTF8 = 0,     //!< UTF-8.
+  kUTF16LE = 1,  //!< UTF-16 little endian.
+  kUTF16BE = 2,  //!< UTF-16 big endian.
+  kUTF32LE = 3,  //!< UTF-32 little endian.
+  kUTF32BE = 4   //!< UTF-32 big endian.
+};
+
+//! Dynamically select encoding according to stream's runtime-specified UTF
+//! encoding type.
+/*! \note This class can be used with AutoUTFInputtStream and
+ * AutoUTFOutputStream, which provides GetType().
+ */
+template <typename CharType>
+struct AutoUTF {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
+                                           unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
+                                                 unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
+                                           unsigned *codepoint) {
+    typedef bool (*DecodeFunc)(InputStream &, unsigned *);
+    static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
+    return (*f[is.GetType()])(is, codepoint);
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
+    static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
+    return (*f[is.GetType()])(is, os);
+  }
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Transcoder
+
+//! Encoding conversion.
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder {
+  //! Take one Unicode codepoint from source encoding, convert it to target
+  //! encoding and put it to the output stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::Encode(os, codepoint);
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::EncodeUnsafe(os, codepoint);
+    return true;
+  }
+
+  //! Validate one Unicode codepoint from an encoded stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Transcode(
+        is, os);  // Since source/target encoding is different, must transcode.
+  }
+};
+
+// Forward declaration.
+template <typename Stream>
+inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
+
+//! Specialization of Transcoder with same source and target encoding.
+template <typename Encoding>
+struct Transcoder<Encoding, Encoding> {
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    os.Put(is.Take());  // Just copy one code unit. This semantic is different
+                        // from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    PutUnsafe(os, is.Take());  // Just copy one code unit. This semantic is
+                               // different from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Encoding::Validate(is, os);  // source/target encoding are the same
+  }
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__))
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ENCODINGS_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/error/en.h

@@ -0,0 +1,104 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ERROR_EN_H_
+#define RAPIDJSON_ERROR_EN_H_
+
+#include "error.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(switch - enum)
+RAPIDJSON_DIAG_OFF(covered - switch - default)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Maps error code of parsing into error message.
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \param parseErrorCode Error code obtained in parsing.
+    \return the error message.
+    \note User can make a copy of this function for localization.
+        Using switch-case is safer for future modification of error codes.
+*/
+inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
+    ParseErrorCode parseErrorCode) {
+  switch (parseErrorCode) {
+    case kParseErrorNone:
+      return RAPIDJSON_ERROR_STRING("No error.");
+
+    case kParseErrorDocumentEmpty:
+      return RAPIDJSON_ERROR_STRING("The document is empty.");
+    case kParseErrorDocumentRootNotSingular:
+      return RAPIDJSON_ERROR_STRING(
+          "The document root must not be followed by other values.");
+
+    case kParseErrorValueInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid value.");
+
+    case kParseErrorObjectMissName:
+      return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
+    case kParseErrorObjectMissColon:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a colon after a name of object member.");
+    case kParseErrorObjectMissCommaOrCurlyBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or '}' after an object member.");
+
+    case kParseErrorArrayMissCommaOrSquareBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or ']' after an array element.");
+
+    case kParseErrorStringUnicodeEscapeInvalidHex:
+      return RAPIDJSON_ERROR_STRING(
+          "Incorrect hex digit after \\u escape in string.");
+    case kParseErrorStringUnicodeSurrogateInvalid:
+      return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
+    case kParseErrorStringEscapeInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
+    case kParseErrorStringMissQuotationMark:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a closing quotation mark in string.");
+    case kParseErrorStringInvalidEncoding:
+      return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
+
+    case kParseErrorNumberTooBig:
+      return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
+    case kParseErrorNumberMissFraction:
+      return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
+    case kParseErrorNumberMissExponent:
+      return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
+
+    case kParseErrorTermination:
+      return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
+    case kParseErrorUnspecificSyntaxError:
+      return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
+
+    default:
+      return RAPIDJSON_ERROR_STRING("Unknown error.");
+  }
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_EN_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/error/error.h

@@ -0,0 +1,186 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ERROR_ERROR_H_
+#define RAPIDJSON_ERROR_ERROR_H_
+
+#include "../rapidjson.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+/*! \file error.h */
+
+/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_CHARTYPE
+
+//! Character type of error messages.
+/*! \ingroup RAPIDJSON_ERRORS
+    The default character type is \c char.
+    On Windows, user can define this macro as \c TCHAR for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_CHARTYPE
+#define RAPIDJSON_ERROR_CHARTYPE char
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_STRING
+
+//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[].
+/*! \ingroup RAPIDJSON_ERRORS
+    By default this conversion macro does nothing.
+    On Windows, user can define this macro as \c _T(x) for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_STRING
+#define RAPIDJSON_ERROR_STRING(x) x
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// ParseErrorCode
+
+//! Error code of parsing.
+/*! \ingroup RAPIDJSON_ERRORS
+    \see GenericReader::Parse, GenericReader::GetParseErrorCode
+*/
+enum ParseErrorCode {
+  kParseErrorNone = 0,  //!< No error.
+
+  kParseErrorDocumentEmpty,            //!< The document is empty.
+  kParseErrorDocumentRootNotSingular,  //!< The document root must not follow by
+                                       //!< other values.
+
+  kParseErrorValueInvalid,  //!< Invalid value.
+
+  kParseErrorObjectMissName,   //!< Missing a name for object member.
+  kParseErrorObjectMissColon,  //!< Missing a colon after a name of object
+                               //!< member.
+  kParseErrorObjectMissCommaOrCurlyBracket,  //!< Missing a comma or '}' after
+                                             //!an
+                                             //!< object member.
+
+  kParseErrorArrayMissCommaOrSquareBracket,  //!< Missing a comma or ']' after
+                                             //!an
+                                             //!< array element.
+
+  kParseErrorStringUnicodeEscapeInvalidHex,  //!< Incorrect hex digit after \\u
+                                             //!< escape in string.
+  kParseErrorStringUnicodeSurrogateInvalid,  //!< The surrogate pair in string
+                                             //!is
+                                             //!< invalid.
+  kParseErrorStringEscapeInvalid,      //!< Invalid escape character in string.
+  kParseErrorStringMissQuotationMark,  //!< Missing a closing quotation mark in
+                                       //!< string.
+  kParseErrorStringInvalidEncoding,    //!< Invalid encoding in string.
+
+  kParseErrorNumberTooBig,        //!< Number too big to be stored in double.
+  kParseErrorNumberMissFraction,  //!< Miss fraction part in number.
+  kParseErrorNumberMissExponent,  //!< Miss exponent in number.
+
+  kParseErrorTermination,           //!< Parsing was terminated.
+  kParseErrorUnspecificSyntaxError  //!< Unspecific syntax error.
+};
+
+//! Result of parsing (wraps ParseErrorCode)
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \code
+        Document doc;
+        ParseResult ok = doc.Parse("[42]");
+        if (!ok) {
+            fprintf(stderr, "JSON parse error: %s (%u)",
+                    GetParseError_En(ok.Code()), ok.Offset());
+            exit(EXIT_FAILURE);
+        }
+    \endcode
+    \see GenericReader::Parse, GenericDocument::Parse
+*/
+struct ParseResult {
+  //!! Unspecified boolean type
+  typedef bool (ParseResult::*BooleanType)() const;
+
+ public:
+  //! Default constructor, no error.
+  ParseResult() : code_(kParseErrorNone), offset_(0) {}
+  //! Constructor to set an error.
+  ParseResult(ParseErrorCode code, size_t offset)
+      : code_(code), offset_(offset) {}
+
+  //! Get the error code.
+  ParseErrorCode Code() const { return code_; }
+  //! Get the error offset, if \ref IsError(), 0 otherwise.
+  size_t Offset() const { return offset_; }
+
+  //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
+  operator BooleanType() const {
+    return !IsError() ? &ParseResult::IsError : NULL;
+  }
+  //! Whether the result is an error.
+  bool IsError() const { return code_ != kParseErrorNone; }
+
+  bool operator==(const ParseResult &that) const { return code_ == that.code_; }
+  bool operator==(ParseErrorCode code) const { return code_ == code; }
+  friend bool operator==(ParseErrorCode code, const ParseResult &err) {
+    return code == err.code_;
+  }
+
+  bool operator!=(const ParseResult &that) const { return !(*this == that); }
+  bool operator!=(ParseErrorCode code) const { return !(*this == code); }
+  friend bool operator!=(ParseErrorCode code, const ParseResult &err) {
+    return err != code;
+  }
+
+  //! Reset error code.
+  void Clear() { Set(kParseErrorNone); }
+  //! Update error code and offset.
+  void Set(ParseErrorCode code, size_t offset = 0) {
+    code_ = code;
+    offset_ = offset;
+  }
+
+ private:
+  ParseErrorCode code_;
+  size_t offset_;
+};
+
+//! Function pointer type of GetParseError().
+/*! \ingroup RAPIDJSON_ERRORS
+
+    This is the prototype for \c GetParseError_X(), where \c X is a locale.
+    User can dynamically change locale in runtime, e.g.:
+\code
+    GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
+    const RAPIDJSON_ERROR_CHARTYPE* s =
+GetParseError(document.GetParseErrorCode()); \endcode
+*/
+typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_ERROR_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/filereadstream.h

@@ -0,0 +1,123 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FILEREADSTREAM_H_
+#define RAPIDJSON_FILEREADSTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! File byte stream for input using fread().
+/*!
+    \note implements Stream concept
+*/
+class FileReadStream {
+ public:
+  typedef char Ch;  //!< Character type (byte).
+
+  //! Constructor.
+  /*!
+      \param fp File pointer opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (readCount_ < bufferSize_) {
+        buffer_[readCount_] = '\0';
+        ++bufferLast_;
+        eof_ = true;
+      }
+    }
+  }
+
+  std::FILE *fp_;
+  Ch *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/filewritestream.h

@@ -0,0 +1,128 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FILEWRITESTREAM_H_
+#define RAPIDJSON_FILEWRITESTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of C file stream for output using fwrite().
+/*!
+    \note implements Stream concept
+*/
+class FileWriteStream {
+ public:
+  typedef char Ch;  //!< Character type. Only support char.
+
+  FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferEnd_(buffer + bufferSize),
+        current_(buffer_) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+  }
+
+  void Put(char c) {
+    if (current_ >= bufferEnd_) Flush();
+
+    *current_++ = c;
+  }
+
+  void PutN(char c, size_t n) {
+    size_t avail = static_cast<size_t>(bufferEnd_ - current_);
+    while (n > avail) {
+      std::memset(current_, c, avail);
+      current_ += avail;
+      Flush();
+      n -= avail;
+      avail = static_cast<size_t>(bufferEnd_ - current_);
+    }
+
+    if (n > 0) {
+      std::memset(current_, c, n);
+      current_ += n;
+    }
+  }
+
+  void Flush() {
+    if (current_ != buffer_) {
+      size_t result =
+          std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
+      if (result < static_cast<size_t>(current_ - buffer_)) {
+        // failure deliberately ignored at this time
+        // added to avoid warn_unused_result build errors
+      }
+      current_ = buffer_;
+    }
+  }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  FileWriteStream(const FileWriteStream &);
+  FileWriteStream &operator=(const FileWriteStream &);
+
+  std::FILE *fp_;
+  char *buffer_;
+  char *bufferEnd_;
+  char *current_;
+};
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(FileWriteStream &stream, char c, size_t n) {
+  stream.PutN(c, n);
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/fwd.h

@@ -0,0 +1,170 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FWD_H_
+#define RAPIDJSON_FWD_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+// encodings.h
+
+template <typename CharType>
+struct UTF8;
+template <typename CharType>
+struct UTF16;
+template <typename CharType>
+struct UTF16BE;
+template <typename CharType>
+struct UTF16LE;
+template <typename CharType>
+struct UTF32;
+template <typename CharType>
+struct UTF32BE;
+template <typename CharType>
+struct UTF32LE;
+template <typename CharType>
+struct ASCII;
+template <typename CharType>
+struct AutoUTF;
+
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder;
+
+// allocators.h
+
+class CrtAllocator;
+
+template <typename BaseAllocator>
+class MemoryPoolAllocator;
+
+// stream.h
+
+template <typename Encoding>
+struct GenericStringStream;
+
+typedef GenericStringStream<UTF8<char>> StringStream;
+
+template <typename Encoding>
+struct GenericInsituStringStream;
+
+typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
+
+// stringbuffer.h
+
+template <typename Encoding, typename Allocator>
+class GenericStringBuffer;
+
+typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
+
+// filereadstream.h
+
+class FileReadStream;
+
+// filewritestream.h
+
+class FileWriteStream;
+
+// memorybuffer.h
+
+template <typename Allocator>
+struct GenericMemoryBuffer;
+
+typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
+
+// memorystream.h
+
+struct MemoryStream;
+
+// reader.h
+
+template <typename Encoding, typename Derived>
+struct BaseReaderHandler;
+
+template <typename SourceEncoding, typename TargetEncoding,
+          typename StackAllocator>
+class GenericReader;
+
+typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
+
+// writer.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class Writer;
+
+// prettywriter.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class PrettyWriter;
+
+// document.h
+
+template <typename Encoding, typename Allocator>
+class GenericMember;
+
+template <bool Const, typename Encoding, typename Allocator>
+class GenericMemberIterator;
+
+template <typename CharType>
+struct GenericStringRef;
+
+template <typename Encoding, typename Allocator>
+class GenericValue;
+
+typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
+
+template <typename Encoding, typename Allocator, typename StackAllocator>
+class GenericDocument;
+
+typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
+                        CrtAllocator>
+    Document;
+
+// pointer.h
+
+template <typename ValueType, typename Allocator>
+class GenericPointer;
+
+typedef GenericPointer<Value, CrtAllocator> Pointer;
+
+// schema.h
+
+template <typename SchemaDocumentType>
+class IGenericRemoteSchemaDocumentProvider;
+
+template <typename ValueT, typename Allocator>
+class GenericSchemaDocument;
+
+typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
+typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
+    IRemoteSchemaDocumentProvider;
+
+template <typename SchemaDocumentType, typename OutputHandler,
+          typename StateAllocator>
+class GenericSchemaValidator;
+
+typedef GenericSchemaValidator<
+    SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
+    SchemaValidator;
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSONFWD_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/biginteger.h

@@ -0,0 +1,295 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_BIGINTEGER_H_
+#define RAPIDJSON_BIGINTEGER_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
+#include <intrin.h>  // for _umul128
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class BigInteger {
+ public:
+  typedef uint64_t Type;
+
+  BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
+    std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+  }
+
+  explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
+
+  BigInteger(const char *decimals, size_t length) : count_(1) {
+    RAPIDJSON_ASSERT(length > 0);
+    digits_[0] = 0;
+    size_t i = 0;
+    const size_t kMaxDigitPerIteration =
+        19;  // 2^64 = 18446744073709551616 > 10^19
+    while (length >= kMaxDigitPerIteration) {
+      AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
+      length -= kMaxDigitPerIteration;
+      i += kMaxDigitPerIteration;
+    }
+
+    if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
+  }
+
+  BigInteger &operator=(const BigInteger &rhs) {
+    if (this != &rhs) {
+      count_ = rhs.count_;
+      std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+    }
+    return *this;
+  }
+
+  BigInteger &operator=(uint64_t u) {
+    digits_[0] = u;
+    count_ = 1;
+    return *this;
+  }
+
+  BigInteger &operator+=(uint64_t u) {
+    Type backup = digits_[0];
+    digits_[0] += u;
+    for (size_t i = 0; i < count_ - 1; i++) {
+      if (digits_[i] >= backup) return *this;  // no carry
+      backup = digits_[i + 1];
+      digits_[i + 1] += 1;
+    }
+
+    // Last carry
+    if (digits_[count_ - 1] < backup) PushBack(1);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint64_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      uint64_t hi;
+      digits_[i] = MulAdd64(digits_[i], u, k, &hi);
+      k = hi;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint32_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      const uint64_t c = digits_[i] >> 32;
+      const uint64_t d = digits_[i] & 0xFFFFFFFF;
+      const uint64_t uc = u * c;
+      const uint64_t ud = u * d;
+      const uint64_t p0 = ud + k;
+      const uint64_t p1 = uc + (p0 >> 32);
+      digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
+      k = p1 >> 32;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator<<=(size_t shift) {
+    if (IsZero() || shift == 0) return *this;
+
+    size_t offset = shift / kTypeBit;
+    size_t interShift = shift % kTypeBit;
+    RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
+
+    if (interShift == 0) {
+      std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
+      count_ += offset;
+    } else {
+      digits_[count_] = 0;
+      for (size_t i = count_; i > 0; i--)
+        digits_[i + offset] = (digits_[i] << interShift) |
+                              (digits_[i - 1] >> (kTypeBit - interShift));
+      digits_[offset] = digits_[0] << interShift;
+      count_ += offset;
+      if (digits_[count_]) count_++;
+    }
+
+    std::memset(digits_, 0, offset * sizeof(Type));
+
+    return *this;
+  }
+
+  bool operator==(const BigInteger &rhs) const {
+    return count_ == rhs.count_ &&
+           std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
+  }
+
+  bool operator==(const Type rhs) const {
+    return count_ == 1 && digits_[0] == rhs;
+  }
+
+  BigInteger &MultiplyPow5(unsigned exp) {
+    static const uint32_t kPow5[12] = {
+        5,
+        5 * 5,
+        5 * 5 * 5,
+        5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
+    if (exp == 0) return *this;
+    for (; exp >= 27; exp -= 27)
+      *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D);  // 5^27
+    for (; exp >= 13; exp -= 13)
+      *this *= static_cast<uint32_t>(1220703125u);  // 5^13
+    if (exp > 0) *this *= kPow5[exp - 1];
+    return *this;
+  }
+
+  // Compute absolute difference of this and rhs.
+  // Assume this != rhs
+  bool Difference(const BigInteger &rhs, BigInteger *out) const {
+    int cmp = Compare(rhs);
+    RAPIDJSON_ASSERT(cmp != 0);
+    const BigInteger *a, *b;  // Makes a > b
+    bool ret;
+    if (cmp < 0) {
+      a = &rhs;
+      b = this;
+      ret = true;
+    } else {
+      a = this;
+      b = &rhs;
+      ret = false;
+    }
+
+    Type borrow = 0;
+    for (size_t i = 0; i < a->count_; i++) {
+      Type d = a->digits_[i] - borrow;
+      if (i < b->count_) d -= b->digits_[i];
+      borrow = (d > a->digits_[i]) ? 1 : 0;
+      out->digits_[i] = d;
+      if (d != 0) out->count_ = i + 1;
+    }
+
+    return ret;
+  }
+
+  int Compare(const BigInteger &rhs) const {
+    if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
+
+    for (size_t i = count_; i-- > 0;)
+      if (digits_[i] != rhs.digits_[i])
+        return digits_[i] < rhs.digits_[i] ? -1 : 1;
+
+    return 0;
+  }
+
+  size_t GetCount() const { return count_; }
+  Type GetDigit(size_t index) const {
+    RAPIDJSON_ASSERT(index < count_);
+    return digits_[index];
+  }
+  bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
+
+ private:
+  void AppendDecimal64(const char *begin, const char *end) {
+    uint64_t u = ParseUint64(begin, end);
+    if (IsZero())
+      *this = u;
+    else {
+      unsigned exp = static_cast<unsigned>(end - begin);
+      (MultiplyPow5(exp) <<= exp) += u;  // *this = *this * 10^exp + u
+    }
+  }
+
+  void PushBack(Type digit) {
+    RAPIDJSON_ASSERT(count_ < kCapacity);
+    digits_[count_++] = digit;
+  }
+
+  static uint64_t ParseUint64(const char *begin, const char *end) {
+    uint64_t r = 0;
+    for (const char *p = begin; p != end; ++p) {
+      RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
+      r = r * 10u + static_cast<unsigned>(*p - '0');
+    }
+    return r;
+  }
+
+  // Assume a * b + k < 2^128
+  static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
+                           uint64_t *outHigh) {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t low = _umul128(a, b, outHigh) + k;
+    if (low < k) (*outHigh)++;
+    return low;
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
+    p += k;
+    *outHigh = static_cast<uint64_t>(p >> 64);
+    return static_cast<uint64_t>(p);
+#else
+    const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
+                   b1 = b >> 32;
+    uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
+    x1 += (x0 >> 32);  // can't give carry
+    x1 += x2;
+    if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
+    uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
+    uint64_t hi = x3 + (x1 >> 32);
+
+    lo += k;
+    if (lo < k) hi++;
+    *outHigh = hi;
+    return lo;
+#endif
+  }
+
+  static const size_t kBitCount = 3328;  // 64bit * 54 > 10^1000
+  static const size_t kCapacity = kBitCount / sizeof(Type);
+  static const size_t kTypeBit = sizeof(Type) * 8;
+
+  Type digits_[kCapacity];
+  size_t count_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_BIGINTEGER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/clzll.h

@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_CLZLL_H_
+#define RAPIDJSON_CLZLL_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER)
+#include <intrin.h>
+#if defined(_WIN64)
+#pragma intrinsic(_BitScanReverse64)
+#else
+#pragma intrinsic(_BitScanReverse)
+#endif
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#if (defined(__GNUC__) && __GNUC__ >= 4) || \
+    RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+#define RAPIDJSON_CLZLL __builtin_clzll
+#else
+
+inline uint32_t clzll(uint64_t x) {
+  // Passing 0 to __builtin_clzll is UB in GCC and results in an
+  // infinite loop in the software implementation.
+  RAPIDJSON_ASSERT(x != 0);
+
+#if defined(_MSC_VER)
+  unsigned long r = 0;
+#if defined(_WIN64)
+  _BitScanReverse64(&r, x);
+#else
+  // Scan the high 32 bits.
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
+
+  // Scan the low 32 bits.
+  _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
+#endif  // _WIN64
+
+  return 63 - r;
+#else
+  uint32_t r;
+  while (!(x & (static_cast<uint64_t>(1) << 63))) {
+    x <<= 1;
+    ++r;
+  }
+
+  return r;
+#endif  // _MSC_VER
+}
+
+#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
+#endif  // (defined(__GNUC__) && __GNUC__ >= 4) ||
+        // RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CLZLL_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/diyfp.h

@@ -0,0 +1,305 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DIYFP_H_
+#define RAPIDJSON_DIYFP_H_
+
+#include <limits>
+#include "../rapidjson.h"
+#include "clzll.h"
+
+#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
+#include <intrin.h>
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+struct DiyFp {
+  DiyFp() : f(), e() {}
+
+  DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
+
+  explicit DiyFp(double d) {
+    union {
+      double d;
+      uint64_t u64;
+    } u = {d};
+
+    int biased_e =
+        static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
+    uint64_t significand = (u.u64 & kDpSignificandMask);
+    if (biased_e != 0) {
+      f = significand + kDpHiddenBit;
+      e = biased_e - kDpExponentBias;
+    } else {
+      f = significand;
+      e = kDpMinExponent + 1;
+    }
+  }
+
+  DiyFp operator-(const DiyFp &rhs) const { return DiyFp(f - rhs.f, e); }
+
+  DiyFp operator*(const DiyFp &rhs) const {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t h;
+    uint64_t l = _umul128(f, rhs.f, &h);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
+    uint64_t h = static_cast<uint64_t>(p >> 64);
+    uint64_t l = static_cast<uint64_t>(p);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#else
+    const uint64_t M32 = 0xFFFFFFFF;
+    const uint64_t a = f >> 32;
+    const uint64_t b = f & M32;
+    const uint64_t c = rhs.f >> 32;
+    const uint64_t d = rhs.f & M32;
+    const uint64_t ac = a * c;
+    const uint64_t bc = b * c;
+    const uint64_t ad = a * d;
+    const uint64_t bd = b * d;
+    uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
+    tmp += 1U << 31;  /// mult_round
+    return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
+#endif
+  }
+
+  DiyFp Normalize() const {
+    int s = static_cast<int>(RAPIDJSON_CLZLL(f));
+    return DiyFp(f << s, e - s);
+  }
+
+  DiyFp NormalizeBoundary() const {
+    DiyFp res = *this;
+    while (!(res.f & (kDpHiddenBit << 1))) {
+      res.f <<= 1;
+      res.e--;
+    }
+    res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
+    res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
+    return res;
+  }
+
+  void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
+    DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
+    DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
+                                   : DiyFp((f << 1) - 1, e - 1);
+    mi.f <<= mi.e - pl.e;
+    mi.e = pl.e;
+    *plus = pl;
+    *minus = mi;
+  }
+
+  double ToDouble() const {
+    union {
+      double d;
+      uint64_t u64;
+    } u;
+    RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
+    if (e < kDpDenormalExponent) {
+      // Underflow.
+      return 0.0;
+    }
+    if (e >= kDpMaxExponent) {
+      // Overflow.
+      return std::numeric_limits<double>::infinity();
+    }
+    const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
+                            ? 0
+                            : static_cast<uint64_t>(e + kDpExponentBias);
+    u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
+    return u.d;
+  }
+
+  static const int kDiySignificandSize = 64;
+  static const int kDpSignificandSize = 52;
+  static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
+  static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
+  static const int kDpMinExponent = -kDpExponentBias;
+  static const int kDpDenormalExponent = -kDpExponentBias + 1;
+  static const uint64_t kDpExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kDpSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kDpHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  uint64_t f;
+  int e;
+};
+
+inline DiyFp GetCachedPowerByIndex(size_t index) {
+  // 10^-348, 10^-340, ..., 10^340
+  static const uint64_t kCachedPowers_F[] = {
+      RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288),
+      RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
+      RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76),
+      RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
+      RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d),
+      RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
+      RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca),
+      RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
+      RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c),
+      RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
+      RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83),
+      RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
+      RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb),
+      RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
+      RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57),
+      RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
+      RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f),
+      RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
+      RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4),
+      RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
+      RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e),
+      RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
+      RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba),
+      RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
+      RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584),
+      RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
+      RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126),
+      RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
+      RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b),
+      RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
+      RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655),
+      RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
+      RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f),
+      RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
+      RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf),
+      RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
+      RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6),
+      RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
+      RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06),
+      RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
+      RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e),
+      RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
+      RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc),
+      RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
+      RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
+      RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
+      RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
+      RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
+      RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
+      RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
+      RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
+      RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
+      RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
+      RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
+      RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
+      RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
+      RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
+      RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
+      RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
+      RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
+      RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
+      RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
+      RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
+      RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
+      RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
+      RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
+      RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
+      RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
+      RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
+      RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
+      RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
+      RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
+      RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
+      RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
+      RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
+      RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
+      RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
+      RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
+      RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
+      RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
+      RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
+      RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
+      RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
+      RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
+      RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
+  static const int16_t kCachedPowers_E[] = {
+      -1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
+      -927,  -901,  -874,  -847,  -821,  -794,  -768,  -741,  -715,  -688, -661,
+      -635,  -608,  -582,  -555,  -529,  -502,  -475,  -449,  -422,  -396, -369,
+      -343,  -316,  -289,  -263,  -236,  -210,  -183,  -157,  -130,  -103, -77,
+      -50,   -24,   3,     30,    56,    83,    109,   136,   162,   189,  216,
+      242,   269,   295,   322,   348,   375,   402,   428,   455,   481,  508,
+      534,   561,   588,   614,   641,   667,   694,   720,   747,   774,  800,
+      827,   853,   880,   907,   933,   960,   986,   1013,  1039,  1066};
+  RAPIDJSON_ASSERT(index < 87);
+  return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
+}
+
+inline DiyFp GetCachedPower(int e, int *K) {
+  // int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
+  double dk = (-61 - e) * 0.30102999566398114 +
+              347;  // dk must be positive, so can do ceiling in positive
+  int k = static_cast<int>(dk);
+  if (dk - k > 0.0) k++;
+
+  unsigned index = static_cast<unsigned>((k >> 3) + 1);
+  *K = -(-348 + static_cast<int>(
+                    index << 3));  // decimal exponent no need lookup table
+
+  return GetCachedPowerByIndex(index);
+}
+
+inline DiyFp GetCachedPower10(int exp, int *outExp) {
+  RAPIDJSON_ASSERT(exp >= -348);
+  unsigned index = static_cast<unsigned>(exp + 348) / 8u;
+  *outExp = -348 + static_cast<int>(index) * 8;
+  return GetCachedPowerByIndex(index);
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DIYFP_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/dtoa.h

@@ -0,0 +1,269 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DTOA_
+#define RAPIDJSON_DTOA_
+
+#include "diyfp.h"
+#include "ieee754.h"
+#include "itoa.h"  // GetDigitsLut()
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(array - bounds)  // some gcc versions generate wrong warnings
+// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
+#endif
+
+inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
+                       uint64_t ten_kappa, uint64_t wp_w) {
+  while (rest < wp_w && delta - rest >= ten_kappa &&
+         (rest + ten_kappa < wp_w ||  /// closer
+          wp_w - rest > rest + ten_kappa - wp_w)) {
+    buffer[len - 1]--;
+    rest += ten_kappa;
+  }
+}
+
+inline int CountDecimalDigit32(uint32_t n) {
+  // Simple pure C++ implementation was faster than __builtin_clz version in
+  // this situation.
+  if (n < 10) return 1;
+  if (n < 100) return 2;
+  if (n < 1000) return 3;
+  if (n < 10000) return 4;
+  if (n < 100000) return 5;
+  if (n < 1000000) return 6;
+  if (n < 10000000) return 7;
+  if (n < 100000000) return 8;
+  // Will not reach 10 digits in DigitGen()
+  // if (n < 1000000000) return 9;
+  // return 10;
+  return 9;
+}
+
+inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
+                     char *buffer, int *len, int *K) {
+  static const uint32_t kPow10[] = {1,         10,        100,     1000,
+                                    10000,     100000,    1000000, 10000000,
+                                    100000000, 1000000000};
+  const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
+  const DiyFp wp_w = Mp - W;
+  uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
+  uint64_t p2 = Mp.f & (one.f - 1);
+  int kappa = CountDecimalDigit32(p1);  // kappa in [0, 9]
+  *len = 0;
+
+  while (kappa > 0) {
+    uint32_t d = 0;
+    switch (kappa) {
+      case 9:
+        d = p1 / 100000000;
+        p1 %= 100000000;
+        break;
+      case 8:
+        d = p1 / 10000000;
+        p1 %= 10000000;
+        break;
+      case 7:
+        d = p1 / 1000000;
+        p1 %= 1000000;
+        break;
+      case 6:
+        d = p1 / 100000;
+        p1 %= 100000;
+        break;
+      case 5:
+        d = p1 / 10000;
+        p1 %= 10000;
+        break;
+      case 4:
+        d = p1 / 1000;
+        p1 %= 1000;
+        break;
+      case 3:
+        d = p1 / 100;
+        p1 %= 100;
+        break;
+      case 2:
+        d = p1 / 10;
+        p1 %= 10;
+        break;
+      case 1:
+        d = p1;
+        p1 = 0;
+        break;
+      default:;
+    }
+    if (d || *len)
+      buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
+    kappa--;
+    uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
+    if (tmp <= delta) {
+      *K += kappa;
+      GrisuRound(buffer, *len, delta, tmp,
+                 static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
+      return;
+    }
+  }
+
+  // kappa = 0
+  for (;;) {
+    p2 *= 10;
+    delta *= 10;
+    char d = static_cast<char>(p2 >> -one.e);
+    if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
+    p2 &= one.f - 1;
+    kappa--;
+    if (p2 < delta) {
+      *K += kappa;
+      int index = -kappa;
+      GrisuRound(buffer, *len, delta, p2, one.f,
+                 wp_w.f * (index < 9 ? kPow10[index] : 0));
+      return;
+    }
+  }
+}
+
+inline void Grisu2(double value, char *buffer, int *length, int *K) {
+  const DiyFp v(value);
+  DiyFp w_m, w_p;
+  v.NormalizedBoundaries(&w_m, &w_p);
+
+  const DiyFp c_mk = GetCachedPower(w_p.e, K);
+  const DiyFp W = v.Normalize() * c_mk;
+  DiyFp Wp = w_p * c_mk;
+  DiyFp Wm = w_m * c_mk;
+  Wm.f++;
+  Wp.f--;
+  DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
+}
+
+inline char *WriteExponent(int K, char *buffer) {
+  if (K < 0) {
+    *buffer++ = '-';
+    K = -K;
+  }
+
+  if (K >= 100) {
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
+    K %= 100;
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else if (K >= 10) {
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K));
+
+  return buffer;
+}
+
+inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
+  const int kk = length + k;  // 10^(kk-1) <= v < 10^kk
+
+  if (0 <= k && kk <= 21) {
+    // 1234e7 -> 12340000000
+    for (int i = length; i < kk; i++) buffer[i] = '0';
+    buffer[kk] = '.';
+    buffer[kk + 1] = '0';
+    return &buffer[kk + 2];
+  } else if (0 < kk && kk <= 21) {
+    // 1234e-2 -> 12.34
+    std::memmove(&buffer[kk + 1], &buffer[kk],
+                 static_cast<size_t>(length - kk));
+    buffer[kk] = '.';
+    if (0 > k + maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[kk + 2];  // Reserve one zero
+    } else
+      return &buffer[length + 1];
+  } else if (-6 < kk && kk <= 0) {
+    // 1234e-6 -> 0.001234
+    const int offset = 2 - kk;
+    std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
+    buffer[0] = '0';
+    buffer[1] = '.';
+    for (int i = 2; i < offset; i++) buffer[i] = '0';
+    if (length - kk > maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = maxDecimalPlaces + 1; i > 2; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[3];  // Reserve one zero
+    } else
+      return &buffer[length + offset];
+  } else if (kk < -maxDecimalPlaces) {
+    // Truncate to zero
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else if (length == 1) {
+    // 1e30
+    buffer[1] = 'e';
+    return WriteExponent(kk - 1, &buffer[2]);
+  } else {
+    // 1234e30 -> 1.234e33
+    std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
+    buffer[1] = '.';
+    buffer[length + 1] = 'e';
+    return WriteExponent(kk - 1, &buffer[0 + length + 2]);
+  }
+}
+
+inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
+  RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
+  Double d(value);
+  if (d.IsZero()) {
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else {
+    if (value < 0) {
+      *buffer++ = '-';
+      value = -value;
+    }
+    int length, K;
+    Grisu2(value, buffer, &length, &K);
+    return Prettify(buffer, length, K, maxDecimalPlaces);
+  }
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DTOA_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/ieee754.h

@@ -0,0 +1,100 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_IEEE754_
+#define RAPIDJSON_IEEE754_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class Double {
+ public:
+  Double() {}
+  Double(double d) : d_(d) {}
+  Double(uint64_t u) : u_(u) {}
+
+  double Value() const { return d_; }
+  uint64_t Uint64Value() const { return u_; }
+
+  double NextPositiveDouble() const {
+    RAPIDJSON_ASSERT(!Sign());
+    return Double(u_ + 1).Value();
+  }
+
+  bool Sign() const { return (u_ & kSignMask) != 0; }
+  uint64_t Significand() const { return u_ & kSignificandMask; }
+  int Exponent() const {
+    return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) -
+                            kExponentBias);
+  }
+
+  bool IsNan() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() != 0;
+  }
+  bool IsInf() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
+  }
+  bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
+  bool IsNormal() const {
+    return (u_ & kExponentMask) != 0 || Significand() == 0;
+  }
+  bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
+
+  uint64_t IntegerSignificand() const {
+    return IsNormal() ? Significand() | kHiddenBit : Significand();
+  }
+  int IntegerExponent() const {
+    return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize;
+  }
+  uint64_t ToBias() const {
+    return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask;
+  }
+
+  static int EffectiveSignificandSize(int order) {
+    if (order >= -1021)
+      return 53;
+    else if (order <= -1074)
+      return 0;
+    else
+      return order + 1074;
+  }
+
+ private:
+  static const int kSignificandSize = 52;
+  static const int kExponentBias = 0x3FF;
+  static const int kDenormalExponent = 1 - kExponentBias;
+  static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000);
+  static const uint64_t kExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  union {
+    double d_;
+    uint64_t u_;
+  };
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_IEEE754_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/itoa.h

@@ -0,0 +1,288 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ITOA_
+#define RAPIDJSON_ITOA_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline const char *GetDigitsLut() {
+  static const char cDigitsLut[200] = {
+      '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
+      '7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
+      '1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
+      '2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
+      '3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
+      '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
+      '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
+      '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
+      '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
+      '7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4',
+      '7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8',
+      '2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
+      '9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
+      '7', '9', '8', '9', '9'};
+  return cDigitsLut;
+}
+
+inline char *u32toa(uint32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+
+  const char *cDigitsLut = GetDigitsLut();
+
+  if (value < 10000) {
+    const uint32_t d1 = (value / 100) << 1;
+    const uint32_t d2 = (value % 100) << 1;
+
+    if (value >= 1000) *buffer++ = cDigitsLut[d1];
+    if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 10) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+  } else if (value < 100000000) {
+    // value = bbbbcccc
+    const uint32_t b = value / 10000;
+    const uint32_t c = value % 10000;
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+    if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 100000) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  } else {
+    // value = aabbbbcccc in decimal
+
+    const uint32_t a = value / 100000000;  // 1 to 42
+    value %= 100000000;
+
+    if (a >= 10) {
+      const unsigned i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+
+    const uint32_t b = value / 10000;  // 0 to 9999
+    const uint32_t c = value % 10000;  // 0 to 9999
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  }
+  return buffer;
+}
+
+inline char *i32toa(int32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint32_t u = static_cast<uint32_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u32toa(u, buffer);
+}
+
+inline char *u64toa(uint64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  const char *cDigitsLut = GetDigitsLut();
+  const uint64_t kTen8 = 100000000;
+  const uint64_t kTen9 = kTen8 * 10;
+  const uint64_t kTen10 = kTen8 * 100;
+  const uint64_t kTen11 = kTen8 * 1000;
+  const uint64_t kTen12 = kTen8 * 10000;
+  const uint64_t kTen13 = kTen8 * 100000;
+  const uint64_t kTen14 = kTen8 * 1000000;
+  const uint64_t kTen15 = kTen8 * 10000000;
+  const uint64_t kTen16 = kTen8 * kTen8;
+
+  if (value < kTen8) {
+    uint32_t v = static_cast<uint32_t>(value);
+    if (v < 10000) {
+      const uint32_t d1 = (v / 100) << 1;
+      const uint32_t d2 = (v % 100) << 1;
+
+      if (v >= 1000) *buffer++ = cDigitsLut[d1];
+      if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
+      if (v >= 10) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+    } else {
+      // value = bbbbcccc
+      const uint32_t b = v / 10000;
+      const uint32_t c = v % 10000;
+
+      const uint32_t d1 = (b / 100) << 1;
+      const uint32_t d2 = (b % 100) << 1;
+
+      const uint32_t d3 = (c / 100) << 1;
+      const uint32_t d4 = (c % 100) << 1;
+
+      if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+      if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+      if (value >= 100000) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+
+      *buffer++ = cDigitsLut[d3];
+      *buffer++ = cDigitsLut[d3 + 1];
+      *buffer++ = cDigitsLut[d4];
+      *buffer++ = cDigitsLut[d4 + 1];
+    }
+  } else if (value < kTen16) {
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    if (value >= kTen15) *buffer++ = cDigitsLut[d1];
+    if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= kTen13) *buffer++ = cDigitsLut[d2];
+    if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
+    if (value >= kTen11) *buffer++ = cDigitsLut[d3];
+    if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
+    if (value >= kTen9) *buffer++ = cDigitsLut[d4];
+
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  } else {
+    const uint32_t a = static_cast<uint32_t>(value / kTen16);  // 1 to 1844
+    value %= kTen16;
+
+    if (a < 10)
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+    else if (a < 100) {
+      const uint32_t i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else if (a < 1000) {
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
+
+      const uint32_t i = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else {
+      const uint32_t i = (a / 100) << 1;
+      const uint32_t j = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+      *buffer++ = cDigitsLut[j];
+      *buffer++ = cDigitsLut[j + 1];
+    }
+
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  }
+
+  return buffer;
+}
+
+inline char *i64toa(int64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint64_t u = static_cast<uint64_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u64toa(u, buffer);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ITOA_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/meta.h

@@ -0,0 +1,243 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_META_H_
+#define RAPIDJSON_INTERNAL_META_H_
+
+#include "../rapidjson.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(6334)
+#endif
+
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+#include <type_traits>
+#endif
+
+//@cond RAPIDJSON_INTERNAL
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
+// matching
+template <typename T>
+struct Void {
+  typedef void Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// BoolType, TrueType, FalseType
+//
+template <bool Cond>
+struct BoolType {
+  static const bool Value = Cond;
+  typedef BoolType Type;
+};
+typedef BoolType<true> TrueType;
+typedef BoolType<false> FalseType;
+
+///////////////////////////////////////////////////////////////////////////////
+// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
+//
+
+template <bool C>
+struct SelectIfImpl {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T1 Type;
+  };
+};
+template <>
+struct SelectIfImpl<false> {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T2 Type;
+  };
+};
+template <bool C, typename T1, typename T2>
+struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
+template <typename C, typename T1, typename T2>
+struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
+
+template <bool Cond1, bool Cond2>
+struct AndExprCond : FalseType {};
+template <>
+struct AndExprCond<true, true> : TrueType {};
+template <bool Cond1, bool Cond2>
+struct OrExprCond : TrueType {};
+template <>
+struct OrExprCond<false, false> : FalseType {};
+
+template <typename C>
+struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
+template <typename C>
+struct NotExpr : SelectIf<C, FalseType, TrueType>::Type {};
+template <typename C1, typename C2>
+struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
+template <typename C1, typename C2>
+struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
+
+///////////////////////////////////////////////////////////////////////////////
+// AddConst, MaybeAddConst, RemoveConst
+template <typename T>
+struct AddConst {
+  typedef const T Type;
+};
+template <bool Constify, typename T>
+struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
+template <typename T>
+struct RemoveConst {
+  typedef T Type;
+};
+template <typename T>
+struct RemoveConst<const T> {
+  typedef T Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsSame, IsConst, IsMoreConst, IsPointer
+//
+template <typename T, typename U>
+struct IsSame : FalseType {};
+template <typename T>
+struct IsSame<T, T> : TrueType {};
+
+template <typename T>
+struct IsConst : FalseType {};
+template <typename T>
+struct IsConst<const T> : TrueType {};
+
+template <typename CT, typename T>
+struct IsMoreConst
+    : AndExpr<
+          IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
+          BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
+
+template <typename T>
+struct IsPointer : FalseType {};
+template <typename T>
+struct IsPointer<T *> : TrueType {};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsBaseOf
+//
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+template <typename B, typename D>
+struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
+
+#else  // simplified version adopted from Boost
+
+template <typename B, typename D>
+struct IsBaseOfImpl {
+  RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
+  RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
+
+  typedef char (&Yes)[1];
+  typedef char (&No)[2];
+
+  template <typename T>
+  static Yes Check(const D *, T);
+  static No Check(const B *, int);
+
+  struct Host {
+    operator const B *() const;
+    operator const D *();
+  };
+
+  enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
+};
+
+template <typename B, typename D>
+struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
+
+#endif  // RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+//////////////////////////////////////////////////////////////////////////
+// EnableIf / DisableIf
+//
+template <bool Condition, typename T = void>
+struct EnableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct EnableIfCond<false, T> { /* empty */
+};
+
+template <bool Condition, typename T = void>
+struct DisableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct DisableIfCond<true, T> { /* empty */
+};
+
+template <typename Condition, typename T = void>
+struct EnableIf : EnableIfCond<Condition::Value, T> {};
+
+template <typename Condition, typename T = void>
+struct DisableIf : DisableIfCond<Condition::Value, T> {};
+
+// SFINAE helpers
+struct SfinaeTag {};
+template <typename T>
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+  typedef T Type;
+};
+
+#define RAPIDJSON_REMOVEFPTR_(type)                          \
+  typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
+      ::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
+
+#define RAPIDJSON_ENABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_DISABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+//@endcond
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_META_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/pow10.h

@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_POW10_
+#define RAPIDJSON_POW10_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Computes integer powers of 10 in double (10.0^n).
+/*! This function uses lookup table for fast and accurate results.
+    \param n non-negative exponent. Must <= 308.
+    \return 10.0^n
+*/
+inline double Pow10(int n) {
+  static const double e[] = {
+      // 1e-0...1e308: 309 * 8 bytes = 2472 bytes
+      1e+0,   1e+1,   1e+2,   1e+3,   1e+4,   1e+5,   1e+6,   1e+7,   1e+8,
+      1e+9,   1e+10,  1e+11,  1e+12,  1e+13,  1e+14,  1e+15,  1e+16,  1e+17,
+      1e+18,  1e+19,  1e+20,  1e+21,  1e+22,  1e+23,  1e+24,  1e+25,  1e+26,
+      1e+27,  1e+28,  1e+29,  1e+30,  1e+31,  1e+32,  1e+33,  1e+34,  1e+35,
+      1e+36,  1e+37,  1e+38,  1e+39,  1e+40,  1e+41,  1e+42,  1e+43,  1e+44,
+      1e+45,  1e+46,  1e+47,  1e+48,  1e+49,  1e+50,  1e+51,  1e+52,  1e+53,
+      1e+54,  1e+55,  1e+56,  1e+57,  1e+58,  1e+59,  1e+60,  1e+61,  1e+62,
+      1e+63,  1e+64,  1e+65,  1e+66,  1e+67,  1e+68,  1e+69,  1e+70,  1e+71,
+      1e+72,  1e+73,  1e+74,  1e+75,  1e+76,  1e+77,  1e+78,  1e+79,  1e+80,
+      1e+81,  1e+82,  1e+83,  1e+84,  1e+85,  1e+86,  1e+87,  1e+88,  1e+89,
+      1e+90,  1e+91,  1e+92,  1e+93,  1e+94,  1e+95,  1e+96,  1e+97,  1e+98,
+      1e+99,  1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
+      1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
+      1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
+      1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
+      1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143,
+      1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152,
+      1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
+      1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
+      1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
+      1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
+      1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
+      1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
+      1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
+      1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
+      1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
+      1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
+      1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
+      1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
+      1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
+      1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
+      1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
+      1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
+      1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
+      1e+306, 1e+307, 1e+308};
+  RAPIDJSON_ASSERT(n >= 0 && n <= 308);
+  return e[n];
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_POW10_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/regex.h

@@ -0,0 +1,753 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_REGEX_H_
+#define RAPIDJSON_INTERNAL_REGEX_H_
+
+#include "../allocators.h"
+#include "../stream.h"
+#include "stack.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(switch - enum)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifndef RAPIDJSON_REGEX_VERBOSE
+#define RAPIDJSON_REGEX_VERBOSE 0
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// DecodedStream
+
+template <typename SourceStream, typename Encoding>
+class DecodedStream {
+ public:
+  DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
+  unsigned Peek() { return codepoint_; }
+  unsigned Take() {
+    unsigned c = codepoint_;
+    if (c)  // No further decoding when '\0'
+      Decode();
+    return c;
+  }
+
+ private:
+  void Decode() {
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
+  }
+
+  SourceStream &ss_;
+  unsigned codepoint_;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericRegex
+
+static const SizeType kRegexInvalidState = ~SizeType(
+    0);  //!< Represents an invalid index in GenericRegex::State::out, out1
+static const SizeType kRegexInvalidRange = ~SizeType(0);
+
+template <typename Encoding, typename Allocator>
+class GenericRegexSearch;
+
+//! Regular expression engine with subset of ECMAscript grammar.
+/*!
+    Supported regular expression syntax:
+    - \c ab     Concatenation
+    - \c a|b    Alternation
+    - \c a?     Zero or one
+    - \c a*     Zero or more
+    - \c a+     One or more
+    - \c a{3}   Exactly 3 times
+    - \c a{3,}  At least 3 times
+    - \c a{3,5} 3 to 5 times
+    - \c (ab)   Grouping
+    - \c ^a     At the beginning
+    - \c a$     At the end
+    - \c .      Any character
+    - \c [abc]  Character classes
+    - \c [a-c]  Character class range
+    - \c [a-z0-9_] Character class combination
+    - \c [^abc] Negated character classes
+    - \c [^a-c] Negated character class range
+    - \c [\b]   Backspace (U+0008)
+    - \c \\| \\\\ ...  Escape characters
+    - \c \\f Form feed (U+000C)
+    - \c \\n Line feed (U+000A)
+    - \c \\r Carriage return (U+000D)
+    - \c \\t Tab (U+0009)
+    - \c \\v Vertical tab (U+000B)
+
+    \note This is a Thompson NFA engine, implemented with reference to
+        Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is
+   slow in Java, Perl, PHP, Python, Ruby,...).",
+        https://swtch.com/~rsc/regexp/regexp1.html
+*/
+template <typename Encoding, typename Allocator = CrtAllocator>
+class GenericRegex {
+ public:
+  typedef Encoding EncodingType;
+  typedef typename Encoding::Ch Ch;
+  template <typename, typename>
+  friend class GenericRegexSearch;
+
+  GenericRegex(const Ch *source, Allocator *allocator = 0)
+      : ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()),
+        allocator_(allocator ? allocator : ownAllocator_),
+        states_(allocator_, 256),
+        ranges_(allocator_, 256),
+        root_(kRegexInvalidState),
+        stateCount_(),
+        rangeCount_(),
+        anchorBegin_(),
+        anchorEnd_() {
+    GenericStringStream<Encoding> ss(source);
+    DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
+    Parse(ds);
+  }
+
+  ~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); }
+
+  bool IsValid() const { return root_ != kRegexInvalidState; }
+
+ private:
+  enum Operator {
+    kZeroOrOne,
+    kZeroOrMore,
+    kOneOrMore,
+    kConcatenation,
+    kAlternation,
+    kLeftParenthesis
+  };
+
+  static const unsigned kAnyCharacterClass = 0xFFFFFFFF;  //!< For '.'
+  static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
+  static const unsigned kRangeNegationFlag = 0x80000000;
+
+  struct Range {
+    unsigned start;  //
+    unsigned end;
+    SizeType next;
+  };
+
+  struct State {
+    SizeType out;   //!< Equals to kInvalid for matching state
+    SizeType out1;  //!< Equals to non-kInvalid for split
+    SizeType rangeStart;
+    unsigned codepoint;
+  };
+
+  struct Frag {
+    Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
+    SizeType start;
+    SizeType out;  //!< link-list of all output states
+    SizeType minIndex;
+  };
+
+  State &GetState(SizeType index) {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  const State &GetState(SizeType index) const {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  Range &GetRange(SizeType index) {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  const Range &GetRange(SizeType index) const {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  template <typename InputStream>
+  void Parse(DecodedStream<InputStream, Encoding> &ds) {
+    Stack<Allocator> operandStack(allocator_, 256);   // Frag
+    Stack<Allocator> operatorStack(allocator_, 256);  // Operator
+    Stack<Allocator> atomCountStack(allocator_,
+                                    256);  // unsigned (Atom per parenthesis)
+
+    *atomCountStack.template Push<unsigned>() = 0;
+
+    unsigned codepoint;
+    while (ds.Peek() != 0) {
+      switch (codepoint = ds.Take()) {
+        case '^':
+          anchorBegin_ = true;
+          break;
+
+        case '$':
+          anchorEnd_ = true;
+          break;
+
+        case '|':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() < kAlternation)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          *operatorStack.template Push<Operator>() = kAlternation;
+          *atomCountStack.template Top<unsigned>() = 0;
+          break;
+
+        case '(':
+          *operatorStack.template Push<Operator>() = kLeftParenthesis;
+          *atomCountStack.template Push<unsigned>() = 0;
+          break;
+
+        case ')':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() != kLeftParenthesis)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          if (operatorStack.Empty()) return;
+          operatorStack.template Pop<Operator>(1);
+          atomCountStack.template Pop<unsigned>(1);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '?':
+          if (!Eval(operandStack, kZeroOrOne)) return;
+          break;
+
+        case '*':
+          if (!Eval(operandStack, kZeroOrMore)) return;
+          break;
+
+        case '+':
+          if (!Eval(operandStack, kOneOrMore)) return;
+          break;
+
+        case '{': {
+          unsigned n, m;
+          if (!ParseUnsigned(ds, &n)) return;
+
+          if (ds.Peek() == ',') {
+            ds.Take();
+            if (ds.Peek() == '}')
+              m = kInfinityQuantifier;
+            else if (!ParseUnsigned(ds, &m) || m < n)
+              return;
+          } else
+            m = n;
+
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
+          ds.Take();
+        } break;
+
+        case '.':
+          PushOperand(operandStack, kAnyCharacterClass);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '[': {
+          SizeType range;
+          if (!ParseRange(ds, &range)) return;
+          SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
+                                kRangeCharacterClass);
+          GetState(s).rangeStart = range;
+          *operandStack.template Push<Frag>() = Frag(s, s, s);
+        }
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '\\':  // Escape character
+          if (!CharacterEscape(ds, &codepoint))
+            return;  // Unsupported escape character
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:  // Pattern character
+          PushOperand(operandStack, codepoint);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+      }
+    }
+
+    while (!operatorStack.Empty())
+      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
+
+    // Link the operand to matching state.
+    if (operandStack.GetSize() == sizeof(Frag)) {
+      Frag *e = operandStack.template Pop<Frag>(1);
+      Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0));
+      root_ = e->start;
+
+#if RAPIDJSON_REGEX_VERBOSE
+      printf("root: %d\n", root_);
+      for (SizeType i = 0; i < stateCount_; i++) {
+        State &s = GetState(i);
+        printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1,
+               (char)s.codepoint);
+      }
+      printf("\n");
+#endif
+    }
+  }
+
+  SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) {
+    State *s = states_.template Push<State>();
+    s->out = out;
+    s->out1 = out1;
+    s->codepoint = codepoint;
+    s->rangeStart = kRegexInvalidRange;
+    return stateCount_++;
+  }
+
+  void PushOperand(Stack<Allocator> &operandStack, unsigned codepoint) {
+    SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint);
+    *operandStack.template Push<Frag>() = Frag(s, s, s);
+  }
+
+  void ImplicitConcatenation(Stack<Allocator> &atomCountStack,
+                             Stack<Allocator> &operatorStack) {
+    if (*atomCountStack.template Top<unsigned>())
+      *operatorStack.template Push<Operator>() = kConcatenation;
+    (*atomCountStack.template Top<unsigned>())++;
+  }
+
+  SizeType Append(SizeType l1, SizeType l2) {
+    SizeType old = l1;
+    while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
+    GetState(l1).out = l2;
+    return old;
+  }
+
+  void Patch(SizeType l, SizeType s) {
+    for (SizeType next; l != kRegexInvalidState; l = next) {
+      next = GetState(l).out;
+      GetState(l).out = s;
+    }
+  }
+
+  bool Eval(Stack<Allocator> &operandStack, Operator op) {
+    switch (op) {
+      case kConcatenation:
+        RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
+        {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          Patch(e1.out, e2.start);
+          *operandStack.template Push<Frag>() =
+              Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
+        }
+        return true;
+
+      case kAlternation:
+        if (operandStack.GetSize() >= sizeof(Frag) * 2) {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(e1.start, e2.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
+          return true;
+        }
+        return false;
+
+      case kZeroOrOne:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e.out, s), e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kZeroOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kOneOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      default:
+        // syntax error (e.g. unclosed kLeftParenthesis)
+        return false;
+    }
+  }
+
+  bool EvalQuantifier(Stack<Allocator> &operandStack, unsigned n, unsigned m) {
+    RAPIDJSON_ASSERT(n <= m);
+    RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
+
+    if (n == 0) {
+      if (m == 0)  // a{0} not support
+        return false;
+      else if (m == kInfinityQuantifier)
+        Eval(operandStack, kZeroOrMore);  // a{0,} -> a*
+      else {
+        Eval(operandStack, kZeroOrOne);  // a{0,5} -> a?
+        for (unsigned i = 0; i < m - 1; i++)
+          CloneTopOperand(operandStack);  // a{0,5} -> a? a? a? a? a?
+        for (unsigned i = 0; i < m - 1; i++)
+          Eval(operandStack, kConcatenation);  // a{0,5} -> a?a?a?a?a?
+      }
+      return true;
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)  // a{3} -> a a a
+      CloneTopOperand(operandStack);
+
+    if (m == kInfinityQuantifier)
+      Eval(operandStack, kOneOrMore);  // a{3,} -> a a a+
+    else if (m > n) {
+      CloneTopOperand(operandStack);   // a{3,5} -> a a a a
+      Eval(operandStack, kZeroOrOne);  // a{3,5} -> a a a a?
+      for (unsigned i = n; i < m - 1; i++)
+        CloneTopOperand(operandStack);  // a{3,5} -> a a a a? a?
+      for (unsigned i = n; i < m; i++)
+        Eval(operandStack, kConcatenation);  // a{3,5} -> a a aa?a?
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)
+      Eval(operandStack,
+           kConcatenation);  // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
+
+    return true;
+  }
+
+  static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
+
+  void CloneTopOperand(Stack<Allocator> &operandStack) {
+    const Frag src =
+        *operandStack
+             .template Top<Frag>();  // Copy constructor to prevent invalidation
+    SizeType count =
+        stateCount_ - src.minIndex;  // Assumes top operand contains states in
+                                     // [src->minIndex, stateCount_)
+    State *s = states_.template Push<State>(count);
+    memcpy(s, &GetState(src.minIndex), count * sizeof(State));
+    for (SizeType j = 0; j < count; j++) {
+      if (s[j].out != kRegexInvalidState) s[j].out += count;
+      if (s[j].out1 != kRegexInvalidState) s[j].out1 += count;
+    }
+    *operandStack.template Push<Frag>() =
+        Frag(src.start + count, src.out + count, src.minIndex + count);
+    stateCount_ += count;
+  }
+
+  template <typename InputStream>
+  bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
+    unsigned r = 0;
+    if (ds.Peek() < '0' || ds.Peek() > '9') return false;
+    while (ds.Peek() >= '0' && ds.Peek() <= '9') {
+      if (r >= 429496729 && ds.Peek() > '5')  // 2^32 - 1 = 4294967295
+        return false;                         // overflow
+      r = r * 10 + (ds.Take() - '0');
+    }
+    *u = r;
+    return true;
+  }
+
+  template <typename InputStream>
+  bool ParseRange(DecodedStream<InputStream, Encoding> &ds, SizeType *range) {
+    bool isBegin = true;
+    bool negate = false;
+    int step = 0;
+    SizeType start = kRegexInvalidRange;
+    SizeType current = kRegexInvalidRange;
+    unsigned codepoint;
+    while ((codepoint = ds.Take()) != 0) {
+      if (isBegin) {
+        isBegin = false;
+        if (codepoint == '^') {
+          negate = true;
+          continue;
+        }
+      }
+
+      switch (codepoint) {
+        case ']':
+          if (start == kRegexInvalidRange)
+            return false;   // Error: nothing inside []
+          if (step == 2) {  // Add trailing '-'
+            SizeType r = NewRange('-');
+            RAPIDJSON_ASSERT(current != kRegexInvalidRange);
+            GetRange(current).next = r;
+          }
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
+          *range = start;
+          return true;
+
+        case '\\':
+          if (ds.Peek() == 'b') {
+            ds.Take();
+            codepoint = 0x0008;  // Escape backspace character
+          } else if (!CharacterEscape(ds, &codepoint))
+            return false;
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:
+          switch (step) {
+            case 1:
+              if (codepoint == '-') {
+                step++;
+                break;
+              }
+              // fall through to step 0 for other characters
+              RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+            case 0: {
+              SizeType r = NewRange(codepoint);
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
+              current = r;
+            }
+              step = 1;
+              break;
+
+            default:
+              RAPIDJSON_ASSERT(step == 2);
+              GetRange(current).end = codepoint;
+              step = 0;
+          }
+      }
+    }
+    return false;
+  }
+
+  SizeType NewRange(unsigned codepoint) {
+    Range *r = ranges_.template Push<Range>();
+    r->start = r->end = codepoint;
+    r->next = kRegexInvalidRange;
+    return rangeCount_++;
+  }
+
+  template <typename InputStream>
+  bool CharacterEscape(DecodedStream<InputStream, Encoding> &ds,
+                       unsigned *escapedCodepoint) {
+    unsigned codepoint;
+    switch (codepoint = ds.Take()) {
+      case '^':
+      case '$':
+      case '|':
+      case '(':
+      case ')':
+      case '?':
+      case '*':
+      case '+':
+      case '.':
+      case '[':
+      case ']':
+      case '{':
+      case '}':
+      case '\\':
+        *escapedCodepoint = codepoint;
+        return true;
+      case 'f':
+        *escapedCodepoint = 0x000C;
+        return true;
+      case 'n':
+        *escapedCodepoint = 0x000A;
+        return true;
+      case 'r':
+        *escapedCodepoint = 0x000D;
+        return true;
+      case 't':
+        *escapedCodepoint = 0x0009;
+        return true;
+      case 'v':
+        *escapedCodepoint = 0x000B;
+        return true;
+      default:
+        return false;  // Unsupported escape character
+    }
+  }
+
+  Allocator *ownAllocator_;
+  Allocator *allocator_;
+  Stack<Allocator> states_;
+  Stack<Allocator> ranges_;
+  SizeType root_;
+  SizeType stateCount_;
+  SizeType rangeCount_;
+
+  static const unsigned kInfinityQuantifier = ~0u;
+
+  // For SearchWithAnchoring()
+  bool anchorBegin_;
+  bool anchorEnd_;
+};
+
+template <typename RegexType, typename Allocator = CrtAllocator>
+class GenericRegexSearch {
+ public:
+  typedef typename RegexType::EncodingType Encoding;
+  typedef typename Encoding::Ch Ch;
+
+  GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
+      : regex_(regex),
+        allocator_(allocator),
+        ownAllocator_(0),
+        state0_(allocator, 0),
+        state1_(allocator, 0),
+        stateSet_() {
+    RAPIDJSON_ASSERT(regex_.IsValid());
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    stateSet_ = static_cast<unsigned *>(allocator_->Malloc(GetStateSetSize()));
+    state0_.template Reserve<SizeType>(regex_.stateCount_);
+    state1_.template Reserve<SizeType>(regex_.stateCount_);
+  }
+
+  ~GenericRegexSearch() {
+    Allocator::Free(stateSet_);
+    RAPIDJSON_DELETE(ownAllocator_);
+  }
+
+  template <typename InputStream>
+  bool Match(InputStream &is) {
+    return SearchWithAnchoring(is, true, true);
+  }
+
+  bool Match(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Match(is);
+  }
+
+  template <typename InputStream>
+  bool Search(InputStream &is) {
+    return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
+  }
+
+  bool Search(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Search(is);
+  }
+
+ private:
+  typedef typename RegexType::State State;
+  typedef typename RegexType::Range Range;
+
+  template <typename InputStream>
+  bool SearchWithAnchoring(InputStream &is, bool anchorBegin, bool anchorEnd) {
+    DecodedStream<InputStream, Encoding> ds(is);
+
+    state0_.Clear();
+    Stack<Allocator> *current = &state0_, *next = &state1_;
+    const size_t stateSetSize = GetStateSetSize();
+    std::memset(stateSet_, 0, stateSetSize);
+
+    bool matched = AddState(*current, regex_.root_);
+    unsigned codepoint;
+    while (!current->Empty() && (codepoint = ds.Take()) != 0) {
+      std::memset(stateSet_, 0, stateSetSize);
+      next->Clear();
+      matched = false;
+      for (const SizeType *s = current->template Bottom<SizeType>();
+           s != current->template End<SizeType>(); ++s) {
+        const State &sr = regex_.GetState(*s);
+        if (sr.codepoint == codepoint ||
+            sr.codepoint == RegexType::kAnyCharacterClass ||
+            (sr.codepoint == RegexType::kRangeCharacterClass &&
+             MatchRange(sr.rangeStart, codepoint))) {
+          matched = AddState(*next, sr.out) || matched;
+          if (!anchorEnd && matched) return true;
+        }
+        if (!anchorBegin) AddState(*next, regex_.root_);
+      }
+      internal::Swap(current, next);
+    }
+
+    return matched;
+  }
+
+  size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; }
+
+  // Return whether the added states is a match state
+  bool AddState(Stack<Allocator> &l, SizeType index) {
+    RAPIDJSON_ASSERT(index != kRegexInvalidState);
+
+    const State &s = regex_.GetState(index);
+    if (s.out1 != kRegexInvalidState) {  // Split
+      bool matched = AddState(l, s.out);
+      return AddState(l, s.out1) || matched;
+    } else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
+      stateSet_[index >> 5] |= (1u << (index & 31));
+      *l.template PushUnsafe<SizeType>() = index;
+    }
+    return s.out ==
+           kRegexInvalidState;  // by using PushUnsafe() above, we can ensure s
+                                // is not validated due to reallocation.
+  }
+
+  bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
+    bool yes = (regex_.GetRange(rangeIndex).start &
+                RegexType::kRangeNegationFlag) == 0;
+    while (rangeIndex != kRegexInvalidRange) {
+      const Range &r = regex_.GetRange(rangeIndex);
+      if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) &&
+          codepoint <= r.end)
+        return yes;
+      rangeIndex = r.next;
+    }
+    return !yes;
+  }
+
+  const RegexType &regex_;
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  Stack<Allocator> state0_;
+  Stack<Allocator> state1_;
+  uint32_t *stateSet_;
+};
+
+typedef GenericRegex<UTF8<>> Regex;
+typedef GenericRegexSearch<Regex> RegexSearch;
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_REGEX_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/stack.h

@@ -0,0 +1,245 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_STACK_H_
+#define RAPIDJSON_INTERNAL_STACK_H_
+
+#include <cstddef>
+#include "../allocators.h"
+#include "swap.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// Stack
+
+//! A type-unsafe stack for storing different types of data.
+/*! \tparam Allocator Allocator for allocating stack memory.
+ */
+template <typename Allocator>
+class Stack {
+ public:
+  // Optimization note: Do not allocate memory for stack_ in constructor.
+  // Do it lazily when first Push() -> Expand() -> Resize().
+  Stack(Allocator *allocator, size_t stackCapacity)
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(0),
+        stackTop_(0),
+        stackEnd_(0),
+        initialCapacity_(stackCapacity) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack(Stack &&rhs)
+      : allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(rhs.stack_),
+        stackTop_(rhs.stackTop_),
+        stackEnd_(rhs.stackEnd_),
+        initialCapacity_(rhs.initialCapacity_) {
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.stack_ = 0;
+    rhs.stackTop_ = 0;
+    rhs.stackEnd_ = 0;
+    rhs.initialCapacity_ = 0;
+  }
+#endif
+
+  ~Stack() { Destroy(); }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack &operator=(Stack &&rhs) {
+    if (&rhs != this) {
+      Destroy();
+
+      allocator_ = rhs.allocator_;
+      ownAllocator_ = rhs.ownAllocator_;
+      stack_ = rhs.stack_;
+      stackTop_ = rhs.stackTop_;
+      stackEnd_ = rhs.stackEnd_;
+      initialCapacity_ = rhs.initialCapacity_;
+
+      rhs.allocator_ = 0;
+      rhs.ownAllocator_ = 0;
+      rhs.stack_ = 0;
+      rhs.stackTop_ = 0;
+      rhs.stackEnd_ = 0;
+      rhs.initialCapacity_ = 0;
+    }
+    return *this;
+  }
+#endif
+
+  void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
+    internal::Swap(allocator_, rhs.allocator_);
+    internal::Swap(ownAllocator_, rhs.ownAllocator_);
+    internal::Swap(stack_, rhs.stack_);
+    internal::Swap(stackTop_, rhs.stackTop_);
+    internal::Swap(stackEnd_, rhs.stackEnd_);
+    internal::Swap(initialCapacity_, rhs.initialCapacity_);
+  }
+
+  void Clear() { stackTop_ = stack_; }
+
+  void ShrinkToFit() {
+    if (Empty()) {
+      // If the stack is empty, completely deallocate the memory.
+      Allocator::Free(stack_);  // NOLINT (+clang-analyzer-unix.Malloc)
+      stack_ = 0;
+      stackTop_ = 0;
+      stackEnd_ = 0;
+    } else
+      Resize(GetSize());
+  }
+
+  // Optimization note: try to minimize the size of this function for force
+  // inline. Expansion is run very infrequently, so it is moved to another
+  // (probably non-inline) function.
+  template <typename T>
+  RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
+    // Expand the stack if needed
+    if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
+                           (stackEnd_ - stackTop_)))
+      Expand<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
+    Reserve<T>(count);
+    return PushUnsafe<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
+    RAPIDJSON_ASSERT(stackTop_);
+    RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
+                     (stackEnd_ - stackTop_));
+    T *ret = reinterpret_cast<T *>(stackTop_);
+    stackTop_ += sizeof(T) * count;
+    return ret;
+  }
+
+  template <typename T>
+  T *Pop(size_t count) {
+    RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
+    stackTop_ -= count * sizeof(T);
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Top() {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  const T *Top() const {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  T *End() {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  const T *End() const {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Bottom() {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  template <typename T>
+  const T *Bottom() const {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  bool HasAllocator() const { return allocator_ != 0; }
+
+  Allocator &GetAllocator() {
+    RAPIDJSON_ASSERT(allocator_);
+    return *allocator_;
+  }
+
+  bool Empty() const { return stackTop_ == stack_; }
+  size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
+  size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
+
+ private:
+  template <typename T>
+  void Expand(size_t count) {
+    // Only expand the capacity if the current stack exists. Otherwise just
+    // create a stack with initial capacity.
+    size_t newCapacity;
+    if (stack_ == 0) {
+      if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+      newCapacity = initialCapacity_;
+    } else {
+      newCapacity = GetCapacity();
+      newCapacity += (newCapacity + 1) / 2;
+    }
+    size_t newSize = GetSize() + sizeof(T) * count;
+    if (newCapacity < newSize) newCapacity = newSize;
+
+    Resize(newCapacity);
+  }
+
+  void Resize(size_t newCapacity) {
+    const size_t size = GetSize();  // Backup the current size
+    stack_ = static_cast<char *>(
+        allocator_->Realloc(stack_, GetCapacity(), newCapacity));
+    stackTop_ = stack_ + size;
+    stackEnd_ = stack_ + newCapacity;
+  }
+
+  void Destroy() {
+    Allocator::Free(stack_);
+    RAPIDJSON_DELETE(ownAllocator_);  // Only delete if it is owned by the stack
+  }
+
+  // Prohibit copy constructor & assignment operator.
+  Stack(const Stack &);
+  Stack &operator=(const Stack &);
+
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  char *stack_;
+  char *stackTop_;
+  char *stackEnd_;
+  size_t initialCapacity_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_STACK_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/strfunc.h

@@ -0,0 +1,74 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
+#define RAPIDJSON_INTERNAL_STRFUNC_H_
+
+#include <cwchar>
+#include "../stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom strlen() which works on different character types.
+/*! \tparam Ch Character type (e.g. char, wchar_t, short)
+    \param s Null-terminated input string.
+    \return Number of characters in the string.
+    \note This has the same semantics as strlen(), the return value is not
+   number of Unicode codepoints.
+*/
+template <typename Ch>
+inline SizeType StrLen(const Ch *s) {
+  RAPIDJSON_ASSERT(s != 0);
+  const Ch *p = s;
+  while (*p) ++p;
+  return SizeType(p - s);
+}
+
+template <>
+inline SizeType StrLen(const char *s) {
+  return SizeType(std::strlen(s));
+}
+
+template <>
+inline SizeType StrLen(const wchar_t *s) {
+  return SizeType(std::wcslen(s));
+}
+
+//! Returns number of code points in a encoded string.
+template <typename Encoding>
+bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
+                          SizeType *outCount) {
+  RAPIDJSON_ASSERT(s != 0);
+  RAPIDJSON_ASSERT(outCount != 0);
+  GenericStringStream<Encoding> is(s);
+  const typename Encoding::Ch *end = s + length;
+  SizeType count = 0;
+  while (is.src_ < end) {
+    unsigned codepoint;
+    if (!Encoding::Decode(is, &codepoint)) return false;
+    count++;
+  }
+  *outCount = count;
+  return true;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_INTERNAL_STRFUNC_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/strtod.h

@@ -0,0 +1,303 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_STRTOD_
+#define RAPIDJSON_STRTOD_
+
+#include <climits>
+#include <limits>
+#include "biginteger.h"
+#include "diyfp.h"
+#include "ieee754.h"
+#include "pow10.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline double FastPath(double significand, int exp) {
+  if (exp < -308)
+    return 0.0;
+  else if (exp >= 0)
+    return significand * internal::Pow10(exp);
+  else
+    return significand / internal::Pow10(-exp);
+}
+
+inline double StrtodNormalPrecision(double d, int p) {
+  if (p < -308) {
+    // Prevent expSum < -308, making Pow10(p) = 0
+    d = FastPath(d, -308);
+    d = FastPath(d, p + 308);
+  } else
+    d = FastPath(d, p);
+  return d;
+}
+
+template <typename T>
+inline T Min3(T a, T b, T c) {
+  T m = a;
+  if (m > b) m = b;
+  if (m > c) m = c;
+  return m;
+}
+
+inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) {
+  const Double db(b);
+  const uint64_t bInt = db.IntegerSignificand();
+  const int bExp = db.IntegerExponent();
+  const int hExp = bExp - 1;
+
+  int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0,
+      hS_Exp5 = 0;
+
+  // Adjust for decimal exponent
+  if (dExp >= 0) {
+    dS_Exp2 += dExp;
+    dS_Exp5 += dExp;
+  } else {
+    bS_Exp2 -= dExp;
+    bS_Exp5 -= dExp;
+    hS_Exp2 -= dExp;
+    hS_Exp5 -= dExp;
+  }
+
+  // Adjust for binary exponent
+  if (bExp >= 0)
+    bS_Exp2 += bExp;
+  else {
+    dS_Exp2 -= bExp;
+    hS_Exp2 -= bExp;
+  }
+
+  // Adjust for half ulp exponent
+  if (hExp >= 0)
+    hS_Exp2 += hExp;
+  else {
+    dS_Exp2 -= hExp;
+    bS_Exp2 -= hExp;
+  }
+
+  // Remove common power of two factor from all three scaled values
+  int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
+  dS_Exp2 -= common_Exp2;
+  bS_Exp2 -= common_Exp2;
+  hS_Exp2 -= common_Exp2;
+
+  BigInteger dS = d;
+  dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<=
+      static_cast<unsigned>(dS_Exp2);
+
+  BigInteger bS(bInt);
+  bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<=
+      static_cast<unsigned>(bS_Exp2);
+
+  BigInteger hS(1);
+  hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
+      static_cast<unsigned>(hS_Exp2);
+
+  BigInteger delta(0);
+  dS.Difference(bS, &delta);
+
+  return delta.Compare(hS);
+}
+
+inline bool StrtodFast(double d, int p, double *result) {
+  // Use fast path for string-to-double conversion if possible
+  // see
+  // http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
+  if (p > 22 && p < 22 + 16) {
+    // Fast Path Cases In Disguise
+    d *= internal::Pow10(p - 22);
+    p = 22;
+  }
+
+  if (p >= -22 && p <= 22 && d <= 9007199254740991.0) {  // 2^53 - 1
+    *result = FastPath(d, p);
+    return true;
+  } else
+    return false;
+}
+
+// Compute an approximation and see if it is within 1/2 ULP
+inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
+                        double *result) {
+  uint64_t significand = 0;
+  int i = 0;  // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
+              // 0x1999999999999999
+  for (; i < dLen; i++) {
+    if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
+        (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
+         decimals[i] > '5'))
+      break;
+    significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
+  }
+
+  if (i < dLen && decimals[i] >= '5')  // Rounding
+    significand++;
+
+  int remaining = dLen - i;
+  const int kUlpShift = 3;
+  const int kUlp = 1 << kUlpShift;
+  int64_t error = (remaining == 0) ? 0 : kUlp / 2;
+
+  DiyFp v(significand, 0);
+  v = v.Normalize();
+  error <<= -v.e;
+
+  dExp += remaining;
+
+  int actualExp;
+  DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
+  if (actualExp != dExp) {
+    static const DiyFp kPow10[] = {
+        DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60),  // 10^1
+        DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57),  // 10^2
+        DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54),  // 10^3
+        DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50),  // 10^4
+        DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47),  // 10^5
+        DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44),  // 10^6
+        DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40)   // 10^7
+    };
+    int adjustment = dExp - actualExp;
+    RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
+    v = v * kPow10[adjustment - 1];
+    if (dLen + adjustment >
+        19)  // has more digits than decimal digits in 64-bit
+      error += kUlp / 2;
+  }
+
+  v = v * cachedPower;
+
+  error += kUlp + (error == 0 ? 0 : 1);
+
+  const int oldExp = v.e;
+  v = v.Normalize();
+  error <<= oldExp - v.e;
+
+  const int effectiveSignificandSize =
+      Double::EffectiveSignificandSize(64 + v.e);
+  int precisionSize = 64 - effectiveSignificandSize;
+  if (precisionSize + kUlpShift >= 64) {
+    int scaleExp = (precisionSize + kUlpShift) - 63;
+    v.f >>= scaleExp;
+    v.e += scaleExp;
+    error = (error >> scaleExp) + 1 + kUlp;
+    precisionSize -= scaleExp;
+  }
+
+  DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
+  const uint64_t precisionBits =
+      (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
+  const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
+  if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
+    rounded.f++;
+    if (rounded.f & (DiyFp::kDpHiddenBit
+                     << 1)) {  // rounding overflows mantissa (issue #340)
+      rounded.f >>= 1;
+      rounded.e++;
+    }
+  }
+
+  *result = rounded.ToDouble();
+
+  return halfWay - static_cast<unsigned>(error) >= precisionBits ||
+         precisionBits >= halfWay + static_cast<unsigned>(error);
+}
+
+inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
+                               int dExp) {
+  RAPIDJSON_ASSERT(dLen >= 0);
+  const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
+  Double a(approx);
+  int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
+  if (cmp < 0)
+    return a.Value();  // within half ULP
+  else if (cmp == 0) {
+    // Round towards even
+    if (a.Significand() & 1)
+      return a.NextPositiveDouble();
+    else
+      return a.Value();
+  } else  // adjustment
+    return a.NextPositiveDouble();
+}
+
+inline double StrtodFullPrecision(double d, int p, const char *decimals,
+                                  size_t length, size_t decimalPosition,
+                                  int exp) {
+  RAPIDJSON_ASSERT(d >= 0.0);
+  RAPIDJSON_ASSERT(length >= 1);
+
+  double result = 0.0;
+  if (StrtodFast(d, p, &result)) return result;
+
+  RAPIDJSON_ASSERT(length <= INT_MAX);
+  int dLen = static_cast<int>(length);
+
+  RAPIDJSON_ASSERT(length >= decimalPosition);
+  RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
+  int dExpAdjust = static_cast<int>(length - decimalPosition);
+
+  RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
+  int dExp = exp - dExpAdjust;
+
+  // Make sure length+dExp does not overflow
+  RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
+
+  // Trim leading zeros
+  while (dLen > 0 && *decimals == '0') {
+    dLen--;
+    decimals++;
+  }
+
+  // Trim trailing zeros
+  while (dLen > 0 && decimals[dLen - 1] == '0') {
+    dLen--;
+    dExp++;
+  }
+
+  if (dLen == 0) {  // Buffer only contains zeros.
+    return 0.0;
+  }
+
+  // Trim right-most digits
+  const int kMaxDecimalDigit = 767 + 1;
+  if (dLen > kMaxDecimalDigit) {
+    dExp += dLen - kMaxDecimalDigit;
+    dLen = kMaxDecimalDigit;
+  }
+
+  // If too small, underflow to zero.
+  // Any x <= 10^-324 is interpreted as zero.
+  if (dLen + dExp <= -324) return 0.0;
+
+  // If too large, overflow to infinity.
+  // Any x >= 10^309 is interpreted as +infinity.
+  if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
+
+  if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
+
+  // Use approximation from StrtodDiyFp and make adjustment with BigInteger
+  // comparison
+  return StrtodBigInteger(result, decimals, dLen, dExp);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_STRTOD_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/internal/swap.h

@@ -0,0 +1,50 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_SWAP_H_
+#define RAPIDJSON_INTERNAL_SWAP_H_
+
+#include "../rapidjson.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom swap() to avoid dependency on C++ <algorithm> header
+/*! \tparam T Type of the arguments to swap, should be instantiated with
+   primitive C++ types only. \note This has the same semantics as std::swap().
+*/
+template <typename T>
+inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
+  T tmp = a;
+  a = b;
+  b = tmp;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_SWAP_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/istreamwrapper.h

@@ -0,0 +1,161 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
+#define RAPIDJSON_ISTREAMWRAPPER_H_
+
+#include <ios>
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4351)  // new behavior: elements of array 'array' will be
+                          // default initialized
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::istringstream
+    - \c std::stringstream
+    - \c std::wistringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wifstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_istream.
+*/
+
+template <typename StreamType>
+class BasicIStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+  */
+  BasicIStreamWrapper(StreamType &stream)
+      : stream_(stream),
+        buffer_(peekBuffer_),
+        bufferSize_(4),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    Read();
+  }
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
+      : stream_(stream),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  BasicIStreamWrapper();
+  BasicIStreamWrapper(const BasicIStreamWrapper &);
+  BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
+
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = bufferSize_;
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
+        readCount_ = static_cast<size_t>(stream_.gcount());
+        *(bufferLast_ = buffer_ + readCount_) = '\0';
+        eof_ = true;
+      }
+    }
+  }
+
+  StreamType &stream_;
+  Ch peekBuffer_[4], *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
+typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ISTREAMWRAPPER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/license.txt

@@ -0,0 +1,57 @@
+Tencent is pleased to support the open source community by making RapidJSON available. 
+ 
+Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip.  All rights reserved.
+
+If you have downloaded a copy of the RapidJSON binary from Tencent, please note that the RapidJSON binary is licensed under the MIT License.
+If you have downloaded a copy of the RapidJSON source code from Tencent, please note that RapidJSON source code is licensed under the MIT License, except for the third-party components listed below which are subject to different license terms.  Your integration of RapidJSON into your own projects may require compliance with the MIT License, as well as the other licenses applicable to the third-party components included within RapidJSON. To avoid the problematic JSON license in your own projects, it's sufficient to exclude the bin/jsonchecker/ directory, as it's the only code under the JSON license.
+A copy of the MIT License is included in this file.
+
+Other dependencies and licenses:
+
+Open Source Software Licensed Under the BSD License:
+--------------------------------------------------------------------
+
+The msinttypes r29 
+Copyright (c) 2006-2013 Alexander Chemeris 
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
+* 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.
+* Neither the name of  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 REGENTS 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 REGENTS AND 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.
+
+Open Source Software Licensed Under the JSON License:
+--------------------------------------------------------------------
+
+json.org 
+Copyright (c) 2002 JSON.org
+All Rights Reserved.
+
+JSON_checker
+Copyright (c) 2002 JSON.org
+All Rights Reserved.
+
+	
+Terms of the JSON License:
+---------------------------------------------------
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+The Software shall be used for Good, not Evil.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+Terms of the MIT License:
+--------------------------------------------------------------------
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/memorybuffer.h

@@ -0,0 +1,78 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_MEMORYBUFFER_H_
+#define RAPIDJSON_MEMORYBUFFER_H_
+
+#include "internal/stack.h"
+#include "stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory output byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedOutputStream or
+   AutoUTFOutputStream.
+
+    It is similar to FileWriteBuffer but the destination is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryBuffer and StringBuffer:
+    1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
+    2. StringBuffer::GetString() returns a null-terminated string.
+   MemoryBuffer::GetBuffer() returns a buffer without terminator.
+
+    \tparam Allocator type for allocating memory buffer.
+    \note implements Stream concept
+*/
+template <typename Allocator = CrtAllocator>
+struct GenericMemoryBuffer {
+  typedef char Ch;  // byte
+
+  GenericMemoryBuffer(Allocator *allocator = 0,
+                      size_t capacity = kDefaultCapacity)
+      : stack_(allocator, capacity) {}
+
+  void Put(Ch c) { *stack_.template Push<Ch>() = c; }
+  void Flush() {}
+
+  void Clear() { stack_.Clear(); }
+  void ShrinkToFit() { stack_.ShrinkToFit(); }
+  Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
+  void Pop(size_t count) { stack_.template Pop<Ch>(count); }
+
+  const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
+
+  size_t GetSize() const { return stack_.GetSize(); }
+
+  static const size_t kDefaultCapacity = 256;
+  mutable internal::Stack<Allocator> stack_;
+};
+
+typedef GenericMemoryBuffer<> MemoryBuffer;
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
+  std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/memorystream.h

@@ -0,0 +1,84 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_MEMORYSTREAM_H_
+#define RAPIDJSON_MEMORYSTREAM_H_
+
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory input byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedInputStream or
+   AutoUTFInputStream.
+
+    It is similar to FileReadBuffer but the source is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryStream and StringStream:
+    1. StringStream has encoding but MemoryStream is a byte stream.
+    2. MemoryStream needs size of the source buffer and the buffer don't need to
+   be null terminated. StringStream assume null-terminated string as source.
+    3. MemoryStream supports Peek4() for encoding detection. StringStream is
+   specified with an encoding so it should not have Peek4(). \note implements
+   Stream concept
+*/
+struct MemoryStream {
+  typedef char Ch;  // byte
+
+  MemoryStream(const Ch *src, size_t size)
+      : src_(src), begin_(src), end_(src + size), size_(size) {}
+
+  Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
+  Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
+  size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
+
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
+
+  const Ch *src_;    //!< Current read position.
+  const Ch *begin_;  //!< Original head of the string.
+  const Ch *end_;    //!< End of stream.
+  size_t size_;      //!< Size of the stream.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/msinttypes/inttypes.h

@@ -0,0 +1,316 @@
+// ISO C9x  compliant inttypes.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// 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 product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_INTTYPES_H_  // [
+#define _MSC_INTTYPES_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+#include "stdint.h"
+
+// miloyip: VC supports inttypes.h since VC2013
+#if _MSC_VER >= 1800
+#include <inttypes.h>
+#else
+
+// 7.8 Format conversion of integer types
+
+typedef struct {
+  intmax_t quot;
+  intmax_t rem;
+} imaxdiv_t;
+
+// 7.8.1 Macros for format specifiers
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_FORMAT_MACROS)  // [   See footnote 185 at page 198
+
+// The fprintf macros for signed integers are:
+#define PRId8 "d"
+#define PRIi8 "i"
+#define PRIdLEAST8 "d"
+#define PRIiLEAST8 "i"
+#define PRIdFAST8 "d"
+#define PRIiFAST8 "i"
+
+#define PRId16 "hd"
+#define PRIi16 "hi"
+#define PRIdLEAST16 "hd"
+#define PRIiLEAST16 "hi"
+#define PRIdFAST16 "hd"
+#define PRIiFAST16 "hi"
+
+#define PRId32 "I32d"
+#define PRIi32 "I32i"
+#define PRIdLEAST32 "I32d"
+#define PRIiLEAST32 "I32i"
+#define PRIdFAST32 "I32d"
+#define PRIiFAST32 "I32i"
+
+#define PRId64 "I64d"
+#define PRIi64 "I64i"
+#define PRIdLEAST64 "I64d"
+#define PRIiLEAST64 "I64i"
+#define PRIdFAST64 "I64d"
+#define PRIiFAST64 "I64i"
+
+#define PRIdMAX "I64d"
+#define PRIiMAX "I64i"
+
+#define PRIdPTR "Id"
+#define PRIiPTR "Ii"
+
+// The fprintf macros for unsigned integers are:
+#define PRIo8 "o"
+#define PRIu8 "u"
+#define PRIx8 "x"
+#define PRIX8 "X"
+#define PRIoLEAST8 "o"
+#define PRIuLEAST8 "u"
+#define PRIxLEAST8 "x"
+#define PRIXLEAST8 "X"
+#define PRIoFAST8 "o"
+#define PRIuFAST8 "u"
+#define PRIxFAST8 "x"
+#define PRIXFAST8 "X"
+
+#define PRIo16 "ho"
+#define PRIu16 "hu"
+#define PRIx16 "hx"
+#define PRIX16 "hX"
+#define PRIoLEAST16 "ho"
+#define PRIuLEAST16 "hu"
+#define PRIxLEAST16 "hx"
+#define PRIXLEAST16 "hX"
+#define PRIoFAST16 "ho"
+#define PRIuFAST16 "hu"
+#define PRIxFAST16 "hx"
+#define PRIXFAST16 "hX"
+
+#define PRIo32 "I32o"
+#define PRIu32 "I32u"
+#define PRIx32 "I32x"
+#define PRIX32 "I32X"
+#define PRIoLEAST32 "I32o"
+#define PRIuLEAST32 "I32u"
+#define PRIxLEAST32 "I32x"
+#define PRIXLEAST32 "I32X"
+#define PRIoFAST32 "I32o"
+#define PRIuFAST32 "I32u"
+#define PRIxFAST32 "I32x"
+#define PRIXFAST32 "I32X"
+
+#define PRIo64 "I64o"
+#define PRIu64 "I64u"
+#define PRIx64 "I64x"
+#define PRIX64 "I64X"
+#define PRIoLEAST64 "I64o"
+#define PRIuLEAST64 "I64u"
+#define PRIxLEAST64 "I64x"
+#define PRIXLEAST64 "I64X"
+#define PRIoFAST64 "I64o"
+#define PRIuFAST64 "I64u"
+#define PRIxFAST64 "I64x"
+#define PRIXFAST64 "I64X"
+
+#define PRIoMAX "I64o"
+#define PRIuMAX "I64u"
+#define PRIxMAX "I64x"
+#define PRIXMAX "I64X"
+
+#define PRIoPTR "Io"
+#define PRIuPTR "Iu"
+#define PRIxPTR "Ix"
+#define PRIXPTR "IX"
+
+// The fscanf macros for signed integers are:
+#define SCNd8 "d"
+#define SCNi8 "i"
+#define SCNdLEAST8 "d"
+#define SCNiLEAST8 "i"
+#define SCNdFAST8 "d"
+#define SCNiFAST8 "i"
+
+#define SCNd16 "hd"
+#define SCNi16 "hi"
+#define SCNdLEAST16 "hd"
+#define SCNiLEAST16 "hi"
+#define SCNdFAST16 "hd"
+#define SCNiFAST16 "hi"
+
+#define SCNd32 "ld"
+#define SCNi32 "li"
+#define SCNdLEAST32 "ld"
+#define SCNiLEAST32 "li"
+#define SCNdFAST32 "ld"
+#define SCNiFAST32 "li"
+
+#define SCNd64 "I64d"
+#define SCNi64 "I64i"
+#define SCNdLEAST64 "I64d"
+#define SCNiLEAST64 "I64i"
+#define SCNdFAST64 "I64d"
+#define SCNiFAST64 "I64i"
+
+#define SCNdMAX "I64d"
+#define SCNiMAX "I64i"
+
+#ifdef _WIN64  // [
+#define SCNdPTR "I64d"
+#define SCNiPTR "I64i"
+#else  // _WIN64 ][
+#define SCNdPTR "ld"
+#define SCNiPTR "li"
+#endif  // _WIN64 ]
+
+// The fscanf macros for unsigned integers are:
+#define SCNo8 "o"
+#define SCNu8 "u"
+#define SCNx8 "x"
+#define SCNX8 "X"
+#define SCNoLEAST8 "o"
+#define SCNuLEAST8 "u"
+#define SCNxLEAST8 "x"
+#define SCNXLEAST8 "X"
+#define SCNoFAST8 "o"
+#define SCNuFAST8 "u"
+#define SCNxFAST8 "x"
+#define SCNXFAST8 "X"
+
+#define SCNo16 "ho"
+#define SCNu16 "hu"
+#define SCNx16 "hx"
+#define SCNX16 "hX"
+#define SCNoLEAST16 "ho"
+#define SCNuLEAST16 "hu"
+#define SCNxLEAST16 "hx"
+#define SCNXLEAST16 "hX"
+#define SCNoFAST16 "ho"
+#define SCNuFAST16 "hu"
+#define SCNxFAST16 "hx"
+#define SCNXFAST16 "hX"
+
+#define SCNo32 "lo"
+#define SCNu32 "lu"
+#define SCNx32 "lx"
+#define SCNX32 "lX"
+#define SCNoLEAST32 "lo"
+#define SCNuLEAST32 "lu"
+#define SCNxLEAST32 "lx"
+#define SCNXLEAST32 "lX"
+#define SCNoFAST32 "lo"
+#define SCNuFAST32 "lu"
+#define SCNxFAST32 "lx"
+#define SCNXFAST32 "lX"
+
+#define SCNo64 "I64o"
+#define SCNu64 "I64u"
+#define SCNx64 "I64x"
+#define SCNX64 "I64X"
+#define SCNoLEAST64 "I64o"
+#define SCNuLEAST64 "I64u"
+#define SCNxLEAST64 "I64x"
+#define SCNXLEAST64 "I64X"
+#define SCNoFAST64 "I64o"
+#define SCNuFAST64 "I64u"
+#define SCNxFAST64 "I64x"
+#define SCNXFAST64 "I64X"
+
+#define SCNoMAX "I64o"
+#define SCNuMAX "I64u"
+#define SCNxMAX "I64x"
+#define SCNXMAX "I64X"
+
+#ifdef _WIN64  // [
+#define SCNoPTR "I64o"
+#define SCNuPTR "I64u"
+#define SCNxPTR "I64x"
+#define SCNXPTR "I64X"
+#else  // _WIN64 ][
+#define SCNoPTR "lo"
+#define SCNuPTR "lu"
+#define SCNxPTR "lx"
+#define SCNXPTR "lX"
+#endif  // _WIN64 ]
+
+#endif  // __STDC_FORMAT_MACROS ]
+
+// 7.8.2 Functions for greatest-width integer types
+
+// 7.8.2.1 The imaxabs function
+#define imaxabs _abs64
+
+// 7.8.2.2 The imaxdiv function
+
+// This is modified version of div() function from Microsoft's div.c found
+// in %MSVC.NET%\crt\src\div.c
+#ifdef STATIC_IMAXDIV  // [
+static
+#else                  // STATIC_IMAXDIV ][
+_inline
+#endif                 // STATIC_IMAXDIV ]
+    imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
+  imaxdiv_t result;
+
+  result.quot = numer / denom;
+  result.rem = numer % denom;
+
+  if (numer < 0 && result.rem > 0) {
+    // did division wrong; must fix up
+    ++result.quot;
+    result.rem -= denom;
+  }
+
+  return result;
+}
+
+// 7.8.2.3 The strtoimax and strtoumax functions
+#define strtoimax _strtoi64
+#define strtoumax _strtoui64
+
+// 7.8.2.4 The wcstoimax and wcstoumax functions
+#define wcstoimax _wcstoi64
+#define wcstoumax _wcstoui64
+
+#endif  // _MSC_VER >= 1800
+
+#endif  // _MSC_INTTYPES_H_ ]

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/msinttypes/stdint.h

@@ -0,0 +1,301 @@
+// ISO C9x  compliant stdint.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// 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 product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_STDINT_H_  // [
+#define _MSC_STDINT_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
+// generates warning with INT64_C(), so change to use this file for vs2010.
+#if _MSC_VER >= 1600  // [
+#include <stdint.h>
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+#undef INT8_C
+#undef INT16_C
+#undef INT32_C
+#undef INT64_C
+#undef UINT8_C
+#undef UINT16_C
+#undef UINT32_C
+#undef UINT64_C
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#else  // ] _MSC_VER >= 1700 [
+
+#include <limits.h>
+
+// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
+// compiling for ARM we have to wrap <wchar.h> include with 'extern "C++" {}'
+// or compiler would give many errors like this:
+//   error C2733: second C linkage of overloaded function 'wmemchr' not allowed
+#if defined(__cplusplus) && !defined(_M_ARM)
+extern "C" {
+#endif
+#include <wchar.h>
+#if defined(__cplusplus) && !defined(_M_ARM)
+}
+#endif
+
+// Define _W64 macros to mark types changing their size, like intptr_t.
+#ifndef _W64
+#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
+#define _W64 __w64
+#else
+#define _W64
+#endif
+#endif
+
+// 7.18.1 Integer types
+
+// 7.18.1.1 Exact-width integer types
+
+// Visual Studio 6 and Embedded Visual C++ 4 doesn't
+// realize that, e.g. char has the same size as __int8
+// so we give up on __intX for them.
+#if (_MSC_VER < 1300)
+typedef signed char int8_t;
+typedef signed short int16_t;
+typedef signed int int32_t;
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned int uint32_t;
+#else
+typedef signed __int8 int8_t;
+typedef signed __int16 int16_t;
+typedef signed __int32 int32_t;
+typedef unsigned __int8 uint8_t;
+typedef unsigned __int16 uint16_t;
+typedef unsigned __int32 uint32_t;
+#endif
+typedef signed __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+
+// 7.18.1.2 Minimum-width integer types
+typedef int8_t int_least8_t;
+typedef int16_t int_least16_t;
+typedef int32_t int_least32_t;
+typedef int64_t int_least64_t;
+typedef uint8_t uint_least8_t;
+typedef uint16_t uint_least16_t;
+typedef uint32_t uint_least32_t;
+typedef uint64_t uint_least64_t;
+
+// 7.18.1.3 Fastest minimum-width integer types
+typedef int8_t int_fast8_t;
+typedef int16_t int_fast16_t;
+typedef int32_t int_fast32_t;
+typedef int64_t int_fast64_t;
+typedef uint8_t uint_fast8_t;
+typedef uint16_t uint_fast16_t;
+typedef uint32_t uint_fast32_t;
+typedef uint64_t uint_fast64_t;
+
+// 7.18.1.4 Integer types capable of holding object pointers
+#ifdef _WIN64  // [
+typedef signed __int64 intptr_t;
+typedef unsigned __int64 uintptr_t;
+#else          // _WIN64 ][
+typedef _W64 signed int intptr_t;
+typedef _W64 unsigned int uintptr_t;
+#endif  // _WIN64 ]
+
+// 7.18.1.5 Greatest-width integer types
+typedef int64_t intmax_t;
+typedef uint64_t uintmax_t;
+
+// 7.18.2 Limits of specified-width integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_LIMIT_MACROS)  // [   See footnote 220 at page 257 and
+// footnote 221 at page 259
+
+// 7.18.2.1 Limits of exact-width integer types
+#define INT8_MIN ((int8_t)_I8_MIN)
+#define INT8_MAX _I8_MAX
+#define INT16_MIN ((int16_t)_I16_MIN)
+#define INT16_MAX _I16_MAX
+#define INT32_MIN ((int32_t)_I32_MIN)
+#define INT32_MAX _I32_MAX
+#define INT64_MIN ((int64_t)_I64_MIN)
+#define INT64_MAX _I64_MAX
+#define UINT8_MAX _UI8_MAX
+#define UINT16_MAX _UI16_MAX
+#define UINT32_MAX _UI32_MAX
+#define UINT64_MAX _UI64_MAX
+
+// 7.18.2.2 Limits of minimum-width integer types
+#define INT_LEAST8_MIN INT8_MIN
+#define INT_LEAST8_MAX INT8_MAX
+#define INT_LEAST16_MIN INT16_MIN
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST32_MIN INT32_MIN
+#define INT_LEAST32_MAX INT32_MAX
+#define INT_LEAST64_MIN INT64_MIN
+#define INT_LEAST64_MAX INT64_MAX
+#define UINT_LEAST8_MAX UINT8_MAX
+#define UINT_LEAST16_MAX UINT16_MAX
+#define UINT_LEAST32_MAX UINT32_MAX
+#define UINT_LEAST64_MAX UINT64_MAX
+
+// 7.18.2.3 Limits of fastest minimum-width integer types
+#define INT_FAST8_MIN INT8_MIN
+#define INT_FAST8_MAX INT8_MAX
+#define INT_FAST16_MIN INT16_MIN
+#define INT_FAST16_MAX INT16_MAX
+#define INT_FAST32_MIN INT32_MIN
+#define INT_FAST32_MAX INT32_MAX
+#define INT_FAST64_MIN INT64_MIN
+#define INT_FAST64_MAX INT64_MAX
+#define UINT_FAST8_MAX UINT8_MAX
+#define UINT_FAST16_MAX UINT16_MAX
+#define UINT_FAST32_MAX UINT32_MAX
+#define UINT_FAST64_MAX UINT64_MAX
+
+// 7.18.2.4 Limits of integer types capable of holding object pointers
+#ifdef _WIN64  // [
+#define INTPTR_MIN INT64_MIN
+#define INTPTR_MAX INT64_MAX
+#define UINTPTR_MAX UINT64_MAX
+#else  // _WIN64 ][
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#endif  // _WIN64 ]
+
+// 7.18.2.5 Limits of greatest-width integer types
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MAX INT64_MAX
+#define UINTMAX_MAX UINT64_MAX
+
+// 7.18.3 Limits of other integer types
+
+#ifdef _WIN64  // [
+#define PTRDIFF_MIN _I64_MIN
+#define PTRDIFF_MAX _I64_MAX
+#else  // _WIN64 ][
+#define PTRDIFF_MIN _I32_MIN
+#define PTRDIFF_MAX _I32_MAX
+#endif  // _WIN64 ]
+
+#define SIG_ATOMIC_MIN INT_MIN
+#define SIG_ATOMIC_MAX INT_MAX
+
+#ifndef SIZE_MAX  // [
+#ifdef _WIN64     // [
+#define SIZE_MAX _UI64_MAX
+#else  // _WIN64 ][
+#define SIZE_MAX _UI32_MAX
+#endif  // _WIN64 ]
+#endif  // SIZE_MAX ]
+
+// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
+#ifndef WCHAR_MIN  // [
+#define WCHAR_MIN 0
+#endif             // WCHAR_MIN ]
+#ifndef WCHAR_MAX  // [
+#define WCHAR_MAX _UI16_MAX
+#endif  // WCHAR_MAX ]
+
+#define WINT_MIN 0
+#define WINT_MAX _UI16_MAX
+
+#endif  // __STDC_LIMIT_MACROS ]
+
+// 7.18.4 Limits of other integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#endif  // _MSC_VER >= 1600 ]
+
+#endif  // _MSC_STDINT_H_ ]

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/ostreamwrapper.h

@@ -0,0 +1,96 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
+#define RAPIDJSON_OSTREAMWRAPPER_H_
+
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::ostringstream
+    - \c std::stringstream
+    - \c std::wpstringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wofstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_ostream.
+*/
+
+template <typename StreamType>
+class BasicOStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+  BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
+
+  void Put(Ch c) { stream_.put(c); }
+
+  void Flush() { stream_.flush(); }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  BasicOStreamWrapper(const BasicOStreamWrapper &);
+  BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
+
+  StreamType &stream_;
+};
+
+typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
+typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_OSTREAMWRAPPER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/prettywriter.h

@@ -0,0 +1,333 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_PRETTYWRITER_H_
+#define RAPIDJSON_PRETTYWRITER_H_
+
+#include "writer.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Combination of PrettyWriter format flags.
+/*! \see PrettyWriter::SetFormatOptions
+ */
+enum PrettyFormatOptions {
+  kFormatDefault = 0,         //!< Default pretty formatting.
+  kFormatSingleLineArray = 1  //!< Format arrays on a single line.
+};
+
+//! Writer with indentation and spacing.
+/*!
+    \tparam OutputStream Type of output os.
+    \tparam SourceEncoding Encoding of source string.
+    \tparam TargetEncoding Encoding of output stream.
+    \tparam StackAllocator Type of allocator for allocating memory of stack.
+*/
+template <typename OutputStream, typename SourceEncoding = UTF8<>,
+          typename TargetEncoding = UTF8<>,
+          typename StackAllocator = CrtAllocator,
+          unsigned writeFlags = kWriteDefaultFlags>
+class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
+                                   StackAllocator, writeFlags> {
+ public:
+  typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
+                 writeFlags>
+      Base;
+  typedef typename Base::Ch Ch;
+
+  //! Constructor
+  /*! \param os Output stream.
+      \param allocator User supplied allocator. If it is null, it will create a
+     private one. \param levelDepth Initial capacity of stack.
+  */
+  explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(os, allocator, levelDepth),
+        indentChar_(' '),
+        indentCharCount_(4),
+        formatOptions_(kFormatDefault) {}
+
+  explicit PrettyWriter(StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  PrettyWriter(PrettyWriter &&rhs)
+      : Base(std::forward<PrettyWriter>(rhs)),
+        indentChar_(rhs.indentChar_),
+        indentCharCount_(rhs.indentCharCount_),
+        formatOptions_(rhs.formatOptions_) {}
+#endif
+
+  //! Set custom indentation.
+  /*! \param indentChar       Character for indentation. Must be whitespace
+     character (' ', '\\t', '\\n', '\\r'). \param indentCharCount  Number of
+     indent characters for each indentation level. \note The default indentation
+     is 4 spaces.
+  */
+  PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
+    RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
+                     indentChar == '\n' || indentChar == '\r');
+    indentChar_ = indentChar;
+    indentCharCount_ = indentCharCount;
+    return *this;
+  }
+
+  //! Set pretty writer formatting options.
+  /*! \param options Formatting options.
+   */
+  PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
+    formatOptions_ = options;
+    return *this;
+  }
+
+  /*! @name Implementation of Handler
+      \see Handler
+  */
+  //@{
+
+  bool Null() {
+    PrettyPrefix(kNullType);
+    return Base::EndValue(Base::WriteNull());
+  }
+  bool Bool(bool b) {
+    PrettyPrefix(b ? kTrueType : kFalseType);
+    return Base::EndValue(Base::WriteBool(b));
+  }
+  bool Int(int i) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt(i));
+  }
+  bool Uint(unsigned u) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint(u));
+  }
+  bool Int64(int64_t i64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt64(i64));
+  }
+  bool Uint64(uint64_t u64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint64(u64));
+  }
+  bool Double(double d) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteDouble(d));
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kStringType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool String(const std::basic_string<Ch> &str) {
+    return String(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool StartObject() {
+    PrettyPrefix(kObjectType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(false);
+    return Base::WriteStartObject();
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy = false) {
+    return String(str, length, copy);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool Key(const std::basic_string<Ch> &str) {
+    return Key(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool EndObject(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));  // not inside an Object
+    RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
+                          ->inArray);  // currently inside an Array, not Object
+    RAPIDJSON_ASSERT(
+        0 ==
+        Base::level_stack_.template Top<typename Base::Level>()->valueCount %
+            2);  // Object has a Key without a Value
+
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndObject());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  bool StartArray() {
+    PrettyPrefix(kArrayType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(true);
+    return Base::WriteStartArray();
+  }
+
+  bool EndArray(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));
+    RAPIDJSON_ASSERT(
+        Base::level_stack_.template Top<typename Base::Level>()->inArray);
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndArray());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  //@}
+
+  /*! @name Convenience extensions */
+  //@{
+
+  //! Simpler but slower overload.
+  bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
+  bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
+
+  //@}
+
+  //! Write a raw JSON value.
+  /*!
+      For user to write a stringified JSON as a value.
+
+      \param json A well-formed JSON value. It should not contain null character
+     within [0, length - 1] range. \param length Length of the json. \param type
+     Type of the root of json. \note When using PrettyWriter::RawValue(), the
+     result json may not be indented correctly.
+  */
+  bool RawValue(const Ch *json, size_t length, Type type) {
+    RAPIDJSON_ASSERT(json != 0);
+    PrettyPrefix(type);
+    return Base::EndValue(Base::WriteRawValue(json, length));
+  }
+
+ protected:
+  void PrettyPrefix(Type type) {
+    (void)type;
+    if (Base::level_stack_.GetSize() != 0) {  // this value is not at root
+      typename Base::Level *level =
+          Base::level_stack_.template Top<typename Base::Level>();
+
+      if (level->inArray) {
+        if (level->valueCount > 0) {
+          Base::os_->Put(
+              ',');  // add comma if it is not the first element in array
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
+        }
+
+        if (!(formatOptions_ & kFormatSingleLineArray)) {
+          Base::os_->Put('\n');
+          WriteIndent();
+        }
+      } else {  // in object
+        if (level->valueCount > 0) {
+          if (level->valueCount % 2 == 0) {
+            Base::os_->Put(',');
+            Base::os_->Put('\n');
+          } else {
+            Base::os_->Put(':');
+            Base::os_->Put(' ');
+          }
+        } else
+          Base::os_->Put('\n');
+
+        if (level->valueCount % 2 == 0) WriteIndent();
+      }
+      if (!level->inArray && level->valueCount % 2 == 0)
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
+                                                // number should be a name
+      level->valueCount++;
+    } else {
+      RAPIDJSON_ASSERT(
+          !Base::hasRoot_);  // Should only has one and only one root.
+      Base::hasRoot_ = true;
+    }
+  }
+
+  void WriteIndent() {
+    size_t count =
+        (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
+        indentCharCount_;
+    PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
+         count);
+  }
+
+  Ch indentChar_;
+  unsigned indentCharCount_;
+  PrettyFormatOptions formatOptions_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  PrettyWriter(const PrettyWriter &);
+  PrettyWriter &operator=(const PrettyWriter &);
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_RAPIDJSON_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/rapidjson.h

@@ -0,0 +1,719 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_RAPIDJSON_H_
+#define RAPIDJSON_RAPIDJSON_H_
+
+/*!\file rapidjson.h
+    \brief common definitions and configuration
+
+    \see RAPIDJSON_CONFIG
+ */
+
+/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
+    \brief Configuration macros for library features
+
+    Some RapidJSON features are configurable to adapt the library to a wide
+    variety of platforms, environments and usage scenarios.  Most of the
+    features can be configured in terms of overridden or predefined
+    preprocessor macros at compile-time.
+
+    Some additional customization is available in the \ref RAPIDJSON_ERRORS
+   APIs.
+
+    \note These macros should be given on the compiler command-line
+          (where applicable)  to avoid inconsistent values when compiling
+          different translation units of a single application.
+ */
+
+#include <cstdlib>  // malloc(), realloc(), free(), size_t
+#include <cstring>  // memset(), memcpy(), memmove(), memcmp()
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_VERSION_STRING
+//
+// ALWAYS synchronize the following 3 macros with corresponding variables in
+// /CMakeLists.txt.
+//
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+// token stringification
+#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
+#define RAPIDJSON_DO_STRINGIFY(x) #x
+
+// token concatenation
+#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
+#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
+#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
+//!@endcond
+
+/*! \def RAPIDJSON_MAJOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Major version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_MINOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Minor version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_PATCH_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Patch version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_VERSION_STRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
+*/
+#define RAPIDJSON_MAJOR_VERSION 1
+#define RAPIDJSON_MINOR_VERSION 1
+#define RAPIDJSON_PATCH_VERSION 0
+#define RAPIDJSON_VERSION_STRING \
+  RAPIDJSON_STRINGIFY(           \
+      RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NAMESPACE_(BEGIN|END)
+/*! \def RAPIDJSON_NAMESPACE
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace
+
+    In order to avoid symbol clashes and/or "One Definition Rule" errors
+    between multiple inclusions of (different versions of) RapidJSON in
+    a single binary, users can customize the name of the main RapidJSON
+    namespace.
+
+    In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
+    to a custom name (e.g. \c MyRapidJSON) is sufficient.  If multiple
+    levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
+    RAPIDJSON_NAMESPACE_END need to be defined as well:
+
+    \code
+    // in some .cpp file
+    #define RAPIDJSON_NAMESPACE my::rapidjson
+    #define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
+    #define RAPIDJSON_NAMESPACE_END   } }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson
+ */
+/*! \def RAPIDJSON_NAMESPACE_BEGIN
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (opening expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+/*! \def RAPIDJSON_NAMESPACE_END
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (closing expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+#ifndef RAPIDJSON_NAMESPACE
+#define RAPIDJSON_NAMESPACE rapidjson
+#endif
+#ifndef RAPIDJSON_NAMESPACE_BEGIN
+#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
+#endif
+#ifndef RAPIDJSON_NAMESPACE_END
+#define RAPIDJSON_NAMESPACE_END }
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_HAS_STDSTRING
+
+#ifndef RAPIDJSON_HAS_STDSTRING
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_HAS_STDSTRING 1  // force generation of documentation
+#else
+#define RAPIDJSON_HAS_STDSTRING 0  // no std::string support by default
+#endif
+/*! \def RAPIDJSON_HAS_STDSTRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable RapidJSON support for \c std::string
+
+    By defining this preprocessor symbol to \c 1, several convenience functions
+   for using \ref rapidjson::GenericValue with \c std::string are enabled,
+   especially for construction and comparison.
+
+    \hideinitializer
+*/
+#endif  // !defined(RAPIDJSON_HAS_STDSTRING)
+
+#if RAPIDJSON_HAS_STDSTRING
+#include <string>
+#endif  // RAPIDJSON_HAS_STDSTRING
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_INT64DEFINE
+
+/*! \def RAPIDJSON_NO_INT64DEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief Use external 64-bit integer types.
+
+    RapidJSON requires the 64-bit integer types \c int64_t and  \c uint64_t
+   types to be available at global scope.
+
+    If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
+    prevent RapidJSON from defining its own types.
+*/
+#ifndef RAPIDJSON_NO_INT64DEFINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && (_MSC_VER < 1800)  // Visual Studio 2013
+#include "msinttypes/inttypes.h"
+#include "msinttypes/stdint.h"
+#else
+// Other compilers should have this.
+#include <inttypes.h>
+#include <stdint.h>
+#endif
+//!@endcond
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_INT64DEFINE
+#endif
+#endif  // RAPIDJSON_NO_INT64TYPEDEF
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_FORCEINLINE
+
+#ifndef RAPIDJSON_FORCEINLINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __forceinline
+#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
+#else
+#define RAPIDJSON_FORCEINLINE
+#endif
+//!@endcond
+#endif  // RAPIDJSON_FORCEINLINE
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ENDIAN
+#define RAPIDJSON_LITTLEENDIAN 0  //!< Little endian machine
+#define RAPIDJSON_BIGENDIAN 1     //!< Big endian machine
+
+//! Endianness of the machine.
+/*!
+    \def RAPIDJSON_ENDIAN
+    \ingroup RAPIDJSON_CONFIG
+
+    GCC 4.6 provided macro for detecting endianness of the target machine. But
+   other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
+    \ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
+
+    Default detection implemented with reference to
+    \li
+   https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
+    \li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
+*/
+#ifndef RAPIDJSON_ENDIAN
+// Detect with GCC 4.6's macro
+#ifdef __BYTE_ORDER__
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __BYTE_ORDER__
+// Detect with GLIBC's endian.h
+#elif defined(__GLIBC__)
+#include <endian.h>
+#if (__BYTE_ORDER == __LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif (__BYTE_ORDER == __BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __GLIBC__
+// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
+#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+// Detect with architecture macros
+#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
+    defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
+    defined(__hppa) || defined(_MIPSEB) || defined(_POWER) ||      \
+    defined(__s390__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) ||  \
+    defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) ||     \
+    defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) ||   \
+    defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
+    defined(_M_X64) || defined(__bfin__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_ENDIAN
+#else
+#    error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif
+#endif  // RAPIDJSON_ENDIAN
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_64BIT
+
+//! Whether using 64-bit architecture
+#ifndef RAPIDJSON_64BIT
+#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
+    defined(_WIN64) || defined(__EMSCRIPTEN__)
+#define RAPIDJSON_64BIT 1
+#else
+#define RAPIDJSON_64BIT 0
+#endif
+#endif  // RAPIDJSON_64BIT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ALIGN
+
+//! Data alignment of the machine.
+/*! \ingroup RAPIDJSON_CONFIG
+    \param x pointer to align
+
+    Some machines require strict data alignment. The default is 8 bytes.
+    User can customize by defining the RAPIDJSON_ALIGN function macro.
+*/
+#ifndef RAPIDJSON_ALIGN
+#define RAPIDJSON_ALIGN(x) \
+  (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_UINT64_C2
+
+//! Construct a 64-bit literal by a pair of 32-bit integer.
+/*!
+    64-bit literal with or without ULL suffix is prone to compiler warnings.
+    UINT64_C() is C macro which cause compilation problems.
+    Use this macro to define 64-bit constants by a pair of 32-bit integer.
+*/
+#ifndef RAPIDJSON_UINT64_C2
+#define RAPIDJSON_UINT64_C2(high32, low32) \
+  ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+//! Use only lower 48-bit address for some pointers.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+
+    This optimization uses the fact that current X86-64 architecture only
+   implement lower 48-bit virtual address. The higher 16-bit can be used for
+   storing other data. \c GenericValue uses this optimization to reduce its size
+   form 24 bytes to 16 bytes in 64-bit architecture.
+*/
+#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
+#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
+    defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
+#else
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
+#endif
+#endif  // RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
+#if RAPIDJSON_64BIT != 1
+#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
+#endif
+#define RAPIDJSON_SETPOINTER(type, p, x)                                       \
+  (p = reinterpret_cast<type *>(                                               \
+       (reinterpret_cast<uintptr_t>(p) &                                       \
+        static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
+       reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
+#define RAPIDJSON_GETPOINTER(type, p)  \
+  (reinterpret_cast<type *>(           \
+      reinterpret_cast<uintptr_t>(p) & \
+      static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
+#else
+#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
+#define RAPIDJSON_GETPOINTER(type, p) (p)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
+
+/*! \def RAPIDJSON_SIMD
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable SSE2/SSE4.2/Neon optimization.
+
+    RapidJSON supports optimized implementations for some parsing operations
+    based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
+    or ARM compatible processors.
+
+    To enable these optimizations, three different symbols can be defined;
+    \code
+    // Enable SSE2 optimization.
+    #define RAPIDJSON_SSE2
+
+    // Enable SSE4.2 optimization.
+    #define RAPIDJSON_SSE42
+    \endcode
+
+    // Enable ARM Neon optimization.
+    #define RAPIDJSON_NEON
+    \endcode
+
+    \c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
+
+    If any of these symbols is defined, RapidJSON defines the macro
+    \c RAPIDJSON_SIMD to indicate the availability of the optimized code.
+*/
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
+    defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_SIMD
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_SIZETYPEDEFINE
+
+#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
+/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-provided \c SizeType definition.
+
+    In order to avoid using 32-bit size types for indexing strings and arrays,
+    define this preprocessor symbol and provide the type rapidjson::SizeType
+    before including RapidJSON:
+    \code
+    #define RAPIDJSON_NO_SIZETYPEDEFINE
+    namespace rapidjson { typedef ::std::size_t SizeType; }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson::SizeType
+*/
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_SIZETYPEDEFINE
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+//! Size type (for string lengths, array sizes, etc.)
+/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
+    instead of using \c size_t. Users may override the SizeType by defining
+    \ref RAPIDJSON_NO_SIZETYPEDEFINE.
+*/
+typedef unsigned SizeType;
+RAPIDJSON_NAMESPACE_END
+#endif
+
+// always import std::size_t to rapidjson namespace
+RAPIDJSON_NAMESPACE_BEGIN
+using std::size_t;
+RAPIDJSON_NAMESPACE_END
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ASSERT
+
+//! Assertion.
+/*! \ingroup RAPIDJSON_CONFIG
+    By default, rapidjson uses C \c assert() for internal assertions.
+    User can override it by defining RAPIDJSON_ASSERT(x) macro.
+
+    \note Parsing errors are handled and can be customized by the
+          \ref RAPIDJSON_ERRORS APIs.
+*/
+#ifndef RAPIDJSON_ASSERT
+#include <cassert>
+#define RAPIDJSON_ASSERT(x) assert(x)
+#endif  // RAPIDJSON_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_STATIC_ASSERT
+
+// Prefer C++11 static_assert, if available
+#ifndef RAPIDJSON_STATIC_ASSERT
+#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
+#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
+#endif  // C++11
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+// Adopt C++03 implementation from boost
+#ifndef RAPIDJSON_STATIC_ASSERT
+#ifndef __clang__
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+template <bool x>
+struct STATIC_ASSERTION_FAILURE;
+template <>
+struct STATIC_ASSERTION_FAILURE<true> {
+  enum { value = 1 };
+};
+template <size_t x>
+struct StaticAssertTest {};
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
+#else
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif
+#ifndef __clang__
+//!@endcond
+#endif
+
+/*! \def RAPIDJSON_STATIC_ASSERT
+    \brief (Internal) macro to check for conditions at compile-time
+    \param x compile-time condition
+    \hideinitializer
+ */
+#define RAPIDJSON_STATIC_ASSERT(x)                               \
+  typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof(        \
+      ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
+      RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__)              \
+          RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
+
+//! Compiler branching hint for expression with high probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression likely to be true.
+*/
+#ifndef RAPIDJSON_LIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
+#else
+#define RAPIDJSON_LIKELY(x) (x)
+#endif
+#endif
+
+//! Compiler branching hint for expression with low probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression unlikely to be true.
+*/
+#ifndef RAPIDJSON_UNLIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
+#else
+#define RAPIDJSON_UNLIKELY(x) (x)
+#endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Helpers
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+
+#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
+#define RAPIDJSON_MULTILINEMACRO_END \
+  }                                  \
+  while ((void)0, 0)
+
+// adopted from Boost
+#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
+
+#if defined(__has_builtin)
+#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#define RAPIDJSON_HAS_BUILTIN(x) 0
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
+
+#if defined(__GNUC__)
+#define RAPIDJSON_GNUC \
+  RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
+#endif
+
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
+
+#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
+#define RAPIDJSON_DIAG_OFF(x) \
+  RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
+
+// push/pop support in Clang and GCC>=4.6
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+#else                       // GCC >= 4.2, < 4.6
+#define RAPIDJSON_DIAG_PUSH /* ignored */
+#define RAPIDJSON_DIAG_POP  /* ignored */
+#endif
+
+#elif defined(_MSC_VER)
+
+// pragma (MSVC specific)
+#define RAPIDJSON_PRAGMA(x) __pragma(x)
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
+
+#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+
+#else
+
+#define RAPIDJSON_DIAG_OFF(x) /* ignored */
+#define RAPIDJSON_DIAG_PUSH   /* ignored */
+#define RAPIDJSON_DIAG_POP    /* ignored */
+
+#endif  // RAPIDJSON_DIAG_*
+
+///////////////////////////////////////////////////////////////////////////////
+// C++11 features
+
+#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#if defined(__clang__)
+#if __has_feature(cxx_rvalue_references) &&           \
+    (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
+     defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1600) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+
+#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1900) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
+#else
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
+#endif
+#endif
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT noexcept
+#else
+#define RAPIDJSON_NOEXCEPT /* noexcept */
+#endif                     // RAPIDJSON_HAS_CXX11_NOEXCEPT
+
+// no automatic detection, yet
+#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
+#if (defined(_MSC_VER) && _MSC_VER >= 1700)
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
+#else
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
+#endif
+#endif
+
+#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1700) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
+#else
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RANGE_FOR
+
+///////////////////////////////////////////////////////////////////////////////
+// C++17 features
+
+#if defined(__has_cpp_attribute)
+#if __has_cpp_attribute(fallthrough)
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+
+//!@endcond
+
+//! Assertion (in non-throwing contexts).
+/*! \ingroup RAPIDJSON_CONFIG
+   Some functions provide a \c noexcept guarantee, if the compiler supports it.
+   In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
+   throw an exception.  This macro adds a separate customization point for
+   such cases.
+
+   Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
+   supported, and to \ref RAPIDJSON_ASSERT otherwise.
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NOEXCEPT_ASSERT
+
+#ifndef RAPIDJSON_NOEXCEPT_ASSERT
+#ifdef RAPIDJSON_ASSERT_THROWS
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT_ASSERT(x)
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_HAS_CXX11_NOEXCEPT
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_ASSERT_THROWS
+#endif  // RAPIDJSON_NOEXCEPT_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// new/delete
+
+#ifndef RAPIDJSON_NEW
+///! customization point for global \c new
+#define RAPIDJSON_NEW(TypeName) new TypeName
+#endif
+#ifndef RAPIDJSON_DELETE
+///! customization point for global \c delete
+#define RAPIDJSON_DELETE(x) delete x
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Type
+
+/*! \namespace rapidjson
+    \brief main RapidJSON namespace
+    \see RAPIDJSON_NAMESPACE
+*/
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Type of JSON value
+enum Type {
+  kNullType = 0,    //!< null
+  kFalseType = 1,   //!< false
+  kTrueType = 2,    //!< true
+  kObjectType = 3,  //!< object
+  kArrayType = 4,   //!< array
+  kStringType = 5,  //!< string
+  kNumberType = 6   //!< number
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSON_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/stream.h

@@ -0,0 +1,242 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#include "rapidjson.h"
+
+#ifndef RAPIDJSON_STREAM_H_
+#define RAPIDJSON_STREAM_H_
+
+#include "encodings.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+//  Stream
+
+/*! \class rapidjson::Stream
+    \brief Concept for reading and writing characters.
+
+    For read-only stream, no need to implement PutBegin(), Put(), Flush() and
+PutEnd().
+
+    For write-only stream, only need to implement Put() and Flush().
+
+\code
+concept Stream {
+    typename Ch;    //!< Character type of the stream.
+
+    //! Read the current character from stream without moving the read cursor.
+    Ch Peek() const;
+
+    //! Read the current character from stream and moving the read cursor to
+next character. Ch Take();
+
+    //! Get the current read cursor.
+    //! \return Number of characters read from start.
+    size_t Tell();
+
+    //! Begin writing operation at the current read pointer.
+    //! \return The begin writer pointer.
+    Ch* PutBegin();
+
+    //! Write a character.
+    void Put(Ch c);
+
+    //! Flush the buffer.
+    void Flush();
+
+    //! End the writing operation.
+    //! \param begin The begin write pointer returned by PutBegin().
+    //! \return Number of characters written.
+    size_t PutEnd(Ch* begin);
+}
+\endcode
+*/
+
+//! Provides additional information for stream.
+/*!
+    By using traits pattern, this type provides a default configuration for
+   stream. For custom stream, this type can be specialized for other
+   configuration. See TEST(Reader, CustomStringStream) in readertest.cpp for
+   example.
+*/
+template <typename Stream>
+struct StreamTraits {
+  //! Whether to make local copy of stream for optimization during parsing.
+  /*!
+      By default, for safety, streams do not use local copy optimization.
+      Stream that can be copied fast should specialize this, like
+     StreamTraits<StringStream>.
+  */
+  enum { copyOptimization = 0 };
+};
+
+//! Reserve n characters for writing to a stream.
+template <typename Stream>
+inline void PutReserve(Stream &stream, size_t count) {
+  (void)stream;
+  (void)count;
+}
+
+//! Write character to a stream, presuming buffer is reserved.
+template <typename Stream>
+inline void PutUnsafe(Stream &stream, typename Stream::Ch c) {
+  stream.Put(c);
+}
+
+//! Put N copies of a character to a stream.
+template <typename Stream, typename Ch>
+inline void PutN(Stream &stream, Ch c, size_t n) {
+  PutReserve(stream, n);
+  for (size_t i = 0; i < n; i++) PutUnsafe(stream, c);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericStreamWrapper
+
+//! A Stream Wrapper
+/*! \tThis string stream is a wrapper for any stream by just forwarding any
+    \treceived message to the origin stream.
+    \note implements Stream concept
+*/
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+template <typename InputStream, typename Encoding = UTF8<>>
+class GenericStreamWrapper {
+ public:
+  typedef typename Encoding::Ch Ch;
+  GenericStreamWrapper(InputStream &is) : is_(is) {}
+
+  Ch Peek() const { return is_.Peek(); }
+  Ch Take() { return is_.Take(); }
+  size_t Tell() { return is_.Tell(); }
+  Ch *PutBegin() { return is_.PutBegin(); }
+  void Put(Ch ch) { is_.Put(ch); }
+  void Flush() { is_.Flush(); }
+  size_t PutEnd(Ch *ch) { return is_.PutEnd(ch); }
+
+  // wrapper for MemoryStream
+  const Ch *Peek4() const { return is_.Peek4(); }
+
+  // wrapper for AutoUTFInputStream
+  UTFType GetType() const { return is_.GetType(); }
+  bool HasBOM() const { return is_.HasBOM(); }
+
+ protected:
+  InputStream &is_;
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_POP
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// StringStream
+
+//! Read-only string stream.
+/*! \note implements Stream concept
+ */
+template <typename Encoding>
+struct GenericStringStream {
+  typedef typename Encoding::Ch Ch;
+
+  GenericStringStream(const Ch *src) : src_(src), head_(src) {}
+
+  Ch Peek() const { return *src_; }
+  Ch Take() { return *src_++; }
+  size_t Tell() const { return static_cast<size_t>(src_ - head_); }
+
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  const Ch *src_;   //!< Current read position.
+  const Ch *head_;  //!< Original head of the string.
+};
+
+template <typename Encoding>
+struct StreamTraits<GenericStringStream<Encoding>> {
+  enum { copyOptimization = 1 };
+};
+
+//! String stream with UTF8 encoding.
+typedef GenericStringStream<UTF8<>> StringStream;
+
+///////////////////////////////////////////////////////////////////////////////
+// InsituStringStream
+
+//! A read-write string stream.
+/*! This string stream is particularly designed for in-situ parsing.
+    \note implements Stream concept
+*/
+template <typename Encoding>
+struct GenericInsituStringStream {
+  typedef typename Encoding::Ch Ch;
+
+  GenericInsituStringStream(Ch *src) : src_(src), dst_(0), head_(src) {}
+
+  // Read
+  Ch Peek() { return *src_; }
+  Ch Take() { return *src_++; }
+  size_t Tell() { return static_cast<size_t>(src_ - head_); }
+
+  // Write
+  void Put(Ch c) {
+    RAPIDJSON_ASSERT(dst_ != 0);
+    *dst_++ = c;
+  }
+
+  Ch *PutBegin() { return dst_ = src_; }
+  size_t PutEnd(Ch *begin) { return static_cast<size_t>(dst_ - begin); }
+  void Flush() {}
+
+  Ch *Push(size_t count) {
+    Ch *begin = dst_;
+    dst_ += count;
+    return begin;
+  }
+  void Pop(size_t count) { dst_ -= count; }
+
+  Ch *src_;
+  Ch *dst_;
+  Ch *head_;
+};
+
+template <typename Encoding>
+struct StreamTraits<GenericInsituStringStream<Encoding>> {
+  enum { copyOptimization = 1 };
+};
+
+//! Insitu string stream with UTF8 encoding.
+typedef GenericInsituStringStream<UTF8<>> InsituStringStream;
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_STREAM_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/stringbuffer.h

@@ -0,0 +1,130 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_STRINGBUFFER_H_
+#define RAPIDJSON_STRINGBUFFER_H_
+
+#include "internal/stack.h"
+#include "stream.h"
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#include <utility>  // std::move
+#endif
+
+#include "internal/stack.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory output stream.
+/*!
+    \tparam Encoding Encoding of the stream.
+    \tparam Allocator type for allocating memory buffer.
+    \note implements Stream concept
+*/
+template <typename Encoding, typename Allocator = CrtAllocator>
+class GenericStringBuffer {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  GenericStringBuffer(Allocator *allocator = 0,
+                      size_t capacity = kDefaultCapacity)
+      : stack_(allocator, capacity) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  GenericStringBuffer(GenericStringBuffer &&rhs)
+      : stack_(std::move(rhs.stack_)) {}
+  GenericStringBuffer &operator=(GenericStringBuffer &&rhs) {
+    if (&rhs != this) stack_ = std::move(rhs.stack_);
+    return *this;
+  }
+#endif
+
+  void Put(Ch c) { *stack_.template Push<Ch>() = c; }
+  void PutUnsafe(Ch c) { *stack_.template PushUnsafe<Ch>() = c; }
+  void Flush() {}
+
+  void Clear() { stack_.Clear(); }
+  void ShrinkToFit() {
+    // Push and pop a null terminator. This is safe.
+    *stack_.template Push<Ch>() = '\0';
+    stack_.ShrinkToFit();
+    stack_.template Pop<Ch>(1);
+  }
+
+  void Reserve(size_t count) { stack_.template Reserve<Ch>(count); }
+  Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
+  Ch *PushUnsafe(size_t count) { return stack_.template PushUnsafe<Ch>(count); }
+  void Pop(size_t count) { stack_.template Pop<Ch>(count); }
+
+  const Ch *GetString() const {
+    // Push and pop a null terminator. This is safe.
+    *stack_.template Push<Ch>() = '\0';
+    stack_.template Pop<Ch>(1);
+
+    return stack_.template Bottom<Ch>();
+  }
+
+  //! Get the size of string in bytes in the string buffer.
+  size_t GetSize() const { return stack_.GetSize(); }
+
+  //! Get the length of string in Ch in the string buffer.
+  size_t GetLength() const { return stack_.GetSize() / sizeof(Ch); }
+
+  static const size_t kDefaultCapacity = 256;
+  mutable internal::Stack<Allocator> stack_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  GenericStringBuffer(const GenericStringBuffer &);
+  GenericStringBuffer &operator=(const GenericStringBuffer &);
+};
+
+//! String buffer with UTF8 encoding
+typedef GenericStringBuffer<UTF8<>> StringBuffer;
+
+template <typename Encoding, typename Allocator>
+inline void PutReserve(GenericStringBuffer<Encoding, Allocator> &stream,
+                       size_t count) {
+  stream.Reserve(count);
+}
+
+template <typename Encoding, typename Allocator>
+inline void PutUnsafe(GenericStringBuffer<Encoding, Allocator> &stream,
+                      typename Encoding::Ch c) {
+  stream.PutUnsafe(c);
+}
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(GenericStringBuffer<UTF8<>> &stream, char c, size_t n) {
+  std::memset(stream.stack_.Push<char>(n), c, n * sizeof(c));
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_STRINGBUFFER_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/3rdparty/rapidjson/writer.h

@@ -0,0 +1,811 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_WRITER_H_
+#define RAPIDJSON_WRITER_H_
+
+#include <new>  // placement new
+#include "internal/clzll.h"
+#include "internal/dtoa.h"
+#include "internal/itoa.h"
+#include "internal/meta.h"
+#include "internal/stack.h"
+#include "internal/strfunc.h"
+#include "stream.h"
+#include "stringbuffer.h"
+
+#if defined(RAPIDJSON_SIMD) && defined(_MSC_VER)
+#include <intrin.h>
+#pragma intrinsic(_BitScanForward)
+#endif
+#ifdef RAPIDJSON_SSE42
+#include <nmmintrin.h>
+#elif defined(RAPIDJSON_SSE2)
+#include <emmintrin.h>
+#elif defined(RAPIDJSON_NEON)
+#include <arm_neon.h>
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4127)  // conditional expression is constant
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// WriteFlag
+
+/*! \def RAPIDJSON_WRITE_DEFAULT_FLAGS
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kWriteDefaultFlags definition.
+
+    User can define this as any \c WriteFlag combinations.
+*/
+#ifndef RAPIDJSON_WRITE_DEFAULT_FLAGS
+#define RAPIDJSON_WRITE_DEFAULT_FLAGS kWriteNoFlags
+#endif
+
+//! Combination of writeFlags
+enum WriteFlag {
+  kWriteNoFlags = 0,               //!< No flags are set.
+  kWriteValidateEncodingFlag = 1,  //!< Validate encoding of JSON strings.
+  kWriteNanAndInfFlag = 2,  //!< Allow writing of Infinity, -Infinity and NaN.
+  kWriteDefaultFlags =
+      RAPIDJSON_WRITE_DEFAULT_FLAGS  //!< Default write flags. Can be customized
+                                     //!< by defining
+                                     //!< RAPIDJSON_WRITE_DEFAULT_FLAGS
+};
+
+//! JSON writer
+/*! Writer implements the concept Handler.
+    It generates JSON text by events to an output os.
+
+    User may programmatically calls the functions of a writer to generate JSON
+   text.
+
+    On the other side, a writer can also be passed to objects that generates
+   events,
+
+    for example Reader::Parse() and Document::Accept().
+
+    \tparam OutputStream Type of output stream.
+    \tparam SourceEncoding Encoding of source string.
+    \tparam TargetEncoding Encoding of output stream.
+    \tparam StackAllocator Type of allocator for allocating memory of stack.
+    \note implements Handler concept
+*/
+template <typename OutputStream, typename SourceEncoding = UTF8<>,
+          typename TargetEncoding = UTF8<>,
+          typename StackAllocator = CrtAllocator,
+          unsigned writeFlags = kWriteDefaultFlags>
+class Writer {
+ public:
+  typedef typename SourceEncoding::Ch Ch;
+
+  static const int kDefaultMaxDecimalPlaces = 324;
+
+  //! Constructor
+  /*! \param os Output stream.
+      \param stackAllocator User supplied allocator. If it is null, it will
+     create a private one. \param levelDepth Initial capacity of stack.
+  */
+  explicit Writer(OutputStream &os, StackAllocator *stackAllocator = 0,
+                  size_t levelDepth = kDefaultLevelDepth)
+      : os_(&os),
+        level_stack_(stackAllocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
+
+  explicit Writer(StackAllocator *allocator = 0,
+                  size_t levelDepth = kDefaultLevelDepth)
+      : os_(0),
+        level_stack_(allocator, levelDepth * sizeof(Level)),
+        maxDecimalPlaces_(kDefaultMaxDecimalPlaces),
+        hasRoot_(false) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Writer(Writer &&rhs)
+      : os_(rhs.os_),
+        level_stack_(std::move(rhs.level_stack_)),
+        maxDecimalPlaces_(rhs.maxDecimalPlaces_),
+        hasRoot_(rhs.hasRoot_) {
+    rhs.os_ = 0;
+  }
+#endif
+
+  //! Reset the writer with a new stream.
+  /*!
+      This function reset the writer with a new stream and default settings,
+      in order to make a Writer object reusable for output multiple JSONs.
+
+      \param os New output stream.
+      \code
+      Writer<OutputStream> writer(os1);
+      writer.StartObject();
+      // ...
+      writer.EndObject();
+
+      writer.Reset(os2);
+      writer.StartObject();
+      // ...
+      writer.EndObject();
+      \endcode
+  */
+  void Reset(OutputStream &os) {
+    os_ = &os;
+    hasRoot_ = false;
+    level_stack_.Clear();
+  }
+
+  //! Checks whether the output is a complete JSON.
+  /*!
+      A complete JSON has a complete root object or array.
+  */
+  bool IsComplete() const { return hasRoot_ && level_stack_.Empty(); }
+
+  int GetMaxDecimalPlaces() const { return maxDecimalPlaces_; }
+
+  //! Sets the maximum number of decimal places for double output.
+  /*!
+      This setting truncates the output with specified number of decimal places.
+
+      For example,
+
+      \code
+      writer.SetMaxDecimalPlaces(3);
+      writer.StartArray();
+      writer.Double(0.12345);                 // "0.123"
+      writer.Double(0.0001);                  // "0.0"
+      writer.Double(1.234567890123456e30);    // "1.234567890123456e30" (do not
+     truncate significand for positive exponent) writer.Double(1.23e-4); //
+     "0.0"                  (do truncate significand for negative exponent)
+      writer.EndArray();
+      \endcode
+
+      The default setting does not truncate any decimal places. You can restore
+     to this setting by calling \code
+      writer.SetMaxDecimalPlaces(Writer::kDefaultMaxDecimalPlaces);
+      \endcode
+  */
+  void SetMaxDecimalPlaces(int maxDecimalPlaces) {
+    maxDecimalPlaces_ = maxDecimalPlaces;
+  }
+
+  /*!@name Implementation of Handler
+      \see Handler
+  */
+  //@{
+
+  bool Null() {
+    Prefix(kNullType);
+    return EndValue(WriteNull());
+  }
+  bool Bool(bool b) {
+    Prefix(b ? kTrueType : kFalseType);
+    return EndValue(WriteBool(b));
+  }
+  bool Int(int i) {
+    Prefix(kNumberType);
+    return EndValue(WriteInt(i));
+  }
+  bool Uint(unsigned u) {
+    Prefix(kNumberType);
+    return EndValue(WriteUint(u));
+  }
+  bool Int64(int64_t i64) {
+    Prefix(kNumberType);
+    return EndValue(WriteInt64(i64));
+  }
+  bool Uint64(uint64_t u64) {
+    Prefix(kNumberType);
+    return EndValue(WriteUint64(u64));
+  }
+
+  //! Writes the given \c double value to the stream
+  /*!
+      \param d The value to be written.
+      \return Whether it is succeed.
+  */
+  bool Double(double d) {
+    Prefix(kNumberType);
+    return EndValue(WriteDouble(d));
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    Prefix(kNumberType);
+    return EndValue(WriteString(str, length));
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    Prefix(kStringType);
+    return EndValue(WriteString(str, length));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool String(const std::basic_string<Ch> &str) {
+    return String(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool StartObject() {
+    Prefix(kObjectType);
+    new (level_stack_.template Push<Level>()) Level(false);
+    return WriteStartObject();
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy = false) {
+    return String(str, length, copy);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool Key(const std::basic_string<Ch> &str) {
+    return Key(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool EndObject(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(level_stack_.GetSize() >=
+                     sizeof(Level));  // not inside an Object
+    RAPIDJSON_ASSERT(!level_stack_.template Top<Level>()
+                          ->inArray);  // currently inside an Array, not Object
+    RAPIDJSON_ASSERT(0 ==
+                     level_stack_.template Top<Level>()->valueCount %
+                         2);  // Object has a Key without a Value
+    level_stack_.template Pop<Level>(1);
+    return EndValue(WriteEndObject());
+  }
+
+  bool StartArray() {
+    Prefix(kArrayType);
+    new (level_stack_.template Push<Level>()) Level(true);
+    return WriteStartArray();
+  }
+
+  bool EndArray(SizeType elementCount = 0) {
+    (void)elementCount;
+    RAPIDJSON_ASSERT(level_stack_.GetSize() >= sizeof(Level));
+    RAPIDJSON_ASSERT(level_stack_.template Top<Level>()->inArray);
+    level_stack_.template Pop<Level>(1);
+    return EndValue(WriteEndArray());
+  }
+  //@}
+
+  /*! @name Convenience extensions */
+  //@{
+
+  //! Simpler but slower overload.
+  bool String(const Ch *const &str) {
+    return String(str, internal::StrLen(str));
+  }
+  bool Key(const Ch *const &str) { return Key(str, internal::StrLen(str)); }
+
+  //@}
+
+  //! Write a raw JSON value.
+  /*!
+      For user to write a stringified JSON as a value.
+
+      \param json A well-formed JSON value. It should not contain null character
+     within [0, length - 1] range. \param length Length of the json. \param type
+     Type of the root of json.
+  */
+  bool RawValue(const Ch *json, size_t length, Type type) {
+    RAPIDJSON_ASSERT(json != 0);
+    Prefix(type);
+    return EndValue(WriteRawValue(json, length));
+  }
+
+  //! Flush the output stream.
+  /*!
+      Allows the user to flush the output stream immediately.
+   */
+  void Flush() { os_->Flush(); }
+
+ protected:
+  //! Information for each nested level
+  struct Level {
+    Level(bool inArray_) : valueCount(0), inArray(inArray_) {}
+    size_t valueCount;  //!< number of values in this level
+    bool inArray;       //!< true if in array, otherwise in object
+  };
+
+  static const size_t kDefaultLevelDepth = 32;
+
+  bool WriteNull() {
+    PutReserve(*os_, 4);
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'u');
+    PutUnsafe(*os_, 'l');
+    PutUnsafe(*os_, 'l');
+    return true;
+  }
+
+  bool WriteBool(bool b) {
+    if (b) {
+      PutReserve(*os_, 4);
+      PutUnsafe(*os_, 't');
+      PutUnsafe(*os_, 'r');
+      PutUnsafe(*os_, 'u');
+      PutUnsafe(*os_, 'e');
+    } else {
+      PutReserve(*os_, 5);
+      PutUnsafe(*os_, 'f');
+      PutUnsafe(*os_, 'a');
+      PutUnsafe(*os_, 'l');
+      PutUnsafe(*os_, 's');
+      PutUnsafe(*os_, 'e');
+    }
+    return true;
+  }
+
+  bool WriteInt(int i) {
+    char buffer[11];
+    const char *end = internal::i32toa(i, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteUint(unsigned u) {
+    char buffer[10];
+    const char *end = internal::u32toa(u, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteInt64(int64_t i64) {
+    char buffer[21];
+    const char *end = internal::i64toa(i64, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (const char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteUint64(uint64_t u64) {
+    char buffer[20];
+    char *end = internal::u64toa(u64, buffer);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteDouble(double d) {
+    if (internal::Double(d).IsNanOrInf()) {
+      if (!(writeFlags & kWriteNanAndInfFlag)) return false;
+      if (internal::Double(d).IsNan()) {
+        PutReserve(*os_, 3);
+        PutUnsafe(*os_, 'N');
+        PutUnsafe(*os_, 'a');
+        PutUnsafe(*os_, 'N');
+        return true;
+      }
+      if (internal::Double(d).Sign()) {
+        PutReserve(*os_, 9);
+        PutUnsafe(*os_, '-');
+      } else
+        PutReserve(*os_, 8);
+      PutUnsafe(*os_, 'I');
+      PutUnsafe(*os_, 'n');
+      PutUnsafe(*os_, 'f');
+      PutUnsafe(*os_, 'i');
+      PutUnsafe(*os_, 'n');
+      PutUnsafe(*os_, 'i');
+      PutUnsafe(*os_, 't');
+      PutUnsafe(*os_, 'y');
+      return true;
+    }
+
+    char buffer[25];
+    char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
+    PutReserve(*os_, static_cast<size_t>(end - buffer));
+    for (char *p = buffer; p != end; ++p)
+      PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(*p));
+    return true;
+  }
+
+  bool WriteString(const Ch *str, SizeType length) {
+    static const typename OutputStream::Ch hexDigits[16] = {
+        '0', '1', '2', '3', '4', '5', '6', '7',
+        '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
+    static const char escape[256] = {
+#define Z16 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+        // 0    1    2    3    4    5    6    7    8    9    A    B    C    D E
+        // F
+        'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'b', 't',
+        'n', 'u', 'f',  'r', 'u', 'u',  // 00
+        'u', 'u', 'u',  'u', 'u', 'u', 'u', 'u', 'u', 'u',
+        'u', 'u', 'u',  'u', 'u', 'u',  // 10
+        0,   0,   '"',  0,   0,   0,   0,   0,   0,   0,
+        0,   0,   0,    0,   0,   0,  // 20
+        Z16, Z16,                     // 30~4F
+        0,   0,   0,    0,   0,   0,   0,   0,   0,   0,
+        0,   0,   '\\', 0,   0,   0,                       // 50
+        Z16, Z16, Z16,  Z16, Z16, Z16, Z16, Z16, Z16, Z16  // 60~FF
+#undef Z16
+    };
+
+    if (TargetEncoding::supportUnicode)
+      PutReserve(*os_, 2 + length * 6);  // "\uxxxx..."
+    else
+      PutReserve(*os_, 2 + length * 12);  // "\uxxxx\uyyyy..."
+
+    PutUnsafe(*os_, '\"');
+    GenericStringStream<SourceEncoding> is(str);
+    while (ScanWriteUnescapedString(is, length)) {
+      const Ch c = is.Peek();
+      if (!TargetEncoding::supportUnicode && static_cast<unsigned>(c) >= 0x80) {
+        // Unicode escaping
+        unsigned codepoint;
+        if (RAPIDJSON_UNLIKELY(!SourceEncoding::Decode(is, &codepoint)))
+          return false;
+        PutUnsafe(*os_, '\\');
+        PutUnsafe(*os_, 'u');
+        if (codepoint <= 0xD7FF ||
+            (codepoint >= 0xE000 && codepoint <= 0xFFFF)) {
+          PutUnsafe(*os_, hexDigits[(codepoint >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(codepoint)&15]);
+        } else {
+          RAPIDJSON_ASSERT(codepoint >= 0x010000 && codepoint <= 0x10FFFF);
+          // Surrogate pair
+          unsigned s = codepoint - 0x010000;
+          unsigned lead = (s >> 10) + 0xD800;
+          unsigned trail = (s & 0x3FF) + 0xDC00;
+          PutUnsafe(*os_, hexDigits[(lead >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(lead)&15]);
+          PutUnsafe(*os_, '\\');
+          PutUnsafe(*os_, 'u');
+          PutUnsafe(*os_, hexDigits[(trail >> 12) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail >> 8) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail >> 4) & 15]);
+          PutUnsafe(*os_, hexDigits[(trail)&15]);
+        }
+      } else if ((sizeof(Ch) == 1 || static_cast<unsigned>(c) < 256) &&
+                 RAPIDJSON_UNLIKELY(escape[static_cast<unsigned char>(c)])) {
+        is.Take();
+        PutUnsafe(*os_, '\\');
+        PutUnsafe(*os_, static_cast<typename OutputStream::Ch>(
+                            escape[static_cast<unsigned char>(c)]));
+        if (escape[static_cast<unsigned char>(c)] == 'u') {
+          PutUnsafe(*os_, '0');
+          PutUnsafe(*os_, '0');
+          PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) >> 4]);
+          PutUnsafe(*os_, hexDigits[static_cast<unsigned char>(c) & 0xF]);
+        }
+      } else if (RAPIDJSON_UNLIKELY(!(
+                     writeFlags & kWriteValidateEncodingFlag
+                         ? Transcoder<SourceEncoding, TargetEncoding>::Validate(
+                               is, *os_)
+                         : Transcoder<SourceEncoding,
+                                      TargetEncoding>::TranscodeUnsafe(is,
+                                                                       *os_))))
+        return false;
+    }
+    PutUnsafe(*os_, '\"');
+    return true;
+  }
+
+  bool ScanWriteUnescapedString(GenericStringStream<SourceEncoding> &is,
+                                size_t length) {
+    return RAPIDJSON_LIKELY(is.Tell() < length);
+  }
+
+  bool WriteStartObject() {
+    os_->Put('{');
+    return true;
+  }
+  bool WriteEndObject() {
+    os_->Put('}');
+    return true;
+  }
+  bool WriteStartArray() {
+    os_->Put('[');
+    return true;
+  }
+  bool WriteEndArray() {
+    os_->Put(']');
+    return true;
+  }
+
+  bool WriteRawValue(const Ch *json, size_t length) {
+    PutReserve(*os_, length);
+    GenericStringStream<SourceEncoding> is(json);
+    while (RAPIDJSON_LIKELY(is.Tell() < length)) {
+      RAPIDJSON_ASSERT(is.Peek() != '\0');
+      if (RAPIDJSON_UNLIKELY(!(
+              writeFlags & kWriteValidateEncodingFlag
+                  ? Transcoder<SourceEncoding, TargetEncoding>::Validate(is,
+                                                                         *os_)
+                  : Transcoder<SourceEncoding, TargetEncoding>::TranscodeUnsafe(
+                        is, *os_))))
+        return false;
+    }
+    return true;
+  }
+
+  void Prefix(Type type) {
+    (void)type;
+    if (RAPIDJSON_LIKELY(level_stack_.GetSize() !=
+                         0)) {  // this value is not at root
+      Level *level = level_stack_.template Top<Level>();
+      if (level->valueCount > 0) {
+        if (level->inArray)
+          os_->Put(',');  // add comma if it is not the first element in array
+        else              // in object
+          os_->Put((level->valueCount % 2 == 0) ? ',' : ':');
+      }
+      if (!level->inArray && level->valueCount % 2 == 0)
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
+                                                // number should be a name
+      level->valueCount++;
+    } else {
+      RAPIDJSON_ASSERT(!hasRoot_);  // Should only has one and only one root.
+      hasRoot_ = true;
+    }
+  }
+
+  // Flush the value if it is the top level one.
+  bool EndValue(bool ret) {
+    if (RAPIDJSON_UNLIKELY(level_stack_.Empty()))  // end of json text
+      Flush();
+    return ret;
+  }
+
+  OutputStream *os_;
+  internal::Stack<StackAllocator> level_stack_;
+  int maxDecimalPlaces_;
+  bool hasRoot_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  Writer(const Writer &);
+  Writer &operator=(const Writer &);
+};
+
+// Full specialization for StringStream to prevent memory copying
+
+template <>
+inline bool Writer<StringBuffer>::WriteInt(int i) {
+  char *buffer = os_->Push(11);
+  const char *end = internal::i32toa(i, buffer);
+  os_->Pop(static_cast<size_t>(11 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteUint(unsigned u) {
+  char *buffer = os_->Push(10);
+  const char *end = internal::u32toa(u, buffer);
+  os_->Pop(static_cast<size_t>(10 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteInt64(int64_t i64) {
+  char *buffer = os_->Push(21);
+  const char *end = internal::i64toa(i64, buffer);
+  os_->Pop(static_cast<size_t>(21 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteUint64(uint64_t u) {
+  char *buffer = os_->Push(20);
+  const char *end = internal::u64toa(u, buffer);
+  os_->Pop(static_cast<size_t>(20 - (end - buffer)));
+  return true;
+}
+
+template <>
+inline bool Writer<StringBuffer>::WriteDouble(double d) {
+  if (internal::Double(d).IsNanOrInf()) {
+    // Note: This code path can only be reached if
+    // (RAPIDJSON_WRITE_DEFAULT_FLAGS & kWriteNanAndInfFlag).
+    if (!(kWriteDefaultFlags & kWriteNanAndInfFlag)) return false;
+    if (internal::Double(d).IsNan()) {
+      PutReserve(*os_, 3);
+      PutUnsafe(*os_, 'N');
+      PutUnsafe(*os_, 'a');
+      PutUnsafe(*os_, 'N');
+      return true;
+    }
+    if (internal::Double(d).Sign()) {
+      PutReserve(*os_, 9);
+      PutUnsafe(*os_, '-');
+    } else
+      PutReserve(*os_, 8);
+    PutUnsafe(*os_, 'I');
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'f');
+    PutUnsafe(*os_, 'i');
+    PutUnsafe(*os_, 'n');
+    PutUnsafe(*os_, 'i');
+    PutUnsafe(*os_, 't');
+    PutUnsafe(*os_, 'y');
+    return true;
+  }
+
+  char *buffer = os_->Push(25);
+  char *end = internal::dtoa(d, buffer, maxDecimalPlaces_);
+  os_->Pop(static_cast<size_t>(25 - (end - buffer)));
+  return true;
+}
+
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42)
+template <>
+inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
+                                                           size_t length) {
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
+
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
+
+  const char *p = is.src_;
+  const char *end = is.head_ + length;
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  const char *endAligned = reinterpret_cast<const char *>(
+      reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
+  if (nextAligned > end) return true;
+
+  while (p != nextAligned)
+    if (*p < 0x20 || *p == '\"' || *p == '\\') {
+      is.src_ = p;
+      return RAPIDJSON_LIKELY(is.Tell() < length);
+    } else
+      os_->PutUnsafe(*p++);
+
+  // The rest of string using SIMD
+  static const char dquote[16] = {'\"', '\"', '\"', '\"', '\"', '\"',
+                                  '\"', '\"', '\"', '\"', '\"', '\"',
+                                  '\"', '\"', '\"', '\"'};
+  static const char bslash[16] = {'\\', '\\', '\\', '\\', '\\', '\\',
+                                  '\\', '\\', '\\', '\\', '\\', '\\',
+                                  '\\', '\\', '\\', '\\'};
+  static const char space[16] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F,
+                                 0x1F, 0x1F, 0x1F, 0x1F};
+  const __m128i dq =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&dquote[0]));
+  const __m128i bs =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&bslash[0]));
+  const __m128i sp =
+      _mm_loadu_si128(reinterpret_cast<const __m128i *>(&space[0]));
+
+  for (; p != endAligned; p += 16) {
+    const __m128i s = _mm_load_si128(reinterpret_cast<const __m128i *>(p));
+    const __m128i t1 = _mm_cmpeq_epi8(s, dq);
+    const __m128i t2 = _mm_cmpeq_epi8(s, bs);
+    const __m128i t3 = _mm_cmpeq_epi8(_mm_max_epu8(s, sp),
+                                      sp);  // s < 0x20 <=> max(s, 0x1F) == 0x1F
+    const __m128i x = _mm_or_si128(_mm_or_si128(t1, t2), t3);
+    unsigned short r = static_cast<unsigned short>(_mm_movemask_epi8(x));
+    if (RAPIDJSON_UNLIKELY(r != 0)) {  // some of characters is escaped
+      SizeType len;
+#ifdef _MSC_VER  // Find the index of first escaped
+      unsigned long offset;
+      _BitScanForward(&offset, r);
+      len = offset;
+#else
+      len = static_cast<SizeType>(__builtin_ffs(r) - 1);
+#endif
+      char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
+
+      p += len;
+      break;
+    }
+    _mm_storeu_si128(reinterpret_cast<__m128i *>(os_->PushUnsafe(16)), s);
+  }
+
+  is.src_ = p;
+  return RAPIDJSON_LIKELY(is.Tell() < length);
+}
+#elif defined(RAPIDJSON_NEON)
+template <>
+inline bool Writer<StringBuffer>::ScanWriteUnescapedString(StringStream &is,
+                                                           size_t length) {
+  if (length < 16) return RAPIDJSON_LIKELY(is.Tell() < length);
+
+  if (!RAPIDJSON_LIKELY(is.Tell() < length)) return false;
+
+  const char *p = is.src_;
+  const char *end = is.head_ + length;
+  const char *nextAligned = reinterpret_cast<const char *>(
+      (reinterpret_cast<size_t>(p) + 15) & static_cast<size_t>(~15));
+  const char *endAligned = reinterpret_cast<const char *>(
+      reinterpret_cast<size_t>(end) & static_cast<size_t>(~15));
+  if (nextAligned > end) return true;
+
+  while (p != nextAligned)
+    if (*p < 0x20 || *p == '\"' || *p == '\\') {
+      is.src_ = p;
+      return RAPIDJSON_LIKELY(is.Tell() < length);
+    } else
+      os_->PutUnsafe(*p++);
+
+  // The rest of string using SIMD
+  const uint8x16_t s0 = vmovq_n_u8('"');
+  const uint8x16_t s1 = vmovq_n_u8('\\');
+  const uint8x16_t s2 = vmovq_n_u8('\b');
+  const uint8x16_t s3 = vmovq_n_u8(32);
+
+  for (; p != endAligned; p += 16) {
+    const uint8x16_t s = vld1q_u8(reinterpret_cast<const uint8_t *>(p));
+    uint8x16_t x = vceqq_u8(s, s0);
+    x = vorrq_u8(x, vceqq_u8(s, s1));
+    x = vorrq_u8(x, vceqq_u8(s, s2));
+    x = vorrq_u8(x, vcltq_u8(s, s3));
+
+    x = vrev64q_u8(x);                                           // Rev in 64
+    uint64_t low = vgetq_lane_u64(vreinterpretq_u64_u8(x), 0);   // extract
+    uint64_t high = vgetq_lane_u64(vreinterpretq_u64_u8(x), 1);  // extract
+
+    SizeType len = 0;
+    bool escaped = false;
+    if (low == 0) {
+      if (high != 0) {
+        uint32_t lz = RAPIDJSON_CLZLL(high);
+        len = 8 + (lz >> 3);
+        escaped = true;
+      }
+    } else {
+      uint32_t lz = RAPIDJSON_CLZLL(low);
+      len = lz >> 3;
+      escaped = true;
+    }
+    if (RAPIDJSON_UNLIKELY(escaped)) {  // some of characters is escaped
+      char *q = reinterpret_cast<char *>(os_->PushUnsafe(len));
+      for (size_t i = 0; i < len; i++) q[i] = p[i];
+
+      p += len;
+      break;
+    }
+    vst1q_u8(reinterpret_cast<uint8_t *>(os_->PushUnsafe(16)), s);
+  }
+
+  is.src_ = p;
+  return RAPIDJSON_LIKELY(is.Tell() < length);
+}
+#endif  // RAPIDJSON_NEON
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(_MSC_VER) || defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_RAPIDJSON_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/CHANGELOG.md

@@ -0,0 +1,53 @@
+# Changelog
+
+All notable changes to this project will be documented in this file.
+
+## [1.2.3]
+### Fixed
+- Optimize framing logic and reduce CPU usage
+- Fixed some known issues
+
+## [1.2.1]
+### Fixed
+- Fix offset time error regarding CustomMsg format message publishment.
+
+## [1.2.0]
+### Added
+- Revise the frame segmentation logic.
+- (Notice!!!) Add Timestamp to each point in Livox pointcloud2 (PointXYZRTLT) format. The PointXYZRTL format has been updated to PointXYZRTLT format. Compatibility needs to be considered.
+### Fixed
+- Improve support for gPTP and GPS synchronizations.
+
+--- 
+## [1.1.3]
+### Fixed
+- Improve performance when running in ROS2 Humble.
+
+--- 
+## [1.1.2]
+### Changed
+- Change publish frequency range to [0.5Hz, 10 Hz].
+### Fixed
+- Fix a high CPU-usage problem.
+
+--- 
+## [1.1.1]
+### Added
+- Offer valid line-number info in the point cloud data of MID-360 Lidar.
+- Enable IMU by default.
+### Changed
+- Update the README slightly.
+
+--- 
+## [1.0.0]
+### Added
+- Support Mid-360 Lidar.
+- Support for Ubuntu 22.04 ROS2 humble.
+- Support multi-topic fuction, the suffix of the topic name corresponds to the ip address of each Lidar. 
+### Changed
+- Remove the embedded SDK.
+- Constraint: Livox ROS Driver 2 for ROS2 does not support message passing with PCL native data types.
+### Fixed
+- Fix IMU packet loss.
+- Fix some conflicts with livox ros driver.
+- Fixed HAP Lidar publishing PointCloud2 and CustomMsg format point clouds with no line number.

src/rm_nav/rm_driver/livox_ros_driver2/src/CMakeLists.txt

@@ -0,0 +1,146 @@
+# Copyright(c) 2020 livoxtech limited.
+
+cmake_minimum_required(VERSION 3.14)
+project(livox_ros_driver2)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules")
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter)
+endif()
+
+# Printf version info
+include(cmake/version.cmake)
+project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX)
+message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}")
+
+#---------------------------------------------------------------------------------------
+# Add ROS Version MACRO
+#---------------------------------------------------------------------------------------
+add_definitions(-DBUILDING_ROS2)
+
+# find dependencies
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+find_package(PCL REQUIRED QUIET)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# check apr
+find_package(PkgConfig)
+pkg_check_modules(APR apr-1)
+if (APR_FOUND)
+  message(${APR_INCLUDE_DIRS})
+  message(${APR_LIBRARIES})
+endif (APR_FOUND)
+
+# generate custom msg headers
+set(LIVOX_INTERFACES livox_interfaces2)
+rosidl_generate_interfaces(${LIVOX_INTERFACES}
+  "msg/CustomPoint.msg"
+  "msg/CustomMsg.msg"
+  DEPENDENCIES builtin_interfaces std_msgs
+  LIBRARY_NAME ${PROJECT_NAME}
+)
+
+## make sure the livox_lidar_sdk_shared library is installed
+find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED)
+
+##
+find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR
+  NAMES "livox_lidar_api.h" "livox_lidar_def.h"
+  REQUIRED)
+
+## PCL library
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+# livox ros2 driver target
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/livox_ros_driver2.cpp
+  src/lddc.cpp
+  src/driver_node.cpp
+  src/lds.cpp
+  src/lds_lidar.cpp
+
+  src/comm/comm.cpp
+  src/comm/ldq.cpp
+  src/comm/semaphore.cpp
+  src/comm/lidar_imu_data_queue.cpp
+  src/comm/cache_index.cpp
+  src/comm/pub_handler.cpp
+
+  src/parse_cfg_file/parse_cfg_file.cpp
+  src/parse_cfg_file/parse_livox_lidar_cfg.cpp
+
+  src/call_back/lidar_common_callback.cpp
+  src/call_back/livox_lidar_callback.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS})
+
+# get include directories of custom msg headers
+if($ENV{ROS_DISTRO} STREQUAL "foxy")
+  set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp")
+  add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES})
+  get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES)
+else()
+  rosidl_get_typesupport_target(cpp_typesupport_target
+  ${LIVOX_INTERFACES} "rosidl_typesupport_cpp")
+  target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
+endif()
+
+# include file direcotry
+target_include_directories(${PROJECT_NAME} PUBLIC
+  ${PCL_INCLUDE_DIRS}
+  ${APR_INCLUDE_DIRS}
+  ${LIVOX_LIDAR_SDK_INCLUDE_DIR}
+  ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES}   # for custom msgs
+  3rdparty
+  src
+)
+
+# link libraries
+target_link_libraries(${PROJECT_NAME}
+  ${LIVOX_LIDAR_SDK_LIBRARY}
+  ${LIVOX_INTERFACE_TARGET}   # for custom msgs
+  ${PPT_LIBRARY}
+  ${Boost_LIBRARY}
+  ${PCL_LIBRARIES}
+  ${APR_LIBRARIES}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "livox_ros::DriverNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

src/rm_nav/rm_driver/livox_ros_driver2/src/LICENSE.txt

@@ -0,0 +1,412 @@
+The following portions of the LIVOX’s Livox ROS Driver2 (“Software” referred to in the terms below) are made available to you under the terms of the MIT License provided below and is also available at https://opensource.org/licenses/MIT.  
+
+livox_ros_driver2
+├── build.sh
+├── cmake
+│   └── version.cmake
+├── CMakeLists.txt
+├── config
+│   ├── display_point_cloud_ROS1.rviz
+│   ├── display_point_cloud_ROS2.rviz
+│   ├── HAP_config.json
+│   ├── MID360_config.json
+│   └── mixed_HAP_MID360_config.json
+├── launch_ROS1
+│   ├── msg_HAP.launch
+│   ├── msg_MID360.launch
+│   ├── msg_mixed.launch
+│   ├── rviz_HAP.launch
+│   ├── rviz_MID360.launch
+│   └── rviz_mixed.launch
+├── launch_ROS2
+│   ├── msg_HAP_launch.py
+│   ├── msg_MID360_launch.py
+│   ├── rviz_HAP_launch.py
+│   ├── rviz_MID360_launch.py
+│   └── rviz_mixed.py
+├── msg
+│   ├── CustomMsg.msg
+│   └── CustomPoint.msg
+├── package_ROS1.xml
+├── package_ROS2.xml
+├── package.xml
+├── README.md
+└── src
+    ├── call_back
+    │   ├── lidar_common_callback.cpp
+    │   ├── lidar_common_callback.h
+    │   ├── livox_lidar_callback.cpp
+    │   └── livox_lidar_callback.h
+    ├── comm
+    │   ├── cache_index.cpp
+    │   ├── cache_index.h
+    │   ├── comm.cpp
+    │   ├── comm.h
+    │   ├── ldq.cpp
+    │   ├── ldq.h
+    │   ├── lidar_imu_data_queue.cpp
+    │   ├── lidar_imu_data_queue.h
+    │   ├── pub_handler.cpp
+    │   ├── pub_handler.h
+    │   ├── semaphore.cpp
+    │   └── semaphore.h
+    ├── driver_node.cpp
+    ├── driver_node.h
+    ├── include
+    │   ├── livox_ros_driver2.h
+    │   ├── ros1_headers.h
+    │   ├── ros2_headers.h
+    │   └── ros_headers.h
+    ├── lddc.cpp
+    ├── lddc.h
+    ├── lds.cpp
+    ├── lds.h
+    ├── lds_lidar.cpp
+    ├── lds_lidar.h
+    ├── livox_ros_driver2.cpp
+    └── parse_cfg_file
+        ├── parse_cfg_file.cpp
+        ├── parse_cfg_file.h
+        ├── parse_livox_lidar_cfg.cpp
+        └── parse_livox_lidar_cfg.h
+
+---------------------------------
+
+The MIT License (MIT)
+
+Copyright (c) 2022 Livox. All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified source code of RapidJSON (https://github.com/Tencent/rapidjson), which is also licensed under MIT license. A copy of the MIT license is provided below and is also available at https://opensource.org/licenses/MIT.
+
+livox_ros_driver2
+├── 3rdparty
+│   └── rapidjson
+│       ├── allocators.h
+│       ├── cursorstreamwrapper.h
+│       ├── document.h
+│       ├── encodedstream.h
+│       ├── encodings.h
+│       ├── error
+│       │   ├── en.h
+│       │   └── error.h
+│       ├── filereadstream.h
+│       ├── filewritestream.h
+│       ├── fwd.h
+│       ├── internal
+│       │   ├── biginteger.h
+│       │   ├── clzll.h
+│       │   ├── diyfp.h
+│       │   ├── dtoa.h
+│       │   ├── ieee754.h
+│       │   ├── itoa.h
+│       │   ├── meta.h
+│       │   ├── pow10.h
+│       │   ├── regex.h
+│       │   ├── stack.h
+│       │   ├── strfunc.h
+│       │   ├── strtod.h
+│       │   └── swap.h
+│       ├── istreamwrapper.h
+│       ├── memorybuffer.h
+│       ├── memorystream.h
+│       ├── msinttypes
+│       │   ├── inttypes.h
+│       │   └── stdint.h
+│       ├── ostreamwrapper.h
+│       ├── pointer.h
+│       ├── prettywriter.h
+│       ├── rapidjson.h
+│       ├── reader.h
+│       ├── schema.h
+│       ├── stream.h
+│       ├── stringbuffer.h
+│       └── writer.h
+
+-------------------------------------------------------------
+
+Tencent is pleased to support the open source community by making RapidJSON
+available.
+
+Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+rights reserved.
+
+Licensed under the MIT License (the "License"); you may not use this file
+except in compliance with the License. You may obtain a copy of the License
+at
+
+http://opensource.org/licenses/MIT
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+License for the specific language governing permissions and limitations under
+the License.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS (https://www.ros.org/), which is also licensed under 3-Clause-BSD license. A copy of the 3-Clause-BSD license is provided below and is also available at https://opensource.org/licenses/BSD-3-Clause.
+
+-------------------------------------------------------------
+
+The 3-Clause BSD License
+
+Copyright (c) 2001 - 2009, The Board of Trustees of the University of Illinois.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+* Redistributions of source code must retain the above 
+  copyright notice, this list of conditions and the 
+  following disclaimer.
+
+* 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.
+
+* Neither the name of the University of Illinois
+  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 OWNER 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.
+
+===============================================================
+
+LIVOX’s Livox ROS Driver2 uses unmodified libraries and interfaces of ROS2-rclcpp (https://github.com/ros2), which is also licensed under Apache License 2.0. A copy of the Apache License 2.0 is provided below and is also available at https://www.apache.org/licenses/LICENSE-2.0.
+
+-------------------------------------------------------------
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/rm_nav/rm_driver/livox_ros_driver2/src/cmake/version.cmake

@@ -0,0 +1,16 @@
+#---------------------------------------------------------------------------------------
+# Get livox_ros_driver2 version from include/livox_ros_driver2.h
+#---------------------------------------------------------------------------------------
+file(READ "${CMAKE_CURRENT_LIST_DIR}/../src/include/livox_ros_driver2.h" LIVOX_ROS_DRIVER2_VERSION_FILE)
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MAJOR ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_major ${CMAKE_MATCH_1})
+
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_MINOR ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_minor ${CMAKE_MATCH_1})
+string(REGEX MATCH "LIVOX_ROS_DRIVER2_VER_PATCH ([0-9]+)" _  "${LIVOX_ROS_DRIVER2_VERSION_FILE}")
+set(ver_patch ${CMAKE_MATCH_1})
+
+if (NOT DEFINED ver_major OR NOT DEFINED ver_minor OR NOT DEFINED ver_patch)
+    message(FATAL_ERROR "Could not extract valid version from include/livox_ros_driver2.h")
+endif()
+set (LIVOX_ROS_DRIVER2_VERSION "${ver_major}.${ver_minor}.${ver_patch}")

src/rm_nav/rm_driver/livox_ros_driver2/src/config/HAP_config.json

@@ -0,0 +1,42 @@
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8
+  },
+  "HAP": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.100",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+

src/rm_nav/rm_driver/livox_ros_driver2/src/config/MID360_config.json

@@ -0,0 +1,41 @@
+{
+  "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.50",
+      "cmd_data_port": 56101,
+      "push_msg_ip": "192.168.1.50",
+      "push_msg_port": 56201,
+      "point_data_ip": "192.168.1.50",
+      "point_data_port": 56301,
+      "imu_data_ip" : "192.168.1.50",
+      "imu_data_port": 56401,
+      "log_data_ip" : "",
+      "log_data_port": 56501
+    }
+  },
+  "lidar_configs" : [
+    {
+      "ip" : "192.168.1.176",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 180.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}

src/rm_nav/rm_driver/livox_ros_driver2/src/config/display_point_cloud.rviz

@@ -0,0 +1,137 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 796
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.20000000298023224
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 150
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /livox/lidar
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: livox_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 14.215983390808105
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.9503982067108154
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.6603963375091553
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1025
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015f000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c3000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1853
+  X: 67
+  Y: 27

src/rm_nav/rm_driver/livox_ros_driver2/src/config/mixed_HAP_MID360_config.json

@@ -0,0 +1,76 @@
+{
+  "lidar_summary_info" : {
+    "lidar_type": 8
+  },
+  "HAP": {
+    "lidar_net_info" : {
+      "cmd_data_port": 56000,
+      "push_msg_port": 0,
+      "point_data_port": 57000,
+      "imu_data_port": 58000,
+      "log_data_port": 59000
+    },
+    "host_net_info" : {
+      "cmd_data_ip" : "192.168.1.5",
+      "cmd_data_port": 56000,
+      "push_msg_ip": "",
+      "push_msg_port": 0,
+      "point_data_ip": "192.168.1.5",
+      "point_data_port": 57000,
+      "imu_data_ip" : "192.168.1.5",
+      "imu_data_port": 58000,
+      "log_data_ip" : "",
+      "log_data_port": 59000
+    }
+  },
+  "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",
+      "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.100",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    },
+    {
+      "ip" : "192.168.1.12",
+      "pcl_data_type" : 1,
+      "pattern_mode" : 0,
+      "extrinsic_parameter" : {
+        "roll": 0.0,
+        "pitch": 0.0,
+        "yaw": 0.0,
+        "x": 0,
+        "y": 0,
+        "z": 0
+      }
+    }
+  ]
+}
+

src/rm_nav/rm_driver/livox_ros_driver2/src/launch/msg_HAP_launch.py

@@ -0,0 +1,55 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 1    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'livox_lidar.rviz')
+user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])

src/rm_nav/rm_driver/livox_ros_driver2/src/launch/msg_MID360_launch.py

@@ -0,0 +1,54 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 4    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])

src/rm_nav/rm_driver/livox_ros_driver2/src/launch/rviz_HAP_launch.py

@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud.rviz')
+user_config_path = os.path.join(cur_config_path, 'HAP_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])

src/rm_nav/rm_driver/livox_ros_driver2/src/launch/rviz_MID360_launch.py

@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud.rviz')
+user_config_path = os.path.join(cur_config_path, 'MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])

src/rm_nav/rm_driver/livox_ros_driver2/src/launch/rviz_mixed.py

@@ -0,0 +1,63 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch
+
+################### user configure parameters for ros2 start ###################
+xfer_format   = 0    # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+multi_topic   = 0    # 0-All LiDARs share the same topic, 1-One LiDAR one topic
+data_src      = 0    # 0-lidar, others-Invalid data src
+publish_freq  = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+output_type   = 0
+frame_id      = 'livox_frame'
+lvx_file_path = '/home/livox/livox_test.lvx'
+cmdline_bd_code = 'livox0000000001'
+
+cur_path = os.path.split(os.path.realpath(__file__))[0] + '/'
+cur_config_path = cur_path + '../config'
+rviz_config_path = os.path.join(cur_config_path, 'display_point_cloud.rviz')
+user_config_path = os.path.join(cur_config_path, 'mixed_HAP_MID360_config.json')
+################### user configure parameters for ros2 end #####################
+
+livox_ros2_params = [
+    {"xfer_format": xfer_format},
+    {"multi_topic": multi_topic},
+    {"data_src": data_src},
+    {"publish_freq": publish_freq},
+    {"output_data_type": output_type},
+    {"frame_id": frame_id},
+    {"lvx_file_path": lvx_file_path},
+    {"user_config_path": user_config_path},
+    {"cmdline_input_bd_code": cmdline_bd_code}
+]
+
+
+def generate_launch_description():
+    livox_driver = Node(
+        package='livox_ros_driver2',
+        executable='livox_ros_driver2_node',
+        name='livox_lidar_publisher',
+        output='screen',
+        parameters=livox_ros2_params
+        )
+
+    livox_rviz = Node(
+            package='rviz2',
+            executable='rviz2',
+            output='screen',
+            arguments=['--display-config', rviz_config_path]
+        )
+
+    return LaunchDescription([
+        livox_driver,
+        livox_rviz,
+        # launch.actions.RegisterEventHandler(
+        #     event_handler=launch.event_handlers.OnProcessExit(
+        #         target_action=livox_rviz,
+        #         on_exit=[
+        #             launch.actions.EmitEvent(event=launch.events.Shutdown()),
+        #         ]
+        #     )
+        # )
+    ])

src/rm_nav/rm_driver/livox_ros_driver2/src/msg/CustomMsg.msg

@@ -0,0 +1,9 @@
+# Livox publish pointcloud msg format.
+
+std_msgs/Header header    # ROS standard message header
+uint64 timebase           # The time of first point
+uint32 point_num          # Total number of pointclouds
+uint8  lidar_id           # Lidar device id number
+uint8[3]  rsvd            # Reserved use
+CustomPoint[] points      # Pointcloud data
+

src/rm_nav/rm_driver/livox_ros_driver2/src/msg/CustomPoint.msg

@@ -0,0 +1,10 @@
+# Livox costom pointcloud format.
+
+uint32 offset_time      # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8 reflectivity      # reflectivity, 0~255
+uint8 tag               # livox tag
+uint8 line              # laser number in lidar
+

src/rm_nav/rm_driver/livox_ros_driver2/src/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>livox_ros_driver2</name>
+  <version>1.0.0</version>
+  <description>The ROS device driver for Livox 3D LiDARs, for ROS2</description>
+  <maintainer email="dev@livoxtech.com">feng</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rcutils</depend>
+  <depend>pcl_conversions</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>libpcl-all-dev</depend>
+
+  <exec_depend>rosbag2</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>git</depend>
+  <depend>apr</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_nav/rm_driver/livox_ros_driver2/src/src/call_back/lidar_common_callback.cpp

@@ -0,0 +1,73 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lidar_common_callback.h"
+
+#include "../lds_lidar.h"
+
+#include <string>
+
+namespace livox_ros {
+
+void LidarCommonCallback::OnLidarPointClounCb(PointFrame* frame, void* client_data) {
+  if (frame == nullptr) {
+    printf("LidarPointCloudCb frame is nullptr.\n");
+    return;
+  }
+
+  if (client_data == nullptr) {
+    printf("Lidar point cloud cb failed, client data is nullptr.\n");
+    return;
+  }
+
+  if (frame->lidar_num ==0) {
+    printf("LidarPointCloudCb lidar_num:%u.\n", frame->lidar_num);
+    return;
+  }
+
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+  
+  //printf("Lidar point cloud, lidar_num:%u.\n", frame->lidar_num);
+
+  lds_lidar->StoragePointData(frame);
+}
+
+void LidarCommonCallback::LidarImuDataCallback(ImuData* imu_data, void *client_data) {
+  if (imu_data == nullptr) {
+    printf("Imu data is nullptr.\n");
+    return;
+  }
+  if (client_data == nullptr) {
+    printf("Lidar point cloud cb failed, client data is nullptr.\n");
+    return;
+  }
+
+  LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+  lds_lidar->StorageImuData(imu_data);
+}
+
+} // namespace livox_ros
+
+
+

src/rm_nav/rm_driver/livox_ros_driver2/src/src/call_back/lidar_common_callback.h

@@ -0,0 +1,40 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_
+#define LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_	
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+
+class LidarCommonCallback {
+ public:
+  static void OnLidarPointClounCb(PointFrame* frame, void* client_data);
+  static void LidarImuDataCallback(ImuData* imu_data, void *client_data);
+};
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LIDAR_COMMON_CALLBACK_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/src/call_back/livox_lidar_callback.cpp

@@ -0,0 +1,333 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "livox_lidar_callback.h"
+
+#include <string>
+#include <thread>
+#include <iostream>
+
+namespace livox_ros {
+
+void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle,
+                                           const LivoxLidarInfo* info,
+                                           void* client_data) {
+  if (client_data == nullptr) {
+    std::cout << "lidar info change callback failed, client data is nullptr" << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  LidarDevice* lidar_device = GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "found lidar not defined in the user-defined config, ip: " << IpNumToString(handle) << std::endl;
+    // add lidar device
+    uint8_t index = 0;
+    int8_t ret = lds_lidar->cache_index_.GetFreeIndex(kLivoxLidarType, handle, index);
+    if (ret != 0) {
+      std::cout << "failed to add lidar device, lidar ip: " << IpNumToString(handle) << std::endl;
+      return;
+    }
+    LidarDevice *p_lidar = &(lds_lidar->lidars_[index]);
+    p_lidar->lidar_type = kLivoxLidarType;
+  } else {
+    // set the lidar according to the user-defined config
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+
+    // lock for modify the lidar device set_bits
+    {
+      std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+      if (config.pcl_data_type != -1 ) {
+        lidar_device->livox_config.set_bits |= kConfigDataType;
+        SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
+                                LivoxLidarCallback::SetDataTypeCallback, lds_lidar);
+        std::cout << "set pcl data type, handle: " << handle << ", data type: "
+                  << static_cast<int32_t>(config.pcl_data_type) << std::endl;
+      }
+      if (config.pattern_mode != -1) {
+        lidar_device->livox_config.set_bits |= kConfigScanPattern;
+        SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
+                              LivoxLidarCallback::SetPatternModeCallback, lds_lidar);
+        std::cout << "set scan pattern, handle: " << handle << ", scan pattern: "
+                  << static_cast<int32_t>(config.pattern_mode) << std::endl;
+      }
+      if (config.blind_spot_set != -1) {
+        lidar_device->livox_config.set_bits |= kConfigBlindSpot;
+        SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
+                              LivoxLidarCallback::SetBlindSpotCallback, lds_lidar);
+
+        std::cout << "set blind spot, handle: " << handle << ", blind spot distance: "
+                  << config.blind_spot_set << std::endl;
+      }
+      if (config.dual_emit_en != -1) {
+        lidar_device->livox_config.set_bits |= kConfigDualEmit;
+        SetLivoxLidarDualEmit(handle, (config.dual_emit_en == 0 ? false : true),
+                              LivoxLidarCallback::SetDualEmitCallback, lds_lidar);
+        std::cout << "set dual emit mode, handle: " << handle << ", enable dual emit: "
+                  << static_cast<int32_t>(config.dual_emit_en) << std::endl;
+      }
+    } // free lock for set_bits
+
+    // set extrinsic params into lidar
+    LivoxLidarInstallAttitude attitude {
+      config.extrinsic_param.roll,
+      config.extrinsic_param.pitch,
+      config.extrinsic_param.yaw,
+      config.extrinsic_param.x,
+      config.extrinsic_param.y,
+      config.extrinsic_param.z
+    };
+    SetLivoxLidarInstallAttitude(config.handle, &attitude,
+                                 LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
+  }
+
+  std::cout << "begin to change work mode to 'Normal', handle: " << handle << std::endl;
+  SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
+  EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
+  return;
+}
+
+void LivoxLidarCallback::WorkModeChangedCallback(livox_status status,
+                                                 uint32_t handle,
+                                                 LivoxLidarAsyncControlResponse *response,
+                                                 void *client_data) {
+  if (status != kLivoxLidarStatusSuccess) {
+    std::cout << "failed to change work mode, handle: " << handle << ", try again..."<< std::endl;
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, WorkModeChangedCallback, nullptr);
+    return;
+  }
+  std::cout << "successfully change work mode, handle: " << handle << std::endl;
+  return;
+}
+
+void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set data type since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDataType));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set data type, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarPclDataType(handle, static_cast<LivoxLidarPointDataType>(config.pcl_data_type),
+                             LivoxLidarCallback::SetDataTypeCallback, client_data);
+    std::cout << "set data type timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set data type, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t handle,
+                                                LivoxLidarAsyncControlResponse *response,
+                                                void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set pattern mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigScanPattern));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set pattern mode, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarScanPattern(handle, static_cast<LivoxLidarScanPattern>(config.pattern_mode),
+                             LivoxLidarCallback::SetPatternModeCallback, client_data);
+    std::cout << "set pattern mode timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set pattern mode, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t handle,
+                                              LivoxLidarAsyncControlResponse *response,
+                                              void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set blind spot since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigBlindSpot));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set blind spot, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarBlindSpot(handle, config.blind_spot_set,
+                           LivoxLidarCallback::SetBlindSpotCallback, client_data);
+    std::cout << "set blind spot timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set blind spot, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set dual emit mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  if (status == kLivoxLidarStatusSuccess) {
+    std::lock_guard<std::mutex> lock(lds_lidar->config_mutex_);
+    lidar_device->livox_config.set_bits &= ~((uint32_t)(kConfigDualEmit));
+    if (!lidar_device->livox_config.set_bits) {
+      lidar_device->connect_state = kConnectStateSampling;
+    }
+    std::cout << "successfully set dual emit mode, handle: " << handle
+              << ", set_bit: " << lidar_device->livox_config.set_bits << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    SetLivoxLidarDualEmit(handle, config.dual_emit_en,
+                          LivoxLidarCallback::SetDualEmitCallback, client_data);
+    std::cout << "set dual emit mode timeout, handle: " << handle
+              << ", try again..." << std::endl;
+  } else {
+    std::cout << "failed to set dual emit mode, handle: " << handle
+              << ", return code: " << response->ret_code
+              << ", error key: " << response->error_key << std::endl;
+  }
+  return;
+}
+
+void LivoxLidarCallback::SetAttitudeCallback(livox_status status, uint32_t handle,
+                                             LivoxLidarAsyncControlResponse *response,
+                                             void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set dual emit mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  if (status == kLivoxLidarStatusSuccess) {
+    std::cout << "successfully set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    std::cout << "set lidar attitude timeout, ip: " << IpNumToString(handle)
+              << ", try again..." << std::endl;
+    const UserLivoxLidarConfig& config = lidar_device->livox_config;
+    LivoxLidarInstallAttitude attitude {
+      config.extrinsic_param.roll,
+      config.extrinsic_param.pitch,
+      config.extrinsic_param.yaw,
+      config.extrinsic_param.x,
+      config.extrinsic_param.y,
+      config.extrinsic_param.z
+    };
+    SetLivoxLidarInstallAttitude(config.handle, &attitude,
+                                 LivoxLidarCallback::SetAttitudeCallback, lds_lidar);
+  } else {
+    std::cout << "failed to set lidar attitude, ip: " << IpNumToString(handle) << std::endl;
+  }
+}
+
+void LivoxLidarCallback::EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
+                                                         LivoxLidarAsyncControlResponse *response,
+                                                         void *client_data) {
+  LidarDevice* lidar_device =  GetLidarDevice(handle, client_data);
+  if (lidar_device == nullptr) {
+    std::cout << "failed to set pattern mode since no lidar device found, handle: "
+              << handle << std::endl;
+    return;
+  }
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+
+  if (response == nullptr) {
+    std::cout << "failed to get response since no lidar IMU sensor found, handle: "
+              << handle << std::endl;
+    return;
+  }
+
+  if (status == kLivoxLidarStatusSuccess) {
+    std::cout << "successfully enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
+  } else if (status == kLivoxLidarStatusTimeout) {
+    std::cout << "enable Livox Lidar imu timeout, ip: " << IpNumToString(handle)
+              << ", try again..." << std::endl;
+    EnableLivoxLidarImuData(handle, LivoxLidarCallback::EnableLivoxLidarImuDataCallback, lds_lidar);
+  } else {
+    std::cout << "failed to enable Livox Lidar imu, ip: " << IpNumToString(handle) << std::endl;
+  }
+}
+
+LidarDevice* LivoxLidarCallback::GetLidarDevice(const uint32_t handle, void* client_data) {
+  if (client_data == nullptr) {
+    std::cout << "failed to get lidar device, client data is nullptr" << std::endl;
+    return nullptr;
+  }
+
+  LdsLidar* lds_lidar = static_cast<LdsLidar*>(client_data);
+  uint8_t index = 0;
+  int8_t ret = lds_lidar->cache_index_.GetIndex(kLivoxLidarType, handle, index);
+  if (ret != 0) {
+    return nullptr;
+  }
+
+  return &(lds_lidar->lidars_[index]);
+}
+
+} // namespace livox_ros

src/rm_nav/rm_driver/livox_ros_driver2/src/src/call_back/livox_lidar_callback.h

@@ -0,0 +1,71 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
+#define LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_
+
+#include "../lds.h"
+#include "../lds_lidar.h"
+#include "../comm/comm.h"
+
+#include "livox_lidar_api.h"
+#include "livox_lidar_def.h"
+
+namespace livox_ros {
+
+class LivoxLidarCallback {
+ public:
+  static void LidarInfoChangeCallback(const uint32_t handle,
+                                      const LivoxLidarInfo* info,
+                                      void* client_data);
+  static void WorkModeChangedCallback(livox_status status,
+                                      uint32_t handle,
+                                      LivoxLidarAsyncControlResponse *response,
+                                      void *client_data);
+  static void SetDataTypeCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void SetPatternModeCallback(livox_status status, uint32_t handle,
+                                     LivoxLidarAsyncControlResponse *response,
+                                     void *client_data);
+  static void SetBlindSpotCallback(livox_status status, uint32_t handle,
+                                   LivoxLidarAsyncControlResponse *response,
+                                   void *client_data);
+  static void SetDualEmitCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void SetAttitudeCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+  static void EnableLivoxLidarImuDataCallback(livox_status status, uint32_t handle,
+                                  LivoxLidarAsyncControlResponse *response,
+                                  void *client_data);
+
+ private:
+  static LidarDevice* GetLidarDevice(const uint32_t handle, void* client_data);
+};
+
+} // namespace livox_ros
+
+#endif  // LIVOX_ROS_DRIVER_LIVOX_LIDAR_CALLBACK_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/cache_index.cpp

@@ -0,0 +1,121 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "cache_index.h"
+#include "livox_lidar_def.h"
+
+namespace livox_ros {
+
+CacheIndex::CacheIndex() {
+  std::array<bool, kMaxLidarCount> index_cache = {0};
+  index_cache_.swap(index_cache);
+}
+
+int8_t CacheIndex::GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+  {
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    if (map_index_.find(key) != map_index_.end()) {
+      index = map_index_[key];
+      return 0;
+    }
+  }
+
+  {
+    printf("GetFreeIndex key:%s.\n", key.c_str());
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    for (size_t i = 0; i < kMaxSourceLidar; ++i) {
+      if (!index_cache_[i]) {
+        index_cache_[i] = 1;
+        map_index_[key] = static_cast<uint8_t>(i);
+        index = static_cast<uint8_t>(i);
+        return 0;
+      }
+    }
+  }
+  return -1;
+}
+
+int8_t CacheIndex::GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key) {
+  if (livox_lidar_type == kLivoxLidarType) {
+    key = "livox_lidar_" + std::to_string(handle);
+  } else {
+    printf("Can not generate index, the livox lidar type is unknown, the livox lidar type:%u\n", livox_lidar_type);
+    return -1;
+  }
+  return 0;
+}
+
+int8_t CacheIndex::GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    index = map_index_[key];
+    return 0;
+  }
+  printf("Can not get index, the livox lidar type:%u, handle:%u\n", livox_lidar_type, handle);
+  return -1;
+}
+
+int8_t CacheIndex::LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(livox_lidar_type, handle, key);
+  if (ret != 0) {
+    return -1;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    index = map_index_[key];
+    return 0;
+  }
+
+  return GetFreeIndex(livox_lidar_type, handle, index);
+}
+
+void CacheIndex::ResetIndex(LidarDevice *lidar) {
+  std::string key;
+  int8_t ret = GenerateIndexKey(lidar->lidar_type, lidar->handle, key);
+  if (ret != 0) {
+    printf("Reset index failed, can not generate index key, lidar type:%u, handle:%u.\n", lidar->lidar_type, lidar->handle);
+    return;
+  }
+
+  if (map_index_.find(key) != map_index_.end()) {
+    uint8_t index = map_index_[key];
+    std::lock_guard<std::mutex> lock(index_mutex_);
+    map_index_.erase(key);
+    index_cache_[index] = 0;
+  }
+}
+
+} // namespace

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/cache_index.h

@@ -0,0 +1,54 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_CACHE_INDEX_H_
+#define LIVOX_ROS_DRIVER_CACHE_INDEX_H_
+
+#include <mutex>
+#include <array>
+#include <map>
+#include <string>
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+
+class CacheIndex {
+ public:
+  CacheIndex();
+  int8_t GetFreeIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  int8_t GetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  int8_t GenerateIndexKey(const uint8_t livox_lidar_type, const uint32_t handle, std::string& key);
+  int8_t LvxGetIndex(const uint8_t livox_lidar_type, const uint32_t handle, uint8_t& index);
+  void ResetIndex(LidarDevice *lidar);
+
+ private:
+  std::mutex index_mutex_;
+  std::map<std::string, uint8_t> map_index_; /* key:handle/slot, val:index */
+  std::array<bool, kMaxSourceLidar> index_cache_;
+};
+
+} // namespace livox_ros
+
+# endif // LIVOX_ROS_DRIVER_CACHE_INDEX_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/comm.cpp

@@ -0,0 +1,70 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "comm/comm.h"
+#include <string.h>
+#include <arpa/inet.h>
+
+namespace livox_ros {
+
+/** Common function --------------------------------------------------------- */
+bool IsFilePathValid(const char *path_str) {
+  int str_len = strlen(path_str);
+
+  if ((str_len > kPathStrMinSize) && (str_len < kPathStrMaxSize)) {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+uint32_t CalculatePacketQueueSize(const double publish_freq) {
+  uint32_t queue_size = 10;
+  if (publish_freq > 10.0) {
+    queue_size = static_cast<uint32_t>(publish_freq) + 1;
+  }
+  return queue_size;
+}
+
+std::string IpNumToString(uint32_t ip_num) {
+  struct in_addr ip;
+  ip.s_addr = ip_num;
+  return std::string(inet_ntoa(ip));
+}
+
+uint32_t IpStringToNum(std::string ip_string) {
+  return static_cast<uint32_t>(inet_addr(ip_string.c_str()));
+}
+
+std::string ReplacePeriodByUnderline(std::string str) {
+  std::size_t pos = str.find(".");
+  while (pos != std::string::npos) {
+    str.replace(pos, 1, "_");
+    pos = str.find(".");
+  }
+  return str;
+}
+
+} // namespace livox_ros
+

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/comm.h

@@ -0,0 +1,304 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER2_COMM_H_
+#define LIVOX_ROS_DRIVER2_COMM_H_
+
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <memory>
+#include <map>
+
+#include "lidar_imu_data_queue.h"
+
+namespace livox_ros {
+
+/** Max lidar data source num */
+const uint8_t kMaxSourceLidar = 32;
+
+
+/** Eth packet relative info parama */
+const uint32_t kMaxPointPerEthPacket = 100;
+const uint32_t kMinEthPacketQueueSize = 32;     /**< must be 2^n */
+const uint32_t kMaxEthPacketQueueSize = 131072; /**< must be 2^n */
+const uint32_t kImuEthPacketQueueSize = 256;
+
+/** Max packet length according to Ethernet MTU */
+const uint32_t KEthPacketMaxLength = 1500;
+const uint32_t KEthPacketHeaderLength = 18;     /**< (sizeof(LivoxEthPacket) - 1) */
+const uint32_t KCartesianPointSize = 13;
+const uint32_t KSphericalPointSzie = 9;
+
+const uint64_t kRosTimeMax = 4294967296000000000; /**< 2^32 * 1000000000ns */
+const int64_t kPacketTimeGap = 1000000;           /**< 1ms = 1000000ns */
+/**< the threshold of packet continuous */
+const int64_t kMaxPacketTimeGap = 1700000;
+/**< the threshold of device disconect */
+const int64_t kDeviceDisconnectThreshold = 1000000000;
+const uint32_t kNsPerSecond = 1000000000; /**< 1s  = 1000000000ns */
+const uint32_t kNsTolerantFrameTimeDeviation = 1000000; /**< 1ms  = 1000000ns */
+const uint32_t kRatioOfMsToNs = 1000000; /**< 1ms  = 1000000ns */
+
+const int kPathStrMinSize = 4;   /**< Must more than 4 char */
+const int kPathStrMaxSize = 256; /**< Must less than 256 char */
+const int kBdCodeSize = 15;
+
+const uint32_t kPointXYZRSize = 16;
+const uint32_t kPointXYZRTRSize = 18;
+
+const double PI = 3.14159265358979323846;
+
+constexpr uint32_t kMaxBufferSize = 0x8000;  // 32k bytes
+
+/** Device Line Number **/
+const uint8_t kLineNumberDefault = 1;
+const uint8_t kLineNumberMid360 = 4;
+const uint8_t kLineNumberHAP = 6;    
+
+// SDK related
+typedef enum {
+  kIndustryLidarType = 1,
+  kVehicleLidarType = 2,
+  kDirectLidarType = 4,
+  kLivoxLidarType = 8
+} LidarProtoType;
+
+// SDK related
+/** Timestamp sync mode define. */
+typedef enum {
+  kTimestampTypeNoSync = 0, /**< No sync signal mode. */
+  kTimestampTypeGptpOrPtp = 1,    /**< gPTP or PTP sync mode */
+  kTimestampTypeGps = 2   /**< GPS sync mode. */
+} TimestampType;
+
+/** Lidar connect state */
+typedef enum {
+  kConnectStateOff = 0,
+  kConnectStateOn = 1,
+  kConnectStateConfig = 2,
+  kConnectStateSampling = 3,
+} LidarConnectState;
+
+/** Device data source type */
+typedef enum {
+  kSourceRawLidar = 0, /**< Data from raw lidar. */
+  kSourceRawHub = 1,   /**< Data from lidar hub. */
+  kSourceLvxFile,      /**< Data from parse lvx file. */
+  kSourceUndef,
+} LidarDataSourceType;
+
+typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
+
+typedef enum {
+  kConfigDataType = 1 << 0,
+  kConfigScanPattern = 1 << 1,
+  kConfigBlindSpot = 1 << 2,
+  kConfigDualEmit = 1 << 3,
+  kConfigUnknown
+} LivoxLidarConfigCodeBit;
+
+typedef enum {
+  kNoneExtrinsicParameter,
+  kExtrinsicParameterFromLidar,
+  kExtrinsicParameterFromXml
+} ExtrinsicParameterType;
+
+typedef struct {
+ uint8_t lidar_type {};
+} LidarSummaryInfo;
+
+/** 8bytes stamp to uint64_t stamp */
+typedef union {
+  struct {
+    uint32_t low;
+    uint32_t high;
+  } stamp_word;
+
+  uint8_t stamp_bytes[8];
+  int64_t stamp;
+} LdsStamp;
+
+#pragma pack(1)
+
+typedef struct {
+  float x;            /**< X axis, Unit:m */
+  float y;            /**< Y axis, Unit:m */
+  float z;            /**< Z axis, Unit:m */
+  float reflectivity; /**< Reflectivity   */
+  uint8_t tag;        /**< Livox point tag   */
+  uint8_t line;       /**< Laser line id     */
+  double timestamp;   /**< Timestamp of point*/
+} LivoxPointXyzrtlt;
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+  float intensity;
+  uint8_t tag;
+  uint8_t line;
+  uint64_t offset_time;
+} PointXyzlt;
+
+typedef struct {
+  uint32_t handle;
+  uint8_t lidar_type; ////refer to LivoxLidarType
+  uint32_t points_num;
+  PointXyzlt* points;
+} PointPacket;
+
+typedef struct {
+  uint64_t base_time[kMaxSourceLidar] {};
+  uint8_t lidar_num {};
+  PointPacket lidar_point[kMaxSourceLidar] {};
+} PointFrame;
+
+#pragma pack()
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  uint64_t base_time;
+  uint32_t points_num;
+  std::vector<PointXyzlt> points;
+} StoragePacket;
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  bool extrinsic_enable;
+  uint32_t point_num;
+  uint8_t data_type;
+  uint8_t line_num;
+  uint64_t time_stamp;
+  uint64_t point_interval;
+  std::vector<uint8_t> raw_data;
+} RawPacket;
+
+typedef struct {
+  StoragePacket *storage_packet;
+  volatile uint32_t rd_idx;
+  volatile uint32_t wr_idx;
+  uint32_t mask;
+  uint32_t size; /**< must be power of 2. */
+} LidarDataQueue;
+
+/*****************************/
+/* About Extrinsic Parameter */
+typedef struct {
+  float roll;  /**< Roll angle, unit: degree. */
+  float pitch; /**< Pitch angle, unit: degree. */
+  float yaw;   /**< Yaw angle, unit: degree. */
+  int32_t x;   /**< X translation, unit: mm. */
+  int32_t y;   /**< Y translation, unit: mm. */
+  int32_t z;   /**< Z translation, unit: mm. */
+} ExtParameter;
+
+typedef float TranslationVector[3]; /**< x, y, z translation, unit: mm. */
+typedef float RotationMatrix[3][3];
+
+typedef struct {
+  TranslationVector trans;
+  RotationMatrix rotation;
+} ExtParameterDetailed;
+
+typedef struct {
+  LidarProtoType lidar_type;
+  uint32_t handle;
+  ExtParameter param;
+} LidarExtParameter;
+
+/** Configuration in json config file for livox lidar */
+typedef struct {
+  char broadcast_code[16];
+  bool enable_connect;
+  bool enable_fan;
+  uint32_t return_mode;
+  uint32_t coordinate;
+  uint32_t imu_rate;
+  uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
+} UserRawConfig;
+
+typedef struct {
+  bool enable_fan;
+  uint32_t return_mode;
+  uint32_t coordinate;              /**< 0 for CartesianCoordinate; others for SphericalCoordinate. */
+  uint32_t imu_rate;
+  uint32_t extrinsic_parameter_source;
+  bool enable_high_sensitivity;
+  volatile uint32_t set_bits;
+  volatile uint32_t get_bits;
+} UserConfig;
+
+typedef struct {
+  uint32_t handle;
+  int8_t pcl_data_type;
+  int8_t pattern_mode;
+  int32_t blind_spot_set;
+  int8_t dual_emit_en;
+  ExtParameter extrinsic_param;
+  volatile uint32_t set_bits;
+  volatile uint32_t get_bits;
+} UserLivoxLidarConfig;
+
+/** Lidar data source info abstract */
+typedef struct {
+  uint8_t lidar_type;
+  uint32_t handle;
+  // union {
+  //   uint8_t slot : 4; //slot for LivoxLidarType::kVehicleLidarType
+  //   uint8_t handle : 4;  // handle for LivoxLidarType::kIndustryLidarType
+  // };
+  uint8_t data_src;                  /**< From raw lidar or livox file. */
+  volatile LidarConnectState connect_state;
+  // DeviceInfo info;
+
+  LidarDataQueue data;
+  LidarImuDataQueue imu_data;
+
+  uint32_t firmware_ver; /**< Firmware version of lidar  */
+  UserLivoxLidarConfig livox_config;
+} LidarDevice;
+
+constexpr uint32_t kMaxProductType = 10;
+constexpr uint32_t kDeviceTypeLidarMid70 = 6;
+
+/***********************************/
+/* Global function for general use */
+bool IsFilePathValid(const char *path_str);
+uint32_t CalculatePacketQueueSize(const double publish_freq);
+std::string IpNumToString(uint32_t ip_num);
+uint32_t IpStringToNum(std::string ip_string);
+std::string ReplacePeriodByUnderline(std::string str);
+
+} // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER2_COMM_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/ldq.cpp

@@ -0,0 +1,150 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include <stdio.h>
+#include <string.h>
+
+#include "ldq.h"
+
+namespace livox_ros {
+
+/* for pointcloud queue process */
+bool InitQueue(LidarDataQueue *queue, uint32_t queue_size) {
+  if (queue == nullptr) {
+    // ROS_WARN("RosDriver Queue: Initialization failed - invalid queue.");
+    return false;
+  }
+
+  if (!IsPowerOf2(queue_size)) {
+    queue_size = RoundupPowerOf2(queue_size);
+    printf("Init queue, real query size:%u.\n", queue_size);
+  }
+
+  if (queue->storage_packet) {
+    delete[] queue->storage_packet;
+    queue->storage_packet = nullptr;
+  }
+
+  queue->storage_packet = new StoragePacket[queue_size];
+  if (queue->storage_packet == nullptr) {
+    // ROS_WARN("RosDriver Queue: Initialization failed - failed to allocate memory.");
+    return false;
+  }
+
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+  queue->size = queue_size;
+  queue->mask = queue_size - 1;
+
+  return true;
+}
+
+bool DeInitQueue(LidarDataQueue *queue) {
+  if (queue == nullptr) {
+    // ROS_WARN("RosDriver Queue: Deinitialization failed - invalid queue.");
+    return false;
+  }
+
+  if (queue->storage_packet) {
+    delete[] queue->storage_packet;
+  }
+
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+  queue->size = 0;
+  queue->mask = 0;
+
+  return true;
+}
+
+void ResetQueue(LidarDataQueue *queue) {
+  queue->rd_idx = 0;
+  queue->wr_idx = 0;
+}
+
+bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
+  if (queue == nullptr || storage_packet == nullptr) {
+    // ROS_WARN("RosDriver Queue: Invalid pointer parameters.");
+    return false;
+  }
+
+  if (QueueIsEmpty(queue)) {
+    // ROS_WARN("RosDriver Queue: Pop failed, since the queue is empty.");
+    return false;
+  }
+
+  uint32_t rd_idx = queue->rd_idx & queue->mask;
+
+  storage_packet->base_time = queue->storage_packet[rd_idx].base_time;
+  storage_packet->points_num = queue->storage_packet[rd_idx].points_num;
+  storage_packet->points.resize(queue->storage_packet[rd_idx].points_num);
+
+  memcpy(storage_packet->points.data(), queue->storage_packet[rd_idx].points.data(), (storage_packet->points_num) * sizeof(PointXyzlt));
+  return true;
+}
+
+void QueuePopUpdate(LidarDataQueue *queue) {
+  queue->rd_idx++;
+}
+
+bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet) {
+  if (!QueuePrePop(queue, storage_packet)) {
+    return false;
+  }
+  QueuePopUpdate(queue);
+
+  return true;
+}
+
+uint32_t QueueUsedSize(LidarDataQueue *queue) {
+  return queue->wr_idx - queue->rd_idx;
+}
+
+uint32_t QueueUnusedSize(LidarDataQueue *queue) {
+  return (queue->size - QueueUsedSize(queue));
+}
+
+bool QueueIsFull(LidarDataQueue *queue) {
+  return ((queue->wr_idx - queue->rd_idx) > queue->mask);
+}
+
+bool QueueIsEmpty(LidarDataQueue *queue) {
+  return (queue->rd_idx == queue->wr_idx);
+}
+
+uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time) {
+  uint32_t wr_idx = queue->wr_idx & queue->mask;
+  PointPacket* lidar_point_data = reinterpret_cast<PointPacket*>(data);
+  queue->storage_packet[wr_idx].base_time = base_time;
+  queue->storage_packet[wr_idx].points_num = lidar_point_data->points_num;
+
+  queue->storage_packet[wr_idx].points.clear();
+  queue->storage_packet[wr_idx].points.resize(lidar_point_data->points_num);
+  memcpy(queue->storage_packet[wr_idx].points.data(), lidar_point_data->points, sizeof(PointXyzlt) * (lidar_point_data->points_num));
+
+  queue->wr_idx++;
+  return 1;
+}
+
+}  // namespace livox_ros

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/ldq.h

@@ -0,0 +1,66 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LDQ_H_
+#define LIVOX_ROS_DRIVER_LDQ_H_
+
+#include <stdint.h>
+#include <vector>
+
+#include "comm/comm.h"
+
+namespace livox_ros {
+  
+inline static bool IsPowerOf2(uint32_t size) {
+  return (size != 0) && ((size & (size - 1)) == 0);
+}
+
+inline static uint32_t RoundupPowerOf2(uint32_t size) {
+  uint32_t power2_val = 0;
+  for (int i = 0; i < 32; i++) {
+    power2_val = ((uint32_t)1) << i;
+    if (size <= power2_val) {
+      break;
+    }
+  }
+
+  return power2_val;
+}
+
+/** queue operate function */
+bool InitQueue(LidarDataQueue *queue, uint32_t queue_size);
+bool DeInitQueue(LidarDataQueue *queue);
+void ResetQueue(LidarDataQueue *queue);
+bool QueuePrePop(LidarDataQueue *queue, StoragePacket *storage_packet);
+void QueuePopUpdate(LidarDataQueue *queue);
+bool QueuePop(LidarDataQueue *queue, StoragePacket *storage_packet);
+uint32_t QueueUsedSize(LidarDataQueue *queue);
+uint32_t QueueUnusedSize(LidarDataQueue *queue);
+bool QueueIsFull(LidarDataQueue *queue);
+bool QueueIsEmpty(LidarDataQueue *queue);
+uint32_t QueuePushAny(LidarDataQueue *queue, uint8_t *data, const uint64_t base_time);
+
+}  // namespace livox_ros
+
+#endif // LIVOX_ROS_DRIVER_LDQ_H_

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/lidar_imu_data_queue.cpp

@@ -0,0 +1,70 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lidar_imu_data_queue.h"
+
+namespace livox_ros {
+
+void LidarImuDataQueue::Push(ImuData* imu_data) {
+  ImuData data;
+  data.lidar_type = imu_data->lidar_type;
+  data.handle = imu_data->handle;
+  data.time_stamp = imu_data->time_stamp;
+
+  data.gyro_x = imu_data->gyro_x;
+  data.gyro_y = imu_data->gyro_y;
+  data.gyro_z = imu_data->gyro_z;
+
+  data.acc_x = imu_data->acc_x;
+  data.acc_y = imu_data->acc_y;
+  data.acc_z = imu_data->acc_z;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  imu_data_queue_.push_back(std::move(data));
+}
+
+bool LidarImuDataQueue::Pop(ImuData& imu_data) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (imu_data_queue_.empty()) {
+    return false;
+  }
+  imu_data = imu_data_queue_.front();
+  imu_data_queue_.pop_front();
+  return true;
+}
+
+bool LidarImuDataQueue::Empty() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return imu_data_queue_.empty();
+}
+
+void LidarImuDataQueue::Clear() {
+  std::list<ImuData> tmp_imu_data_queue;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    imu_data_queue_.swap(tmp_imu_data_queue);
+  }
+}
+
+} // namespace livox_ros

src/rm_nav/rm_driver/livox_ros_driver2/src/src/comm/lidar_imu_data_queue.h

@@ -0,0 +1,77 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+#define LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+
+#include <list>
+#include <mutex>
+#include <cstdint>
+
+namespace livox_ros {
+
+// Based on the IMU Data Type in Livox communication protocol
+// TODO: add a link to the protocol
+typedef struct {
+  float gyro_x;        /**< Gyroscope X axis, Unit:rad/s */
+  float gyro_y;        /**< Gyroscope Y axis, Unit:rad/s */
+  float gyro_z;        /**< Gyroscope Z axis, Unit:rad/s */
+  float acc_x;         /**< Accelerometer X axis, Unit:g */
+  float acc_y;         /**< Accelerometer Y axis, Unit:g */
+  float acc_z;         /**< Accelerometer Z axis, Unit:g */
+} RawImuPoint;
+
+typedef struct {
+  uint8_t lidar_type;
+  uint32_t handle;
+  uint8_t slot;
+  // union {
+  //   uint8_t handle;
+  //   uint8_t slot;
+  // };
+  uint64_t time_stamp;
+  float gyro_x;        /**< Gyroscope X axis, Unit:rad/s */
+  float gyro_y;        /**< Gyroscope Y axis, Unit:rad/s */
+  float gyro_z;        /**< Gyroscope Z axis, Unit:rad/s */
+  float acc_x;         /**< Accelerometer X axis, Unit:g */
+  float acc_y;         /**< Accelerometer Y axis, Unit:g */
+  float acc_z;         /**< Accelerometer Z axis, Unit:g */
+} ImuData;
+
+class LidarImuDataQueue {
+ public:
+  void Push(ImuData* imu_data);
+  bool Pop(ImuData& imu_data);
+  bool Empty();
+  void Clear();
+
+ private:
+  std::mutex mutex_;
+  std::list<ImuData> imu_data_queue_;
+};
+
+} // namespace
+
+#endif // LIVOX_ROS_DRIVER_LIDAR_IMU_DATA_QUEUE_H_
+