[real] 整理最终比赛 ROS 2 Sim2Real 部署

2026-07-27 16:09:32 +08:00
286 changed files with 222799 additions and 857378 deletions
@@ -29,8 +29,10 @@ log/
 !05_software/real/sim2real/vendored/odin1_imu/build/
 !05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
 !05_software/real/sim2real/vendored/odin1_imu/lib/*.a
-!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
+
-!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
+# Required vendored Odin SDK libraries in the final ROS 2 deployment
 !05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_arm.a
 !05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_amd.a
 # Training outputs
 logs/
@@ -14,53 +14,7 @@
 | `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
 | `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
 | `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
-| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
+| `v0.9.0` | 最终比赛部署 | ROS 2/C++ 真机闭环、Odin、CAN、导航与屏幕 UI |
 | `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
 | `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
 | `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 |
 | `v0.12.0` | 里程计导航联调 | 纯里程计 fallback、A_min 路线、TF 冲突保护和 model_9600 |
 > 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
 ## `v0.9.0` 的 Python Sim2Real v2
 - 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。
 - 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。
 - 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
 - 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
 ## `v0.10.0` 的 ROS 2/C++ Sim2Real 初版
 - 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。
 - 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。
 - 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。
 - 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
 - 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
 ## `v0.11.0` 的 ROS 2 Sim2Real v2
 - 归档 `real/sim2real_ros2_v2`，保持 `v0.10.0` 的 ROS 2/C++ 控制契约。
 - 增加 `simple_nav_node.py`、PCD 点击工具、任务点/任务序列配置和 Web 导航控制入口。
 - 默认命令源从遥控切换为 `NAV`，加入简单导航状态、PCD 位姿和地图显示链路。
 - 原始 `map1.pcd`、`map6.pcd` 分别约 49.05 MiB、44.24 MiB，归档时确定性抽样到 10 MB 以下并记录哈希。
 - 原始快照中的 Odin 驱动仍为空目录；排除设备运行日志和开发草稿。
 ## `v0.11.1` 的 Odin、TensorRT 与站姿调参
 - 归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，在同一 `sim2real_ros2_v2` 目录中记录真实差异。
 - 首次随部署工程保留完整 Odin ROS 驱动、Apache-2.0 许可证、设备标定参数和预编译 SDK 静态库。
 - 策略运行时增加 TensorRT、ONNX 回退、Rough/Crawl 模式切换、事件日志和更完整的电机失效诊断。
 - 默认 Rough 策略为 `NEWmodel_1900`，默认站姿为髋俯仰 `0.670`、膝关节 `-1.390`；Crawl 配置使用 IK 后端。
 - `map_b.pcd` 从 1,080,047 点确定性抽样为 270,012 点，并保留原始和抽样哈希。
 - 排除嵌套 Git、Odin 运行日志、缓存、开发草稿和未被配置引用的候选策略。
 ## `v0.12.0` 的里程计导航联调
 - 归档 `real/sim2real_ros2_v2(odom)`，继续沿用 `sim2real_ros2_v2` 目录的线性演进。
 - 将 Rough 策略切换为 `model_9600`，默认站姿恢复为髋俯仰 `0.550`、膝关节 `-1.125`。
 - Odin `custom_map_mode` 固定为纯里程计，加入 odom 新鲜度、外部 map/odom TF 冲突和任务结束交接保护。
 - 增加 A_min 路线、PCD 地图编辑工具和三份抽样点云；原始大 PCD 不直接进入 Git。
 - 保留完整 Odin 驱动、标定参数和 SDK 静态库；未找到的 `map_a.bin` 仍不伪造，重定位闭环不在本 Tag 声称已复现。
 ## `v0.4.0` 的模型变化
@@ -118,3 +72,13 @@
 - 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
 - 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
 - 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。
 ## `v0.9.0` 的最终比赛部署
 - 归档比赛得分 1050 所对应的 `last_not_slalom_1050` ROS 2 工作区；1050 是成绩，不是策略编号。
 - 保留 53D→16D C++ 策略运行时、200 Hz CAN 硬件桥、命令仲裁、安全监控和统一启动包。
 - 保留 Rough `model_6800`、Wall `model_84` 的 ONNX 与比赛 TensorRT engine；Crawl 使用 IK 后端。
 - 保留 Odin ROS 驱动及 Apache-2.0 许可证、五份比赛路线、抽样 PCD 和 Orin 触控屏 UI。
 - 排除嵌套 Git、缓存、日志、备份、候选模型、开发草稿和重复地图工具。
 - 原始备份缺少配置所引用的 Odin `1hao.bin`，因此重定位模式仍需从比赛设备补回该外部资产；纯里程计模式不受此限制。
 - 自研 ROS 包仍保留原工程的 `Proprietary` 清单字段，公开到 GitHub 前必须由权利人统一选择开源许可证。
@@ -9,9 +9,7 @@
 └─ real/
   ├─ ik_real/        # IK 轨迹与早期真机控制
   ├─ sim2real/       # 第一代 Python 策略真机部署
-   ├─ sim2real_v2/    # Python Sim2Real v2
+   └─ sim2real_ros2/  # 最终比赛 ROS 2/C++ 真机部署
   ├─ sim2real_ros2/  # ROS 2/C++ Sim2Real 初版
   └─ sim2real_ros2_v2/ # ROS 2 导航原型及后续演进
 ```
 ## 数据流
@@ -26,14 +24,14 @@ MJCF + mjlab task
        |
        +----> Sim2Sim 策略验证
        |
-        +----> Python Sim2Real / v2 ----> 电机 / IMU
+        +----> Python Sim2Real ----> 电机 / IMU
        |
-        +----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
+        +----> ROS 2/C++ Sim2Real -> CAN / Odin / 导航 / 屏幕
 IK real --------------------------------> 电机
 ```
-`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`，纯里程计导航联调对应 `v0.12.0`。
+`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，最终比赛 ROS 2 部署见 `v0.9.0`。
 详细说明见：
@@ -1,6 +1,6 @@
-# 真机控制版本演进
+# 真机控制与部署
-本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
+本目录保存 16DOF 轮足机器人从早期接口验证到最终比赛 ROS 2 部署的演进。
 ## `ik_real`
@@ -20,25 +20,17 @@
 部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
 ## `sim2real_v2`
 Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。
 部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
 ## `sim2real_ros2`
-ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。
+`last_not_slalom_1050` 最终比赛工程的规范化归档，包含：
-原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
+- ROS 2 Humble + C++ 运行时
 - 50 Hz 策略推理与 200 Hz CAN 电机热路径
 - Rough `model_6800`、Wall `model_84` 和 Crawl IK 模式
 - Odin IMU/里程计驱动、简单导航、命令仲裁和触控屏 UI
 - 比赛路线、抽样 PCD、Docker 与部署说明
-## `sim2real_ros2_v2`
+`1050` 是比赛得分，不是模型编号。完整入口与缺失的 Odin 重定位地图边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
 ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。该版本对应重排主线的 `v0.11.0`。
 `v0.11.1` 在同一目录继续演进，首次随工程归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。各 Tag 可恢复对应阶段，当前目录说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
 `v0.12.0` 继续在同一目录保存 odom 快照，固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。
 ## 实机记录
@@ -1,6 +1,14 @@
 build/
 install/
 log/
 logs_v2_web/
 map/load/
 src/odin_ros_driver/log/
 src/odin_ros_driver/recorddata/
 src/odin_ros_driver/image/
 *.bak_*
 __pycache__/
 *.py[cod]
 .colcon/
 .vscode/
 compile_commands.json
@@ -156,7 +156,7 @@ sudo udevadm trigger
 统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
 * `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
-* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
+* `launch_nav2`（默认：`false`）：按需启动 ROS2 Navigation2；比赛默认使用 `simple_nav_node.py` 的路线跟踪。
 #### 1. 完整真实硬件闭环（默认）
 启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
@@ -175,4 +175,3 @@ ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driv
 ```bash
 ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
 ```
@@ -62,6 +62,7 @@ COPY src/sim2real_nav2 sim2real_nav2
 # 拷贝策略文件与运行脚本
 WORKDIR /sim2real_ws
 COPY policies policies
 COPY map map
 COPY start_sim2real.sh start_sim2real.sh
 RUN chmod +x start_sim2real.sh
@@ -1,74 +1,104 @@
-# ROS 2/C++ Sim2Real 初版
+# ROS 2 最终比赛 Sim2Real
-本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
+本目录归档 `last_not_slalom_1050` 真机工程，对应 RC_WheelLeg 在 RoboCon 仿生足式障碍赛使用的最终 ROS 2 部署栈。`1050` 是比赛得分，不是模型编号；比赛 Rough 策略为 `model_6800.onnx`。
-本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
+该里程碑计划标记为 `v0.9.0`。训练架构和策略来源见 `v0.6.0`，比赛 Rough 模型首次归档见 `v0.8.0`，导航打点与路线演进见 `v0.8.1`。
- `53D` 策略观测契约不变
+## 系统闭环
 - `16D` 动作契约不变
 - `50Hz` 策略循环与训练对齐
 - `200Hz` 电机循环为专用 C++ 热路径
 - ROS 2 作为导航、TF、诊断和启动管理的系统集成层
 ## 工作区布局
 - `src/sim2real_interfaces`
  硬件桥接与策略运行时共享的 ROS 2 消息定义。
 - `src/sim2real_common`
  共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。
 - `src/sim2real_hw`
  面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。
 - `src/sim2real_runtime`
  策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。
  同时包含 `odom_relay_node`（里程计中继与 TF 广播）。
 - `src/sim2real_nav2`
  ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。
 - `src/sim2real_bringup`
  统一启动文件与运行时参数配置。
 - `src/odin_ros_driver`
  Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。
 - `docs`
  架构说明与迁移计划。
 ## 目标架构
 ```text
-Odin / IMU / Odom  --->  sim2real_hw  --->  sim2real_runtime  --->  sim2real_hw
+Odin IMU / Odom ──> hardware bridge ──> RuntimeState
-                              |                  |                    |
+                                              |
-                              v                  v                    v
+导航 / 遥控 / 屏幕 ──> cmd mux ──> policy runtime (50 Hz)
-                         RuntimeState       RuntimeTarget          电机 CAN 指令
+                                              |
-                              |                  |
+                                      RuntimeTarget
-                              +-------> 诊断 / 遥测
+                                              |
-
+                              hardware bridge / CAN (200 Hz)
 Nav2 / cmd_vel  ------------------------------>  sim2real_runtime
                   (经 odom_relay_node 提供 odom→base_link TF)
 ```
-## 当前状态
+核心约束：
-已完成 Phase 0-5 的全部迁移：
+- 53 维策略观测、16 维动作输出。
 - Rough：`model_6800`，优先 TensorRT，失败时回退 ONNX Runtime。
 - Wall：`model_84`，同样保留 TensorRT 与 ONNX 两种文件。
 - Crawl：比赛配置使用解析 IK，不加载 Crawl RL 权重。
 - 默认站姿：髋俯仰 `0.550`、膝关节 `-1.125`。
 - 默认命令源：`NAV`；默认定位模式：`relocal`。
-1. ✅ 冻结部署契约（deployment_contract.hpp）
+## 目录
 2. ✅ ROS 2 包结构搭建
 3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
 4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
 5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
-## 契约来源
+```text
 sim2real_ros2/
 ├─ src/
 │  ├─ sim2real_interfaces/  # RuntimeState / RuntimeTarget 消息
 │  ├─ sim2real_common/      # 部署契约、滤波、平衡和安全监控
 │  ├─ sim2real_hw/          # SocketCAN、IMU 和 200 Hz 电机热路径
 │  ├─ sim2real_runtime/     # 策略、命令仲裁、导航、Web API
 │  ├─ sim2real_nav2/        # Nav2 配置入口
 │  ├─ sim2real_bringup/     # 统一参数和启动文件
 │  └─ odin_ros_driver/      # Odin ROS 驱动（Apache-2.0）
 ├─ policies/                # 比赛实际使用的 Rough / Wall 模型
 ├─ map/                     # 比赛路线和抽样 PCD
 ├─ screen/                  # Orin 800×600 触控面板
 ├─ docs/                    # 架构、遥控、Web 和迁移说明
 ├─ Dockerfile
 └─ start_sim2real.sh
 ```
-迁移过程中以下文件被视为真值源：
+## 构建与运行
- `sim2real/deployment_manifest.yaml`
+目标环境是 Ubuntu 22.04、ROS 2 Humble 和 Jetson Orin。系统依赖和 Docker 流程见 [`DEPLOYMENT_GUIDE.md`](DEPLOYMENT_GUIDE.md)。
 - `sim2real/interface/motor_mapping.py`
 - `sim2real/interface/real_io.py`
 - `sim2real/policy/policy_runner.py`
 - `sim2real/web/session.py`
-## 注意事项
+```bash
 cd 05_software/real/sim2real_ros2
 colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 ./start_sim2real.sh
 ```
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
+运行参数和模型/路线均使用工作区根目录相对路径，因此应从本目录启动。常用启动覆盖：
- Windows 仅作为编辑环境使用。
+
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
+```bash
-  除非训练与部署同步更新。
+# 纯里程计模式，不等待 Odin 重定位地图
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
+./start_sim2real.sh localization_mode:=odom \
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
+  odin_config_file:=src/odin_ros_driver/config/control_command_odom.yaml
 # 禁止驱动，仅做软件链路检查
 ./start_sim2real.sh launch_driver:=false launch_remote:=false
 ```
 ## 必须补充的部署资产
 最终源目录配置引用了 Odin `map/1hao.bin`，但工作区备份中不存在这个文件；全盘检索也未找到同名文件。为避免用来源不明的 `.bin` 冒充比赛地图，本仓库不伪造该资产。
 使用 `relocal` 前必须：
 1. 从比赛 Orin 或 Odin 建图备份取得真实 `1hao.bin`。
 2. 修改 `src/odin_ros_driver/config/control_command_relocal.yaml` 中的 `relocalization_map_abs_path` 为目标机绝对路径。
 3. 核对文件哈希并在发布说明中补充来源。
 缺少该文件时请使用 `localization_mode:=odom`，不要宣称重定位闭环已复现。地图和路线边界见 [`map/README.md`](map/README.md)。
 ## 归档边界
 已保留：
 - 最终六个 ROS 2 包、Odin 驱动源码、比赛设备标定参数和预编译 SDK 静态库。
 - 最终 Rough/Wall ONNX 与比赛机 TensorRT engine。
 - 五份最终工程路线、1 号场地抽样 PCD、屏幕 UI 和启动脚本。
 - Odin 驱动 Apache-2.0 许可证。
 未保留：
 - 嵌套 `.git`、`__pycache__`、日志、备份、构建/安装目录。
 - 未被比赛配置引用的候选模型与候选 TensorRT engine。
 - 开发计划、任务草稿、重复地图工具和运行时轨迹。
 - 原备份中大小为 0 的浏览器静态页面；HTTP JSON API 和屏幕 UI 源码仍保留。
 TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关；其他机器应从同名 ONNX 重新生成，不应默认复用比赛 engine。模型哈希见 [`policies/README.md`](policies/README.md)。
 ## 安全与开源状态
 - 真机运行前必须架空轮组验证 CAN 映射、方向、零位、急停和限幅。
 - `deployment_contract.hpp` 是电机映射和动作缩放真值源；参考 YAML 不会自动修改 C++ 契约。
 - 自研 ROS 包的 `package.xml` 仍保留原工程的 `Proprietary` 字段。迁移到 GitHub 公共开源前，需要由项目负责人选择许可证并统一修改；本次整理不代替权利人作许可证决定。
 - 当前 Windows 环境只能做静态检查，不能证明 ROS 2、SocketCAN、Odin SDK 或 TensorRT 真机运行成功。
@@ -21,7 +21,7 @@ src/sim2real_runtime/src/remote_uart_node.py
 ## 2. 通道映射
-通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
+通道映射与本仓库第一代 Python Sim2Real 实现中的遥控器配置保持一致。
 | 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
 |---|---|---|---:|
@@ -64,6 +64,10 @@ remote_invert_vy: false
 remote_invert_yaw: true
 remote_publish_inactive_zero: true
 remote_estop_latch: true
 remote_estop_channel: 7
 remote_estop_level: "high"
 remote_estop_debounce_frames: 3
 remote_estop_require_remote_mode: true
 remote_poll_hz: 50.0
 ```
@@ -276,7 +280,7 @@ ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
 /safety/estop: true
 ```
-由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
+由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
 ### 8.4 机器人行为效果
@@ -0,0 +1,164 @@
 # Pure Pursuit Yaw检查优化 - 最终配置
 ## 🎯 核心问题
 **现象**: 机器人没转到位就开始前进 → 斜着撞杆子
 **根本原因**: Pure Pursuit的yaw检查不够严格
 ---
 ## ✅ 已修改的参数
 ### runtime.yaml
 ```yaml
 # 关键修改1: Script模式yaw门限
 nav_slalom_script_yaw_gate_deg: 8.0  # 从12度 → 8度
 # 关键修改2: 全局yaw容差
 nav_goal_yaw_tolerance_deg: 8.0      # 从12度 → 8度
 ```
 **作用**:
 - yaw偏差 > 8度时：只转向，不前进
 - yaw偏差 ≤ 8度时：才允许前进
 ---
 ## 📊 完整配置总结
 ### 1. 禁用干扰的规划器
 ```yaml
 nav_local_planner_enabled: false
 nav_astar_enabled: false
 ```
 ### 2. 速度和精度
 ```yaml
 nav_slalom_max_vx: 0.50
 nav_slalom_lookahead: 0.15
 nav_slalom_tolerance: 0.05
 ```
 ### 3. Yaw控制（新增）
 ```yaml
 nav_goal_yaw_tolerance_deg: 8.0
 nav_slalom_script_yaw_gate_deg: 8.0
 ```
 ---
 ## 🔍 工作原理
 ### Pure Pursuit算法流程
 ```
 1. 看前方lookahead距离的目标点
 2. 计算到目标的方向和距离
 3. 检查yaw偏差
   - 如果 yaw偏差 > yaw_gate (8度):
       只发wz转向，vx=0
   - 如果 yaw偏差 ≤ yaw_gate (8度):
       发vx前进 + wz微调
 4. 到达目标点，推进下一个
 ```
 ### 之前的问题
 ```
 yaw_gate = 12度（太宽松）
 ↓
 yaw偏差11度时就开始前进
 ↓
 还没对准就冲出去
 ↓
 斜着撞杆子
 ```
 ### 修改后
 ```
 yaw_gate = 8度（更严格）
 ↓
 yaw偏差必须≤8度才前进
 ↓
 基本对准后才移动
 ↓
 不会斜着撞
 ```
 ---
 ## 📁 使用的文件
 **路线**: `points_nav1007_optimized.json`
 - 总航点: 47
 - 绕杆航点: 21
 - 参数: speed=0.50, lookahead=0.15, tolerance=0.05
 **地形**: `tools/nav_tools/xml/A.xml`
 **配置**: `runtime.yaml` (已修改)
 ---
 ## 🚀 下一步
 ### 实机测试
 ```bash
 # 1. 重启系统加载新配置
 ros2 launch sim2real_bringup sim2real_system.launch.py
 # 2. 验证参数
 ros2 param get /sim2real_simple_nav_node nav_slalom_script_yaw_gate_deg
 # 应该显示: 8.0
 ros2 param get /sim2real_simple_nav_node nav_goal_yaw_tolerance_deg
 # 应该显示: 8.0
 # 3. 加载路线
 # 使用: sim2real_ros2_v2_ooo/map/routes/points_nav1007_optimized.json
 # 4. 监控
 ros2 topic echo /cmd_vel_nav
 # 观察: 转向时vx应该接近0，对准后才有vx速度
 ```
 ---
 ## 💡 预期效果
 **修改前**:
 - 机器人边转边进
 - yaw偏差大时仍有前进速度
 - 导致斜着撞杆子
 **修改后**:
 - 转向时几乎不前进（vx≈0）
 - 对准后才快速前进（vx=0.5）
 - 动作分离：先转向，后前进
 ---
 ## ⚠️ 如果还有问题
 ### 场景A: 还是斜着撞
 可能需要进一步收紧:
 ```yaml
 nav_slalom_script_yaw_gate_deg: 5.0  # 改为5度
 ```
 ### 场景B: 太慢，一直在转
 说明yaw_gate太严格:
 ```yaml
 nav_slalom_script_yaw_gate_deg: 10.0  # 放宽到10度
 ```
 ### 场景C: 卡顿
 检查是否在等待yaw对准:
 ```bash
 ros2 topic echo /simple_nav/status
 # 看是否一直在"aligning"状态
 ```
 ---
 **配置已优化完成，ready for testing!** 🎯
@@ -1,20 +0,0 @@
 Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150, 
        0.002390, -0.000340, -1.000000, -0.011850, 
        0.999960, -0.009160, 0.002390, 0.005360, 
        0.000000, 0.000000, 0.000000, 1.000000]
 cam_0: 
   image_width: 1600
   image_height: 1296
   k2: 0.000656
   k3: -0.028961
   k4: 0.045390
   k5: -0.064513
   k6: 0.038735
   k7: -0.009903
   p1: 0.000000
   p2: 0.000000
   A11: 736.894262
   A12: -0.161150
   A22: 736.611354
   u0: 806.125535
   v0: 639.650710
@@ -0,0 +1,11 @@
 # 比赛地图与路线
 `routes/` 保存 `last_not_slalom_1050` 工程中的五份路线快照，保留原文件名以维持运行时配置和版本演进关系。默认运行路线是 `routes/1hao_reall.json`。
 `1hao.pcd` 是 `v0.8.1` 导航工具中同一份抽样点云，包含 199,215 点、大小 9,876,010 字节，SHA-256 为 `48B231C52BECA51316F352300C8B2046133E92359E0855227D93DEB0D927AD34`。它用于本地规划和路线显示，不替代原始高密度点云。
 ## 缺失的 Odin 重定位地图
 比赛配置需要 Odin 专用二进制地图 `1hao.bin`，但源备份没有该文件。源目录中另有两个名称和时间不同的 `.bin`，无法证明它们就是比赛使用地图，因此没有复制或重命名。
 重定位部署时应从比赛设备取回真实文件，并把 `control_command_relocal.yaml` 中的占位绝对路径改为目标机实际位置。纯里程计模式不需要该文件。
@@ -0,0 +1,14 @@
 # 比赛部署策略
 这里只保留最终运行配置实际使用的 Rough 和 Wall 策略。Crawl 在比赛配置中使用 IK 后端，因此不归档未使用的 Crawl RL engine。
 | 用途 | 文件 | SHA-256 |
 | --- | --- | --- |
 | Rough ONNX | `model_6800.onnx` | `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13` |
 | Rough TensorRT | `model_6800_fp16.engine` | `BDC6583AFD594E84D9A4AAA4BB2EEE279C7EBF207A57CF4802A2DF5309C5F663` |
 | Wall ONNX | `model_84.onnx` | `E3A447782DF6C6E11E66C3ACBE41697FF88EBF5E75DCAB65EFCF9A2EB1CA639F` |
 | Wall TensorRT | `model_84_fp16.engine` | `0FE2001113B7146B589D7A62A6E54BAD68574765EBA45782FB41A26F65A8721D` |
 `model_6800.onnx` 与 `05_software/train/rc_mjlab/model_6800.onnx` 哈希一致。此处重复保留是为了让真机工作区可以独立部署。
 TensorRT 文件只作为比赛机历史产物；更换 JetPack、TensorRT 或 GPU 后应从 ONNX 重新构建并重新核对数值误差。
@@ -0,0 +1,22 @@
 # Orin 触控屏控制面板
 `fullscreen_quit.py` 是比赛 Orin 外接 `800×600` 屏幕使用的控制面板，通过本机 `http://127.0.0.1:18080/api/*` 调用 ROS 2 Web bridge，不建立第二套控制协议。
 ```bash
 cd <sim2real_ros2工作区>
 DISPLAY=:0 python3 screen/fullscreen_quit.py
 ```
 程序默认从脚本父目录自动确定工作区，也可设置 `SIM2REAL_REPO_DIR` 覆盖。显示权限检查：
 ```bash
 python3 screen/check_display.py
 ```
 自启动脚本：
 - `install_autostart.sh`：图形桌面登录后启动。
 - `install_boot_service.sh <用户名>`：安装 systemd 服务。
 - `uninstall_boot_service.sh`：卸载服务。
 使用屏幕启动系统前，先完成工作区构建、Odin 配置、CAN 映射检查和急停验证。
@@ -0,0 +1,114 @@
 #!/usr/bin/env python3
 """
 Print display-related diagnostics for launching the fullscreen quit screen.
 """
 import glob
 import getpass
 import os
 import shlex
 import subprocess
 from typing import List, Optional
 def run(command: List[str]) -> str:
    try:
        result = subprocess.run(
            command,
            check=False,
            capture_output=True,
            text=True,
        )
    except OSError as exc:
        return f"<failed: {exc}>"
    output = result.stdout.strip()
    error = result.stderr.strip()
    if error:
        return f"{output}\n{error}".strip()
    return output
 def current_user() -> str:
    try:
        return getpass.getuser()
    except OSError:
        return os.environ.get("USER", "unknown")
 def xauth_from_x_processes() -> List[str]:
    output = run(["ps", "-eo", "args"])
    candidates = []
    for line in output.splitlines():
        if "Xorg" not in line and "Xwayland" not in line:
            continue
        try:
            parts = shlex.split(line)
        except ValueError:
            parts = line.split()
        for index, part in enumerate(parts[:-1]):
            if part == "-auth":
                candidates.append(parts[index + 1])
    return candidates
 def print_path_status(label: str, path: Optional[str]) -> None:
    if not path:
        print(f"{label}: <unset>")
        return
    exists = os.path.exists(path)
    readable = os.access(path, os.R_OK) if exists else False
    print(f"{label}: {path} exists={exists} readable={readable}")
 def main() -> int:
    uid = os.getuid()
    print(f"user: {run(['whoami'])}")
    print(f"uid: {uid}")
    print(f"DISPLAY: {os.environ.get('DISPLAY', '<unset>')}")
    print(f"XAUTHORITY: {os.environ.get('XAUTHORITY', '<unset>')}")
    print()
    print("candidate XAUTHORITY files:")
    candidates = [
        os.environ.get("XAUTHORITY"),
        *xauth_from_x_processes(),
        os.path.expanduser("~/.Xauthority"),
        f"/run/user/{uid}/gdm/Xauthority",
        f"/run/user/{uid}/Xauthority",
        *glob.glob(f"/run/user/{uid}/*Xauthority*"),
    ]
    seen = set()
    for candidate in candidates:
        key = candidate or "<unset>"
        if key in seen:
            continue
        seen.add(key)
        print_path_status("  -", candidate)
    print()
    print("X server processes:")
    x_lines = [
        line
        for line in run(["ps", "-eo", "user,args"]).splitlines()
        if "Xorg" in line or "Xwayland" in line
    ]
    if x_lines:
        for line in x_lines:
            print(f"  {line}")
    else:
        print("  <none found>")
    print()
    print("recommended SSH launch:")
    print("  cd <workspace>/screen")
    print("  bash run_fullscreen_quit.sh")
    print()
    print("if authorization fails, run this on the Nano desktop terminal once:")
    print(f"  DISPLAY=:0 xhost +SI:localuser:{current_user()}")
    return 0
 if __name__ == "__main__":
    raise SystemExit(main())
@@ -0,0 +1,27 @@
 #!/usr/bin/env bash
 set -euo pipefail
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 AUTOSTART_DIR="$HOME/.config/autostart"
 DESKTOP_FILE="$AUTOSTART_DIR/sim2real-screen.desktop"
 mkdir -p "$AUTOSTART_DIR"
 chmod +x "$SCRIPT_DIR/run_fullscreen_quit.sh"
 cat > "$DESKTOP_FILE" <<EOF
 [Desktop Entry]
 Type=Application
 Name=sim2real Screen
 Comment=Start the sim2real touch control panel
 Exec=$SCRIPT_DIR/run_fullscreen_quit.sh
 Path=$SCRIPT_DIR
 Terminal=false
 X-GNOME-Autostart-enabled=true
 StartupNotify=false
 EOF
 echo "Installed desktop autostart:"
 echo "  $DESKTOP_FILE"
 echo
 echo "It will start after this user logs into the graphical desktop."
 echo "For boot-time use, enable automatic login for this user on the Orin desktop."
@@ -0,0 +1,73 @@
 #!/usr/bin/env bash
 set -euo pipefail
 if ! command -v systemctl >/dev/null 2>&1; then
    echo "systemctl not found; this installer is for systemd-based Linux." >&2
    exit 1
 fi
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 SERVICE_NAME="sim2real-screen.service"
 SERVICE_PATH="/etc/systemd/system/$SERVICE_NAME"
 if [ "${EUID:-$(id -u)}" -eq 0 ]; then
    RUN_USER="${1:-${SUDO_USER:-rc2}}"
 else
    RUN_USER="${1:-$(id -un)}"
 fi
 if ! id "$RUN_USER" >/dev/null 2>&1; then
    echo "User '$RUN_USER' does not exist. Usage: bash install_boot_service.sh rc2" >&2
    exit 1
 fi
 RUN_GROUP="$(id -gn "$RUN_USER")"
 RUN_HOME="$(getent passwd "$RUN_USER" | cut -d: -f6)"
 AUTOSTART_FILE="$RUN_HOME/.config/autostart/sim2real-screen.desktop"
 chmod +x "$SCRIPT_DIR/run_fullscreen_quit.sh" "$SCRIPT_DIR/run_boot_screen_service.sh"
 if [ -f "$AUTOSTART_FILE" ]; then
    rm -f "$AUTOSTART_FILE"
    echo "Removed desktop autostart to avoid duplicate screen instances:"
    echo "  $AUTOSTART_FILE"
 fi
 SERVICE_CONTENT="[Unit]
 Description=sim2real touchscreen control panel
 Wants=display-manager.service
 After=systemd-user-sessions.service display-manager.service
 StartLimitIntervalSec=0
 [Service]
 Type=simple
 User=$RUN_USER
 Group=$RUN_GROUP
 WorkingDirectory=$SCRIPT_DIR
 Environment=HOME=$RUN_HOME
 Environment=DISPLAY=:0
 Environment=PYTHONUNBUFFERED=1
 ExecStart=$SCRIPT_DIR/run_boot_screen_service.sh
 Restart=always
 RestartSec=2
 KillSignal=SIGINT
 TimeoutStopSec=20
 [Install]
 WantedBy=graphical.target
 "
 printf "%s" "$SERVICE_CONTENT" | sudo tee "$SERVICE_PATH" >/dev/null
 sudo systemctl daemon-reload
 sudo systemctl enable "$SERVICE_NAME"
 echo "Installed and enabled:"
 echo "  $SERVICE_PATH"
 echo
 echo "Start now:"
 echo "  sudo systemctl restart $SERVICE_NAME"
 echo
 echo "Check status/logs:"
 echo "  systemctl status $SERVICE_NAME --no-pager"
 echo "  journalctl -u $SERVICE_NAME -f"
 echo
 echo "Important: the graphical desktop for user '$RUN_USER' must auto-login, otherwise Tk cannot open DISPLAY=:0."
@@ -0,0 +1,23 @@
 #!/usr/bin/env bash
 set -euo pipefail
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 export DISPLAY="${DISPLAY:-:0}"
 export PYTHONUNBUFFERED=1
 echo "[sim2real-screen] service starting as $(id -un), DISPLAY=$DISPLAY"
 for _ in $(seq 1 120); do
    if [ -S "/tmp/.X11-unix/X${DISPLAY#:}" ]; then
        break
    fi
    sleep 1
 done
 if [ ! -S "/tmp/.X11-unix/X${DISPLAY#:}" ]; then
    echo "[sim2real-screen] X11 socket for DISPLAY=$DISPLAY not found" >&2
    exit 1
 fi
 exec "$SCRIPT_DIR/run_fullscreen_quit.sh"
@@ -0,0 +1,24 @@
 #!/usr/bin/env bash
 set -u
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 cd "$SCRIPT_DIR"
 export DISPLAY="${DISPLAY:-:0}"
 if [ -z "${XAUTHORITY:-}" ]; then
    UID_VALUE="$(id -u)"
    for candidate in \
        "$HOME/.Xauthority" \
        "/run/user/$UID_VALUE/gdm/Xauthority" \
        "/run/user/$UID_VALUE/Xauthority"
    do
        if [ -f "$candidate" ]; then
            export XAUTHORITY="$candidate"
            break
        fi
    done
 fi
 python3 fullscreen_quit.py
@@ -0,0 +1,11 @@
 #!/usr/bin/env bash
 set -euo pipefail
 SERVICE_NAME="sim2real-screen.service"
 SERVICE_PATH="/etc/systemd/system/$SERVICE_NAME"
 sudo systemctl disable --now "$SERVICE_NAME" 2>/dev/null || true
 sudo rm -f "$SERVICE_PATH"
 sudo systemctl daemon-reload
 echo "Removed $SERVICE_NAME"
@@ -0,0 +1,13 @@
 v0.11.0 2026_0618
 Required Minimum Firmware Version：0.12.0
 1.Reduce cloud slam latency
 2.Improve get mapping result 
 v0.10.5 2026_0525
 1.modify recorddata format，add device_id、algorithm_version key
 2.fix download map fail when in slam mode(USB2.0)
 3.fix upload map fail when in relocalization mode(USB2.0)
 v0.10.4 2026_0522
 1.fix usb2.0 heartBeat timeout to cause soft detaching
 2.add custom_init_pose_search_radius and custom_init_pose_max_rot_deg in control_command.yaml
@@ -144,12 +144,27 @@ if(ROS_VERSION STREQUAL "ROS1")
        cv_bridge
        tf
        image_transport
        message_generation
    )
-    
+
    # ---- AE/AWB debug services ----
    # Declare custom .srv files; catkin generates ros1 message headers for them.
    add_service_files(
        FILES
        GetAe.srv
        GetAwb.srv
        SetAe.srv
        SetAwb.srv
    )
    generate_messages(
        DEPENDENCIES
        std_msgs
    )
    include_directories(${catkin_INCLUDE_DIRS})
-    
+
    catkin_package(
-        CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport
+        CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport message_runtime
        INCLUDE_DIRS include
    )
@@ -174,6 +189,9 @@ if(ROS_VERSION STREQUAL "ROS1")
    	pthread  
        usb-1.0
    )
    # Make sure the AE/AWB srv headers are generated before host_sdk_sample
    # is compiled (catkin's generate_messages produces this target).
    add_dependencies(host_sdk_sample ${PROJECT_NAME}_generate_messages_cpp)
    add_library(pointcloud_depth_converter src/pointcloud_depth_converter.cpp)
    target_link_libraries(pointcloud_depth_converter
@@ -249,7 +267,19 @@ elseif(ROS_VERSION STREQUAL "ROS2")
    find_package(tf2_ros REQUIRED)
    find_package(geometry_msgs REQUIRED)
    find_package(tf2_geometry_msgs REQUIRED)
-    
+    find_package(ament_index_cpp REQUIRED)
    # ---- AE/AWB debug services ----
    # Generate C++ bindings for the 4 .srv files in srv/. The generated
    # headers are linked into host_sdk_sample so it can host the services.
    find_package(rosidl_default_generators REQUIRED)
    rosidl_generate_interfaces(${PROJECT_NAME}
        "srv/GetAe.srv"
        "srv/GetAwb.srv"
        "srv/SetAe.srv"
        "srv/SetAwb.srv"
    )
    # Create executable
    add_executable(host_sdk_sample 
        src/host_sdk_sample.cpp
@@ -264,7 +294,13 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        yaml-cpp
        usb-1.0
    )
-    
+
    # Link the generated AE/AWB srv typesupport into host_sdk_sample so it
    # can host the 4 debug services.
    rosidl_get_typesupport_target(cpp_typesupport_target
        ${PROJECT_NAME} "rosidl_typesupport_cpp")
    target_link_libraries(host_sdk_sample "${cpp_typesupport_target}")
    # Add ROS2 dependencies
    ament_target_dependencies(host_sdk_sample
        rclcpp 
@@ -278,6 +314,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        tf2_ros
        geometry_msgs
        tf2_geometry_msgs
        ament_index_cpp
    )
    add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
@@ -366,6 +403,9 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        pcd2depth_ros2_node
        cloud_reprojection_ros2_node
        image_overlay_node
        depth_image_ros2_node_lib
        pointcloud_depth_converter_ros2
        cloud_reprojector_ros2
        EXPORT export_${PROJECT_NAME}
        ARCHIVE DESTINATION lib
        LIBRARY DESTINATION lib
@@ -419,6 +459,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
        tf2_ros
        geometry_msgs
        tf2_geometry_msgs
        ament_index_cpp
    )
    ament_package()
@@ -1,5 +1,821 @@
-# Odin 驱动依赖占位
+# Odin_ROS_Driver Readme
-`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
+ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) 
-因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
+Odin1 wiki: https://manifoldtechltd.github.io/wiki/Odin1/Cover.html
 ## Odin_ROS_Driver
 Compatibility:
 ● ROS 1(LTS Release: Noetic recommended)
 ● ROS 2(LTS Release: Humble recommended)
 ## Important Notice:
 This driver package provides core functionality for point cloud SLAM applications and targets specific use cases. It is intended exclusively for technical professionals conducting secondary development. End users must perform scenario-specific optimization and custom development to align with operational requirements in practical deployment environments.
 ## 1. Version
 Current version: v0.12.0
 Required device firmware version: v0.12.0
 ## 2. Preparation
 ### 2.1 OS Requirement
 ● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy;
 ● Ubuntu 22.04 for ROS2 Humble;
 ● Ubuntu 18.04 is currently not supported;
 ● Ubuntu 24.04 is not officially supported but may work with some modifications.
 ### 2.2 Dependencies
 ● Opencv >= 4.2.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed)
 ● yaml-cpp
 ● thread
 ● OpenSSL
 ● Eigen3
 ### 2.3 Dependencies Install
 #### 2.3.1 System
 ```shell
 sudo apt update
 sudo apt-get install build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
 ```
 #### 2.3.2 yaml-cpp
 ```shell
 sudo apt update
 sudo apt install -y libyaml-cpp-dev
 ```
 #### 2.3.3 libusb
 ```shell
 sudo apt update
 sudo apt install -y libusb-1.0-0-dev
 ```
 #### 2.3.4 opencv
 ```shell
 sudo apt update
 sudo apt-get install libopencv-dev
 ```
 #### 2.3.4 ROS install
 For ROS Noetic installation, please refer to:
 [ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation)
 For ROS2 Foxy installation, please refer to:
 [ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
 For ROS2 Humble installation, please refer to:
 [ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
 ## 3. Preparation
 ### 3.1 Create Udev rules 
 ```shell
 sudo vim /etc/udev/rules.d/99-odin-usb.rules
 ```
 Add the following content to the 99-odin-usb.rules file
 ```shell
 SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
 ```
 Reload rules and reinsert devices
 ```shell
 sudo udevadm control --reload
 sudo udevadm trigger
 ```
 ### 3.2 OS Requirement
 ```shell
 git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
 ```
 Note:
 Please clone the source code into the "[ros_workspace]/src/" folder, otherwise compilation errors will occur.
 ### 3.3 make
 #### 3.3.1 ROS1 (Noetic for example):
 ```shell
 source /opt/ros/noetic/setup.bash
 ./script/build_ros.sh
 ```
 #### 3.3.2 ROS2 (Foxy for example):
 ```shell
 source /opt/ros/foxy/setup.bash
 ./script/build_ros2.sh
 ```
 ### 3.4 run:
 #### 3.4.1 ROS1 (Noetic for example):
 ```shell
 source [ros_workspace]/devel/setup.bash
 roslaunch odin_ros_driver [launch file]
 ```
 ● odin_ros_driver: package name;
 ● launch file: launch file;
 ● ros_workspace: User's ROS environment workspace;
 ```shell
 roslaunch odin_ros_driver odin1_ros1.launch
 ```
 #### 3.4.2 ROS2 (Foxy for example):
 ```shell
 source [ros2_workspace]/install/setup.bash
 ros2 launch odin_ros_driver [launch file]
 ```
 ● odin_ros_driver: package name;
 ● launch file: launch file;
 ● ros2_workspace: User's ROS2 environment workspace;
 ROS2 Demo Launch Instructions:
 ```shell
 ros2 launch odin_ros_driver odin1_ros2.launch.py
 ```
 ### 3.5 Operation Mode:
 The operation mode can be configured via the `custom_map_mode` parameter in config/control_command.yaml.
 #### Odometry mode
 Set `custom_map_mode = 0` to enable odometry mode. In this mode, the map frame and odom frame share the same pose.
 If the odom data is found to drift, the script command "./set_param.sh algo_reset 1" can be used to dynamically reset the algorithm.
 #### SLAM mode
 Set `custom_map_mode = 1` to enable slam mode. This mode provides a complete SLAM system that builds upon the Odometry Mode by adding **loop closure detection** and **map saving** capabilities.
 After launching the driver, odin1 will automatically perform mapping and cache map data. When the scene capture is complete, users need to execute `./set_param.sh save_map 1` in the driver's source directory to save all map data collected since the program started. The map will be saved to the location specified by the `mapping_result_dest_dir` and `mapping_result_file_name` parameters in config/control_command.yaml. If these parameters are not specified, default values will be used.
 After the initial save, you can execute the command again to save a new map. Each save operation will generate a new map file. (Please allow at least 5 seconds between consecutive save operations)
 The map origin corresponds to the odom coordinate system's origin at the program's startup.
 ##### Relocalization mode
 To enable relocalization, set `custom_map_mode = 2` and specify the absolute path to the pre-built map using the `relocalization_map_abs_path` parameter in config/control_command.yaml.
 Once launched, odin1 will initiate the relocalization process based on the current viewpoint and the specified map. To ensure a high success rate, it is recommended to starting within 1 meter ±10 degrees of the original position and orientation from the SLAM trajectory.
 Note that relocalization performance is highly environment-dependent. In highly distinctive scenes, successful matching may occur even beyond the 1m/10° range, while other environments may require more stringent conditions. We advise testing in your target environment to determine practical tolerances.
 If relocalization fails initially, the system will temporarily operate in a fallback SLAM mode (map saving is disabled in this state). During this time, you can freely move odin1. It will continue relocalization attempts in the background. Once successful, the TF between map and odom frames will be published. (Tip: Gently shaking or moving the device after initialization can help improve relocalization accuracy.)
 The following topics are published in the odom frame: `/odin1/cloud_slam, /odin1/odom, /odin1/highodom and /odin1/path`. To obtain these in the map frame, apply the TF from odom frame to map frame.
 ## 4. File structure and data format
 ### 4.1 File structure
 ```shell
 Odin_ROS_Driver/                // ROS1/ROS2 driver package
    3rdparty/                   // Third-party libraries
    src/
        host_sdk_sample.cpp     // Example source code
        yaml_parser.cpp         // Source code for reading yaml parameters
        rawCloudRender.cpp      // Source code for RenderCloud
        depth_image_ros_node.cpp //depth_image_ros_node
        depth_image_ros2_node.cpp //depth_image_ros2_node
        pcd2depth_ros.cpp       //Source code for pcd2depth_ros
        pcd2depth_ros2.cpp      //Source code for pcd2depth_ros2
        pointcloud_depth_converter.cpp //Source code for pointcloud_depth_converter
        cloud_reprojection_ros.cpp //Source code for cloud reprojection node (ROS1/ROS2)
        cloud_reprojector.cpp   //Core logic for cloud reprojection
    lib/
        liblydHostApi_amd.a     // Static library for AMD platform
        liblydHostApi_arm.a     // Static library for ARM platform
    include/
        host_sdk_sample.h       // Example header file
        lidar_api_type.h        // API data structure header file
        lidar_api.h             // API function declarations
        yaml_parser.h           // Parameter file reading header file
        rawCloudRender.h        // API about RenderCloud
        data_logger.h           // LOG about save_data
        depth_image_ros_node.hpp // depth_image_ros_node
        depth_image_ros2_node.hpp // depth_image_ros2_node
        pointcloud_depth_converter.hpp // pointcloud_depth_convert
        cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node (ROS1/ROS2)
        cloud_reprojector.hpp   // Core class for cloud reprojection
    config/
        control_command.yaml    // Control parameter file for driver
        calib.yaml              // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver
    launch_ROS1/
        odin1_ros1.launch       // ROS1 launch file
    launch_ROS2/
        odin1_ros2.launch.py    // ROS2 launch file
    script/
        build_ros1.sh           // Installation script for ROS1
        build_ros2.sh           // Installation script for ROS2
    recorddata/                 // holds recorded data that can import into MindCloud
    log/                        // holds log files
        Driver_{timestamp}/     // holds all log folders for each time driver started
            Conn_{timestamp}/   // holds all log files for each odin1 device connection
                dev_status.csv  // device status log file
    README.md                   // Usage instructions
    CMakeLists.txt              // CMake build file
    License                     // License file
 ```
 ### 4.2 File structure
 | Launch File Name         | Description |
 |--------------------------|-------------|
 | odin1_ros1.launch        | Launch file for ROS1 - Odin1 Basic Operations Demo |
 | odin1_ros2.launch.py     | Launch file for ROS2 - Odin1 Basic Operations Demo |
 ### 4.3 ROS topics
 Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters:
 | Topic               |control_command.yaml  | Detailed Description |
 |---------------------|----------------------|----------------------|
 | odin1/imu                     | sendimu           | Imu Topic |
 | odin1/image                   | sendrgb           | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format |
 | odin1/image_undistort         | sendrgbundistort  | undistorted RGB Camera Topic, processed with calib.yaml from device |
 | odin1/image/compressed        | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device |
 | odin1/cloud_raw               | senddtof          | Raw_Cloud Topic |
 | odin1/cloud_render            | sendcloudrender   | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device |
 | odin1/cloud_slam              | sendcloudslam     | Slam_PointCloud Topic |
 | odin1/odometry                | sendodom          | Odom Topic |
 | odin1/odometry_high           | sendodom          | high frequency Odom Topic |
 | odin1/path                    | showpath          | Odom Path Topic |
 | tf                            | sendodom          | tf tree Topic |
 | odin1/depth_img_competetion   | senddepth         | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. |
 | odin1/depth_img_competetion_cloud  | senddepth         | Dense Depth_Cloud Topic. Demo, high computing power required |
 | odin1/reprojected_image       | sendreprojection  | Reprojected cloud to image Topic. Projects cloud_slam to camera image using odometry. Processed on host device. |
 ### 4.4 Data format
 1. The raw point cloud (cloud_raw) has the following fields:
 ```
 float32 x             // X axis, in meters
 float32 y             // Y axis, in meters
 float32 z             // Z axis, in meters
 uint8  intensity      // Reflectivity, range 0–255
 uint16 confidence     // Point confidence, actual value range from 0 to around 1300 in typical scene, higher value means more reliable. Recommanded filtering threshold is 30-35, should be adjusted accordingly.
 float32 offset_time   // Time offset relative to the base timestamp unit: s 
 ```
 To work with this custom format in PCL, first define the point type:
 ```cpp
 /*** LS ***/
 namespace ls_ros {
    struct EIGEN_ALIGN16 Point {
        float x;
        float y;
        float z;
        uint8_t intensity;
        uint16_t confidence;
        float offset_time;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 }  // namespace ls_ros
 POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point,
      (float, x, x)
      (float, y, y)
      (float, z, z)
      (uint8_t, intensity, intensity)
      (uint16_t, confidence, confidence)
      (float offset_time , offset_time)
 )
 ```
 Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL point cloud:
 ```
 pcl::PointCloud<ls_ros::Point> ls_cloud;
 pcl::fromROSMsg(*msg, ls_cloud);
 ```
 2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields:
 ```
 float32 x             // X axis, in meters
 float32 y             // Y axis, in meters
 float32 z             // Z axis, in meters
 float32 rgb           // RGB value
 ```
 ### 4.5 Other functionalities
 |control_command.yaml   | Detailed Description |
 |-----------------------|----------------------|
 | use_host_ros_time     | Time synchronization mode: 0 - use odin internal system time as data timestamp (typical and recommended); 1 - use host ROS time upon receive (not recommended for most users); 2 - align odin1 time to host time via NTP-like synchronization, timestamp is the sensor data reception time on host time axis. |
 | strict_usb3.0_check   | Strict USB3.0 check, if off, allow connection even if usb connection is below usb 3.0 |
 | recorddata            | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. The per-frame timestamps written into the recorded files (IMU / image / point cloud / pose / rotate) follow the same alignment policy as `use_host_ros_time`, so under NTP mode (`use_host_ros_time=1` or `2`) the recorded timestamps are NTP-aligned host time instead of odin1 boot time. <br>录制文件 (IMU / 图像 / 点云 / Pose / Rotate) 中每帧的时间戳与 `use_host_ros_time` 采用相同对齐策略：在 NTP 模式 (`use_host_ros_time=1` 或 `2`) 下，录制时间戳为 NTP 对齐后的主机时间，而非 odin1 开机时间。 |
 | devstatuslog          | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. |
 | showcamerapose        | Display Camera Pose and Field of View. |
 | custom_map_mode        | Operation Modes: Mode 0 - Odometry mode: The map frame and odom frame share the same pose. Mode 1 - Mapping (with loop closure) mode: This mode supports map saving. Mode 2 - Relocalization mode: Requires specifying the absolute path to the map file. After successful relocalization, it will output the TF relationship between the map and odom frames.|
 | custom_init_pos        | Initialization Position (currently unused). |
 | relocalization_map_abs_path        | Absolute Path to Map File: Used for relocalization mode. |
 | mapping_result_dest_dir and mapping_result_file_name| Path and Name for Saving Maps in Mapping Mode: If not specified, default values will be used. |
 ### 4.6 Runtime AE/AWB Tuning via ROS Service / 通过 ROS Service 在线调节 AE/AWB
 The driver hosts four ROS services that let a side terminal tune the
 camera's auto exposure (AE) and auto white balance (AWB) at runtime,
 while the main data streams keep flowing. The same SDK call is shared
 with the driver's main control path and serialised by an internal
 mutex, so it is safe to invoke these services concurrently with normal
 operation.
 驱动启动后会注册 4 个 ROS Service，允许在不重启 driver 的前提下，从另一个终端动态调节
 相机的自动曝光（AE）和自动白平衡（AWB）。底层 SDK 调用与驱动主控制路径共享同一把
 互斥锁，因此可以与正常数据流并发调用。
 **Service list / Service 一览**
 | Service name | Type / 类型 | Purpose / 用途 |
 |---|---|---|
 | `/odin1/get_ae`  | `odin_ros_driver/srv/GetAe`  | Query current AE status / 查询当前 AE 状态 |
 | `/odin1/get_awb` | `odin_ros_driver/srv/GetAwb` | Query current AWB status / 查询当前 AWB 状态 |
 | `/odin1/set_ae`  | `odin_ros_driver/srv/SetAe`  | Set AE mode and (manual) exposure / gain / 设置 AE 模式和手动曝光/增益 |
 | `/odin1/set_awb` | `odin_ros_driver/srv/SetAwb` | Set AWB mode and (manual) R/B gain / 设置 AWB 模式和手动 R/B 增益 |
 #### 4.6.1 Request fields, ranges, physical meaning / 请求字段、范围与物理含义
 **`SetAe.Request`**
 | Field | Range / 范围 | Meaning / 含义 |
 |---|---|---|
 | `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AE loop, the two floats below are ignored / 设备自动调 AE，下方参数被忽略<br>`1` = device locks AE and applies the provided values / 设备锁 AE 并应用提供的值 |
 | `exposure_time` | `0.0001` ~ `0.033` s (manual only / 仅手动模式) | Sensor exposure time per frame. Longer = brighter but more motion blur / 每帧传感器曝光时间。越长越亮但运动模糊增大 |
 | `gain` | `1.0` ~ `64.0` (manual only / 仅手动模式) | Analog gain. Higher = brighter output but worse SNR / 模拟增益。越大越亮但信噪比越差 |
 **`SetAwb.Request`**
 | Field | Range / 范围 | Meaning / 含义 |
 |---|---|---|
 | `mode`  | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AWB loop / 设备自动 AWB<br>`1` = device locks AWB and applies provided gains / 设备锁定 AWB 并应用所给增益 |
 | `rgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | R channel gain. Higher `rgain` vs `bgain` shifts the image warm (yellow/red) / R 通道增益，相对 bgain 越大，画面越偏暖 |
 | `bgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | B channel gain. Higher `bgain` vs `rgain` shifts the image cool (blue) / B 通道增益，相对 rgain 越大，画面越偏冷 |
 > Gr / Gb channels are fixed to 1.0 by the device and are not adjustable.
 > Gr / Gb 通道被设备固定为 1.0，不可调节。
 #### 4.6.2 Response fields / 响应字段
 All four services return a `success` (bool) and `rc` (int32). Get
 services additionally return the queried state.
 4 个 Service 都返回 `success` (bool) 与 `rc` (int32)。Get 类还会返回查询到的状态字段。
 **`GetAe.Response`**
 | Field | Typical range / 典型范围 | Meaning / 含义 |
 |---|---|---|
 | `exposure_time` | `0.0001`~`0.033` s | Current exposure / 当前曝光时间 |
 | `gain` | `1.0`~`64.0` | Current analog gain / 当前模拟增益 |
 | `iso` | `100`~`6400` | Equivalent ISO / 等效 ISO |
 | `brightness` | `0`~`255` | Average frame brightness / 平均帧亮度 |
 | `is_converged` | `0` or `1` | `1` = AE settled / AE 已收敛 |
 | `env_lv` | `0`~`15` | Ambient luminance index, higher = brighter / 环境光强度指数，越大越亮 |
 | `fps` | `~10` / `~14.5` / `~29` | Current frame rate / 当前帧率 |
 **`GetAwb.Response`**
 | Field | Typical range / 典型范围 | Meaning / 含义 |
 |---|---|---|
 | `rgain` / `bgain` | `0.1`~`4.0` | R / B channel gain / R / B 通道增益 |
 | `grgain` / `gbgain` | `1.0` (fixed / 固定) | Gr / Gb gain, device-fixed / Gr / Gb 增益，设备固定 |
 | `cct` | `2500`~`8000` K | Correlated color temperature / 相关色温 |
 | `ccri` | `-50`~`50` | Color temp deviation index, 0 = on Planckian locus / 色温偏离指数，0 表示在普朗克轨迹上 |
 | `is_converged` | `0` or `1` | `1` = AWB settled / AWB 已收敛 |
 #### 4.6.3 `rc` return code / `rc` 返回码
 | `rc` | Meaning / 含义 |
 |---|---|
 | `0` | Success / 成功 |
 | `400` | Device payload too short / 设备载荷过短 |
 | `401` | Device opcode not supported / 设备不支持该 opcode |
 | `402` | Device parameter length wrong / 参数长度错误 |
 | `403` | **Parameter out of range** / 参数越界 — most common when manual values exceed the table above / 手动值超出上表范围时最常见 |
 | `404` | Device-side socket error / 设备端 socket 错误 |
 | `405` | Device-side `ae_control` did not respond / 设备端 `ae_control` 无应答（确认 lydapp 已运行） |
 | `255` (`0xFF`) | Unknown opcode reported by ae_control / ae_control 报未知 opcode |
 | `-1` | SDK not initialised / SDK 未初始化 |
 | `-2` ~ `-5` | USB transfer / timeout / malformed reply / USB 传输异常、超时、应答畸形 |
 | `-100` | **Driver has not opened the device yet** / driver 还未打开设备，请等设备连接成功 |
 #### 4.6.4 Usage examples / 调用示例
 ROS2 (Humble) — start the driver in one terminal, then in a side terminal:
 ROS2（Humble）—— 在一个终端启动 driver，在另一个终端：
 ```bash
 source install/setup.bash
 # Query current state / 查询当前状态
 ros2 service call /odin1/get_ae  odin_ros_driver/srv/GetAe
 ros2 service call /odin1/get_awb odin_ros_driver/srv/GetAwb
 # Set AE to AUTO / 设置 AE 为自动
 ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}"
 # Set AE to MANUAL with 10 ms exposure and gain 4.0
 # 设置 AE 为手动，10 毫秒曝光，增益 4.0
 ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe \
  "{mode: 1, exposure_time: 0.010, gain: 4.0}"
 # Set AWB to MANUAL with rgain=1.5, bgain=2.0
 # 设置 AWB 为手动，rgain=1.5、bgain=2.0
 ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb \
  "{mode: 1, rgain: 1.5, bgain: 2.0}"
 # Restore AUTO / 一键回自动
 ros2 service call /odin1/set_ae  odin_ros_driver/srv/SetAe  "{mode: 0}"
 ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb "{mode: 0}"
 # Inspect srv definition / 查看 srv 完整定义
 ros2 interface show odin_ros_driver/srv/SetAe
 ```
 ROS1 (Noetic) — start the driver, then in a side terminal:
 ROS1（Noetic）—— 启动 driver 后，新开终端：
 ```bash
 source devel/setup.bash
 # Query / 查询
 rosservice call /odin1/get_ae
 rosservice call /odin1/get_awb
 # Set AE manual / 设置 AE 手动
 rosservice call /odin1/set_ae  "{mode: 1, exposure_time: 0.010, gain: 4.0}"
 # Set AWB manual / 设置 AWB 手动
 rosservice call /odin1/set_awb "{mode: 1, rgain: 1.5, bgain: 2.0}"
 # Restore AUTO (ROS1 requires all fields to be present)
 # 一键回自动（ROS1 要求填齐全部字段）
 rosservice call /odin1/set_ae  "{mode: 0, exposure_time: 0.0, gain: 0.0}"
 rosservice call /odin1/set_awb "{mode: 0, rgain: 0.0, bgain: 0.0}"
 # Inspect srv definition / 查看 srv 完整定义
 rossrv show odin_ros_driver/SetAe
 ```
 #### 4.6.5 Recommended starting points by scene / 不同场景推荐起步参数
 **AE (`exposure_time`, `gain`)**
 | Scene / 场景 | `exposure_time` | `gain` |
 |---|---|---|
 | Bright outdoor / 明亮室外 | `0.001` ~ `0.005` s | `1.0` ~ `2.0` |
 | Normal indoor / 普通室内 | `0.008` ~ `0.015` s | `2.0` ~ `8.0` |
 | Dim light / 暗光环境 | `0.020` ~ `0.030` s | `8.0` ~ `32.0` |
 | Very dark / 极暗 | `0.033` s | `32.0` ~ `64.0` |
 **AWB (`rgain`, `bgain`)**
 | Target tone / 目标色调 | `rgain` | `bgain` |
 |---|---|---|
 | Warm (tungsten, sunset) / 暖（钨丝灯、夕阳） | `2.0` ~ `2.5` | `1.0` ~ `1.2` |
 | Neutral (D65 daylight) / 中性（D65 日光） | `1.5` ~ `1.7` | `1.8` ~ `2.0` |
 | Cool (cloudy, fluorescent) / 冷（阴天、荧光） | `1.2` ~ `1.4` | `2.2` ~ `2.6` |
 | Very cool / 极冷 | `1.0` | `3.0` ~ `4.0` |
 #### 4.6.6 Caveats / 注意事项
 - The service blocks for up to ~10 s waiting for the device to reply;
  typical latency is tens of milliseconds.
  Service 最长阻塞约 10 秒等设备应答；正常几十毫秒返回。
 - Manual mode is **not** persisted across driver / device restart;
  it falls back to AUTO on each new connection.
  手动模式**不会**跨重启保留；每次重连默认回到 AUTO。
 - `rc = -100` means the driver has not yet opened the device.
  Wait until the driver logs `device connected` before calling.
  返回 `rc = -100` 表示 driver 还没打开设备，等到 driver 日志显示 `device connected` 再调用。
 - The effective maximum `exposure_time` is bounded by the frame
  period `1 / fps`. With `dtof_fps = 290` (29 Hz, period ~34 ms)
  the upper limit 0.033 s is already at the frame boundary.
  最大可用 `exposure_time` 受帧周期 `1/fps` 限制。在 `dtof_fps = 290`（29 Hz、周期 ~34 ms）下，上限 0.033 s 已经贴到帧边界。
 ## 5. FAQ
 ### 5.1 Segmentation fault upon re-launching host SDK
 **Error Message**  
 No device connected after 60 seconds 
 **Solution**  
 1. Please power on Odin module again # Disconnect and reconnect odin power
 2. Reinitialize Odin SDK # Execute SDK after device reboot
 ### 5.2 Library binding failure during compilation
 **Error Message**  
 ld: cannot find -llydHostApi or symbol lookup errors
 **Resolution** 
 1. Clean previous build artifacts
 ROS1 
 ```shell
 rm -rf devel/ build/  
 ```
 ROS2
 ```shell
 rm -rf devel/ install/ log/ 
 ```
 2. Re-run script installation
 ### 5.3 Docker GUI passthrough failure
 **Error Message**  
 Unable to open X display or No protocol specified
 **Resolution** 
 ```shell
 xhost + #This command enables graphical passthrough to Docker containers
 ```
 ### 5.4 ROS driver exit with get version failed error
 **Error Message**  
 ```shell
 <ERROR><api.cpp:lidar_get_version:672>: get device version fail.
 get version failed.
 ```
 **Resolution** 
 Device firmware version is too low, please update to latest version.
 ### 5.5 RVIZ has not responded for a long time
 **Error Message**  
 Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection...
 **Resolution** 
 Please power on Odin module again
 ### 5.6 Device not responding
 **Error Message**  
 Missed ok response from device,probably wrong interaction procedure.
 **Resolution** 
 Please adopt the solution mentioned in 5.1
 ### 5.7 Device has no external calibration file 
 **Error Message**  
 ERROR：Missing camera node 'cam_0'
 **Resolution** 
 Please plug and unplug the USB again
 ### 5.8 ROS Driver report device disconnected immediately after stream started
 **Error Message**  
 ```shell
 Device ready and streams activated
 Device detaching...
 Wating for device reconnection...
 Device disconnected, waiting for reconnection...
 ```
 **Reason**
 Mostly common on ros2 environment and connected to complex network environment, such as office wifi & ethernet. ROS2 default to broadcast, and complex network environment will cause ros2 publish to block, leading to device disconnection.
 **Resolution** 
 If cross-device communication is not required, please restrict ros2 to localhost only with:
 ```shell
 export ROS_LOCALHOST_ONLY=1
 ```
 If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended.
 ### 5.9 ROS Driver died immediately after stream started
 **Error Message**  
 ```shell
 Device ready and streams activated
 [host_sdk_sample-2] process has died ......
 ```
 **Test**
 Disable odin1/image	with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system.
 **Resolution** 
 Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again.
 ### 5.10 ROS Driver printing "TF_OLD_DATA ignoring data" warning
 **Error Message**  
 ```shell
 [rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame odin1_base_link at time 20.547632 according to authority Authority undetectable
 [rviz2-3] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
 [rviz2-3]          at line 294 in ./src/buffer_core.cpp
 ```
 **Reason**
 This is a ros & rviz feature to warn user that some tf data is being ignored due to timestamp conflicts. It happens when user keeps ros driver running and power-cycles odin device, which cause odin's internal system time being reset and now data timestamps conflicts with old data recieved by rviz during last run.
 **Resolution** 
 There's a reset button on bottom of rviz gui. Click on this button will reset rviz's internal state and stop the warning.
 ### 5.11 ROS Driver printing "unknown cmd code: xx" error
 **Error Message**  
 ```shell
 <ERROR><api.cpp:cmd_data_deal:418>: unknow command code 21.
 ```
 **Reason**
 This is due to ros driver version mismatch with device firmware version, resulting in ros driver unable to decode new data added in newer firmware.
 **Resolution** 
 Please make sure you are using most up-to-date ros driver and device firmware.
 ### 5.12 USB device access error (LIBUSB_ERROR_BUSY or LIBUSB_ERROR_ACCESS)
 **Error Message**  
 ```shell
 libusb: error [udev_hotplug_event] ignoring udev action bind
 LIBUSB_ERROR_BUSY
 ```
 or
 ```shell
 libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13
 LIBUSB_ERROR_ACCESS
 ```
 **Reason**
 - **LIBUSB_ERROR_BUSY**: Another process is already using the USB device. This commonly happens when multiple instances of the ROS driver are running, or another application (such as a previous crashed instance) still holds the device handle.
 - **LIBUSB_ERROR_ACCESS**: The current user does not have permission to access the USB device. This is typically caused by missing udev rules or insufficient user privileges.
 **Resolution** 
 For **LIBUSB_ERROR_BUSY**:
 1. Check if another instance of the driver is running:
 ```shell
 ps aux | grep host_sdk_sample
 ```
 2. Kill any existing instances:
 ```shell
 killall host_sdk_sample
 ```
 3. If the issue persists, unplug and replug the USB device to reset the device state.
 For **LIBUSB_ERROR_ACCESS**:
 1. Add udev rules for the device. Create a file `/etc/udev/rules.d/99-odin.rules` with the following content:
 ```shell
 SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
 ```
 2. Reload udev rules:
 ```shell
 sudo udevadm control --reload-rules
 sudo udevadm trigger
 ```
 3. Alternatively, run the driver with sudo (not recommended for production):
 ```shell
 sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py
 ```
 4. Make sure your user is in the `plugdev` group:
 ```shell
 sudo usermod -aG plugdev $USER
 ```
 Then log out and log back in for the group change to take effect.
 ### 5.13 ros2 bag drops high-frequency topics (IMU / odometry_highfreq) / ros2 bag 录制丢失高频话题（IMU / odometry_highfreq）
 **Symptom / 现象**
 When recording with `ros2 bag record`, low-frequency topics (cloud, image, odometry, wiwc) are intact, but `/odin1/imu` (400 Hz) and `/odin1/odometry_highfreq` (400 Hz) show missing samples — analysis scripts report inter-message intervals that are 2× or more of the expected period, while no drop is reported on the SDK side or by an online subscriber such as `ros2 topic hz`.
 使用 `ros2 bag record` 录制时，低频话题（cloud、image、odometry、wiwc）完整无丢，但 `/odin1/imu`（400 Hz）和 `/odin1/odometry_highfreq`（400 Hz）会出现丢帧——分析脚本上看到消息间隔达到正常周期的 2 倍以上，而 SDK 侧不报丢，独立的 `ros2 topic hz` 订阅者也看不到丢。
 **Reason / 原因**
 The driver publishes `/odin1/imu` and `/odin1/odometry_highfreq` with `RELIABLE` QoS. By default `ros2 bag record` subscribes with `history = keep_last`, `depth = 10`, which only buffers ~25 ms of samples at 400 Hz. Whenever the recorder is briefly delayed (disk flush, mcap/sqlite chunk write, scheduler jitter), its subscription queue overflows and DDS silently drops the oldest samples on the **subscriber side**. The SDK and publisher are unaffected, which is why no drop appears in the driver logs or in `ros2 topic hz`.
 驱动以 `RELIABLE` QoS 发布 `/odin1/imu` 与 `/odin1/odometry_highfreq`。`ros2 bag record` 默认订阅使用 `history = keep_last`、`depth = 10`，在 400 Hz 下只能缓冲约 25 ms。一旦录制端有短暂阻塞（落盘 flush、mcap/sqlite chunk 写入、调度抖动），订阅队列就会溢出，DDS 在**订阅端**静默丢掉最旧的样本。SDK 与 publisher 不受影响，因此驱动日志和 `ros2 topic hz` 都看不到丢。
 **Resolution / 解决方案**
 Use the provided QoS override file `script/rosbag2_qos.yaml` to raise the subscriber-side queue depth on the recorder for the two high-rate topics:
 使用本仓库提供的 QoS 配置 `script/rosbag2_qos.yaml`，把高频话题的录制订阅 depth 拉大：
 ```yaml
 # script/rosbag2_qos.yaml
 /odin1/imu:
  reliability: reliable
  history: keep_last
  depth: 4000
 /odin1/odometry_highfreq:
  reliability: reliable
  history: keep_last
  depth: 4000
 ```
 Apply it when recording / 录制时通过 `--qos-profile-overrides-path` 应用：
 ```shell
 ros2 bag record -a \
    --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
    -o my_bag
 ```
 Or only the high-rate topics / 也可以只录制高频话题：
 ```shell
 ros2 bag record \
    --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
    -o my_bag \
    /odin1/imu /odin1/odometry_highfreq /odin1/odometry /odin1/wiwc /odin1/cloud_raw
 ```
 **Optional further tuning / 可选的进一步优化**
 If drops still occur after applying the override (typically on slower disks), try the following in addition / 套用上述 override 后仍有丢包时（通常发生在慢盘上），可叠加以下措施：
 ```shell
 # Use mcap backend with a larger internal cache (faster than sqlite3).
 # 使用 mcap 后端 + 更大的内部缓存（比 sqlite3 快）。
 ros2 bag record -s mcap --max-cache-size 1073741824 \
    --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
    -o my_bag \
    /odin1/imu /odin1/odometry_highfreq ...
 # Enlarge kernel UDP socket buffers (the most common hidden bottleneck for
 # 400 Hz RELIABLE traffic, default is only 208 KB).
 # 放大内核 UDP socket buffer（400 Hz RELIABLE 流量最常见的隐藏瓶颈，默认仅 208 KB）。
 sudo sysctl -w net.core.rmem_max=33554432
 sudo sysctl -w net.core.wmem_max=33554432
 ```
 **Does ROS1 have the same problem? / ROS1 是否存在同样的问题？**
 No. ROS1 uses TCP-based publish/subscribe with a single `queue_size` parameter on each side, and has no QoS profile mismatch between publisher and subscriber. The ROS1 publisher path in this driver already sizes the IMU and `odometry_highfreq` publishers to `queue_size = 4000` (`include/host_sdk_sample.h`, see `initialize_publishers` ROS1 branch), and `rosbag record` uses TCP transport which is reliable by construction. As a result this specific drop pattern does not occur under ROS1; no additional configuration is required.
 不存在。ROS1 使用基于 TCP 的发布/订阅，发布端与订阅端各自只有一个 `queue_size` 参数，不存在 ROS2 那种 QoS profile 不匹配的问题。本驱动 ROS1 路径已经把 IMU 与 `odometry_highfreq` 的发布队列设置为 `queue_size = 4000`（见 `include/host_sdk_sample.h` 中 `initialize_publishers` 的 ROS1 分支），并且 `rosbag record` 使用 TCP 传输本身即可靠传递。因此在 ROS1 下不会出现该丢帧现象，也不需要额外配置。
 ## 6.  Contact Information
 You can contact our support through support@manifoldtech.cn
 To help diagnose the issue, please provide the following details to our FAE engineer:
 1. Current firmware version 
 ```shell
 [device_version_capture]: ros_driver_version: [Version Number]
 ```
 2. Photos of power adapter and converter cable in use.
 3. Does the issue happen occasionally or consistently?
 4. Provide images of the problem scenario.
 5. Did the troubleshooting methods in Section V resolve the issue?
 6. Expected timeline for issue resolution.
@@ -90,9 +90,11 @@ register_keys:
  showpath: 0             # 0: off; 1: on
  showcamerapose: 0       # 0: off; 1: on
-  custom_map_mode: 0      # 0: Odometry mode 1: SLAM mode 2: Relocalization mode. Keep 0 for pure odom fallback to avoid Odin map/odom TF conflicts.
+  custom_map_mode: 2      # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
  custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
-  relocalization_map_abs_path: "/absolute/path/to/map_a.bin" # unused in pure odometry mode; edit before Relocalization
+  custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
  custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
  relocalization_map_abs_path: "/absolute/path/to/1hao.bin" # required for Relocalization mode; edit on the target computer
  # To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
  mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
@@ -0,0 +1,110 @@
 register_keys:
  # if off, allow connection even if usb connection is below usb 3.0
  # ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer
  strict_usb3.0_check: 0  # 0: off: 1: on;
  # 0: use odin internal system time as data time stamp, typical and recommended;
  # 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users
  # 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis
  use_host_ros_time: 1
  streamctrl: 1           # 0: off; 1: on
  # original rgb data in jpeg format from device
  sendrgbcompressed: 1    # 0: off; 1: on
  # RGB data, decoded from original jpeg data from device, bgr8 format
  # Processed on host device
  sendrgb: 1              # 0: off; 1: on
  # undistort rgb image processed from decoded rgb data.
  # depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml
  # Processed on host device
  sendrgbundistort: 0    # 0: off; 1: on.
  # IMU data
  sendimu: 1              # 0: off; 1: on
  # SDK IMU smooth sending feature
  # When enabled, SDK will send IMU data at precise intervals (default 400Hz)
  # using a dedicated high-priority thread to reduce jitter and timing variance
  enable_imu_smooth: 1    # 0: disable SDK IMU smooth sending; 1: enable (default)
  # SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
  imu_smooth_frequency: 400  # 1-1000 Hz, recommended 400 Hz
  # Odometry data
  sendodom: 1             # 0: off; 1: on
  # TF from odom to base_link. Leave it on unless you specifically need it off.
  # ATTENTION: critical for rviz to show cloud_raw.
  send_odom_baselink_tf: 0 # 0: off; 1: on.
  # raw dtof data
  senddtof: 1             # 0: off; 1: on
  cloud_raw_confidence_threshold: 35 # please refer to readme for more details
  # dtof sensor frame rate. Supported values: 100 (10fps) or 145 (14.5fps)
  # Higher frame rate provides smoother point cloud data but may increase data bandwidth
  # Note: value is multiplied by 10 (e.g., 145 means 14.5fps)
  dtof_fps: 100           # 100: 10fps; 145: 14.5fps
  # slam cloud data
  sendcloudslam: 1        # 0: off; 1: on
  # processed with raw point cloud, rgb image, and calib.yaml from device
  # Processed on host device
  sendcloudrender: 1      # 0: off; 1: on
  # depth completion demo, high computing resource usage.
  # for more information please refer to the readme file.
  # Processed on host device
  senddepth: 0            # 0: off; 1: on
  # cloud reprojection demo, projects cloud_slam to camera image using odometry
  # Processed on host device
  sendreprojection: 0    # 0: off; 1: on
  # image overlay settings - overlays reprojected points on camera image
  # Processed on host device
  sendoverlay: 0                                    # 0: off; 1: on
  overlay_reprojected_topic: "/odin1/reprojected_image"  # reprojected image topic
  overlay_camera_topic: "/odin1/image/undistorted"       # camera image topic (undistorted)
  overlay_output_topic: "/odin1/overlay_image"           # output overlay image topic
  overlay_alpha: 0.6                                     # blend alpha (0.0-1.0, higher = more reproj color)
  # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
  # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
  # ATTENTION: please copy the full folder for post-processing.
  recorddata: 0           # 0: off; 1: on
  # Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv
  devstatuslog: 1         # 0: off; 1: on.
  save_log: 0 # 0: off; 1: on;
  # raw dtof sensor intensity data in gray format, mostly for debug purpose.
  pubintensitygray: 0     # 0: off; 1: on
  showpath: 0             # 0: off; 1: on
  showcamerapose: 0       # 0: off; 1: on
  # Screen LOC=ODOM profile. Keep Odin in odometry mode; sim2real_web_udp_bridge_node
  # may publish the pure-odom map->odom fallback anchor.
  custom_map_mode: 0      # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
  custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
  custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
  custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
  relocalization_map_abs_path: "" # unused in odometry mode
  # To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
  mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
  mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
  # Image mask transfer settings
  sendimagemask: 0        # 0: off; 1: on - transfer image mask to device on startup
  image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296 resolution)
  # Algorithm reset settings
  resetalgo: 0            # 0: off; 1: on - send algo_reset command to device on startup
@@ -0,0 +1,110 @@
 register_keys:
  # if off, allow connection even if usb connection is below usb 3.0
  # ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer
  strict_usb3.0_check: 0  # 0: off: 1: on;
  # 0: use odin internal system time as data time stamp, typical and recommended;
  # 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users
  # 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis
  use_host_ros_time: 1
  streamctrl: 1           # 0: off; 1: on
  # original rgb data in jpeg format from device
  sendrgbcompressed: 1    # 0: off; 1: on
  # RGB data, decoded from original jpeg data from device, bgr8 format
  # Processed on host device
  sendrgb: 1              # 0: off; 1: on
  # undistort rgb image processed from decoded rgb data.
  # depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml
  # Processed on host device
  sendrgbundistort: 0    # 0: off; 1: on.
  # IMU data
  sendimu: 1              # 0: off; 1: on
  # SDK IMU smooth sending feature
  # When enabled, SDK will send IMU data at precise intervals (default 400Hz)
  # using a dedicated high-priority thread to reduce jitter and timing variance
  enable_imu_smooth: 1    # 0: disable SDK IMU smooth sending; 1: enable (default)
  # SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
  imu_smooth_frequency: 400  # 1-1000 Hz, recommended 400 Hz
  # Odometry data
  sendodom: 1             # 0: off; 1: on
  # TF from odom to base_link. Leave it on unless you specifically need it off.
  # ATTENTION: critical for rviz to show cloud_raw.
  send_odom_baselink_tf: 0 # 0: off; 1: on.
  # raw dtof data
  senddtof: 1             # 0: off; 1: on
  cloud_raw_confidence_threshold: 35 # please refer to readme for more details
  # dtof sensor frame rate. Supported values: 100 (10fps) or 145 (14.5fps)
  # Higher frame rate provides smoother point cloud data but may increase data bandwidth
  # Note: value is multiplied by 10 (e.g., 145 means 14.5fps)
  dtof_fps: 100           # 100: 10fps; 145: 14.5fps
  # slam cloud data
  sendcloudslam: 1        # 0: off; 1: on
  # processed with raw point cloud, rgb image, and calib.yaml from device
  # Processed on host device
  sendcloudrender: 1      # 0: off; 1: on
  # depth completion demo, high computing resource usage.
  # for more information please refer to the readme file.
  # Processed on host device
  senddepth: 0            # 0: off; 1: on
  # cloud reprojection demo, projects cloud_slam to camera image using odometry
  # Processed on host device
  sendreprojection: 0    # 0: off; 1: on
  # image overlay settings - overlays reprojected points on camera image
  # Processed on host device
  sendoverlay: 0                                    # 0: off; 1: on
  overlay_reprojected_topic: "/odin1/reprojected_image"  # reprojected image topic
  overlay_camera_topic: "/odin1/image/undistorted"       # camera image topic (undistorted)
  overlay_output_topic: "/odin1/overlay_image"           # output overlay image topic
  overlay_alpha: 0.6                                     # blend alpha (0.0-1.0, higher = more reproj color)
  # record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
  # save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
  # ATTENTION: please copy the full folder for post-processing.
  recorddata: 0           # 0: off; 1: on
  # Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv
  devstatuslog: 1         # 0: off; 1: on.
  save_log: 0 # 0: off; 1: on;
  # raw dtof sensor intensity data in gray format, mostly for debug purpose.
  pubintensitygray: 0     # 0: off; 1: on
  showpath: 0             # 0: off; 1: on
  showcamerapose: 0       # 0: off; 1: on
  # Screen LOC=RELOC profile. Odin loads relocalization_map_abs_path and publishes
  # the map/odom TF after relocalization succeeds; pure-odom fallback is disabled.
  custom_map_mode: 2      # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
  custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
  custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
  custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
  relocalization_map_abs_path: "/absolute/path/to/1hao.bin" # required for Relocalization mode; edit on the target computer
  # To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
  mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
  mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
  # Image mask transfer settings
  sendimagemask: 0        # 0: off; 1: on - transfer image mask to device on startup
  image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296 resolution)
  # Algorithm reset settings
  resetalgo: 0            # 0: off; 1: on - send algo_reset command to device on startup
@@ -19,7 +19,17 @@ limitations under the License.
    #include <sensor_msgs/msg/image.hpp>
    #include <nav_msgs/msg/odometry.hpp>
    #include <image_transport/image_transport.hpp>
-    #include <cv_bridge/cv_bridge.h>
+    // cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
    // Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
    #if defined(__has_include)
    #  if __has_include(<cv_bridge/cv_bridge.hpp>)
    #    include <cv_bridge/cv_bridge.hpp>
    #  else
    #    include <cv_bridge/cv_bridge.h>
    #  endif
    #else
    #  include <cv_bridge/cv_bridge.h>
    #endif
    #include <message_filters/subscriber.h>
    #include <message_filters/time_synchronizer.h>
    #include <message_filters/sync_policies/exact_time.h>
@@ -23,7 +23,17 @@ limitations under the License.
 #include <pcl/point_cloud.h>
 #include <opencv2/opencv.hpp>
-#include <cv_bridge/cv_bridge.h>
+// cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
 // Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
 #if defined(__has_include)
 #  if __has_include(<cv_bridge/cv_bridge.hpp>)
 #    include <cv_bridge/cv_bridge.hpp>
 #  else
 #    include <cv_bridge/cv_bridge.h>
 #  endif
 #else
 #  include <cv_bridge/cv_bridge.h>
 #endif
 #include <image_transport/image_transport.hpp>
 #include <message_filters/subscriber.h>
@@ -26,7 +26,17 @@ limitations under the License.
 #include <iomanip>
 #include <cstring>
 #include <opencv2/opencv.hpp>
-#include <cv_bridge/cv_bridge.h>
+// cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
 // Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
 #if defined(__has_include)
 #  if __has_include(<cv_bridge/cv_bridge.hpp>)
 #    include <cv_bridge/cv_bridge.hpp>
 #  else
 #    include <cv_bridge/cv_bridge.h>
 #  endif
 #else
 #  include <cv_bridge/cv_bridge.h>
 #endif
 #include <thread>
 #include <Eigen/Dense>
 #include <atomic>
@@ -180,6 +190,32 @@ inline uint64_t ros_time_to_ns(const ros::Time &t) {
    #endif
 }
 // Compute an "aligned" nanosecond timestamp for offline recording (recorddata files).
 // Mirrors the policy used by make_aligned_stamp() so that recorded timestamps stay
 // consistent with the timestamps that are published over ROS topics.
 //   g_use_host_ros_time == 0 : raw sensor timestamp (odin1 boot time, no alignment)
 //   g_use_host_ros_time == 1 : host wall-clock now (NTP-synced if the host is NTP-synced)
 //   g_use_host_ros_time == 2 : sensor timestamp aligned via smoothed PTP offset (NTP/PTP mode)
 //
 //   g_use_host_ros_time == 0 :
 //   g_use_host_ros_time == 1 : 
 //   g_use_host_ros_time == 2 : 
 inline uint64_t aligned_stamp_ns(uint64_t sensor_timestamp_ns) {
    if (g_use_host_ros_time == 1) {
        const auto now = std::chrono::system_clock::now().time_since_epoch();
        return static_cast<uint64_t>(
            std::chrono::duration_cast<std::chrono::nanoseconds>(now).count());
    }
    if (g_use_host_ros_time == 2) {
        const double offset_s = get_ptp_smoothed_offset();
        const int64_t offset_ns = static_cast<int64_t>(offset_s * 1e9);
        const int64_t base_ns = static_cast<int64_t>(sensor_timestamp_ns);
        const int64_t aligned_ns = base_ns - offset_ns;
        return (aligned_ns < 0) ? 0ULL : static_cast<uint64_t>(aligned_ns);
    }
    return sensor_timestamp_ns;
 }
 inline ros::Time make_aligned_stamp(uint64_t sensor_timestamp_ns
 #ifdef ROS2
                                    , const rclcpp::Node::SharedPtr& node
@@ -306,7 +342,8 @@ public:
        #endif
        if(data_logger_) {
-            const double ts_sec = static_cast<double>(stream->stamp) / 1e9;
+            // Align IMU timestamp with the same policy as ROS publish path
            const double ts_sec = static_cast<double>(aligned_stamp_ns(stream->stamp)) / 1e9;
            float ax = imu_msg.linear_acceleration.x;
            float ay = imu_msg.linear_acceleration.y;
            float az = imu_msg.linear_acceleration.z;
@@ -800,7 +837,8 @@ void publishRgb(capture_Image_List_t *stream) {
        // Enqueue binary logging for image
        if (data_logger_) {
            const uint32_t idx_now = image_index_.fetch_add(1, std::memory_order_relaxed);
-            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp) / 1e9;
+            // Align image timestamp with the same policy as ROS publish path (NTP mode -> NTP time)
            const double ts_sec = static_cast<double>(aligned_stamp_ns(stream->imageList[0].timestamp)) / 1e9;
            const uint32_t jpeg_size = static_cast<uint32_t>(jpeg_data.size());
            std::vector<uint8_t> blob;
            blob.reserve(sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t) + jpeg_size);
@@ -957,7 +995,8 @@ void publishRgb(capture_Image_List_t *stream) {
        // Enqueue binary logging for point cloud (XYZRGB per point)
        if (data_logger_ && points > 0) {
-            const double ts_sec = static_cast<double>(stream->imageList[0].timestamp) / 1e9;
+            // Align point cloud timestamp with the same policy as ROS publish path (NTP mode -> NTP time)
            const double ts_sec = static_cast<double>(aligned_stamp_ns(stream->imageList[0].timestamp)) / 1e9;
            const uint32_t idx_now = cloud_index_.fetch_add(1, std::memory_order_relaxed);
            // Compute total blob size: header + per-point payload
            const size_t header_size = sizeof(uint32_t) + sizeof(double) + sizeof(uint32_t);
@@ -1005,7 +1044,8 @@ void publishRgb(capture_Image_List_t *stream) {
            if (data_len == sizeof(ros_odom_convert_complete_t)) {
                ros_odom_convert_complete_t* odom_data = (ros_odom_convert_complete_t*)stream->imageList[0].pAddr;
                const uint32_t idx_now = wcwi_index_.fetch_add(1, std::memory_order_relaxed);
-                const double ts_sec = static_cast<double>(odom_data->timestamp_ns) / 1e9;
+                // Align WIWC/rotate timestamp with the same policy as ROS publish path (NTP mode -> NTP time)
                const double ts_sec = static_cast<double>(aligned_stamp_ns(odom_data->timestamp_ns)) / 1e9;
                float pose_arr[4];
                pose_arr[0] = static_cast<float>((odom_data->orient[0]) / 1e6);
                pose_arr[1] = static_cast<float>((odom_data->orient[1]) / 1e6);
@@ -1221,7 +1261,8 @@ void publishRgb(capture_Image_List_t *stream) {
                // Enqueue binary logging for pose
                if ((odom_type == OdometryType::STANDARD) && data_logger_) {
                    const uint32_t idx_now = pose_index_.fetch_add(1, std::memory_order_relaxed);
-                    const double ts_sec = static_cast<double>(odom_data->timestamp_ns) / 1e9;
+                    // Align pose timestamp with the same policy as ROS publish path (NTP mode -> NTP time)
                    const double ts_sec = static_cast<double>(aligned_stamp_ns(odom_data->timestamp_ns)) / 1e9;
                    float pose_arr[7];
                    pose_arr[0] = static_cast<float>(msg.pose.pose.position.x);
                    pose_arr[1] = static_cast<float>(msg.pose.pose.position.y);
@@ -1675,12 +1716,12 @@ private:
    void initialize_publishers() {
        #ifdef ROS2
            // Small data with queue depth 1
-            auto qos_small = rclcpp::QoS(1)
+            auto qos_small = rclcpp::QoS(4000)
                                    .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
                                    .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
            // Large sensor data with larger queue to avoid blocking
-            auto qos_sensor = rclcpp::QoS(10)
+            auto qos_sensor = rclcpp::QoS(5)
                                    .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
                                    .durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
@@ -1688,33 +1729,33 @@ private:
            rgb_pub_ = node_->create_publisher<ros::Image>("odin1/image", qos_sensor);
            cloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_raw", qos_sensor);
            xyzrgbacloud_pub_ = node_->create_publisher<ros::PointCloud2>("odin1/cloud_slam", qos_sensor);
-            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_small);
+            odom_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry", qos_sensor);
            odom_highfreq_publisher_ = node_->create_publisher<ros::Odometry>("odin1/odometry_highfreq", qos_small);
            path_publisher_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/path", qos_sensor);
            pub_camera_pose_visual_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>("odin1/camera_pose_visual", qos_sensor);
            rgbcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>("odin1/cloud_render", qos_sensor);
-            compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_small);
+            compressed_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::CompressedImage>("odin1/image/compressed", qos_sensor);
            undistort_rgb_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/undistorted", qos_sensor);
            intensity_gray_pub_ = node_->create_publisher<sensor_msgs::msg::Image>("odin1/image/intensity_gray", qos_sensor);
-            wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_small);
+            wiwc_publisher_ = node_->create_publisher<ros::Odometry>("odin1/wiwc", qos_sensor);
            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(node_);
        #endif
    }
    #ifdef ROS1
        void initialize_publishers(ros::NodeHandle& nh) {
            imu_pub_ = nh.advertise<ros::Imu>("odin1/imu", 4000);
-            rgb_pub_ = nh.advertise<ros::Image>("odin1/image", 100);
+            rgb_pub_ = nh.advertise<ros::Image>("odin1/image", 5);
-            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 100);
+            cloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_raw", 5);
-            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 100);
+            xyzrgbacloud_pub_ = nh.advertise<ros::PointCloud2>("odin1/cloud_slam", 5);
-            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 100);
+            odom_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry", 5);
            odom_highfreq_publisher_ = nh.advertise<ros::Odometry>("odin1/odometry_highfreq", 4000);
-            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 100);
+            path_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/path", 5);
-            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 100);
+            pub_camera_pose_visual_ = nh.advertise<visualization_msgs::MarkerArray>("odin1/camera_pose_visual", 5);
-            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 100);
+            rgbcloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odin1/cloud_render", 5);
-            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 100);
+            compressed_rgb_pub_ = nh.advertise<sensor_msgs::CompressedImage>("odin1/image/compressed", 5);
-            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 100);
+            undistort_rgb_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/undistorted", 5);
-            intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 100);
+            intensity_gray_pub_ = nh.advertise<sensor_msgs::Image>("odin1/image/intensity_gray", 5);
-            wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 100);
+            wiwc_publisher_ = nh.advertise<ros::Odometry>("odin1/wiwc", 5);
            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
        }
    #endif
@@ -16,7 +16,17 @@ limitations under the License.
 #ifdef ROS2
    #include <rclcpp/rclcpp.hpp>
    #include <sensor_msgs/msg/image.hpp>
-    #include <cv_bridge/cv_bridge.h>
+    // cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
    // Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
    #if defined(__has_include)
    #  if __has_include(<cv_bridge/cv_bridge.hpp>)
    #    include <cv_bridge/cv_bridge.hpp>
    #  else
    #    include <cv_bridge/cv_bridge.h>
    #  endif
    #else
    #  include <cv_bridge/cv_bridge.h>
    #endif
    #include <mutex>
 #else
    #include <ros/ros.h>
@@ -525,8 +525,202 @@ int lidar_enable_imu_smooth_sending(int enable);
 */
 int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
 /**
 * @brief Reset the USB connection to the device
 *
 * Performs a USB port reset on the device (libusb_reset_device). This
 * re-enumerates the device on the host without requiring a physical
 * re-plug. The device handle should typically be reopened after the
 * device reconnects.
 *
 * @param device Handle to the target device
 * @return int 0 on success, negative error code on failure
 */
 int lidar_reset_usb(device_handle device);
 /**
 * @brief Query the current device initial/running state.
 *
 * Returns the latest state reported by the device through heartbeats. The
 * value corresponds to ::lidar_device_initial_state_e (NONE,
 * NOT_INITIALIZED, INITIALIZED, STREAMING, STREAM_STOPPED). Callers that
 * need to wait until the device is fully booted (for example before
 * uploading a relocalization map) should poll this until it becomes
 * LIDAR_DEVICE_STREAMING (or LIDAR_DEVICE_INITIALIZED at minimum).
 *
 *
 * @param state Output pointer that receives the current state. Must not be NULL.
 * @return int 0 on success, negative error code on failure.
 */
 int lidar_get_device_state(lidar_device_initial_state_e *state);
 /**
 * @brief Send a host-defined pass-through user-data blob to the device.
 *
 * The device forwards data[] as-is to SLAM via shared memory
 * ("user_data_shm"). No acknowledgment is returned from the device
 * (fire-and-forget). An SDK-internal monotonic counter is packed into
 * the on-wire frameId field so SLAM can still distinguish consecutive
 * frames without the caller having to manage an id.
 *
 * Preconditions:
 *  - SDK initialized and the device opened (lidar_open_device).
 *  - SLAM has been started on the device side; otherwise the device
 *    silently drops the frame and logs a warning.
 *
 * Constraints:
 *  - blob != NULL.
 *  - 0 < blob_len <= 8 MiB (8 * 1024 * 1024).
 *  - Sending faster than SLAM can consume causes device-side timeouts
 *    and dropped frames. Pace according to SLAM throughput.
 *
 * @param device   Handle returned by lidar_create_device / lidar_open_device.
 * @param blob     Pointer to user payload.
 * @param blob_len Length in bytes.
 * @return int 0 on success, negative error code on failure.
 */
 int lidar_send_user_data(device_handle device,
                         const void *blob, uint32_t blob_len);
 /* ---------------------------------------------------------------------
 * Camera AE / AWB control APIs.
 *
 * All four APIs are synchronous (send + wait response) and serialised by
 * an internal mutex (the same mutex protecting other control commands).
 * Return value convention:
 *   == 0   success
 *   >  0   device-side error, see lidar_ae_error_e (400..405 or 0xFF)
 *   <  0   SDK-side error (not initialised, bad argument, USB failure,
 *          response timeout, malformed reply, ...)
 *
 * Wire-level details: sdk/api/Host_USB_AE_Protocol.md.
 * ------------------------------------------------------------------- */
 /**
 * @brief Query current AE (auto exposure) state.
 *
 * Sends AE opcode 0x01 and decodes the 25-byte little-endian payload
 * returned by the device-side ae_control service.
 *
 * Output fields and their physical meaning:
 *   exposure_time : current exposure time in seconds (manual range
 *                   0.0001 .. 0.033; in auto mode it varies with
 *                   scene illumination).
 *   gain          : current analog gain (manual range 1.0 .. 64.0;
 *                   higher = brighter but noisier).
 *   iso           : equivalent ISO, typically 100 .. 6400.
 *   brightness    : average frame brightness (0 .. 255).
 *   is_converged  : 1 = AE has settled, 0 = still adjusting.
 *   env_lv        : ambient luminance index, typically 0 .. 15
 *                   (higher = brighter scene).
 *   fps           : actual frame rate, follows dtof_fps config
 *                   (~10 / 14.5 / 29 Hz).
 *
 * @param device Device handle returned by lidar_create_device / lidar_open_device.
 * @param out    Output buffer, must not be NULL.
 * @return See return value convention above.
 */
 int lidar_get_ae_info(device_handle device, lidar_ae_info_t *out);
 /**
 * @brief Query current AWB (auto white balance) state.
 *
 * Sends AE opcode 0x30 and decodes the 25-byte little-endian payload.
 *
 * Output fields and their physical meaning:
 *   rgain        : R  channel gain (manual range 0.1 .. 4.0).
 *   grgain       : Gr channel gain, always 1.0 (device-fixed).
 *   gbgain       : Gb channel gain, always 1.0 (device-fixed).
 *   bgain        : B  channel gain (manual range 0.1 .. 4.0).
 *   cct          : correlated color temperature in Kelvin
 *                  (typically 2500 .. 8000 K).
 *   ccri         : color temperature deviation index (-50 .. 50,
 *                  signed; 0 means on the Planckian locus).
 *   is_converged : 1 = AWB has settled, 0 = still adjusting.
 *
 * @param device Device handle.
 * @param out    Output buffer, must not be NULL.
 * @return See return value convention above.
 */
 int lidar_get_awb_info(device_handle device, lidar_awb_info_t *out);
 /**
 * @brief Set AE mode and (in manual mode) exposure/gain.
 *
 * Behaviour:
 *   - mode == LIDAR_CAM_MODE_AUTO   : sends opcode 0x02 only;
 *                                     exposure_time / gain are ignored.
 *   - mode == LIDAR_CAM_MODE_MANUAL : sends opcode 0x03 to switch to
 *                                     manual AE, then opcode 0x06 to
 *                                     apply (exposure_time, gain).
 *
 * Parameter ranges and physical meaning
 * (manual mode only; out-of-range returns rc = 403):
 *
 *   exposure_time : 0.0001 s .. 0.033 s
 *       Sensor exposure time per frame. Longer = brighter but more
 *       motion blur and lower effective fps if it exceeds the frame
 *       period (1/fps). For dtof_fps = 290 (29 Hz, period ~34 ms) the
 *       upper bound 0.033 s is already at the frame limit.
 *
 *   gain          : 1.0 .. 64.0
 *       Analog gain applied to the raw sensor signal. Higher = brighter
 *       output but worse SNR. Typical sweet spot: 1.0 .. 8.0 for daylight,
 *       8.0 .. 32.0 for indoor / dim light, 32.0 .. 64.0 only when image
 *       must be visible at any cost.
 *
 * Recommended starting points by scene:
 *   bright outdoor : exposure 0.001~0.005 s, gain 1.0~2.0
 *   normal indoor  : exposure 0.008~0.015 s, gain 2.0~8.0
 *   dim light      : exposure 0.020~0.030 s, gain 8.0~32.0
 *
 * @param device        Device handle.
 * @param mode          See lidar_cam_mode_e.
 * @param exposure_time Exposure time in seconds (manual mode only).
 * @param gain          Analog gain (manual mode only).
 * @return See return value convention above.
 */
 int lidar_set_ae_param(device_handle device, lidar_cam_mode_e mode,
                       float exposure_time, float gain);
 /**
 * @brief Set AWB mode and (in manual mode) R/B channel gains.
 *
 * Behaviour:
 *   - mode == LIDAR_CAM_MODE_AUTO   : sends opcode 0x31 only;
 *                                     rgain / bgain are ignored.
 *   - mode == LIDAR_CAM_MODE_MANUAL : sends opcode 0x32 to switch to
 *                                     manual AWB, then opcode 0x33 to
 *                                     apply (rgain, bgain). Gr/Gb are
 *                                     fixed to 1.0 by the device.
 *
 * Parameter ranges and physical meaning
 * (manual mode only; out-of-range returns rc = 403):
 *
 *   rgain : 0.1 .. 4.0
 *       Multiplier on the R channel before color matrix. Higher rgain
 *       relative to bgain shifts the image toward warm (yellow/red).
 *
 *   bgain : 0.1 .. 4.0
 *       Multiplier on the B channel. Higher bgain relative to rgain
 *       shifts the image toward cool (blue).
 *
 * Color-temperature cookbook (approximate):
 *   warm (tungsten, sunset) : rgain ~2.0..2.5, bgain ~1.0..1.2
 *   neutral (daylight D65)  : rgain ~1.5..1.7, bgain ~1.8..2.0
 *   cool (cloudy, fluor.)   : rgain ~1.2..1.4, bgain ~2.2..2.6
 *
 * @param device Device handle.
 * @param mode   See lidar_cam_mode_e.
 * @param rgain  R channel gain (manual mode only).
 * @param bgain  B channel gain (manual mode only).
 * @return See return value convention above.
 */
 int lidar_set_awb_param(device_handle device, lidar_cam_mode_e mode,
                        float rgain, float bgain);
 #ifdef __cplusplus
 }
 #endif
-#endif // LIDAR_API_H
+#endif // LIDAR_API_H
@@ -0,0 +1,443 @@
 #ifndef LIDAR_TYPES_H
 #define LIDAR_TYPES_H
 #include <stdbool.h>
 #include <stdlib.h>
 #include <stdint.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
 #define LIDAR_SERIAL_MAX 64
 #define LIDAR_MODEL_MAX  64
 #define LIDAR_IP_MAX     64
 typedef void * device_handle;
 typedef enum {
    LIDAR_LOG_ERROR = 0,
    LIDAR_LOG_WARN,
    LIDAR_LOG_INFO,
    LIDAR_LOG_DEBUG,
 } lidar_log_level_e;
 typedef enum {
    LIDAR_OTA_ALGORITHM,
    LIDAR_OTA_FIRMWARE,
    LIDAR_OTA_SCRIPT,
    LIDAR_OTA_CALIBRATION
 } lidar_ota_type_e;
 typedef enum {
    LIDAR_MODE_RAW,
    LIDAR_MODE_SLAM,
 } lidar_mode_e;
 /**
 * @brief Data types for lidar_data_callback_t
 * 
 * Each type corresponds to a specific stream format in lidar_data_t.stream (capture_Image_List_t).
 * 
 * ┌─────────────────────────────────────────────────────────────────────────────────────────────┐
 * │ LIDAR_DT_RAW_RGB                                                                            │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: NV12 image data                                                             │
 * │     - pAddr: uint8_t* (Y plane followed by UV plane)                                        │
 * │     - width: 1536, height: 1280                                                             │
 * │     - length: width * height * 3 / 2 bytes                                                  │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_RAW_IMU                                                                            │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: IMU data                                                                    │
 * │     - pAddr: imu_convert_data_t*                                                            │
 * │       - accel[3]: float (m/s^2)                                                             │
 * │       - gyro[3]: float (rad/s)                                                              │
 * │       - stamp: uint64_t (ns)                                                                │
 * │       - sequence: uint64_t                                                                  │
 * │     - length: sizeof(imu_convert_data_t)                                                    │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_RAW_DTOF                                                                           │
 * │   imageCount: 4                                                                             │
 * │   Resolution: 256 x 192                                                                     │
 * │   imageList[0]: Depth image                                                                 │
 * │     - pAddr: float* (depth in meters)                                                       │
 * │     - length: 256 * 192 * sizeof(float)                                                     │
 * │   imageList[1]: Point cloud XYZ                                                             │
 * │     - pAddr: float* (x,y,z interleaved)                                                     │
 * │     - length: 256 * 192 * 3 * sizeof(float)                                                 │
 * │   imageList[2]: Confidence                                                                  │
 * │     - pAddr: uint8_t*                                                                       │
 * │     - length: 256 * 192 * sizeof(uint8_t)                                                   │
 * │   imageList[3]: Intensity/Reflectivity                                                      │
 * │     - pAddr: uint16_t*                                                                      │
 * │     - length: 256 * 192 * sizeof(uint16_t)                                                  │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_SLAM_CLOUD                                                                         │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: SLAM point cloud (XYZRGBA, fixed-point on the wire)                         │
 * │     - pAddr: slam_cloud_point_t*  (7 * int32_t per point, see struct below)                 │
 * │     - length: num_points * sizeof(slam_cloud_point_t)  ( == num_points * 28 bytes )         │
 * │     xyz are stored in 0.1 mm units: meters = xyz * SLAM_CLOUD_XYZ_TO_M                      │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_SLAM_ODOMETRY / LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ / LIDAR_DT_SLAM_ODOMETRY_TF        │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: Odometry data                                                               │
 * │     - pAddr: ros_odom_convert_complete_t*                                                   │
 * │       - timestamp_ns: uint64_t                                                              │
 * │       - pos[3]: int64_t (x,y,z in μm, divide by 1e6 for meters)                             │
 * │       - orient[4]: int64_t (quaternion x,y,z,w, divide by 1e6)                              │
 * │       - linear_velocity[3]: int64_t                                                         │
 * │       - angular_velocity[3]: int64_t                                                        │
 * │       - pose_cov[36]: double                                                                │
 * │       - twist_cov[36]: double                                                               │
 * │     - length: sizeof(ros_odom_convert_complete_t)                                           │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_DEV_STATUS                                                                         │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: Device status                                                               │
 * │     - pAddr: lidar_device_status_t*                                                         │
 * │     - length: sizeof(lidar_device_status_t)                                                 │
 * ├─────────────────────────────────────────────────────────────────────────────────────────────┤
 * │ LIDAR_DT_NTP                                                                                │
 * │   imageCount: 1                                                                             │
 * │   imageList[0]: PTP/NTP sync data                                                           │
 * │     - pAddr: ptp_sync_data_t*                                                               │
 * │       - delay: double                                                                       │
 * │       - offset: double                                                                      │
 * │     - length: sizeof(ptp_sync_data_t)                                                       │
 * └─────────────────────────────────────────────────────────────────────────────────────────────┘
 */
 typedef enum {
    LIDAR_DT_NONE = 0,              /**< No data */
    LIDAR_DT_RAW_RGB,               /**< RGB image (NV12 format, 1536x1280) */
    LIDAR_DT_RAW_IMU,               /**< IMU data (imu_convert_data_t) */
    LIDAR_DT_RAW_DTOF,              /**< DTOF raw data (depth + xyz + confidence + intensity, 256x192) */
    LIDAR_DT_SLAM_CLOUD,            /**< SLAM point cloud (XYZRGBA) */
    LIDAR_DT_SLAM_ODOMETRY,         /**< SLAM odometry (ros_odom_convert_complete_t) */
    LIDAR_DT_DEV_STATUS,            /**< Device status (lidar_device_status_t) */
    LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,/**< High frequency odometry (ros_odom_convert_complete_t) */
    LIDAR_DT_SLAM_ODOMETRY_TF,      /**< Map-Odom TF transform (ros_odom_convert_complete_t) */
    LIDAR_DT_SLAM_WIWC,             /**< WIWC odometry */
    LIDAR_DT_NTP                    /**< PTP/NTP sync data (ptp_sync_data_t) */
 } lidar_data_type_e;
 typedef struct {
    int8_t serial[LIDAR_SERIAL_MAX];
    int8_t model[LIDAR_MODEL_MAX];
    bool online;
    uint32_t initial_state;
 } lidar_device_info_t;
 typedef struct {
    float x, y, z;
    float intensity;
 } lidar_point_t;
 typedef struct {
    float intrinsics[9];
    float extrinsics[16];
 } lidar_calibration_t;
 #define DEVICE_MAX_CH_NUMBER 4
 typedef struct {
    uint64_t timestamp_ns;
    int64_t pos[3];
    int64_t orient[4];
 } ros2_odom_convert_t;
 typedef struct {
    uint64_t timestamp_ns;
    int64_t pos[3];       // x, y, z in μm
    int64_t orient[4];    // quaternion x, y, z, w in 1e6 precision
    int64_t linear_velocity[3];
    int64_t angular_velocity[3];
    double pose_cov[36];
    double twist_cov[36];
 } ros_odom_convert_complete_t;
 typedef struct {
    double delay;
    double offset;
 } ptp_sync_data_t;
 /* ---------------------------------------------------------------------
 * SLAM cloud wire format.
 *
 * One LIDAR_DT_SLAM_CLOUD point on the bus is a 7 * int32_t record:
 *     xyz[0..2] : x, y, z in 0.1 mm fixed-point.
 *                 meters = xyz * SLAM_CLOUD_XYZ_TO_M.
 *     rgba[0..3]: r, g, b, a; each stored in the low byte of an int32_t.
 *
 * Consumers (SDK hooks, ROS driver, etc.) should reference this struct
 * and the scale macro below as the single source of truth rather than
 * re-hardcoding the stride or the divisor.
 * ------------------------------------------------------------------- */
 #define SLAM_CLOUD_XYZ_TO_M   (1.0e-4)   /* device 0.1mm units -> meters */
 #define SLAM_CLOUD_XYZ_FROM_M (1.0e4)    /* meters -> device 0.1mm units */
 typedef struct {
    int32_t xyz[3];    /* x, y, z in 0.1 mm fixed-point */
    int32_t rgba[4];   /* r, g, b, a; only low byte of each is meaningful */
 } slam_cloud_point_t;
 typedef struct icm_6aixs_data_t {
 	int16_t aacx;
 	int16_t aacy; 
 	int16_t aacz;
 	int16_t gyrox;
 	int16_t gyroy;
 	int16_t gyroz;
 	uint8_t valid;
 	uint32_t nums;
 	uint8_t fsync_pack;
 	uint16_t interval;
 	uint64_t stamp;
 } icm_6aixs_data_t;
 typedef struct {
    float accel_x;
    float accel_y;
    float accel_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
    uint64_t stamp;
    uint64_t sequence;
 } imu_convert_data_t;
 typedef struct {
    uint32_t length;
    uint64_t sequence;
    uint64_t timestamp;
    uint64_t interval;
    void* pAddr;
    uint32_t width;
    uint32_t height;
 } buffer_List_t;
 typedef struct capture_Image_List_t {
    uint32_t imageCount;
    buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
 } capture_Image_List_t;
 typedef struct {
    uint32_t type;
    capture_Image_List_t stream;
 } lidar_data_t;
 typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
 typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
 typedef struct {
    lidar_data_callback_t data_callback;
    void *user_data;
 } lidar_data_callback_info_t;
 typedef struct {
    int major;
    int minor;
    int patch;
 }lidar_version_t;
 typedef struct {
    lidar_version_t kernel_version;
    lidar_version_t mcu_version;
    lidar_version_t soc_version;
    lidar_version_t Daemon_proc_version;
    lidar_version_t slam_version;
 } lidar_fireware_version_t;
 /**
 * @brief RGB image sensor frame rate
 * 
 */
 typedef struct{
    int configured_odr;/*rgb image sensor frame rate, offset: */
    int tx_odr;/*The actual rgb image sensor frame rate, offset: */
 } lidar_rgb_sensor_status_t;
 /**
 * @brief DTOF Lidar frame rate
 * 
 */
 typedef struct{
    int configured_odr;/* dtof lidar sensor frame rate, offset: */
    int tx_odr;/*The actual dtof lidar sensor frame rate, offset: */
    int subframe_odr;/*DTOF 6行为一组 这个是组间隔时间*/
    short tx_temp;/* dtof lidar tx temp  offset: */
    short rx_temp;/* dtof lidar rx temp offset:  */
 } lidar_dtof_sensor_status_t;
 /**
 * @brief IMU Sensor
 * 
 */
 typedef struct{
    int configured_odr;
    int tx_odr;
 } lidar_imu_sensor_status_t;
 typedef struct{
    int package_temp;/*SOC整体温度*/
    // int bigcore_temp;/*大核集群温度：4*A76*/
    // int littlecore_temp;/*小核集群温度：4*A53*/
    int cpu_temp;
    int center_temp;/*SOC中心温度：4*A53*/
    int gpu_temp;/* GPU模块温度 */
    int npu_temp;/* NPU模块温度 */
 } lidar_soc_thermal_t;
 typedef struct
 {
    double uptime_seconds;
    lidar_soc_thermal_t soc_thermal; /*offset: 0*/
    int cpu_use_rate[8];/*cpu 使用率,offset:  */
    int ram_use_rate;/*运行内存使用率 ,offset:  */
    lidar_rgb_sensor_status_t rgb_sensor;
    lidar_dtof_sensor_status_t dtof_sensor;
    lidar_imu_sensor_status_t imu_sensor;
    int slam_cloud_tx_odr; /* Actual frame rate of slam cloud offset:  */
    int slam_odom_tx_odr; /* Actual frame rate of slam odom offset:  */
    int slam_odom_highfreq_tx_odr; /* Actual frame rate of slam odom offset:  */
 } lidar_device_status_t;
 typedef enum {
    LIDAR_DEVICE_NONE = 0,
    LIDAR_DEVICE_NOT_INITIALIZED,
    LIDAR_DEVICE_INITIALIZED,
    LIDAR_DEVICE_STREAMING,
    LIDAR_DEVICE_STREAM_STOPPED,
 } lidar_device_initial_state_e;
 typedef enum {
    LIDAR_DEPTH_ODR_10HZ   = 0,
    LIDAR_DEPTH_ODR_14_5HZ = 1,
    LIDAR_DEPTH_ODR_29HZ   = 2,
 } lidar_depth_odr_e;
 typedef struct {
    lidar_depth_odr_e odr;
 } lidar_depth_para_t;
 /* ---------------------------------------------------------------------
 * Camera AE / AWB control types.
 *
 * The host SDK forwards AE/AWB requests through the USB control channel
 * (CMD_CODE_CONTROL_CMD + SYS_CONTROL_AE_UDP). The device-side lydapp
 * relays them via UDP loopback to its ISP service. See
 * sdk/api/Host_USB_AE_Protocol.md for the wire-level details.
 * ------------------------------------------------------------------- */
 /**
 * @brief AE/AWB control mode used by lidar_set_ae_param / lidar_set_awb_param.
 *
 * - AUTO   : the device runs its own AE/AWB convergence loop. The two
 *            float parameters of the corresponding Set call are ignored.
 * - MANUAL : the device locks AE/AWB and applies the user-supplied
 *            (exposure_time, gain) or (rgain, bgain). Out-of-range
 *            values are rejected with rc = 403
 *            (LIDAR_AE_PARAM_OUT_OF_RANGE).
 */
 typedef enum {
    LIDAR_CAM_MODE_AUTO   = 0, /**< switch to auto AE / AWB */
    LIDAR_CAM_MODE_MANUAL = 1, /**< switch to manual AE / AWB and apply params */
 } lidar_cam_mode_e;
 /**
 * @brief Current AE status returned by lidar_get_ae_info().
 *
 * Field-by-field meaning and typical range:
 *
 *   exposure_time : current sensor exposure time, in seconds.
 *                   Manual-mode valid range: 0.0001 .. 0.033.
 *                   In auto mode varies with scene illumination.
 *   gain          : current analog gain (linear, not dB).
 *                   Manual-mode valid range: 1.0 .. 64.0.
 *                   Higher value = brighter output but worse SNR.
 *   iso           : equivalent ISO speed, typically 100 .. 6400.
 *                   Derived from gain; informational only.
 *   brightness    : average frame brightness in [0, 255]. AE target
 *                   converges toward a mid-range value.
 *   is_converged  : 1 = AE has settled, 0 = still adjusting.
 *   env_lv        : ambient luminance index, typically 0 .. 15
 *                   (higher = brighter scene).
 *   fps           : actual frame rate in Hz, follows dtof_fps config
 *                   (~10 / 14.5 / 29).
 */
 typedef struct {
    float    exposure_time; /**< current exposure time (s), 0.0001..0.033 */
    float    gain;          /**< current analog gain, 1.0..64.0 */
    int32_t  iso;           /**< equivalent ISO, ~100..6400 */
    float    brightness;    /**< average frame brightness, 0..255 */
    uint8_t  is_converged;  /**< 1 = AE converged, 0 = not converged */
    float    env_lv;        /**< ambient luminance level, ~0..15 */
    float    fps;           /**< current frame rate (Hz) */
 } lidar_ae_info_t;
 /**
 * @brief Current AWB status returned by lidar_get_awb_info().
 *
 * Field-by-field meaning and typical range:
 *
 *   rgain        : R channel gain. Manual-mode valid range: 0.1 .. 4.0.
 *                  Raising rgain relative to bgain shifts the image
 *                  toward warm (yellow/red).
 *   grgain       : Gr channel gain. Device-fixed at 1.0, not adjustable.
 *   gbgain       : Gb channel gain. Device-fixed at 1.0, not adjustable.
 *   bgain        : B channel gain. Manual-mode valid range: 0.1 .. 4.0.
 *                  Raising bgain relative to rgain shifts the image
 *                  toward cool (blue).
 *   cct          : correlated color temperature in Kelvin, typically
 *                  2500 .. 8000 K.
 *   ccri         : color temperature deviation index, signed value
 *                  roughly in -50 .. 50; 0 = on the Planckian locus.
 *   is_converged : 1 = AWB has settled, 0 = still adjusting.
 */
 typedef struct {
    float    rgain;         /**< R channel gain, 0.1..4.0 */
    float    grgain;        /**< Gr channel gain, device-fixed 1.0 */
    float    gbgain;        /**< Gb channel gain, device-fixed 1.0 */
    float    bgain;         /**< B channel gain, 0.1..4.0 */
    float    cct;           /**< color temperature (K), ~2500..8000 */
    float    ccri;          /**< color temperature deviation, ~-50..50 */
    uint8_t  is_converged;  /**< 1 = AWB converged, 0 = not converged */
 } lidar_awb_info_t;
 /**
 * @brief AE/AWB device-side error codes.
 *
 * Mapped to positive return values of lidar_get_ae_info / lidar_set_ae_param /
 * lidar_get_awb_info / lidar_set_awb_param when the device replies with
 * CMD_CODE_FAIL. See Host_USB_AE_Protocol.md section 6.
 */
 typedef enum {
    LIDAR_AE_OK                   = 0,
    LIDAR_AE_BAD_REQUEST          = 400, /**< payload too short */
    LIDAR_AE_UNSUPPORTED_OPCODE   = 401,
    LIDAR_AE_BAD_PARAM_LEN        = 402,
    LIDAR_AE_PARAM_OUT_OF_RANGE   = 403,
    LIDAR_AE_SOCKET_ERROR         = 404,
    LIDAR_AE_NO_RESPONSE          = 405, /**< ae_control UDP timeout */
    LIDAR_AE_UNKNOWN_OPCODE       = 0xFF,/**< status byte from ae_control */
 } lidar_ae_error_e;
 #ifdef __cplusplus
 }
 #endif
 #endif
@@ -26,14 +26,12 @@ def generate_launch_description():
        default_value=os.path.join(package_dir, 'config', 'odin_ros2.rviz'),
        description='Path to RViz2 config file'
    )
-    
+
    # Declare launch rviz parameter
    launch_rviz_arg = DeclareLaunchArgument(
        'launch_rviz',
        default_value='false',
        description='Whether to launch RViz2'
    )
    # Create main node
    host_sdk_node = Node(
@@ -100,7 +98,7 @@ def generate_launch_description():
    ld = LaunchDescription()
    ld.add_action(config_file_arg)
    ld.add_action(rviz_config_arg)  # Add RViz configuration argument
-    ld.add_action(launch_rviz_arg)  # Add launch_rviz argument
+    ld.add_action(launch_rviz_arg)
    ld.add_action(host_sdk_node)
    ld.add_action(pcd2depth_node)
    ld.add_action(cloud_reprojection_node)
@@ -108,4 +106,3 @@ def generate_launch_description():
    ld.add_action(rviz_node)  # Add RViz node
    return ld
@@ -21,6 +21,11 @@
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>ament_index_cpp</depend>
  <!-- AE/AWB debug services (custom .srv files in srv/) -->
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
  <!-- Specify build type as ament -->
  <export>  
    <build_type>ament_cmake</build_type>  
@@ -16,7 +16,11 @@
  <depend>nav_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>image_transport</depend>
-  
+
  <!-- AE/AWB debug services (custom .srv files in srv/) -->
  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  <!-- System dependencies -->
  <depend>eigen</depend>
  <depend>opencv</depend>
@@ -21,6 +21,11 @@
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>ament_index_cpp</depend>
  <!-- AE/AWB debug services (custom .srv files in srv/) -->
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
  <!-- Specify build type as ament -->
  <export>  
    <build_type>ament_cmake</build_type>  
@@ -67,13 +67,27 @@ build_workspace() {
    cd $WS_DIR
    rm -rf build install log
-    # Ensure ROS2 environment is loaded
+    # Ensure ROS2 environment is loaded.
-    if [ -f "/opt/ros/foxy/setup.bash" ]; then
+    # 1) Prefer the currently active distro (ROS_DISTRO env var) if its setup.bash exists.
-        source "/opt/ros/foxy/setup.bash"
+    # 2) Otherwise probe a known list of ROS2 distros from newest to oldest.
-    elif [ -f "/opt/ros/galactic/setup.bash" ]; then
+    ROS2_DISTRO_CANDIDATES=("rolling" "jazzy" "iron" "humble" "galactic" "foxy")
-        source "/opt/ros/galactic/setup.bash"
+    ROS2_SETUP_BASH=""
-    elif [ -f "/opt/ros/humble/setup.bash" ]; then
+
-        source "/opt/ros/humble/setup.bash"
+    if [ -n "${ROS_DISTRO}" ] && [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then
        ROS2_SETUP_BASH="/opt/ros/${ROS_DISTRO}/setup.bash"
    else
        for distro in "${ROS2_DISTRO_CANDIDATES[@]}"; do
            if [ -f "/opt/ros/${distro}/setup.bash" ]; then
                ROS2_SETUP_BASH="/opt/ros/${distro}/setup.bash"
                break
            fi
        done
    fi
    if [ -n "${ROS2_SETUP_BASH}" ]; then
        echo -e "${GREEN}Sourcing ROS2 environment: ${ROS2_SETUP_BASH}${NC}"
        # shellcheck disable=SC1090
        source "${ROS2_SETUP_BASH}"
    else
        echo -e "${RED}Could not find ROS2 setup.bash file. Please ensure ROS2 is installed.${NC}"
        return 1
@@ -0,0 +1,18 @@
 # rosbag2_qos.yaml
 # QoS overrides for rosbag2 recording
 # 用于 rosbag2 录制的 QoS 配置
 # 
 # Usage / 使用方法:
 #   ros2 bag record -a --qos-profile-overrides-path rosbag2_qos.yaml
 # High frequency data (IMU 400Hz, Odom 100Hz)
 # 高频数据 (IMU 400Hz, 里程计 100Hz)
 /odin1/imu:
  reliability: reliable
  history: keep_last
  depth: 4000
 /odin1/odometry_highfreq:
  reliability: reliable
  history: keep_last
  depth: 4000
@@ -45,13 +45,23 @@ limitations under the License.
 #ifdef ROS2
    #include <ament_index_cpp/get_package_share_directory.hpp>
    #include <rclcpp/rclcpp.hpp>
    // AE/AWB debug services (custom .srv generated from this package).
    #include "odin_ros_driver/srv/get_ae.hpp"
    #include "odin_ros_driver/srv/get_awb.hpp"
    #include "odin_ros_driver/srv/set_ae.hpp"
    #include "odin_ros_driver/srv/set_awb.hpp"
 #else
    #include <ros/package.h>
-    #include <ros/ros.h> 
+    #include <ros/ros.h>
    // AE/AWB debug services (custom .srv generated by catkin).
    #include "odin_ros_driver/GetAe.h"
    #include "odin_ros_driver/GetAwb.h"
    #include "odin_ros_driver/SetAe.h"
    #include "odin_ros_driver/SetAwb.h"
 #endif
-#define ros_driver_version "0.10.5"
+#define ros_driver_version "0.12.0"
 #define required_firmware_version_major 0
-#define required_firmware_version_minor 11
+#define required_firmware_version_minor 12
 #define required_firmware_version_patch 0
 // Global variable declarations
@@ -102,7 +112,7 @@ static std::thread g_imu_thread;
 static std::queue<imu_convert_data_t> g_imu_queue;
 static std::mutex g_imu_queue_mutex;
 static std::condition_variable g_imu_queue_cv;
-static const size_t IMU_QUEUE_MAX_SIZE = 200;
+static const size_t IMU_QUEUE_MAX_SIZE = 8000;
 double get_ptp_smoothed_delay() {
    return g_ptp_delay_smooth.load(std::memory_order_relaxed);
@@ -364,10 +374,27 @@ static void signal_handler(int signum) {
            dev_status_csv_file = nullptr;
        }
-        // Shutdown ROS
+        // IMPORTANT: destroy ROS publishers/subscribers BEFORE shutting down
        // the ROS context. Otherwise the global g_ros_object (a shared_ptr)
        // is destroyed by static finalizers AFTER rclcpp::shutdown(), which
        // triggers a flood of "Failed to delete datawriter" /
        // "Error in destruction of rcl publisher handle: cannot publish data".
        // Mirrors the clean-exit ordering at the end of main().
        // 必须在关闭 ROS 上下文之前先析构 publisher/subscriber。否则全局
        // g_ros_object（shared_ptr）会在 rclcpp::shutdown() 之后由静态析构器
        // 销毁，rmw 层会刷出大量 "Failed to delete datawriter" /
        // "Error in destruction of rcl publisher handle" 噪声。此处与 main()
        // 末尾的正常退出顺序保持一致。
        #ifdef ROS2
            if (g_ros_object) {
                g_ros_object.reset();
            }
            rclcpp::shutdown();
        #else
            if (g_ros_object) {
                delete g_ros_object;
                g_ros_object = nullptr;
            }
            ros::shutdown();
        #endif
@@ -1364,7 +1391,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
                RCLCPP_INFO(rclcpp::get_logger(__func__), "Daemon_proc_version: V%d.%d.%d",version.Daemon_proc_version.major,version.Daemon_proc_version.minor,version.Daemon_proc_version.patch);
                RCLCPP_INFO(rclcpp::get_logger(__func__), "slam_version: V%d.%d.%d",version.slam_version.major,version.slam_version.minor,version.slam_version.patch);
            #else
-                ROS_INFO("ros_driver_version:%s, recommended_firmware_version:%d.%d.%d", ros_driver_version, required_firmware_version_major, required_firmware_version_minor, required_firmware_version_patch);
+                ROS_INFO("ros_driver_version:%s, recommended_min_firmware_version:%d.%d.%d", ros_driver_version, required_firmware_version_major, required_firmware_version_minor, required_firmware_version_patch);
                ROS_INFO("get version success.");
                ROS_INFO("kernel_version: V%d.%d.%d",version.kernel_version.major,version.kernel_version.minor,version.kernel_version.patch);
                ROS_INFO("mcu_version: V%d.%d.%d",version.mcu_version.major,version.mcu_version.minor,version.mcu_version.patch);
@@ -1373,7 +1400,17 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
                ROS_INFO("slam_version: V%d.%d.%d",version.slam_version.major,version.slam_version.minor,version.slam_version.patch);
            #endif
-            if (version.soc_version.major < required_firmware_version_major || (version.soc_version.minor < required_firmware_version_minor) || (version.soc_version.patch < required_firmware_version_patch)) {
+            // Lexicographic compare on (major, minor, patch): treat the three
            // fields as a single ordered tuple. e.g. 0.12.0 must be considered
            // higher than 0.11.99, and 0.11.11 lower than 0.12.0.
            const bool soc_version_too_low =
                (version.soc_version.major <  required_firmware_version_major) ||
                (version.soc_version.major == required_firmware_version_major &&
                    version.soc_version.minor <  required_firmware_version_minor) ||
                (version.soc_version.major == required_firmware_version_major &&
                    version.soc_version.minor == required_firmware_version_minor &&
                    version.soc_version.patch <  required_firmware_version_patch);
            if (soc_version_too_low) {
                #ifdef ROS2
                    RCLCPP_ERROR(rclcpp::get_logger(__func__),"The soc version is too low, please upgrade the device firmware to at least %d.%d.%d\n",required_firmware_version_major,required_firmware_version_minor,required_firmware_version_patch);
                #else
@@ -1558,6 +1595,8 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
            dtofpara.odr = LIDAR_DEPTH_ODR_10HZ;
        } else if (g_dtof_fps == 145) {
            dtofpara.odr = LIDAR_DEPTH_ODR_14_5HZ;
        } else if (g_dtof_fps == 290){
            dtofpara.odr = LIDAR_DEPTH_ODR_29HZ;
        } else {
            // Default to 14.5Hz if invalid value
            dtofpara.odr = LIDAR_DEPTH_ODR_14_5HZ;
@@ -1833,7 +1872,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        }
        g_param_monitor_running = true;
        g_param_monitor_thread = std::thread(custom_parameter_monitor);
-        
+
        #ifdef ROS2
            RCLCPP_INFO(rclcpp::get_logger("device_cb"),
                       "Command interface ready. Use: echo 'set save_map 1' > %s", g_command_file_path.c_str());
@@ -1873,7 +1912,7 @@ static void lidar_device_callback(const lidar_device_info_t* device, bool attach
        if (g_param_monitor_thread.joinable()) {
            g_param_monitor_thread.join();
        }
-        
+
        clear_all_queues();
@@ -1897,10 +1936,232 @@ int main(int argc, char *argv[])
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("lydros_node");
    g_ros_object = std::make_shared<MultiSensorPublisher>(node);
    // -----------------------------------------------------------------
    // AE/AWB debug services (ROS2). Allow a side terminal to tune the
    // camera AE/AWB at runtime via `ros2 service call`, while the
    // driver keeps streaming.
    //
    //   /odin1/get_ae   odin_ros_driver/srv/GetAe   - query AE state
    //   /odin1/get_awb  odin_ros_driver/srv/GetAwb  - query AWB state
    //   /odin1/set_ae   odin_ros_driver/srv/SetAe   - set AE mode/params
    //   /odin1/set_awb  odin_ros_driver/srv/SetAwb  - set AWB mode/params
    //
    // Request semantics (set_ae / set_awb):
    //   mode == 0 (AUTO)   : device runs its own AE/AWB loop; the
    //                        float fields in the request are ignored.
    //   mode == 1 (MANUAL) : device locks AE/AWB and applies the
    //                        provided values. Out-of-range values are
    //                        rejected by the device with rc = 403.
    //
    // Parameter ranges (manual mode):
    //   set_ae .exposure_time : 0.0001 s .. 0.033 s
    //   set_ae .gain          : 1.0     .. 64.0
    //   set_awb.rgain         : 0.1     .. 4.0
    //   set_awb.bgain         : 0.1     .. 4.0
    //
    // Response rc convention (all 4 services):
    //   0      success                          (success = true)
    //   400..  device-side error, see lidar_ae_error_e
    //   -100   driver has not opened the device yet
    //   <0     other SDK-side error (USB / timeout / malformed reply)
    //
    // Concurrency: the underlying SDK calls are serialised by an
    // internal mutex shared with other control commands, so no extra
    // locking is required here.
    // -----------------------------------------------------------------
    auto srv_get_ae = node->create_service<odin_ros_driver::srv::GetAe>(
        "/odin1/get_ae",
        [](const std::shared_ptr<odin_ros_driver::srv::GetAe::Request>  /*req*/,
                 std::shared_ptr<odin_ros_driver::srv::GetAe::Response> res) {
            if (odinDevice == nullptr) {
                res->success = false;
                res->rc = -100;
                return;
            }
            lidar_ae_info_t info{};
            int rc = lidar_get_ae_info(odinDevice, &info);
            res->rc            = rc;
            res->success       = (rc == 0);
            res->exposure_time = info.exposure_time;
            res->gain          = info.gain;
            res->iso           = info.iso;
            res->brightness    = info.brightness;
            res->is_converged  = info.is_converged;
            res->env_lv        = info.env_lv;
            res->fps           = info.fps;
        });
    auto srv_get_awb = node->create_service<odin_ros_driver::srv::GetAwb>(
        "/odin1/get_awb",
        [](const std::shared_ptr<odin_ros_driver::srv::GetAwb::Request>  /*req*/,
                 std::shared_ptr<odin_ros_driver::srv::GetAwb::Response> res) {
            if (odinDevice == nullptr) {
                res->success = false;
                res->rc = -100;
                return;
            }
            lidar_awb_info_t info{};
            int rc = lidar_get_awb_info(odinDevice, &info);
            res->rc           = rc;
            res->success      = (rc == 0);
            res->rgain        = info.rgain;
            res->grgain       = info.grgain;
            res->gbgain       = info.gbgain;
            res->bgain        = info.bgain;
            res->cct          = info.cct;
            res->ccri         = info.ccri;
            res->is_converged = info.is_converged;
        });
    auto srv_set_ae = node->create_service<odin_ros_driver::srv::SetAe>(
        "/odin1/set_ae",
        [](const std::shared_ptr<odin_ros_driver::srv::SetAe::Request>  req,
                 std::shared_ptr<odin_ros_driver::srv::SetAe::Response> res) {
            if (odinDevice == nullptr) {
                res->success = false;
                res->rc = -100;
                return;
            }
            lidar_cam_mode_e mode = (req->mode == 1)
                ? LIDAR_CAM_MODE_MANUAL : LIDAR_CAM_MODE_AUTO;
            int rc = lidar_set_ae_param(odinDevice, mode,
                                        req->exposure_time, req->gain);
            res->rc      = rc;
            res->success = (rc == 0);
        });
    auto srv_set_awb = node->create_service<odin_ros_driver::srv::SetAwb>(
        "/odin1/set_awb",
        [](const std::shared_ptr<odin_ros_driver::srv::SetAwb::Request>  req,
                 std::shared_ptr<odin_ros_driver::srv::SetAwb::Response> res) {
            if (odinDevice == nullptr) {
                res->success = false;
                res->rc = -100;
                return;
            }
            lidar_cam_mode_e mode = (req->mode == 1)
                ? LIDAR_CAM_MODE_MANUAL : LIDAR_CAM_MODE_AUTO;
            int rc = lidar_set_awb_param(odinDevice, mode,
                                         req->rgain, req->bgain);
            res->rc      = rc;
            res->success = (rc == 0);
        });
    RCLCPP_INFO(node->get_logger(),
        "AE/AWB debug services ready: "
        "/odin1/get_ae /odin1/get_awb /odin1/set_ae /odin1/set_awb");
 #else
    ros::init(argc, argv, "lydros_node");
    ros::NodeHandle nh;
    g_ros_object = new MultiSensorPublisher(nh);
    // -----------------------------------------------------------------
    // AE/AWB debug services (ROS1). Same service names, srv schema,
    // parameter ranges and rc convention as the ROS2 branch above
    // (see the comment block before the ROS2 create_service calls).
    // A side terminal can invoke these via `rosservice call ...`.
    // -----------------------------------------------------------------
    ros::ServiceServer srv_get_ae = nh.advertiseService<
        odin_ros_driver::GetAe::Request,
        odin_ros_driver::GetAe::Response>(
        "/odin1/get_ae",
        boost::function<bool(odin_ros_driver::GetAe::Request&,
                             odin_ros_driver::GetAe::Response&)>(
            [](odin_ros_driver::GetAe::Request  & /*req*/,
               odin_ros_driver::GetAe::Response &res) -> bool {
                if (odinDevice == nullptr) {
                    res.success = false;
                    res.rc = -100;
                    return true;
                }
                lidar_ae_info_t info{};
                int rc = lidar_get_ae_info(odinDevice, &info);
                res.rc            = rc;
                res.success       = (rc == 0);
                res.exposure_time = info.exposure_time;
                res.gain          = info.gain;
                res.iso           = info.iso;
                res.brightness    = info.brightness;
                res.is_converged  = info.is_converged;
                res.env_lv        = info.env_lv;
                res.fps           = info.fps;
                return true;
            }));
    ros::ServiceServer srv_get_awb = nh.advertiseService<
        odin_ros_driver::GetAwb::Request,
        odin_ros_driver::GetAwb::Response>(
        "/odin1/get_awb",
        boost::function<bool(odin_ros_driver::GetAwb::Request&,
                             odin_ros_driver::GetAwb::Response&)>(
            [](odin_ros_driver::GetAwb::Request  & /*req*/,
               odin_ros_driver::GetAwb::Response &res) -> bool {
                if (odinDevice == nullptr) {
                    res.success = false;
                    res.rc = -100;
                    return true;
                }
                lidar_awb_info_t info{};
                int rc = lidar_get_awb_info(odinDevice, &info);
                res.rc           = rc;
                res.success      = (rc == 0);
                res.rgain        = info.rgain;
                res.grgain       = info.grgain;
                res.gbgain       = info.gbgain;
                res.bgain        = info.bgain;
                res.cct          = info.cct;
                res.ccri         = info.ccri;
                res.is_converged = info.is_converged;
                return true;
            }));
    ros::ServiceServer srv_set_ae = nh.advertiseService<
        odin_ros_driver::SetAe::Request,
        odin_ros_driver::SetAe::Response>(
        "/odin1/set_ae",
        boost::function<bool(odin_ros_driver::SetAe::Request&,
                             odin_ros_driver::SetAe::Response&)>(
            [](odin_ros_driver::SetAe::Request  &req,
               odin_ros_driver::SetAe::Response &res) -> bool {
                if (odinDevice == nullptr) {
                    res.success = false;
                    res.rc = -100;
                    return true;
                }
                lidar_cam_mode_e mode = (req.mode == 1)
                    ? LIDAR_CAM_MODE_MANUAL : LIDAR_CAM_MODE_AUTO;
                int rc = lidar_set_ae_param(odinDevice, mode,
                                            req.exposure_time, req.gain);
                res.rc      = rc;
                res.success = (rc == 0);
                return true;
            }));
    ros::ServiceServer srv_set_awb = nh.advertiseService<
        odin_ros_driver::SetAwb::Request,
        odin_ros_driver::SetAwb::Response>(
        "/odin1/set_awb",
        boost::function<bool(odin_ros_driver::SetAwb::Request&,
                             odin_ros_driver::SetAwb::Response&)>(
            [](odin_ros_driver::SetAwb::Request  &req,
               odin_ros_driver::SetAwb::Response &res) -> bool {
                if (odinDevice == nullptr) {
                    res.success = false;
                    res.rc = -100;
                    return true;
                }
                lidar_cam_mode_e mode = (req.mode == 1)
                    ? LIDAR_CAM_MODE_MANUAL : LIDAR_CAM_MODE_AUTO;
                int rc = lidar_set_awb_param(odinDevice, mode,
                                             req.rgain, req.bgain);
                res.rc      = rc;
                res.success = (rc == 0);
                return true;
            }));
    ROS_INFO("AE/AWB debug services ready: "
             "/odin1/get_ae /odin1/get_awb /odin1/set_ae /odin1/set_awb");
 #endif
    // Register signal handlers for Ctrl+C
@@ -2260,7 +2521,7 @@ int main(int argc, char *argv[])
    if (g_param_monitor_thread.joinable()) {
        g_param_monitor_thread.join();
    }
-    
+
    // lidar_system_deinit();
@@ -0,0 +1,13 @@
 # Query current AE (auto exposure) state.
 # Request: empty.
 # Response: success + raw return code + decoded fields.
 ---
 bool success            # true if rc == 0
 int32 rc                # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
 float32 exposure_time   # seconds (manual range 0.0001 .. 0.033)
 float32 gain            # analog gain
 int32 iso               # equivalent ISO
 float32 brightness      # average frame brightness
 uint8 is_converged      # 1=converged, 0=not converged
 float32 env_lv          # ambient luminance level
 float32 fps             # current frame rate
@@ -0,0 +1,13 @@
 # Query current AWB (auto white balance) state.
 # Request: empty.
 # Response: success + raw return code + decoded fields.
 ---
 bool success            # true if rc == 0
 int32 rc                # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
 float32 rgain
 float32 grgain
 float32 gbgain
 float32 bgain
 float32 cct             # color temperature in Kelvin
 float32 ccri            # color temperature deviation
 uint8 is_converged      # 1=converged, 0=not converged
@@ -0,0 +1,14 @@
 # Set AE (auto exposure) mode and (in manual mode) exposure / gain.
 #
 # mode == 0 (AUTO)   : sends opcode 0x02 only; exposure_time and gain ignored.
 # mode == 1 (MANUAL) : sends opcode 0x03 then 0x06 (exposure_time, gain).
 #
 # Valid manual ranges (device-enforced; out-of-range returns rc=403):
 #   exposure_time : 0.0001 s .. 0.033 s
 #   gain          : 1.0     .. 64.0
 uint8 mode
 float32 exposure_time
 float32 gain
 ---
 bool success
 int32 rc                # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
@@ -0,0 +1,15 @@
 # Set AWB (auto white balance) mode and (in manual mode) R/B gains.
 #
 # mode == 0 (AUTO)   : sends opcode 0x31 only; rgain and bgain ignored.
 # mode == 1 (MANUAL) : sends opcode 0x32 then 0x33 (rgain, bgain).
 #                     Gr/Gb are fixed to 1.0 by the device.
 #
 # Valid manual ranges:
 #   rgain : 0.1 .. 4.0
 #   bgain : 0.1 .. 4.0
 uint8 mode
 float32 rgain
 float32 bgain
 ---
 bool success
 int32 rc                # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
@@ -47,7 +47,7 @@ action:
    - rr_wheel
  wheel_indices: [12, 13, 14, 15]
  scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
-  default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
+  default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
 motor_mapping:
  can_id_map:
@@ -4,8 +4,47 @@
    motor_hz: 200.0
    status_hz: 10.0
    target_timeout_ms: 150.0
-    model_path: policies/model_rough.onnx
+    event_log_dir: "logs_v2_web"
-    use_cuda: true  # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
+    model_engine_path: policies/model_6800_fp16.engine
    prefer_tensorrt: true
    model_path: policies/model_6800.onnx
    rough_model_engine_path: policies/model_6800_fp16.engine
    crawl_model_path: policies/model_crawl.onnx  # unused while crawl_backend is "ik"
    crawl_model_engine_path: ""
    wall_model_path: policies/model_84.onnx
    wall_model_engine_path: policies/model_84_fp16.engine
    rough_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
    wall_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
    crawl_backend: "ik"
    crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0]
    crawl_ik_wheel_linear_gain: 12.5
    crawl_ik_wheel_yaw_gain: 8.0
    crawl_ik_max_wheel_speed: 12.0
    crawl_ik_abduction_clip: 0.45
    crawl_ik_yaw_rate_kp: 0.5
    crawl_ik_imu_posture: false
    crawl_ik_encoder_posture_kp: 0.0
    crawl_ik_encoder_posture_max: 0.03
    crawl_ik_encoder_guard: false
    crawl_ik_encoder_guard_start: 0.28
    crawl_ik_encoder_guard_stop: 0.65
    crawl_ik_imu_guard: true
    crawl_ik_imu_guard_start_deg: 12.0
    crawl_ik_imu_guard_stop_deg: 28.0
    model_switch_transition_s: 0.9
    model_switch_min_transition_s: 0.4
    model_switch_to_stand_transition_scale: 2.1
    model_switch_to_model_transition_scale: 2.4
    model_switch_stand_hold_s: 0.45
    model_switch_stand_max_err: 0.18
    model_switch_stand_max_vel: 0.8
    model_switch_release_scale: 1.0
    runtime_max_vx: 0.9
    runtime_max_vy: 0.5
    runtime_max_yaw_rate: 0.85
    debug_trace_enabled: true
    debug_trace_decimation: 1
    use_cuda: true  # enable CUDA Execution Provider on Orin Nano GPU
    contract_file: deployment_contract.yaml
    dry_run: false
    can0_name: "can0"
@@ -13,39 +52,60 @@
    imu_topic: "/odin1/imu"
    odom_topic: "/odom"
-    # Remote UART / SBUS parameters, aligned with the Python deployment
+    # Remote UART / SBUS parameters, aligned with the first-generation Python deployment
-    remote_enabled: true
+    remote_enabled: false # true
    remote_port: "/dev/ttyACM0"
    remote_baudrate: 100000
    remote_timeout: 0.02
    remote_axis_deadzone: 40
    remote_active_threshold: 40
    remote_axis_full_scale: 660.0
-    remote_max_vx: 0.8
+    remote_max_vx: 0.9
-    remote_max_vy: 0.3
+    remote_max_vy: 0.5
-    remote_max_yaw_rate: 0.5
+    remote_max_yaw_rate: 0.85
    remote_invert_vx: true
    remote_invert_vy: false
    remote_invert_yaw: true
    remote_publish_inactive_zero: true
    remote_estop_latch: true
    remote_poll_hz: 50.0
    remote_model_switch_enabled: true
    remote_model_switch_channel: 10
    remote_model_switch_debounce_frames: 3
    remote_model_switch_rough_level: "low"
    remote_model_switch_ik_level: "high"
    # Command mux parameters
-    cmd_mux_default_mode: "REMOTE"
+    cmd_mux_default_mode: "NAV"
    cmd_mux_output_hz: 50.0
    cmd_mux_remote_timeout_ms: 250.0
    cmd_mux_web_timeout_ms: 300.0
    cmd_mux_nav_timeout_ms: 500.0
-    cmd_mux_max_vx: 0.8
+    cmd_mux_max_vx: 0.9
-    cmd_mux_max_vy: 0.3
+    cmd_mux_max_vy: 0.5
-    cmd_mux_max_yaw_rate: 0.5
+    cmd_mux_max_yaw_rate: 0.85
    cmd_mux_max_vx_acc: 1.0
    cmd_mux_max_vy_acc: 1.0
    cmd_mux_max_yaw_acc: 1.5
    cmd_mux_max_vx_decel: 2.0
    cmd_mux_max_vy_decel: 2.0
    cmd_mux_max_yaw_decel: 2.0
    # The rough locomotion policy has an approximately 0.2 m/s linear command dead zone.
    # Skip that ineffective band on start-up, but still allow exact zero for braking/estop.
    cmd_mux_linear_deadzone_epsilon: 0.05
    cmd_mux_yaw_deadzone_epsilon: 0.02
    cmd_mux_min_effective_vx: 0.22
    cmd_mux_min_effective_vy: 0.22
    cmd_mux_min_effective_yaw_rate: 0.0
    cmd_mux_deadzone_sources: "nav"
    # Windows/Nano Web UDP bridge parameters
    web_bridge_enabled: true
    web_http_host: "0.0.0.0"
    web_http_port: 18080
    web_static_dir: ""
    # Odom task actual path export; files can be opened by nav_tools over the PCD.
    odom_trace_export_dir: "map/load"
    web_udp_listen_host: "0.0.0.0"
    web_udp_listen_port: 15000
    web_udp_remote_host: ""
@@ -53,22 +113,46 @@
    web_udp_state_hz: 20.0
    web_udp_cmd_timeout_ms: 300.0
    web_udp_max_packet_bytes: 8192
-    web_udp_max_vx: 0.8
+    web_udp_max_vx: 0.9
    web_udp_max_vy: 0.3
-    web_udp_max_yaw_rate: 0.5
+    web_udp_max_yaw_rate: 0.85
    web_udp_estop_on_timeout: false
    # Safety parameters
    safety_enabled: true
-    max_target_offset: 0.6
+    max_target_offset: 2.4
-    hard_target_offset: 2.0
+    model_switch_max_target_offset: 1.8
-    max_ang_vel: 10.0
+    hard_target_offset: 3.0
    max_ang_vel: 30.0
    max_tilt_z: -0.3
    clip_to_brake: 0
    imu_age_warn_ms: 60.0
-    imu_age_stop_ms: 200.0
+    imu_age_stop_ms: 500.0
    wheel_no_effect_command_threshold: 1.0
    wheel_no_effect_min_response_ratio: 0.20
    wheel_no_effect_velocity_epsilon: 0.25
    wheel_no_effect_max_temperature_c: 90.0
    wheel_no_effect_min_bus_voltage_v: 18.0
    wheel_no_effect_command_warmup_cycles: 12
    wheel_no_effect_trigger_cycles: 30
    wheel_no_effect_attempt_limit: 2
    wheel_no_effect_cooldown_ms: 1200
    wheel_recovery_verify_timeout_ms: 180
    wheel_no_effect_diag_freshness_ms: 350
    wheel_no_effect_diag_request_period_ms: 80
    leg_no_effect_position_error_threshold: 0.18
    leg_no_effect_velocity_epsilon: 0.12
    leg_no_effect_max_estimated_current_arms: 4.0
    leg_no_effect_max_abs_torque_nm: 5.0
    leg_no_effect_max_temperature_c: 100.0
    leg_no_effect_min_bus_voltage_v: 18.0
    leg_no_effect_command_warmup_cycles: 40
    leg_no_effect_trigger_cycles: 25
    leg_no_effect_attempt_limit: 2
    leg_no_effect_cooldown_ms: 1200
    leg_recovery_verify_timeout_ms: 220
-    # Policy alignment with the Python deployment
+    # Policy alignment with the first-generation Python deployment
    command_release_s: 0.35
    release_command_hold_s: 0.12
    release_posture_max_err: 0.35
@@ -78,3 +162,143 @@
    enable_zero_cmd_suppression: true
    require_active_command_to_release: true
    zero_cmd_use_yaw_rate: true
    # Simple navigation parameters
    localization_mode: "relocal" # relocal: wait for Odin map/odom TF; odom: bridge map->odom fallback
    nav_map_frame: "map"
    nav_odom_frame: "odom"
    nav_base_frame: "base_link"
    nav_control_hz: 20.0
    nav_goal_tolerance: 0.20
    nav_yaw_stop_threshold: 0.80
    nav_max_vx: 0.90
    nav_max_vy: 0.50
    nav_max_wz: 0.85
    nav_kp_dist: 0.80
    nav_kp_yaw: 1.80
    nav_goal_exit_tolerance_margin: 0.08
    nav_goal_complete_stable_cycles: 2
    nav_final_align_kp_yaw_scale: 0.60
    nav_final_align_max_wz: 0.45
    nav_final_align_creep_speed: 0.05
    nav_goal_yaw_tolerance_deg: 12.0
    nav_astar_enabled: true
    nav_astar_resolution: 0.10
    nav_astar_pcd_sample_step: 5
    nav_astar_allow_diagonal: true
    nav_astar_smooth_enabled: true
    nav_astar_corner_blend_dist: 0.20
    nav_astar_waypoint_reach_dist: 0.18
    nav_astar_lookahead_dist: 0.35
    nav_astar_snap_radius: 0.60
    nav_astar_max_expansions: 120000
    # Slalom is treated as a continuous path by simple_nav even if the route JSON
    # was saved without precisionFollow/stableCycles/lookahead metadata.
    nav_slalom_auto_precision_enabled: true
    nav_slalom_auto_precision_force: true
    nav_slalom_task_names: "slalom"
    nav_slalom_stable_cycles: 0
    nav_slalom_lookahead: 0.35
    nav_slalom_yaw_rate_limit: 0.45
    nav_slalom_tolerance: 0.15
    nav_slalom_max_vx: 0.58
    nav_slalom_min_vx: 0.22
    nav_slalom_curvature_slowdown_enabled: true
    nav_slalom_min_turn_speed_scale: 0.45
    # Execute waypoints marked slalomStraight as odometry-closed scripted moves.
    nav_slalom_script_enabled: true
    nav_slalom_script_start_tolerance: 0.22
    nav_slalom_script_pos_tolerance: 0.10
    nav_slalom_script_yaw_tolerance_deg: 5.0
    nav_slalom_script_drive_yaw_deadband_deg: 8.0
    nav_slalom_script_stable_cycles: 1
    nav_slalom_script_rotate_steps_enabled: false
    nav_slalom_script_final_rotate_enabled: false
    nav_slalom_script_require_yaw_at_step: false
    nav_slalom_script_kp_dist: 1.00
    nav_slalom_script_kp_yaw: 1.20
    nav_slalom_script_max_vx: 0.58
    nav_slalom_script_max_vy: 0.50
    nav_slalom_script_max_wz: 0.50
    nav_slalom_script_min_cmd_linear: 0.22
    nav_slalom_script_min_cmd_angular: 0.20
    nav_slalom_script_min_cmd_epsilon: 0.05
    nav_slalom_script_min_step_distance: 0.02
    nav_slalom_script_yaw_gate_deg: 8.0
    nav_slalom_script_lateral_gate: 0.07
    nav_slalom_script_lateral_slow_gate: 0.15
    nav_slalom_script_lateral_creep_vx: 0.22
    nav_slalom_script_drive_yaw_source: "segment"
    nav_slalom_script_segment_yaw_min_dist: 0.45
    nav_precision_lateral_control_enabled: true
    nav_precision_lateral_kp: 0.80
    nav_precision_lateral_max_vy: 0.22
    # Lightweight DWA-style local safety layer over the route/avoid polygons.
    nav_local_planner_enabled: true
    nav_local_planner_tasks: "slalom"
    nav_local_planner_precision_enabled: true
    nav_local_planner_sim_time: 0.9
    nav_local_planner_sim_dt: 0.1
    nav_local_planner_v_samples: 5
    nav_local_planner_w_samples: 7
    nav_local_planner_vy_samples: 3
    nav_local_planner_obstacle_margin: 0.08
    nav_local_planner_recovery_clearance_epsilon: 0.005
    # 0.0 means auto: use the nav_tools body+wheel lateral footprint.
    nav_local_planner_robot_radius: 0.0
    nav_local_planner_clearance_weight: 2.0
    nav_local_planner_path_weight: 2.0
    nav_local_planner_heading_weight: 0.7
    nav_local_planner_speed_weight: 0.3
    nav_local_planner_nominal_weight: 1.0
    nav_local_planner_min_vx: 0.22
    nav_slalom_script_safety_filter_enabled: true
    nav_local_planner_use_astar_grid: false
    nav_turn_in_place_enabled: true
    nav_turn_in_place_enter_yaw_deg: 70.0
    nav_turn_in_place_exit_yaw_deg: 18.0
    nav_turn_in_place_max_wz: 0.80
    nav_pre_dock_enabled: true
    nav_pre_dock_distance: 0.35
    nav_pre_dock_tolerance: 0.18
    nav_pre_dock_skip_within_goal_dist: 0.45
    nav_goals_file: ""
    nav_missions_file: ""
    # Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below.
    nav_route_file: ""
    # Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes.
    # The web "odom" button uses the first waypoint of this route as the fixed odom fallback start pose.
    nav_route_task_file: map/routes/1hao_reall.json
    nav_route_auto_align_enabled: false
    nav_route_rotation_offset_deg: 0.0
    nav_route_align_max_angle_deg: 6.0
    nav_route_align_angle_step_deg: 0.5
    nav_route_align_search_radius: 0.35
    nav_avoid_regions_enabled: true
    nav_avoid_region_margin: 0.0
    # 0.0 means auto: use the nav_tools body+wheel lateral footprint for avoid-region inflation.
    nav_avoid_footprint_radius: 0.0
    nav_robot_body_length: 0.356
    nav_robot_body_width: 0.235
    nav_robot_body_center_x: 0.1518
    nav_robot_origin_from_front: 0.105
    nav_robot_pose_hip: 0.550
    nav_robot_pose_knee: -1.125
    nav_robot_wheel_vis_length: 0.16
    nav_robot_wheel_vis_width: 0.055
    nav_robot_footprint_padding: 0.02
    odom_fallback_require_odom_fresh: true
    odom_fallback_max_odom_age_ms: 500.0
    odom_fallback_block_existing_map_odom_tf: true
    odom_fallback_tf_conflict_window_s: 1.0
    odom_fallback_tf_conflict_xy_tolerance: 0.05
    odom_fallback_tf_conflict_yaw_tolerance_deg: 2.0
    # Keep odom fallback running if Odin relocalizes mid-task; hand off after mission end or Exit odom.
    odom_fallback_stop_on_external_tf: false
    pcd_nav_file: map/1hao.pcd
    pcd_floor_z_min: -1.6
    pcd_floor_z_max: 0.4
    pcd_sample_step: 25
    pcd_robot_radius: 0.18
@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch.conditions import IfCondition
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterFile
@@ -27,7 +27,7 @@ def generate_launch_description():
    launch_nav2_arg = DeclareLaunchArgument(
        'launch_nav2',
-        default_value='true',
+        default_value='false',
        description='Whether to launch the Nav2 navigation stack'
    )
@@ -43,6 +43,36 @@ def generate_launch_description():
        description='Whether to launch the Windows/Nano UDP web debug bridge'
    )
    launch_simple_nav_arg = DeclareLaunchArgument(
        'launch_simple_nav',
        default_value='true',
        description='Whether to launch the simple waypoint navigation node'
    )
    localization_mode_arg = DeclareLaunchArgument(
        'localization_mode',
        default_value='relocal',
        description='Localization profile: odom uses bridge fallback; relocal waits for Odin map/odom TF'
    )
    odin_config_file_arg = DeclareLaunchArgument(
        'odin_config_file',
        default_value=PathJoinSubstitution([
            FindPackageShare('odin_ros_driver'),
            'config',
            'control_command_relocal.yaml',
        ]),
        description='Odin control config YAML for the selected localization profile'
    )
    event_log_dir_arg = DeclareLaunchArgument(
        'event_log_dir',
        default_value=PythonExpression([
            "'logs_v2_web/run_' + __import__('datetime').datetime.now().strftime('%Y-%m-%d_%H-%M-%S_%f')[:-3]"
        ]),
        description='Per-run event log directory'
    )
    # Include odin_ros_driver launch
    driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
@@ -52,7 +82,10 @@ def generate_launch_description():
                'odin1_ros2.launch.py'
            ])
        ),
-        launch_arguments={'launch_rviz': 'false'}.items(),
+        launch_arguments={
            'launch_rviz': 'false',
            'config_file': LaunchConfiguration('odin_config_file'),
        }.items(),
        condition=IfCondition(LaunchConfiguration('launch_driver'))
    )
@@ -73,19 +106,23 @@ def generate_launch_description():
        launch_nav2_arg,
        launch_remote_arg,
        launch_web_bridge_arg,
        launch_simple_nav_arg,
        localization_mode_arg,
        odin_config_file_arg,
        event_log_dir_arg,
        Node(
            package="sim2real_hw",
            executable="sim2real_hw_node",
            name="sim2real_hw_node",
            output="screen",
-            parameters=[runtime_params],
+            parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
        ),
        Node(
            package="sim2real_runtime",
            executable="sim2real_runtime_node",
            name="sim2real_runtime_node",
            output="screen",
-            parameters=[runtime_params],
+            parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
        ),
        Node(
            package="sim2real_runtime",
@@ -99,7 +136,7 @@ def generate_launch_description():
            executable="web_udp_bridge_node.py",
            name="sim2real_web_udp_bridge_node",
            output="screen",
-            parameters=[runtime_params],
+            parameters=[runtime_params, {"localization_mode": LaunchConfiguration("localization_mode")}],
            condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
        ),
        Node(
@@ -110,6 +147,14 @@ def generate_launch_description():
            parameters=[runtime_params],
            condition=IfCondition(LaunchConfiguration('launch_remote')),
        ),
        Node(
            package="sim2real_runtime",
            executable="simple_nav_node.py",
            name="sim2real_simple_nav_node",
            output="screen",
            parameters=[runtime_params],
            condition=IfCondition(LaunchConfiguration('launch_simple_nav')),
        ),
        Node(
            package="sim2real_runtime",
            executable="odom_relay_node",
@@ -126,4 +171,3 @@ def generate_launch_description():
        driver_launch,
        nav2_launch,
    ])
@@ -19,8 +19,8 @@ struct DeploymentContract
  static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
  static constexpr float kLegKp = 50.0f;
  static constexpr float kLegKd = 1.5f;
-  static constexpr float kLegHoldKp = 80.0f;
+  static constexpr float kLegHoldKp = kLegKp;
-  static constexpr float kLegHoldKd = 4.0f;
+  static constexpr float kLegHoldKd = kLegKd;
  static constexpr float kWheelKd = 1.0f;
  static constexpr std::array<int, 16> kCanBusMap = {
@@ -64,10 +64,10 @@ struct DeploymentContract
  };
  static constexpr std::array<float, 16> kDefaultDofPos = {
-    0.0f, 0.9f, -1.8f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.9f, -1.8f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.9f, -1.8f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.9f, -1.8f,
+    0.0f, 0.550f, -1.125f,
    0.0f, 0.0f, 0.0f, 0.0f
  };
 };
--- a/Show More
+++ b/Show More