Compare commits
10 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
fbb396708c | ||
|
zeitvex
|
117b02bec7 | ||
|
zeitvex
|
196044b993 | ||
|
zeitvex
|
645819a65f | ||
|
zeitvex
|
1b551f974b | ||
|
zeitvex
|
765173676f | ||
|
zeitvex
|
dc7a2c2a67 | ||
|
zeitvex
|
c8aa490b37 | ||
|
zeitvex
|
302b6acc64 | ||
|
zeitvex
|
3bf6eb0c4c |
@@ -0,0 +1,2 @@
|
||||
# Keep structured point-cloud assets byte-identical across platforms.
|
||||
*.pcd -text
|
||||
+3
-4
@@ -29,10 +29,9 @@ 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
|
||||
|
||||
# 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
|
||||
!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/*.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
|
||||
+7
-6
@@ -2,13 +2,14 @@
|
||||
|
||||
本目录用于保存 16DOF 轮足项目自身的技术文档和使用说明。
|
||||
|
||||
后续建议按主题组织：
|
||||
当前文档结构：
|
||||
|
||||
```text
|
||||
01_doc/
|
||||
├─ architecture/ # 系统架构和数据流
|
||||
├─ control/ # 控制与强化学习原理
|
||||
├─ deployment/ # Sim2Sim 和 Sim2Real 部署
|
||||
├─ hardware/ # 接线、标定和硬件兼容性
|
||||
└─ user_guide/ # 安装、运行和调试说明
|
||||
├─ architecture/
|
||||
│ └─ early_software_stack.md # 第一代训练—仿真—真机闭环
|
||||
├─ training_evolution.md # v0.4～v0.6 训练架构演进
|
||||
└─ version_history.md # 全项目 Tag 与里程碑
|
||||
```
|
||||
|
||||
具体运行说明放在对应工程目录内，避免在顶层重复并逐渐失真：训练见 `05_software/train/rc_mjlab/`，真机部署见 `05_software/real/`。
|
||||
|
||||
@@ -34,4 +34,4 @@
|
||||
比赛得分：1050
|
||||
```
|
||||
|
||||
`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。
|
||||
`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它已随比赛部署归档；当前规范目录为 `05_software/real/sim2real_ros2_v3/policies/`。
|
||||
|
||||
+78
-12
@@ -1,6 +1,6 @@
|
||||
# 版本演进
|
||||
|
||||
本项目使用同一条 `16dof` 主线和里程碑 Tag 保存线性演进，不在源码目录中复制历史版本。
|
||||
本项目使用里程碑 Tag 保存时间演进；同一架构的小步迭代不复制目录。ROS 2 的无后缀、`_v2`、`_v3` 目录分别代表三个架构大版本，并在 `v1.1.0` 中同时保留。
|
||||
|
||||
| Tag | 阶段 | 核心内容 |
|
||||
| --- | --- | --- |
|
||||
@@ -14,7 +14,83 @@
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | 最终比赛部署 | ROS 2/C++ 真机闭环、Odin、CAN、导航与屏幕 UI |
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `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 |
|
||||
| `v1.0.0` | 比赛最终部署初次归档 | last_not_slalom_1050、model_6800/model_84、最终路线和触控屏；当时暂存于无后缀目录 |
|
||||
| `v1.0.1` | 比赛成果媒体补充 | 最终机器人图片与 1050 分比赛视频 |
|
||||
| `v1.0.2` | 文档一致性修正 | 统一历史 Tag、当前快照和成果媒体的描述 |
|
||||
| `v1.1.0` | 三代目录规范化 | 恢复无后缀初版、保留 v2 里程计版、明确最终比赛 v3，并校准训练 README |
|
||||
|
||||
> 原先临时归档为 `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 声称已复现。
|
||||
|
||||
## `v1.0.0` 的比赛最终部署
|
||||
|
||||
- 首次归档原始 `sim2real_ros2_v2(last_not_slalom_1050)` 最终 ROS 2/C++ 真机工程；该 Tag 中暂存于无后缀 `real/sim2real_ros2`，目录命名在 `v1.1.0` 才修正为 `real/sim2real_ros2_v3`。`1050` 是比赛成绩，不是模型编号。
|
||||
- Rough 使用 `model_6800`，Wall 使用 `model_84`，Crawl 按比赛配置使用 IK 后端。
|
||||
- 保留最终五份路线、1 号场地抽样 PCD、Odin 驱动、CAN 硬件桥、命令仲裁、导航和 Orin 触控屏 UI。
|
||||
- 最终配置默认命令源为 `NAV`、定位模式为 `relocal`，但真实 Odin `1hao.bin` 不在备份中，重定位闭环需要从比赛设备补回。
|
||||
- 排除嵌套 Git、日志、备份、候选策略、构建产物和开发草稿；TensorRT engine 仅代表比赛机环境。
|
||||
|
||||
## `v1.0.1` 的比赛成果媒体补充
|
||||
|
||||
- 保持 `v1.0.0` 的比赛最终代码和部署内容不变。
|
||||
- 补充最终机器人图片和比赛视频，成绩为 1050 分、第七名（前 5%）。
|
||||
- 代码复现可查看 `v1.0.0`，包含成果媒体的对应快照可查看 `v1.0.1`。
|
||||
|
||||
## `v1.0.2` 的文档一致性修正
|
||||
|
||||
- 统一 ROS 2 历史 Tag、当前工作树和媒体补丁的说明。
|
||||
- 该版本仅修正文档，没有改变训练或真机运行代码。
|
||||
|
||||
## `v1.1.0` 的目录与说明规范化
|
||||
|
||||
- 从 `v0.10.0` 恢复无后缀 `sim2real_ros2` 初版快照。
|
||||
- `sim2real_ros2_v2` 保持 `v0.12.0` 里程计联调快照。
|
||||
- 将 `last_not_slalom_1050` 最终比赛部署正式命名为 `sim2real_ros2_v3`。
|
||||
- 依据当前源码重新校准 `rc_mjlab` README 中的物理步长、控制频率、环境数、执行器、地形、奖励和随机化说明。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
@@ -72,13 +148,3 @@
|
||||
- 补充 `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 前必须由权利人统一选择开源许可证。
|
||||
|
||||
@@ -3,3 +3,5 @@
|
||||
本目录用于保存 16DOF 轮足机器人的自研电路、接线图、BOM、传感器与计算平台说明。
|
||||
|
||||
第三方硬件资料不作为自研成果提交。
|
||||
|
||||
当前目录仅有范围说明，尚未归档 16DOF 平台的自研原理图、PCB、BOM 或正式接线图，不应将本目录视为已完成的硬件开源包。
|
||||
|
||||
@@ -3,3 +3,5 @@
|
||||
本目录用于保存运行在 MCU 或其他嵌入式控制器上的固件源码和构建工程。
|
||||
|
||||
编译生成的 `.hex`、`.bin`、`.elf`、`.axf` 等文件不进入源码目录，可在需要时作为 Release 附件发布。
|
||||
|
||||
当前 16DOF 主线尚未在本目录归档 MCU/Keil 工程；该目录是预留入口。8DOF 大疆 A 板 Keil 工程由 `8dof` 分支和 `v0.1.0` 保存。
|
||||
|
||||
@@ -9,9 +9,14 @@
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
└─ sim2real_ros2/ # 最终比赛 ROS 2/C++ 真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
├─ sim2real_ros2_v2/ # ROS 2 导航原型及里程计演进
|
||||
└─ sim2real_ros2_v3/ # 最终比赛 ROS 2/C++ 部署
|
||||
```
|
||||
|
||||
当前工作树按架构大版本同时保留三个 ROS 2 目录：无后缀目录是初版，`_v2` 是第二版演进的最终里程计快照，`_v3` 是 `last_not_slalom_1050` 最终比赛部署。各目录内部的小阶段仍可通过对应 Tag 恢复。
|
||||
|
||||
## 数据流
|
||||
|
||||
```text
|
||||
@@ -24,14 +29,14 @@ MJCF + mjlab task
|
||||
|
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
+----> Python Sim2Real ----> 电机 / IMU
|
||||
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
||||
|
|
||||
+----> ROS 2/C++ Sim2Real -> CAN / Odin / 导航 / 屏幕
|
||||
+----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
|
||||
|
||||
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`，最终比赛 ROS 2 部署见 `v0.9.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`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`，纯里程计导航联调对应 `v0.12.0`，1050 分比赛最终部署对应 `v1.0.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
+22
-10
@@ -1,6 +1,6 @@
|
||||
# 真机控制与部署
|
||||
# 真机控制版本演进
|
||||
|
||||
本目录保存 16DOF 轮足机器人从早期接口验证到最终比赛 ROS 2 部署的演进。
|
||||
本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
@@ -20,17 +20,29 @@
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_ros2`
|
||||
## `sim2real_v2`
|
||||
|
||||
`last_not_slalom_1050` 最终比赛工程的规范化归档，包含：
|
||||
Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具，对应 `v0.9.0`。
|
||||
|
||||
- ROS 2 Humble + C++ 运行时
|
||||
- 50 Hz 策略推理与 200 Hz CAN 电机热路径
|
||||
- Rough `model_6800`、Wall `model_84` 和 Crawl IK 模式
|
||||
- Odin IMU/里程计驱动、简单导航、命令仲裁和触控屏 UI
|
||||
- 比赛路线、抽样 PCD、Docker 与部署说明
|
||||
部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
|
||||
|
||||
`1050` 是比赛得分，不是模型编号。完整入口与缺失的 Odin 重定位地图边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
## ROS 2/C++ 版本线
|
||||
|
||||
### `sim2real_ros2`（初版，`v0.10.0`）
|
||||
|
||||
无后缀目录固定表示 ROS 2/C++ Sim2Real 初版：将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。原始快照未随工程保存 Odin ROS 2 驱动源码，依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
### `sim2real_ros2_v2`（`v0.11.0`～`v0.12.0`）
|
||||
|
||||
在 `v0.11.0` 中，该目录是 ROS 2 Sim2Real v2 导航原型，增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。
|
||||
|
||||
`v0.11.1` 在同一路径继续演进，首次归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。
|
||||
|
||||
`v0.12.0` 仍在同一路径上形成里程计导航联调快照：固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。当前该目录保持 `v0.12.0` 快照，阶段说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
|
||||
### `sim2real_ros2_v3`（最终比赛版；代码快照 `v1.0.0`，规范目录 `v1.1.0`）
|
||||
|
||||
第三版来自原始目录 `sim2real_ros2_v2(last_not_slalom_1050)`，整理时正式命名为 `sim2real_ros2_v3`。它是 1050 分比赛最终部署，包含 `model_6800` Rough、`model_84` Wall、最终路线、完整 Odin 驱动、CAN 和触控屏。部署说明见 [`sim2real_ros2_v3/README.md`](sim2real_ros2_v3/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
|
||||
@@ -1,10 +1,12 @@
|
||||
# `sim2real` 部署说明
|
||||
|
||||
> 版本范围：第一代 Python Sim2Real（`v0.3.0`）。本文“当前”均指该快照。
|
||||
|
||||
## 模型
|
||||
|
||||
当前只使用：
|
||||
|
||||
- `sim2real/policies/model_rough.pt`
|
||||
- `policies/model_rough.pt`
|
||||
|
||||
## 模型契约
|
||||
|
||||
@@ -38,7 +40,7 @@
|
||||
|
||||
## 纯 Python 命令
|
||||
|
||||
默认前提：当前目录就是 `sim2real/`
|
||||
默认前提：当前目录是 `05_software/real/sim2real/`
|
||||
|
||||
```bash
|
||||
python -m pip install -r requirements-orin.txt
|
||||
|
||||
@@ -1,9 +1,11 @@
|
||||
# `FACTS_AND_ASSUMPTIONS`
|
||||
|
||||
> 版本范围：第一代 Python Sim2Real（`v0.3.0`）。事实项只适用于该快照。
|
||||
|
||||
## 已确认
|
||||
|
||||
- 当前部署模型：`sim2real/policies/model_rough.pt`
|
||||
- 源模型：`model_2000.pt`
|
||||
- 当前部署模型：`policies/model_rough.pt`
|
||||
- 原始说明记录的源模型名：`model_2000.pt`；同名文件未随本目录归档
|
||||
- actor 输入：`53D`
|
||||
- actor 输出：`16D`
|
||||
- 当前 actor 不吃 `base_lin_vel`
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# `Orin Nano` 部署说明
|
||||
|
||||
> 版本范围：第一代 Python Sim2Real（`v0.3.0`）。本文不是最终 ROS 2 v3 部署指南。
|
||||
|
||||
## 是否必须转 ONNX
|
||||
|
||||
不必须。
|
||||
@@ -26,7 +28,7 @@
|
||||
|
||||
## 纯 Python 部署命令
|
||||
|
||||
默认前提：当前目录就是 `sim2real/`
|
||||
默认前提：当前目录是 `05_software/real/sim2real/`
|
||||
|
||||
```bash
|
||||
python3 -m pip install -r requirements-orin.txt
|
||||
|
||||
@@ -1,11 +1,13 @@
|
||||
# `sim2real`
|
||||
|
||||
当前版本只部署现在这套 `53D -> 16D` 模型，不再兼容旧版 `crawl`、多策略和历史观测。
|
||||
本目录是第一代 Python Sim2Real 快照，对应 `v0.3.0`。下文“当前”均指该历史快照，不指仓库 `main` 的最终 ROS 2 v3。
|
||||
|
||||
该快照只部署当时的 `53D -> 16D` Rough 模型，不兼容更早的 Crawl、多策略和其他历史观测契约。
|
||||
|
||||
## 当前部署模型
|
||||
|
||||
- 使用文件：`sim2real/policies/model_rough.pt`
|
||||
- 来源文件：`model_2000.pt`
|
||||
- 使用文件：`policies/model_rough.pt`
|
||||
- 原始说明记录的来源名：`model_2000.pt`；该同名源文件未随本目录归档，仓库只保留重命名后的 `policies/model_rough.pt`
|
||||
|
||||
## 当前 actor 输入
|
||||
|
||||
@@ -52,7 +54,7 @@
|
||||
|
||||
## 启动命令
|
||||
|
||||
默认前提：当前目录就是 `sim2real/`
|
||||
默认前提：当前目录是 `05_software/real/sim2real/`
|
||||
|
||||
纯 `python`：
|
||||
|
||||
@@ -65,13 +67,12 @@ python main.py
|
||||
python web/server.py --host 0.0.0.0 --port 8080
|
||||
```
|
||||
|
||||
Windows 本机：
|
||||
Windows 本机可使用目标虚拟环境中的 Python；不要依赖个人机器的绝对安装路径：
|
||||
|
||||
```bash
|
||||
D:\Minicoda3\envs\py10\python.exe -m pip install -r requirements-orin.txt
|
||||
D:\Minicoda3\envs\py10\python.exe tools\alignment_check.py --policy policies\model_rough.pt --manifest deployment_manifest.yaml
|
||||
D:\Minicoda3\envs\py10\python.exe tools\standalone_check.py
|
||||
D:\Minicoda3\envs\py10\python.exe main.py
|
||||
D:\Minicoda3\envs\py10\python.exe web\server.py --host 0.0.0.0 --port 8080
|
||||
python -m pip install -r requirements-orin.txt
|
||||
python tools\alignment_check.py --policy policies\model_rough.pt --manifest deployment_manifest.yaml
|
||||
python tools\standalone_check.py
|
||||
python main.py
|
||||
python web\server.py --host 0.0.0.0 --port 8080
|
||||
```
|
||||
#sim2real/policies/model_rough.pt
|
||||
@@ -1,14 +1,6 @@
|
||||
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
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||
# ROS 2/C++ Sim2Real 初版部署指南
|
||||
|
||||
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||
本工作区保存 `v0.10.0` 的 ROS 2 Humble/C++ 初版。控制栈主体源码已归档，但 Odin 驱动源码在该快照中缺失，因此不是自包含部署包；完成真实传感器闭环前必须补充兼容驱动。
|
||||
|
||||
---
|
||||
|
||||
@@ -156,7 +156,7 @@ sudo udevadm trigger
|
||||
统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
|
||||
|
||||
* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
|
||||
* `launch_nav2`（默认：`false`）：按需启动 ROS2 Navigation2；比赛默认使用 `simple_nav_node.py` 的路线跟踪。
|
||||
* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
|
||||
|
||||
#### 1. 完整真实硬件闭环（默认）
|
||||
启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
|
||||
@@ -175,3 +175,4 @@ 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,7 +62,6 @@ 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,104 +1,74 @@
|
||||
# ROS 2 最终比赛 Sim2Real
|
||||
# ROS 2/C++ Sim2Real 初版
|
||||
|
||||
本目录归档 `last_not_slalom_1050` 真机工程，对应 RC_WheelLeg 在 RoboCon 仿生足式障碍赛使用的最终 ROS 2 部署栈。`1050` 是比赛得分，不是模型编号；比赛 Rough 策略为 `model_6800.onnx`。
|
||||
本目录归档 `real/sim2real_ros2`，对应 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
|
||||
|
||||
该里程碑计划标记为 `v0.9.0`。训练架构和策略来源见 `v0.6.0`，比赛 Rough 模型首次归档见 `v0.8.0`，导航打点与路线演进见 `v0.8.1`。
|
||||
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||
|
||||
## 系统闭环
|
||||
- `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`
|
||||
仅保留依赖边界说明；原始 `v0.10.0` 快照没有归档 Odin 驱动源码，不能独立提供 IMU、点云和里程计发布。
|
||||
- `docs`
|
||||
架构说明与迁移计划。
|
||||
|
||||
## 目标架构
|
||||
|
||||
```text
|
||||
Odin IMU / Odom ──> hardware bridge ──> RuntimeState
|
||||
|
|
||||
导航 / 遥控 / 屏幕 ──> cmd mux ──> policy runtime (50 Hz)
|
||||
|
|
||||
RuntimeTarget
|
||||
|
|
||||
hardware bridge / CAN (200 Hz)
|
||||
Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw
|
||||
| | |
|
||||
v v v
|
||||
RuntimeState RuntimeTarget 电机 CAN 指令
|
||||
| |
|
||||
+-------> 诊断 / 遥测
|
||||
|
||||
Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||
(经 odom_relay_node 提供 odom→base_link TF)
|
||||
```
|
||||
|
||||
核心约束：
|
||||
## 当前状态
|
||||
|
||||
- 53 维策略观测、16 维动作输出。
|
||||
- Rough：`model_6800`，优先 TensorRT，失败时回退 ONNX Runtime。
|
||||
- Wall：`model_84`，同样保留 TensorRT 与 ONNX 两种文件。
|
||||
- Crawl：比赛配置使用解析 IK，不加载 Crawl RL 权重。
|
||||
- 默认站姿：髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- 默认命令源：`NAV`；默认定位模式：`relocal`。
|
||||
已完成 Phase 0-5 的全部迁移：
|
||||
|
||||
## 目录
|
||||
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
|
||||
```
|
||||
## 契约来源
|
||||
|
||||
## 构建与运行
|
||||
迁移过程中以下文件被视为真值源：
|
||||
|
||||
目标环境是 Ubuntu 22.04、ROS 2 Humble 和 Jetson Orin。系统依赖和 Docker 流程见 [`DEPLOYMENT_GUIDE.md`](DEPLOYMENT_GUIDE.md)。
|
||||
- `../sim2real/deployment_manifest.yaml`
|
||||
- `../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
|
||||
```
|
||||
## 注意事项
|
||||
|
||||
运行参数和模型/路线均使用工作区根目录相对路径，因此应从本目录启动。常用启动覆盖：
|
||||
|
||||
```bash
|
||||
# 纯里程计模式，不等待 Odin 重定位地图
|
||||
./start_sim2real.sh localization_mode:=odom \
|
||||
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 真机运行成功。
|
||||
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
|
||||
- Windows 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
|
||||
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
# 迁移计划
|
||||
|
||||
> 版本范围：ROS 2/C++ 初版（`v0.10.0`）。这是历史迁移记录，不是最终比赛版验收报告；本快照未归档 Odin 驱动源码。
|
||||
|
||||
## Phase 1: 硬件核心迁移 ✅ 已完成
|
||||
|
||||
将当前高频热路径从 Python 迁出。
|
||||
@@ -39,11 +41,7 @@
|
||||
|
||||
## Phase 3: ROS 2 系统集成 ✅ 已完成
|
||||
|
||||
参考的源项目：
|
||||
|
||||
- `00_ reference/odin_ros_driver`
|
||||
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||
- `00_ reference/rl_sar`
|
||||
参考过的外部项目包括 Odin ROS 驱动、EDULITE A3 ROS 工程和 RL-SAR；这些外部参考目录不属于本仓库公开内容。
|
||||
|
||||
交付物：
|
||||
|
||||
@@ -76,5 +74,5 @@
|
||||
| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
|
||||
| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
|
||||
| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
|
||||
| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
|
||||
| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||
| 传感器驱动 | 外部 Odin 驱动 | 本快照未归档源码，不能视为自包含组件 |
|
||||
| 点云转换 | `pointcloud_to_laserscan` | 外部 ROS 包；/odin1/cloud_slam → /scan（供 AMCL 使用） |
|
||||
|
||||
@@ -21,7 +21,7 @@ src/sim2real_runtime/src/remote_uart_node.py
|
||||
|
||||
## 2. 通道映射
|
||||
|
||||
通道映射与本仓库第一代 Python Sim2Real 实现中的遥控器配置保持一致。
|
||||
通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
|
||||
|
||||
| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
|
||||
|---|---|---|---:|
|
||||
@@ -64,10 +64,6 @@ 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
|
||||
```
|
||||
|
||||
@@ -280,7 +276,7 @@ ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
/safety/estop: true
|
||||
```
|
||||
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
|
||||
### 8.4 机器人行为效果
|
||||
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
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
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,821 +1,5 @@
|
||||
# Odin_ROS_Driver Readme
|
||||
# Odin 驱动依赖占位
|
||||
|
||||
ROS driver suite for Odin sensor modules (Manifold Tech Ltd.)
|
||||
`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
|
||||
|
||||
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.
|
||||
因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
|
||||
|
||||
@@ -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.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]
|
||||
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]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
|
||||
@@ -4,47 +4,8 @@
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
event_log_dir: "logs_v2_web"
|
||||
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
|
||||
model_path: policies/model_rough.onnx
|
||||
use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
|
||||
contract_file: deployment_contract.yaml
|
||||
dry_run: false
|
||||
can0_name: "can0"
|
||||
@@ -52,60 +13,39 @@
|
||||
imu_topic: "/odin1/imu"
|
||||
odom_topic: "/odom"
|
||||
|
||||
# Remote UART / SBUS parameters, aligned with the first-generation Python deployment
|
||||
remote_enabled: false # true
|
||||
# Remote UART / SBUS parameters, aligned with the Python deployment
|
||||
remote_enabled: 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.9
|
||||
remote_max_vy: 0.5
|
||||
remote_max_yaw_rate: 0.85
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
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: "NAV"
|
||||
cmd_mux_default_mode: "REMOTE"
|
||||
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.9
|
||||
cmd_mux_max_vy: 0.5
|
||||
cmd_mux_max_yaw_rate: 0.85
|
||||
cmd_mux_max_vx: 0.8
|
||||
cmd_mux_max_vy: 0.3
|
||||
cmd_mux_max_yaw_rate: 0.5
|
||||
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: ""
|
||||
@@ -113,46 +53,22 @@
|
||||
web_udp_state_hz: 20.0
|
||||
web_udp_cmd_timeout_ms: 300.0
|
||||
web_udp_max_packet_bytes: 8192
|
||||
web_udp_max_vx: 0.9
|
||||
web_udp_max_vx: 0.8
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.85
|
||||
web_udp_max_yaw_rate: 0.5
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 2.4
|
||||
model_switch_max_target_offset: 1.8
|
||||
hard_target_offset: 3.0
|
||||
max_ang_vel: 30.0
|
||||
max_target_offset: 0.6
|
||||
hard_target_offset: 2.0
|
||||
max_ang_vel: 10.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.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
|
||||
imu_age_stop_ms: 200.0
|
||||
|
||||
# Policy alignment with the first-generation Python deployment
|
||||
# Policy alignment with the Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
@@ -162,143 +78,3 @@
|
||||
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
|
||||
|
||||
|
||||
|
||||
+7
-51
@@ -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, PythonExpression
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
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='false',
|
||||
default_value='true',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
@@ -43,36 +43,6 @@ 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(
|
||||
@@ -82,10 +52,7 @@ def generate_launch_description():
|
||||
'odin1_ros2.launch.py'
|
||||
])
|
||||
),
|
||||
launch_arguments={
|
||||
'launch_rviz': 'false',
|
||||
'config_file': LaunchConfiguration('odin_config_file'),
|
||||
}.items(),
|
||||
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||
)
|
||||
|
||||
@@ -106,23 +73,19 @@ 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, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
@@ -136,7 +99,7 @@ def generate_launch_description():
|
||||
executable="web_udp_bridge_node.py",
|
||||
name="sim2real_web_udp_bridge_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params, {"localization_mode": LaunchConfiguration("localization_mode")}],
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||
),
|
||||
Node(
|
||||
@@ -147,14 +110,6 @@ 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",
|
||||
@@ -171,3 +126,4 @@ def generate_launch_description():
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
|
||||
+6
-6
@@ -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 = kLegKp;
|
||||
static constexpr float kLegHoldKd = kLegKd;
|
||||
static constexpr float kLegHoldKp = 80.0f;
|
||||
static constexpr float kLegHoldKd = 4.0f;
|
||||
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.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f
|
||||
};
|
||||
};
|
||||
|
||||
+32
-12
@@ -1,11 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
@@ -19,15 +18,12 @@ public:
|
||||
StandBalanceController(double control_dt = 0.02)
|
||||
: control_dt_(control_dt)
|
||||
{
|
||||
profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f};
|
||||
profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f};
|
||||
profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f};
|
||||
reset();
|
||||
}
|
||||
|
||||
void setNominalLegPose(float hip_pitch, float knee)
|
||||
{
|
||||
nominal_hip_pitch_ = hip_pitch;
|
||||
nominal_knee_ = knee;
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
@@ -38,8 +34,9 @@ public:
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
const float hip_base = nominal_hip_pitch_;
|
||||
const float knee_base = nominal_knee_;
|
||||
float hip_base = 0.9f;
|
||||
float knee_base = -1.8f;
|
||||
estimateBaseLegPose(hip_base, knee_base);
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
@@ -87,9 +84,28 @@ private:
|
||||
pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz)));
|
||||
}
|
||||
|
||||
void estimateBaseLegPose(float& hip, float& knee)
|
||||
{
|
||||
float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back());
|
||||
hip = interpolate(h_clamp, profile_h_, profile_hip_);
|
||||
knee = interpolate(h_clamp, profile_h_, profile_knee_);
|
||||
}
|
||||
|
||||
float interpolate(float x, const std::vector<float>& xp, const std::vector<float>& fp)
|
||||
{
|
||||
if (x <= xp.front()) return fp.front();
|
||||
if (x >= xp.back()) return fp.back();
|
||||
for (std::size_t i = 0; i < xp.size() - 1; ++i) {
|
||||
if (x >= xp[i] && x <= xp[i+1]) {
|
||||
float f = (x - xp[i]) / (xp[i+1] - xp[i]);
|
||||
return fp[i] + f * (fp[i+1] - fp[i]);
|
||||
}
|
||||
}
|
||||
return fp.back();
|
||||
}
|
||||
|
||||
double control_dt_;
|
||||
float nominal_hip_pitch_{DeploymentContract::kDefaultDofPos[1]};
|
||||
float nominal_knee_{DeploymentContract::kDefaultDofPos[2]};
|
||||
float height_{0.33f};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
@@ -101,6 +117,10 @@ private:
|
||||
float stable_gyro_deg_s_{45.0f};
|
||||
float enter_hold_s_{1.0f};
|
||||
|
||||
std::vector<float> profile_h_;
|
||||
std::vector<float> profile_hip_;
|
||||
std::vector<float> profile_knee_;
|
||||
|
||||
float stable_time_{0.0f};
|
||||
};
|
||||
|
||||
|
||||
+2
-169
@@ -12,41 +12,16 @@
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/event_logger.hpp"
|
||||
#include "sim2real_common/low_pass_filter.hpp"
|
||||
#include "sim2real_common/mahony_filter.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
struct can_frame;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
enum class RecoveryKind
|
||||
{
|
||||
None,
|
||||
Stale,
|
||||
NoEffect
|
||||
};
|
||||
|
||||
enum class RecoveryStage
|
||||
{
|
||||
Idle,
|
||||
AwaitInitFeedback,
|
||||
AwaitEffectVerification
|
||||
};
|
||||
|
||||
enum class ActiveModelMode
|
||||
{
|
||||
Rough,
|
||||
Crawl,
|
||||
Wall
|
||||
};
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
@@ -61,49 +36,14 @@ struct MotorStateInternal
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
float bus_voltage{0.0f};
|
||||
float estimated_current_arms{0.0f};
|
||||
float last_command_sim{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{0};
|
||||
std::uint32_t command_active_count{0};
|
||||
std::uint32_t no_effect_count{0};
|
||||
std::uint16_t fault_code{0};
|
||||
std::uint16_t fault_detail_1{0};
|
||||
std::uint16_t fault_detail_2{0};
|
||||
// Hold-over state
|
||||
float last_valid_pos{0.0f};
|
||||
float last_valid_vel{0.0f};
|
||||
float last_valid_torque{0.0f};
|
||||
std::uint32_t prev_update_count{0};
|
||||
bool has_valid_data{false};
|
||||
bool has_bus_voltage{false};
|
||||
bool has_fault_snapshot{false};
|
||||
bool stale_reported{false};
|
||||
bool recovered_reported{false};
|
||||
bool disable_reported{false};
|
||||
bool command_effect_monitoring_active{false};
|
||||
bool no_effect_reported{false};
|
||||
bool high_temp_reported{false};
|
||||
bool high_current_reported{false};
|
||||
bool high_voltage_reported{false};
|
||||
bool low_voltage_reported{false};
|
||||
bool fault_code_reported{false};
|
||||
bool init_confirmed{false};
|
||||
std::uint32_t init_attempt_count{0};
|
||||
std::uint32_t recovery_attempt_count{0};
|
||||
std::uint32_t no_effect_recovery_attempt_count{0};
|
||||
std::chrono::steady_clock::time_point last_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_no_effect_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_snapshot_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_request_time_{};
|
||||
std::chrono::steady_clock::time_point recovery_stage_deadline_{};
|
||||
std::uint32_t recovery_start_update_count{0};
|
||||
std::uint32_t recovery_active_attempt_number{0};
|
||||
RecoveryKind recovery_kind{RecoveryKind::None};
|
||||
RecoveryStage recovery_stage{RecoveryStage::Idle};
|
||||
std::string recovery_trigger;
|
||||
std::string last_power_event_reason;
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
@@ -114,7 +54,6 @@ public:
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onModelStatus(const std_msgs::msg::String::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
@@ -125,83 +64,25 @@ private:
|
||||
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id, bool clear_fault = false);
|
||||
bool writeParameterInt(int fd, int motor_id, std::uint16_t param_id, std::uint32_t value);
|
||||
bool disableMotor(int fd, int motor_id);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
bool readParameter(int fd, int motor_id, std::uint16_t param_id);
|
||||
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||
bool initializeMotor(std::size_t index, const std::string & reason, int max_attempts = 3);
|
||||
bool initializeMotorsOnBus(int bus_id, const std::string & reason);
|
||||
bool waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout);
|
||||
void processCanFrame(const struct can_frame & frame, int bus_id);
|
||||
void drainCanFrames(int fd, int bus_id, int timeout_us);
|
||||
bool isLegMotor(std::size_t index) const;
|
||||
bool isWheelMotor(std::size_t index) const;
|
||||
bool motorHasBlockingFault(std::size_t index) const;
|
||||
bool isNoEffectConditionPresent(std::size_t index) const;
|
||||
std::uint32_t noEffectCommandWarmupCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectTriggerCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectAttemptLimit(std::size_t index) const;
|
||||
std::uint32_t noEffectCooldownMs(std::size_t index) const;
|
||||
std::uint32_t noEffectVerifyTimeoutMs(std::size_t index) const;
|
||||
bool hasFreshNoEffectDiagnostics(std::size_t index) const;
|
||||
void requestMotorDiagnostics(std::size_t index);
|
||||
std::string classifyNoEffectSuspect(std::size_t index) const;
|
||||
std::string buildNoEffectSummary(std::size_t index) const;
|
||||
void updateMotorCommandTracking(std::size_t index, float sim_command, const std::string & target_source);
|
||||
void updateNoEffectDetection(std::size_t index);
|
||||
bool startMotorRecoverySequence(
|
||||
std::size_t index,
|
||||
const std::string & trigger,
|
||||
RecoveryKind kind,
|
||||
std::uint32_t attempt_number);
|
||||
void processMotorRecoverySequence(std::size_t index);
|
||||
void clearMotorRecoverySequence(std::size_t index);
|
||||
bool shouldAttemptMotorRecovery(std::size_t index) const;
|
||||
bool attemptMotorRecovery(std::size_t index, const std::string & trigger);
|
||||
bool shouldAttemptNoEffectRecovery(std::size_t index) const;
|
||||
bool attemptNoEffectRecovery(std::size_t index, const std::string & trigger);
|
||||
const char * jointName(std::size_t index) const;
|
||||
std::string motorTag(std::size_t index) const;
|
||||
float estimateCurrentArms(float torque_nm) const;
|
||||
std::string decodeFaultCode(std::uint16_t fault_code) const;
|
||||
std::string decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const;
|
||||
std::string buildMotorFaultSummary(std::size_t index) const;
|
||||
std::string formatProtectionReason(const std::string & trigger, const std::string & reason) const;
|
||||
void logProtectionEvent(const std::string & trigger, const std::string & reason, const std::string & action);
|
||||
void logMotorPowerEvent(std::size_t index, const std::string & state, const std::string & reason);
|
||||
void logMotorDiagnosticEvent(std::size_t index, const std::string & event, const std::string & reason, const char * level = "WARN");
|
||||
void updateMotorTelemetry(std::size_t index, float pos_sim, float vel_sim, float torque_sim, float temperature_c);
|
||||
void handleParameterResponse(const struct can_frame & frame, int bus_id);
|
||||
void updateMotorDiagnostics(std::size_t index);
|
||||
void pollMotorDiagnostics();
|
||||
void finalizeRunSummary();
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr model_status_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
|
||||
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||
rclcpp::TimerBase::SharedPtr write_timer_;
|
||||
rclcpp::CallbackGroup::SharedPtr motor_callback_group_;
|
||||
rclcpp::CallbackGroup::SharedPtr sensor_callback_group_;
|
||||
rclcpp::CallbackGroup::SharedPtr control_callback_group_;
|
||||
|
||||
std::mutex target_mutex_;
|
||||
std::array<float, 16> latest_target_{};
|
||||
std::array<float, 16> latest_raw_action_{};
|
||||
std::string latest_target_source_{"boot_hold"};
|
||||
rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::array<float, 16> rough_default_dof_pos_{};
|
||||
std::array<float, 16> crawl_default_dof_pos_{};
|
||||
std::array<float, 16> wall_default_dof_pos_{};
|
||||
std::array<float, 16> active_default_dof_pos_{};
|
||||
ActiveModelMode active_model_mode_{ActiveModelMode::Rough};
|
||||
bool model_switch_active_{false};
|
||||
std::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
@@ -220,7 +101,6 @@ private:
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
static constexpr std::uint32_t kMotorDropReportThreshold = 40;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<MotorConfig, 16> motors_;
|
||||
@@ -253,71 +133,24 @@ private:
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_wheels_;
|
||||
std::unique_ptr<sim2real_common::MahonyFilter> mahony_filter_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> model_switch_safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
std::atomic<bool> mahony_initialized_{false};
|
||||
bool mahony_initialized_{false};
|
||||
rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time last_diag_poll_time_{0, 0, RCL_ROS_TIME};
|
||||
std::size_t diag_poll_motor_index_{0};
|
||||
|
||||
// Telemetry
|
||||
std::uint32_t fresh_count_{0};
|
||||
std::uint32_t holdover_count_{0};
|
||||
std::uint32_t stale_max_{0};
|
||||
std::uint32_t holdover_events_total_{0};
|
||||
std::uint32_t protection_trigger_count_{0};
|
||||
std::uint32_t motor_drop_event_count_{0};
|
||||
std::uint32_t motor_recover_event_count_{0};
|
||||
std::uint32_t motor_fault_event_count_{0};
|
||||
bool timeout_hold_logged_{false};
|
||||
bool clip_active_logged_{false};
|
||||
|
||||
bool dry_run_{false};
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
sim2real_common::EventLogger event_logger_;
|
||||
std::string run_log_dir_;
|
||||
float motor_temp_warn_c_{100.0f};
|
||||
float motor_temp_fault_c_{135.0f};
|
||||
float motor_bus_overvoltage_v_{60.0f};
|
||||
float motor_bus_undervoltage_v_{12.0f};
|
||||
float motor_current_warn_arms_{10.5f};
|
||||
float motor_current_peak_arms_{14.0f};
|
||||
float motor_torque_warn_nm_{13.0f};
|
||||
float motor_torque_peak_nm_{17.0f};
|
||||
double diag_poll_period_s_{0.10};
|
||||
float wheel_no_effect_command_threshold_{1.0f};
|
||||
float wheel_no_effect_min_response_ratio_{0.20f};
|
||||
float wheel_no_effect_velocity_epsilon_{0.25f};
|
||||
float wheel_no_effect_max_temperature_c_{90.0f};
|
||||
float wheel_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t wheel_no_effect_command_warmup_cycles_{12};
|
||||
std::uint32_t wheel_no_effect_trigger_cycles_{30};
|
||||
std::uint32_t wheel_no_effect_attempt_limit_{2};
|
||||
std::uint32_t wheel_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t wheel_recovery_verify_timeout_ms_{180};
|
||||
std::uint32_t wheel_no_effect_diag_freshness_ms_{350};
|
||||
std::uint32_t wheel_no_effect_diag_request_period_ms_{80};
|
||||
float leg_no_effect_position_error_threshold_{0.18f};
|
||||
float leg_no_effect_velocity_epsilon_{0.12f};
|
||||
float leg_no_effect_max_estimated_current_arms_{4.0f};
|
||||
float leg_no_effect_max_abs_torque_nm_{5.0f};
|
||||
float leg_no_effect_max_temperature_c_{100.0f};
|
||||
float leg_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t leg_no_effect_command_warmup_cycles_{40};
|
||||
std::uint32_t leg_no_effect_trigger_cycles_{25};
|
||||
std::uint32_t leg_no_effect_attempt_limit_{2};
|
||||
std::uint32_t leg_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t leg_recovery_verify_timeout_ms_{220};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
void logEvent(
|
||||
const std::string & level,
|
||||
const std::string & event,
|
||||
const std::string & message);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -133,7 +133,7 @@ planner_server:
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: true
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
|
||||
@@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_runtime)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(CUDAToolkit QUIET)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
@@ -32,33 +31,10 @@ find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||
|
||||
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||
|
||||
find_path(TENSORRT_INCLUDE_DIR NvInfer.h
|
||||
PATHS
|
||||
/usr/include
|
||||
/usr/include/aarch64-linux-gnu
|
||||
/usr/local/include
|
||||
)
|
||||
|
||||
find_library(TENSORRT_LIBRARY NAMES nvinfer
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/lib/aarch64-linux-gnu
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/local/lib
|
||||
)
|
||||
|
||||
if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY)
|
||||
message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.")
|
||||
endif()
|
||||
|
||||
set(SIM2REAL_RUNTIME_HAS_TENSORRT FALSE)
|
||||
if(TENSORRT_INCLUDE_DIR AND TENSORRT_LIBRARY AND CUDAToolkit_FOUND)
|
||||
set(SIM2REAL_RUNTIME_HAS_TENSORRT TRUE)
|
||||
message(STATUS "TensorRT support enabled for sim2real_runtime")
|
||||
else()
|
||||
message(STATUS "TensorRT support disabled for sim2real_runtime (missing TensorRT or CUDA toolkit)")
|
||||
endif()
|
||||
|
||||
add_executable(sim2real_runtime_node
|
||||
src/policy_runtime_node.cpp
|
||||
)
|
||||
@@ -67,60 +43,23 @@ add_executable(odom_relay_node
|
||||
src/odom_relay_node.cpp
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
add_executable(compare_onnx_trt
|
||||
src/compare_onnx_trt.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
include
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
${TENSORRT_INCLUDE_DIR}
|
||||
)
|
||||
target_include_directories(compare_onnx_trt PRIVATE
|
||||
${TENSORRT_INCLUDE_DIR}
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(odom_relay_node PRIVATE include)
|
||||
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${TENSORRT_LIBRARY}
|
||||
CUDA::cudart
|
||||
)
|
||||
target_link_libraries(compare_onnx_trt
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
${TENSORRT_LIBRARY}
|
||||
CUDA::cudart
|
||||
)
|
||||
target_compile_definitions(sim2real_runtime_node PRIVATE
|
||||
SIM2REAL_RUNTIME_HAS_TENSORRT=1
|
||||
)
|
||||
target_compile_definitions(compare_onnx_trt PRIVATE
|
||||
SIM2REAL_RUNTIME_HAS_TENSORRT=1
|
||||
)
|
||||
endif()
|
||||
|
||||
set_target_properties(sim2real_runtime_node PROPERTIES
|
||||
BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
)
|
||||
|
||||
target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17)
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
target_compile_features(compare_onnx_trt PRIVATE cxx_std_17)
|
||||
endif()
|
||||
|
||||
ament_target_dependencies(sim2real_runtime_node
|
||||
geometry_msgs
|
||||
@@ -132,12 +71,6 @@ ament_target_dependencies(sim2real_runtime_node
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
ament_target_dependencies(compare_onnx_trt
|
||||
sim2real_common
|
||||
)
|
||||
endif()
|
||||
|
||||
ament_target_dependencies(odom_relay_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
@@ -155,26 +88,11 @@ install(
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(SIM2REAL_RUNTIME_HAS_TENSORRT)
|
||||
install(
|
||||
TARGETS compare_onnx_trt
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
endif()
|
||||
|
||||
install(
|
||||
PROGRAMS
|
||||
src/remote_uart_node.py
|
||||
src/cmd_mux_node.py
|
||||
src/web_udp_bridge_node.py
|
||||
src/simple_nav_node.py
|
||||
src/pcd_nav_click_tool.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
FILES
|
||||
src/deadzone_velocity_limiter.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
+7
-180
@@ -5,18 +5,15 @@
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/event_logger.hpp"
|
||||
#include "sim2real_common/stand_balance_controller.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
@@ -24,11 +21,6 @@
|
||||
// ONNXRuntime C++ API
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT
|
||||
#include <NvInfer.h>
|
||||
#include <cuda_runtime_api.h>
|
||||
#endif
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
@@ -36,86 +28,13 @@ class PolicyRuntimeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PolicyRuntimeNode();
|
||||
~PolicyRuntimeNode() override;
|
||||
|
||||
private:
|
||||
enum class InferenceBackend {
|
||||
None,
|
||||
TensorRT,
|
||||
OnnxRuntime,
|
||||
};
|
||||
|
||||
enum class ModelMode {
|
||||
Rough,
|
||||
Crawl,
|
||||
Wall,
|
||||
};
|
||||
|
||||
enum class CrawlBackend {
|
||||
Ik,
|
||||
Rl,
|
||||
};
|
||||
|
||||
enum class ModelSwitchState {
|
||||
Idle,
|
||||
ToStand,
|
||||
StandHold,
|
||||
ToModelPose,
|
||||
};
|
||||
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
|
||||
enum class PostureHoldMode {
|
||||
None,
|
||||
Keep,
|
||||
ReturnDefault,
|
||||
};
|
||||
|
||||
void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg);
|
||||
void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
|
||||
void onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg);
|
||||
void onPostureCmd(const std_msgs::msg::String::SharedPtr msg);
|
||||
void applyCmdVel(float vx, float vy, float vyaw);
|
||||
void onPolicyLoop();
|
||||
bool initInferenceBackend();
|
||||
bool initTensorRt();
|
||||
bool initOnnxRuntime();
|
||||
void shutdownOnnxRuntime();
|
||||
void shutdownTensorRt();
|
||||
std::string deriveTensorRtEnginePath(const std::string & onnx_model_path) const;
|
||||
const std::array<float, 16> & defaultPoseForMode(ModelMode mode) const;
|
||||
const std::string & modelPathForMode(ModelMode mode) const;
|
||||
const std::string & modelEnginePathForMode(ModelMode mode) const;
|
||||
bool switchInferenceModel(ModelMode target_mode);
|
||||
void publishModelStatus();
|
||||
bool modeUsesInference(ModelMode mode) const;
|
||||
std::array<float, 16> computeHoldTarget(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd);
|
||||
std::array<float, 16> computeIkCrawlTarget(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd);
|
||||
float computeCrawlIkCommandScale(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 16> & leg_target) const;
|
||||
float projectedGravityTiltRad(const std::array<float, 3> & projected_gravity) const;
|
||||
const char * startupStateName(StartupState state) const;
|
||||
const char * modelModeName(ModelMode mode) const;
|
||||
const char * modelSwitchStateName(ModelSwitchState state) const;
|
||||
const char * postureHoldModeName(PostureHoldMode mode) const;
|
||||
const char * inferenceBackendName() const;
|
||||
const char * crawlBackendName() const;
|
||||
void initializeDebugTrace();
|
||||
void appendDebugTrace(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const sim2real_interfaces::msg::RuntimeTarget & target);
|
||||
|
||||
std::array<float, 53> buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
@@ -125,19 +44,11 @@ private:
|
||||
std::array<float, 16> runPolicy(const std::array<float, 53> & obs);
|
||||
bool isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const;
|
||||
bool isCommandActive(const std::array<float, 3> & cmd) const;
|
||||
void startPostureTransition(
|
||||
PostureHoldMode mode,
|
||||
const std::array<float, 16> & start_pose,
|
||||
const std::array<float, 16> & target_pose,
|
||||
const rclcpp::Time & now_time);
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr model_status_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_stamped_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr model_switch_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr posture_cmd_sub_;
|
||||
rclcpp::TimerBase::SharedPtr policy_timer_;
|
||||
|
||||
std::mutex mutex_;
|
||||
@@ -150,6 +61,13 @@ private:
|
||||
std::uint32_t sequence_{0};
|
||||
|
||||
// Startup State Machine
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
StartupState startup_state_{StartupState::BOOT_HOLD};
|
||||
std::array<float, 16> start_pose_{};
|
||||
std::array<float, 16> startup_delta_{};
|
||||
@@ -158,65 +76,9 @@ private:
|
||||
double hold_time_{1.0};
|
||||
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||
|
||||
ModelMode current_model_mode_{ModelMode::Rough};
|
||||
ModelMode requested_model_mode_{ModelMode::Rough};
|
||||
ModelMode loaded_model_mode_{ModelMode::Rough};
|
||||
CrawlBackend crawl_backend_{CrawlBackend::Ik};
|
||||
ModelSwitchState model_switch_state_{ModelSwitchState::Idle};
|
||||
bool model_switch_requested_{false};
|
||||
bool hold_active_model_pose_when_unreleased_{false};
|
||||
std::array<float, 16> rough_default_dof_pos_{};
|
||||
std::array<float, 16> crawl_default_dof_pos_{};
|
||||
std::array<float, 16> wall_default_dof_pos_{};
|
||||
std::array<float, 16> active_default_dof_pos_{};
|
||||
std::array<float, 16> safety_reference_dof_pos_{};
|
||||
std::array<float, 16> keep_pose_dof_pos_{};
|
||||
std::array<float, 16> posture_start_pose_{};
|
||||
std::array<float, 16> posture_target_pose_{};
|
||||
std::array<float, 16> posture_delta_{};
|
||||
PostureHoldMode posture_hold_mode_{PostureHoldMode::None};
|
||||
bool posture_transition_active_{false};
|
||||
rclcpp::Time posture_transition_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double posture_transition_s_{0.8};
|
||||
std::array<float, 16> switch_start_pose_{};
|
||||
std::array<float, 16> switch_delta_{};
|
||||
rclcpp::Time model_switch_state_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double model_switch_transition_s_{1.2};
|
||||
double model_switch_to_stand_transition_scale_{1.35};
|
||||
double model_switch_to_model_transition_scale_{1.55};
|
||||
double model_switch_min_transition_s_{0.35};
|
||||
double model_switch_stand_hold_s_{0.45};
|
||||
double model_switch_stand_max_err_{0.18};
|
||||
double model_switch_stand_max_vel_{0.8};
|
||||
double active_switch_transition_s_{1.2};
|
||||
|
||||
std::string rough_model_path_{"policies/model_rough.onnx"};
|
||||
std::string rough_model_engine_path_{""};
|
||||
std::string crawl_model_path_{"policies/model_crawl.onnx"};
|
||||
std::string crawl_model_engine_path_{""};
|
||||
std::string wall_model_path_{"policies/model_wall.onnx"};
|
||||
std::string wall_model_engine_path_{""};
|
||||
float crawl_ik_wheel_linear_gain_{6.25f};
|
||||
float crawl_ik_wheel_yaw_gain_{4.0f};
|
||||
float crawl_ik_max_wheel_speed_{6.0f};
|
||||
float crawl_ik_abduction_clip_{0.45f};
|
||||
float crawl_ik_yaw_rate_kp_{0.0f};
|
||||
bool crawl_ik_imu_posture_{false};
|
||||
float crawl_ik_encoder_posture_kp_{0.0f};
|
||||
float crawl_ik_encoder_posture_max_{0.03f};
|
||||
bool crawl_ik_encoder_guard_{true};
|
||||
float crawl_ik_encoder_guard_start_{0.28f};
|
||||
float crawl_ik_encoder_guard_stop_{0.65f};
|
||||
bool crawl_ik_imu_guard_{true};
|
||||
float crawl_ik_imu_guard_start_rad_{0.20943952f};
|
||||
float crawl_ik_imu_guard_stop_rad_{0.48869219f};
|
||||
|
||||
// ONNX Runtime members
|
||||
std::string model_path_{"policies/model_rough.onnx"};
|
||||
std::string model_engine_path_{""};
|
||||
bool prefer_tensorrt_{true};
|
||||
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||
InferenceBackend inference_backend_{InferenceBackend::None};
|
||||
std::unique_ptr<Ort::Env> env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
@@ -229,35 +91,19 @@ private:
|
||||
std::vector<std::int64_t> input_shape_;
|
||||
std::vector<std::int64_t> output_shape_;
|
||||
|
||||
#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT
|
||||
nvinfer1::IRuntime * trt_runtime_{nullptr};
|
||||
nvinfer1::ICudaEngine * trt_engine_{nullptr};
|
||||
nvinfer1::IExecutionContext * trt_context_{nullptr};
|
||||
cudaStream_t trt_stream_{nullptr};
|
||||
void * trt_input_buffer_{nullptr};
|
||||
void * trt_output_buffer_{nullptr};
|
||||
std::string trt_input_name_;
|
||||
std::string trt_output_name_;
|
||||
#endif
|
||||
|
||||
// Command filter and release states
|
||||
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
float runtime_max_vx_{1.0f};
|
||||
float runtime_max_vy_{0.3f};
|
||||
float runtime_max_yaw_rate_{1.0f};
|
||||
float release_alpha_{0.0f};
|
||||
float command_release_s_{0.35f};
|
||||
float release_command_hold_s_{0.12f};
|
||||
float release_posture_max_err_{0.35f};
|
||||
float release_target_blend_s_{0.30f};
|
||||
float model_switch_release_scale_{1.3f};
|
||||
float clip_obs_{100.0f};
|
||||
bool hold_zero_command_pose_{true};
|
||||
bool enable_zero_cmd_suppression_{true};
|
||||
bool require_active_command_to_release_{true};
|
||||
bool zero_cmd_use_yaw_rate_{false};
|
||||
bool runtime_released_{false};
|
||||
bool slow_release_after_model_switch_{false};
|
||||
float release_active_time_{0.0f};
|
||||
float zero_cmd_lin_thresh_{0.05f};
|
||||
float zero_cmd_yaw_thresh_{0.05f};
|
||||
@@ -269,29 +115,10 @@ private:
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
sim2real_common::EventLogger event_logger_;
|
||||
std::string run_log_dir_;
|
||||
std::string debug_trace_path_;
|
||||
bool debug_trace_enabled_{true};
|
||||
std::uint32_t debug_trace_decimation_{1};
|
||||
std::uint32_t debug_trace_counter_{0};
|
||||
std::uint32_t protection_trigger_count_{0};
|
||||
std::uint32_t target_clip_count_{0};
|
||||
bool clip_active_logged_{false};
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> model_switch_safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
void logEvent(
|
||||
const std::string & level,
|
||||
const std::string & event,
|
||||
const std::string & message);
|
||||
void logProtectionEvent(
|
||||
const std::string & trigger,
|
||||
const std::string & reason,
|
||||
const std::string & action);
|
||||
void finalizeRunSummary();
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
@@ -12,13 +12,10 @@
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
<exec_depend>python3-matplotlib</exec_depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
|
||||
<export>
|
||||
|
||||
@@ -10,15 +10,12 @@ from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
from deadzone_velocity_limiter import limit_deadzone_axis
|
||||
|
||||
|
||||
class ControlMode(str, Enum):
|
||||
DISABLED = "DISABLED"
|
||||
REMOTE = "REMOTE"
|
||||
WEB = "WEB"
|
||||
NAV = "NAV"
|
||||
KEEP = "KEEP"
|
||||
|
||||
|
||||
class CmdMuxNode(Node):
|
||||
@@ -36,37 +33,6 @@ class CmdMuxNode(Node):
|
||||
self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value)
|
||||
self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value)
|
||||
self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value)
|
||||
self.max_vx_decel = float(
|
||||
self.declare_parameter("cmd_mux_max_vx_decel", self.max_vx_acc).value
|
||||
)
|
||||
self.max_vy_decel = float(
|
||||
self.declare_parameter("cmd_mux_max_vy_decel", self.max_vy_acc).value
|
||||
)
|
||||
self.max_yaw_decel = float(
|
||||
self.declare_parameter("cmd_mux_max_yaw_decel", self.max_yaw_acc).value
|
||||
)
|
||||
self.linear_deadzone_epsilon = float(
|
||||
self.declare_parameter("cmd_mux_linear_deadzone_epsilon", 0.0).value
|
||||
)
|
||||
self.yaw_deadzone_epsilon = float(
|
||||
self.declare_parameter("cmd_mux_yaw_deadzone_epsilon", 0.0).value
|
||||
)
|
||||
self.min_effective_vx = float(
|
||||
self.declare_parameter("cmd_mux_min_effective_vx", 0.0).value
|
||||
)
|
||||
self.min_effective_vy = float(
|
||||
self.declare_parameter("cmd_mux_min_effective_vy", 0.0).value
|
||||
)
|
||||
self.min_effective_yaw = float(
|
||||
self.declare_parameter("cmd_mux_min_effective_yaw_rate", 0.0).value
|
||||
)
|
||||
self.deadzone_sources = {
|
||||
item.strip().lower()
|
||||
for item in str(
|
||||
self.declare_parameter("cmd_mux_deadzone_sources", "nav").value
|
||||
).split(",")
|
||||
if item.strip()
|
||||
}
|
||||
|
||||
self.mode = self.parse_mode(self.default_mode)
|
||||
self.estop = False
|
||||
@@ -163,16 +129,9 @@ class CmdMuxNode(Node):
|
||||
elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now):
|
||||
target = self.latest_nav
|
||||
source = "nav"
|
||||
elif self.mode == ControlMode.KEEP:
|
||||
source = "keep"
|
||||
|
||||
target = self.limit_twist(target)
|
||||
if self.estop:
|
||||
target = Twist()
|
||||
self.last_output = Twist()
|
||||
self.last_pub_time = now
|
||||
else:
|
||||
target = self.accel_limit(target, now, source in self.deadzone_sources)
|
||||
target = self.accel_limit(target, now)
|
||||
self.cmd_pub.publish(target)
|
||||
self.mode_pub.publish(String(data=self.mode.value))
|
||||
self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}"))
|
||||
@@ -190,46 +149,12 @@ class CmdMuxNode(Node):
|
||||
out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw)
|
||||
return out
|
||||
|
||||
def accel_limit(
|
||||
self,
|
||||
target: Twist,
|
||||
now: rclpy.time.Time,
|
||||
apply_deadzone: bool,
|
||||
) -> Twist:
|
||||
def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist:
|
||||
dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3)
|
||||
min_effective_vx = self.min_effective_vx if apply_deadzone else 0.0
|
||||
min_effective_vy = self.min_effective_vy if apply_deadzone else 0.0
|
||||
min_effective_yaw = self.min_effective_yaw if apply_deadzone else 0.0
|
||||
linear_deadzone_epsilon = self.linear_deadzone_epsilon if apply_deadzone else 0.0
|
||||
yaw_deadzone_epsilon = self.yaw_deadzone_epsilon if apply_deadzone else 0.0
|
||||
out = Twist()
|
||||
out.linear.x = limit_deadzone_axis(
|
||||
self.last_output.linear.x,
|
||||
target.linear.x,
|
||||
dt,
|
||||
self.max_vx_acc,
|
||||
self.max_vx_decel,
|
||||
min_effective_vx,
|
||||
linear_deadzone_epsilon,
|
||||
)
|
||||
out.linear.y = limit_deadzone_axis(
|
||||
self.last_output.linear.y,
|
||||
target.linear.y,
|
||||
dt,
|
||||
self.max_vy_acc,
|
||||
self.max_vy_decel,
|
||||
min_effective_vy,
|
||||
linear_deadzone_epsilon,
|
||||
)
|
||||
out.angular.z = limit_deadzone_axis(
|
||||
self.last_output.angular.z,
|
||||
target.angular.z,
|
||||
dt,
|
||||
self.max_yaw_acc,
|
||||
self.max_yaw_decel,
|
||||
min_effective_yaw,
|
||||
yaw_deadzone_epsilon,
|
||||
)
|
||||
out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt)
|
||||
out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt)
|
||||
out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt)
|
||||
self.last_output = out
|
||||
self.last_pub_time = now
|
||||
return out
|
||||
@@ -238,6 +163,16 @@ class CmdMuxNode(Node):
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, float(value)))
|
||||
|
||||
@staticmethod
|
||||
def step(current: float, target: float, max_delta: float) -> float:
|
||||
delta = target - current
|
||||
if delta > max_delta:
|
||||
return current + max_delta
|
||||
if delta < -max_delta:
|
||||
return current - max_delta
|
||||
return target
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CmdMuxNode()
|
||||
|
||||
+101
-1531
File diff suppressed because it is too large
Load Diff
@@ -10,7 +10,7 @@ import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool, String
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
@@ -23,22 +23,7 @@ SWITCH_HIGH = 1
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch5: int = SWITCH_MID
|
||||
ch6: int = SWITCH_MID
|
||||
ch7: int = SWITCH_MID
|
||||
ch8: int = SWITCH_MID
|
||||
ch9: int = SWITCH_MID
|
||||
ch10: int = SWITCH_MID
|
||||
|
||||
def get(self, channel: int) -> Optional[int]:
|
||||
return {
|
||||
5: self.ch5,
|
||||
6: self.ch6,
|
||||
7: self.ch7,
|
||||
8: self.ch8,
|
||||
9: self.ch9,
|
||||
10: self.ch10,
|
||||
}.get(int(channel))
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -129,14 +114,7 @@ class SbusUartReceiver:
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
switches=RemoteSwitchState(
|
||||
ch5=self._decode_switch(channels[4]),
|
||||
ch6=self._decode_switch(channels[5]),
|
||||
ch7=self._decode_switch(channels[6]),
|
||||
ch8=self._decode_switch(channels[7]),
|
||||
ch9=self._decode_switch(channels[8]),
|
||||
ch10=self._decode_switch(channels[9]),
|
||||
),
|
||||
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||
frame_ok=True,
|
||||
)
|
||||
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||
@@ -176,31 +154,12 @@ class RemoteUartNode(Node):
|
||||
self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value)
|
||||
self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value)
|
||||
self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value)
|
||||
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).strip().upper()
|
||||
self.model_switch_enabled = bool(self.declare_parameter("remote_model_switch_enabled", True).value)
|
||||
self.model_switch_channel = int(self.declare_parameter("remote_model_switch_channel", 10).value)
|
||||
self.model_switch_debounce_frames = max(int(self.declare_parameter("remote_model_switch_debounce_frames", 3).value), 1)
|
||||
self.model_switch_rough_level = self.parse_switch_level(
|
||||
str(self.declare_parameter("remote_model_switch_rough_level", "low").value)
|
||||
)
|
||||
legacy_ik_level = str(self.declare_parameter("remote_model_switch_crawl_level", "").value).strip()
|
||||
ik_level_default = legacy_ik_level if legacy_ik_level else "high"
|
||||
self.model_switch_ik_level = self.parse_switch_level(
|
||||
str(self.declare_parameter("remote_model_switch_ik_level", ik_level_default).value)
|
||||
)
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10)
|
||||
self.receiver: Optional[SbusUartReceiver] = None
|
||||
self.estop_published = False
|
||||
self.open_error_logged = False
|
||||
self.remote_mode_active = self.default_mode == "REMOTE"
|
||||
self.model_switch_candidate: Optional[int] = None
|
||||
self.model_switch_candidate_count = 0
|
||||
self.model_switch_stable: Optional[int] = None
|
||||
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
|
||||
if self.enabled:
|
||||
self.receiver = SbusUartReceiver(
|
||||
@@ -254,8 +213,6 @@ class RemoteUartNode(Node):
|
||||
self.estop_pub.publish(Bool(data=False))
|
||||
self.estop_published = False
|
||||
|
||||
self.handle_model_switch(state)
|
||||
|
||||
active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||
if active or self.publish_inactive_zero:
|
||||
cmd = Twist()
|
||||
@@ -267,15 +224,6 @@ class RemoteUartNode(Node):
|
||||
def publish_zero_cmd(self) -> None:
|
||||
self.cmd_pub.publish(Twist())
|
||||
|
||||
def on_mode_state(self, msg: String) -> None:
|
||||
mode = str(msg.data).strip().upper()
|
||||
remote_mode_active = mode == "REMOTE"
|
||||
if remote_mode_active == self.remote_mode_active:
|
||||
return
|
||||
|
||||
self.remote_mode_active = remote_mode_active
|
||||
self.reset_model_switch_tracking()
|
||||
|
||||
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
@@ -284,53 +232,6 @@ class RemoteUartNode(Node):
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
@staticmethod
|
||||
def parse_switch_level(value: str) -> int:
|
||||
normalized = value.strip().lower()
|
||||
if normalized == "low":
|
||||
return SWITCH_LOW
|
||||
if normalized == "high":
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
def handle_model_switch(self, state: RemoteControlState) -> None:
|
||||
if not self.model_switch_enabled or not self.remote_mode_active:
|
||||
return
|
||||
|
||||
switch_level = state.switches.get(self.model_switch_channel)
|
||||
if switch_level is None:
|
||||
return
|
||||
|
||||
if switch_level == self.model_switch_candidate:
|
||||
self.model_switch_candidate_count += 1
|
||||
else:
|
||||
self.model_switch_candidate = switch_level
|
||||
self.model_switch_candidate_count = 1
|
||||
|
||||
if self.model_switch_candidate_count < self.model_switch_debounce_frames:
|
||||
return
|
||||
|
||||
if switch_level == self.model_switch_stable:
|
||||
return
|
||||
|
||||
self.model_switch_stable = switch_level
|
||||
|
||||
if switch_level == self.model_switch_rough_level:
|
||||
self.model_cmd_pub.publish(String(data="rough"))
|
||||
self.get_logger().info(
|
||||
f"Remote model switch: CH{self.model_switch_channel} -> rough"
|
||||
)
|
||||
elif switch_level == self.model_switch_ik_level:
|
||||
self.model_cmd_pub.publish(String(data="ik"))
|
||||
self.get_logger().info(
|
||||
f"Remote model switch: CH{self.model_switch_channel} -> ik"
|
||||
)
|
||||
|
||||
def reset_model_switch_tracking(self) -> None:
|
||||
self.model_switch_candidate = None
|
||||
self.model_switch_candidate_count = 0
|
||||
self.model_switch_stable = None
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
STATE_LOCK = threading.Lock()
|
||||
LATEST_STATE: dict = {"type": "state", "connected": False}
|
||||
NANO_ADDR: tuple[str, int]
|
||||
UDP_SOCK: socket.socket
|
||||
|
||||
|
||||
class Handler(SimpleHTTPRequestHandler):
|
||||
def do_GET(self) -> None:
|
||||
if self.path == "/api/state":
|
||||
with STATE_LOCK:
|
||||
data = json.dumps(LATEST_STATE).encode("utf-8")
|
||||
self._json(200, data)
|
||||
return
|
||||
super().do_GET()
|
||||
|
||||
def do_POST(self) -> None:
|
||||
if self.path not in ("/api/control", "/api/state"):
|
||||
self.send_error(404)
|
||||
return
|
||||
length = int(self.headers.get("Content-Length", "0"))
|
||||
body = self.rfile.read(length) if length else b"{}"
|
||||
try:
|
||||
payload = json.loads(body.decode("utf-8"))
|
||||
send_udp(payload)
|
||||
self._json(200, b'{"ok":true}')
|
||||
except Exception as exc:
|
||||
self._json(400, json.dumps({"ok": False, "error": str(exc)}).encode())
|
||||
|
||||
def _json(self, code: int, data: bytes) -> None:
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", "application/json")
|
||||
self.send_header("Content-Length", str(len(data)))
|
||||
self.send_header("Access-Control-Allow-Origin", "*")
|
||||
self.end_headers()
|
||||
self.wfile.write(data)
|
||||
|
||||
def log_message(self, format: str, *args: object) -> None:
|
||||
return
|
||||
|
||||
|
||||
def send_udp(payload: dict) -> None:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
UDP_SOCK.sendto(data, NANO_ADDR)
|
||||
|
||||
|
||||
def udp_rx_loop(sock: socket.socket) -> None:
|
||||
global LATEST_STATE
|
||||
while True:
|
||||
try:
|
||||
data, _ = sock.recvfrom(65535)
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
payload["connected"] = True
|
||||
payload["local_receive_time"] = time.time()
|
||||
with STATE_LOCK:
|
||||
LATEST_STATE = payload
|
||||
except Exception:
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
def heartbeat_loop() -> None:
|
||||
while True:
|
||||
try:
|
||||
send_udp({"type": "ping", "stamp": time.time()})
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
global NANO_ADDR, UDP_SOCK
|
||||
parser = argparse.ArgumentParser(description="Windows local web debug UI for sim2real_ros2")
|
||||
parser.add_argument("--nano-host", required=True, help="Nano IP address")
|
||||
parser.add_argument("--nano-port", type=int, default=15000, help="Nano UDP listen port")
|
||||
parser.add_argument("--listen-host", default="0.0.0.0", help="Local HTTP host")
|
||||
parser.add_argument("--http-port", type=int, default=8088, help="Local HTTP port")
|
||||
parser.add_argument("--udp-port", type=int, default=15001, help="Local UDP receive port")
|
||||
args = parser.parse_args()
|
||||
|
||||
NANO_ADDR = (args.nano_host, args.nano_port)
|
||||
UDP_SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
UDP_SOCK.bind(("0.0.0.0", args.udp_port))
|
||||
|
||||
threading.Thread(target=udp_rx_loop, args=(UDP_SOCK,), daemon=True).start()
|
||||
threading.Thread(target=heartbeat_loop, daemon=True).start()
|
||||
|
||||
static_dir = Path(__file__).resolve().parent / "static"
|
||||
handler = lambda *a, **kw: Handler(*a, directory=str(static_dir), **kw)
|
||||
httpd = ThreadingHTTPServer((args.listen_host, args.http_port), handler)
|
||||
print(f"Open http://127.0.0.1:{args.http_port}")
|
||||
print(f"UDP Nano={args.nano_host}:{args.nano_port} local={args.udp_port}")
|
||||
httpd.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,259 @@
|
||||
'use strict';
|
||||
|
||||
const JOINT_NAMES = [
|
||||
'FL_H_ABD','FL_H_PIT','FL_KNEE',
|
||||
'FR_H_ABD','FR_H_PIT','FR_KNEE',
|
||||
'RL_H_ABD','RL_H_PIT','RL_KNEE',
|
||||
'RR_H_ABD','RR_H_PIT','RR_KNEE',
|
||||
'FL_WHEEL','FR_WHEEL','RL_WHEEL','RR_WHEEL',
|
||||
];
|
||||
|
||||
const $ = id => document.getElementById(id);
|
||||
const cmd = { vx: 0, vy: 0, yaw: 0 };
|
||||
let cmdSendTimer = null;
|
||||
let currentMode = 'UNKNOWN';
|
||||
|
||||
// ── API ──────────────────────────────────────────────────────────────────────
|
||||
async function post(payload) {
|
||||
try {
|
||||
await fetch('/api/control', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(payload),
|
||||
});
|
||||
} catch (e) {
|
||||
appendEvent('API_ERROR', e.message, 'bad');
|
||||
}
|
||||
}
|
||||
|
||||
function sendCmd() {
|
||||
post({
|
||||
type: 'cmd_vel',
|
||||
linear: { x: cmd.vx, y: cmd.vy, z: 0 },
|
||||
angular: { x: 0, y: 0, z: cmd.yaw },
|
||||
});
|
||||
$('cmd-display').textContent =
|
||||
`vx=${cmd.vx.toFixed(2)} vy=${cmd.vy.toFixed(2)} yaw=${cmd.yaw.toFixed(2)}`;
|
||||
}
|
||||
|
||||
function zeroAll() {
|
||||
cmd.vx = 0; cmd.vy = 0; cmd.yaw = 0;
|
||||
$('cmd-vx').value = 0;
|
||||
$('cmd-vy').value = 0;
|
||||
$('cmd-yaw').value = 0;
|
||||
$('cmd-vx-v').textContent = '0.00';
|
||||
$('cmd-vy-v').textContent = '0.00';
|
||||
$('cmd-yaw-v').textContent = '0.00';
|
||||
$('cmd-display').textContent = 'vx=0.00 vy=0.00 yaw=0.00';
|
||||
$('stick').style.transform = 'translate(-50%, -50%)';
|
||||
post({ type: 'zero' });
|
||||
}
|
||||
|
||||
// ── Buttons ──────────────────────────────────────────────────────────────────
|
||||
function setMode(mode) {
|
||||
if (mode === 'WEB' && !confirm('确认切换到 WEB 控制？\n请确认机器人安全且速度为 0。')) return;
|
||||
post({ type: 'mode', mode });
|
||||
appendEvent('MODE_SET', `→ ${mode}`, 'ok');
|
||||
}
|
||||
|
||||
function highlightMode(mode) {
|
||||
for (const m of ['DISABLED', 'REMOTE', 'WEB', 'NAV']) {
|
||||
const btn = $('btn-' + m.toLowerCase());
|
||||
if (btn) btn.classList.toggle('active-mode', m === mode);
|
||||
}
|
||||
const el = $('stage');
|
||||
if (el) {
|
||||
el.textContent = mode;
|
||||
el.className = 'stage ' + mode;
|
||||
}
|
||||
currentMode = mode;
|
||||
}
|
||||
|
||||
$('btn-disabled').onclick = () => { zeroAll(); setMode('DISABLED'); };
|
||||
$('btn-remote').onclick = () => setMode('REMOTE');
|
||||
$('btn-web').onclick = () => setMode('WEB');
|
||||
$('btn-nav').onclick = () => setMode('NAV');
|
||||
$('btn-zero').onclick = zeroAll;
|
||||
$('btn-estop').onclick = () => {
|
||||
if (confirm('确认触发软急停？')) {
|
||||
post({ type: 'estop', data: true });
|
||||
zeroAll();
|
||||
appendEvent('ESTOP', '软急停已触发', 'bad');
|
||||
}
|
||||
};
|
||||
|
||||
// ── Sliders ──────────────────────────────────────────────────────────────────
|
||||
for (const [id, key] of [['cmd-vx','vx'],['cmd-vy','vy'],['cmd-yaw','yaw']]) {
|
||||
$(id).addEventListener('input', e => {
|
||||
cmd[key] = parseFloat(e.target.value);
|
||||
$(id + '-v').textContent = cmd[key].toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
});
|
||||
}
|
||||
|
||||
// ── Joystick ─────────────────────────────────────────────────────────────────
|
||||
const joystick = $('joystick');
|
||||
const stick = $('stick');
|
||||
let dragging = false;
|
||||
|
||||
function updateJoystick(clientX, clientY) {
|
||||
const rect = joystick.getBoundingClientRect();
|
||||
const cx = rect.left + rect.width / 2;
|
||||
const cy = rect.top + rect.height / 2;
|
||||
const maxR = rect.width * 0.42;
|
||||
let dx = clientX - cx;
|
||||
let dy = clientY - cy;
|
||||
const dist = Math.hypot(dx, dy);
|
||||
if (dist > maxR) { dx = dx / dist * maxR; dy = dy / dist * maxR; }
|
||||
stick.style.transform = `translate(calc(-50% + ${dx}px), calc(-50% + ${dy}px))`;
|
||||
cmd.vx = parseFloat((-(dy / maxR) * 0.8).toFixed(3));
|
||||
cmd.vy = parseFloat(( (dx / maxR) * 0.3).toFixed(3));
|
||||
$('cmd-vx').value = cmd.vx;
|
||||
$('cmd-vy').value = cmd.vy;
|
||||
$('cmd-vx-v').textContent = cmd.vx.toFixed(2);
|
||||
$('cmd-vy-v').textContent = cmd.vy.toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
}
|
||||
|
||||
joystick.addEventListener('pointerdown', e => {
|
||||
dragging = true;
|
||||
joystick.setPointerCapture(e.pointerId);
|
||||
updateJoystick(e.clientX, e.clientY);
|
||||
});
|
||||
joystick.addEventListener('pointermove', e => { if (dragging) updateJoystick(e.clientX, e.clientY); });
|
||||
joystick.addEventListener('pointerup', () => { dragging = false; zeroAll(); });
|
||||
joystick.addEventListener('pointercancel', () => { dragging = false; zeroAll(); });
|
||||
|
||||
// ── Joints grid init ─────────────────────────────────────────────────────────
|
||||
function initJointsGrid() {
|
||||
const grid = $('joints-grid');
|
||||
if (!grid) return;
|
||||
grid.innerHTML = JOINT_NAMES.map((name, i) => `
|
||||
<div class="motor-row" id="mi-${i}">
|
||||
<span class="stale" id="ms-${i}" style="color:#ef4444">●</span>
|
||||
<span class="name">${name}</span>
|
||||
<span class="val pos" id="mp-${i}">0.00</span>
|
||||
<span class="val vel" id="mv-${i}">0.00</span>
|
||||
<span class="val tau" id="mt-${i}">0.00</span>
|
||||
</div>`).join('');
|
||||
}
|
||||
|
||||
function updateJointsGrid(robot) {
|
||||
if (!robot) return;
|
||||
const pos = robot.joint_pos || [];
|
||||
const vel = robot.joint_vel || [];
|
||||
const tau = robot.joint_torque || [];
|
||||
const upd = robot.update_counts || [];
|
||||
for (let i = 0; i < 16; i++) {
|
||||
const dot = $('ms-' + i);
|
||||
const cnt = upd[i] ?? 0;
|
||||
if (dot) dot.style.color = cnt > 0 ? '#30d158' : '#ef4444';
|
||||
const p = $('mp-' + i); if (p) p.textContent = (pos[i] || 0).toFixed(2);
|
||||
const v = $('mv-' + i); if (v) v.textContent = (vel[i] || 0).toFixed(2);
|
||||
const t = $('mt-' + i);
|
||||
if (t) {
|
||||
t.textContent = (tau[i] || 0).toFixed(2);
|
||||
t.style.color = Math.abs(tau[i] || 0) > 16 ? '#ff453a' : '#ff9f0a';
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── State polling ─────────────────────────────────────────────────────────────
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
function applyState(data) {
|
||||
const connected = data.connected &&
|
||||
(!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||
|
||||
const stage = $('stage');
|
||||
if (stage) {
|
||||
if (!connected) {
|
||||
stage.textContent = 'DISCONNECTED';
|
||||
stage.className = 'stage DISCONNECTED';
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const mode = data.mode || 'UNKNOWN';
|
||||
if (mode !== currentMode) highlightMode(mode);
|
||||
|
||||
const rt = data.runtime || {};
|
||||
const src = rt.target_source || '--';
|
||||
const srcCls = src === 'safety_brake' ? 'bad'
|
||||
: src === 'runtime_policy' ? 'ok'
|
||||
: src === 'runtime_zero_hold' ? 'active' : '';
|
||||
setText('d-source', src, srcCls);
|
||||
setText('d-released', String(rt.runtime_released ?? '--'),
|
||||
rt.runtime_released ? 'ok' : '');
|
||||
setText('d-alpha', (rt.release_alpha ?? '--') !== '--'
|
||||
? Number(rt.release_alpha).toFixed(2) : '--');
|
||||
setText('d-zero', String(rt.zero_command ?? '--'));
|
||||
setText('d-estop', String(data.estop ?? '--'),
|
||||
data.estop ? 'bad' : 'ok');
|
||||
setText('d-mux', data.mux_status || '--');
|
||||
|
||||
const robot = data.robot || {};
|
||||
const imuAge = robot.imu_age_ms ?? null;
|
||||
setText('d-imu-fresh', String(robot.imu_fresh ?? '--'),
|
||||
robot.imu_fresh ? 'ok' : 'bad');
|
||||
setText('d-imu-age', imuAge !== null ? imuAge.toFixed(1) : '--',
|
||||
imuAge !== null ? (imuAge > 200 ? 'bad' : imuAge > 60 ? 'warn' : 'ok') : '');
|
||||
|
||||
const grav = robot.projected_gravity;
|
||||
setText('d-gravity', grav ? grav.map(v => Number(v).toFixed(2)).join(', ') : '--',
|
||||
grav && grav[2] < -0.5 ? 'ok' : 'warn');
|
||||
|
||||
setText('d-holdover', String(robot.holdover_count ?? '--'),
|
||||
(robot.holdover_count || 0) > 10 ? 'warn' : '');
|
||||
|
||||
const odomAge = robot.odom_age_ms ?? null;
|
||||
setText('d-odom-age', odomAge !== null ? odomAge.toFixed(1) : '--',
|
||||
odomAge !== null ? (odomAge > 500 ? 'bad' : odomAge > 200 ? 'warn' : 'ok') : '');
|
||||
|
||||
const lp = robot.odom_local_pos;
|
||||
setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--');
|
||||
|
||||
const cv = data.cmd_vel || {};
|
||||
const lin = cv.linear || {};
|
||||
const ang = cv.angular || {};
|
||||
setText('cv-vx', (lin.x ?? 0).toFixed(3));
|
||||
setText('cv-vy', (lin.y ?? 0).toFixed(3));
|
||||
setText('cv-yaw', (ang.z ?? 0).toFixed(3));
|
||||
|
||||
updateJointsGrid(robot);
|
||||
}
|
||||
|
||||
async function poll() {
|
||||
try {
|
||||
const res = await fetch('/api/state');
|
||||
const data = await res.json();
|
||||
applyState(data);
|
||||
} catch (_) {}
|
||||
}
|
||||
|
||||
// ── WEB mode heartbeat ────────────────────────────────────────────────────────
|
||||
setInterval(() => {
|
||||
if (currentMode === 'WEB' && !dragging) sendCmd();
|
||||
}, 50);
|
||||
|
||||
// ── Event log ─────────────────────────────────────────────────────────────────
|
||||
function appendEvent(kind, detail, cls) {
|
||||
const el = $('events-log');
|
||||
if (!el) return;
|
||||
const div = document.createElement('div');
|
||||
const t = new Date().toLocaleTimeString();
|
||||
div.innerHTML = `<span class="ev-t">${t}</span> <span class="ev-${cls || 'ok'}">${kind}</span> <span style="color:#8e8e93">${detail || ''}</span>`;
|
||||
el.appendChild(div);
|
||||
while (el.children.length > 200) el.removeChild(el.firstChild);
|
||||
el.scrollTop = el.scrollHeight;
|
||||
}
|
||||
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
initJointsGrid();
|
||||
setInterval(poll, 100);
|
||||
appendEvent('READY', '页面已加载，等待 Nano 连接', 'ok');
|
||||
@@ -0,0 +1,98 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN" data-theme="dark">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<title>sim2real ROS2 控制台</title>
|
||||
<link rel="stylesheet" href="style.css">
|
||||
</head>
|
||||
<body>
|
||||
<header class="glass-panel top-bar">
|
||||
<div class="top-bar-left">
|
||||
<h1>sim2real ROS2</h1>
|
||||
<span class="stage" id="stage">DISCONNECTED</span>
|
||||
</div>
|
||||
<div class="top-bar-center">
|
||||
<span class="label">控制模式</span>
|
||||
<button class="btn" id="btn-disabled">DISABLED</button>
|
||||
<button class="btn btn-remote" id="btn-remote">REMOTE</button>
|
||||
<button class="btn btn-web" id="btn-web">WEB</button>
|
||||
<button class="btn btn-nav" id="btn-nav">NAV</button>
|
||||
<div class="divider"></div>
|
||||
<button class="btn" id="btn-zero">速度归零</button>
|
||||
</div>
|
||||
<div class="top-bar-right">
|
||||
<button class="btn btn-danger" id="btn-estop">软急停</button>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="glass-panel side-panel left-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">运行状态</h2>
|
||||
<div class="diag-row"><span class="diag-label">target_source</span><span class="diag-value" id="d-source">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">runtime_released</span><span class="diag-value" id="d-released">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">release_alpha</span><span class="diag-value" id="d-alpha">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">zero_command</span><span class="diag-value" id="d-zero">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">estop</span><span class="diag-value" id="d-estop">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">mux</span><span class="diag-value" id="d-mux">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">IMU & 里程计</h2>
|
||||
<div class="diag-row"><span class="diag-label">IMU fresh</span><span class="diag-value" id="d-imu-fresh">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">IMU age ms</span><span class="diag-value" id="d-imu-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">projected_gravity</span><span class="diag-value" id="d-gravity">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">holdover</span><span class="diag-value" id="d-holdover">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom age ms</span><span class="diag-value" id="d-odom-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom local pos</span><span class="diag-value" id="d-odom-pos">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section flex-1">
|
||||
<h2 class="panel-title">关节状态 (16轴)</h2>
|
||||
<div id="joints-grid" class="motors-grid-list"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="glass-panel side-panel right-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">Web 手动控制</h2>
|
||||
<div class="joystick-area">
|
||||
<div class="joystick" id="joystick"><div id="stick"></div></div>
|
||||
<p class="hint">拖动控制前后(vx)和横移(vy)，松开归零</p>
|
||||
</div>
|
||||
<div class="slider-group">
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vx</span>
|
||||
<input type="range" id="cmd-vx" class="glass-slider" min="-0.8" max="0.8" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vx-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vy</span>
|
||||
<input type="range" id="cmd-vy" class="glass-slider" min="-0.3" max="0.3" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vy-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">yaw</span>
|
||||
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.5" max="0.5" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-yaw-v">0.00</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="cmd-display" id="cmd-display">vx=0.00 vy=0.00 yaw=0.00</div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">当前输出 /cmd_vel</h2>
|
||||
<div class="diag-row"><span class="diag-label">linear.x</span><span class="diag-value" id="cv-vx">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">linear.y</span><span class="diag-value" id="cv-vy">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">angular.z</span><span class="diag-value" id="cv-yaw">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section log-section flex-1">
|
||||
<h2 class="panel-title">事件流</h2>
|
||||
<div id="events-log" class="log"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script src="app.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,263 @@
|
||||
/* sim2real ROS2 Web Debug — Apple Glass Design */
|
||||
:root {
|
||||
--bg-primary: #000000;
|
||||
--glass-bg: rgba(20, 20, 22, 0.65);
|
||||
--glass-border: rgba(255, 255, 255, 0.12);
|
||||
--glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
|
||||
--text-primary: #ffffff;
|
||||
--text-secondary: #ebebf5;
|
||||
--text-tertiary: #8e8e93;
|
||||
--accent: #0a84ff;
|
||||
--accent-hover: #409cff;
|
||||
--success: #30d158;
|
||||
--warning: #ffd60a;
|
||||
--danger: #ff453a;
|
||||
--blur-amount: 24px;
|
||||
--saturation: 180%;
|
||||
--spring: cubic-bezier(0.4, 0, 0.2, 1);
|
||||
--panel-radius: 16px;
|
||||
--font-family: -apple-system, BlinkMacSystemFont, "SF Pro Display", "PingFang SC", sans-serif;
|
||||
}
|
||||
|
||||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
body {
|
||||
font-family: var(--font-family);
|
||||
background: radial-gradient(circle at top left, #1a1a24 0%, #000000 100%);
|
||||
color: var(--text-primary);
|
||||
-webkit-font-smoothing: antialiased;
|
||||
min-height: 100vh;
|
||||
overflow-x: hidden;
|
||||
}
|
||||
|
||||
.glass-panel {
|
||||
background: var(--glass-bg);
|
||||
backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
-webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
border: 0.5px solid var(--glass-border);
|
||||
box-shadow: var(--glass-shadow);
|
||||
z-index: 50;
|
||||
}
|
||||
|
||||
/* Top Bar */
|
||||
.top-bar {
|
||||
position: fixed;
|
||||
top: 16px;
|
||||
left: 50%;
|
||||
transform: translateX(-50%);
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
padding: 8px 16px;
|
||||
border-radius: 24px;
|
||||
width: 96%;
|
||||
max-width: 1400px;
|
||||
gap: 16px;
|
||||
}
|
||||
.top-bar-left, .top-bar-center, .top-bar-right {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
.top-bar-center { flex: 1; justify-content: center; }
|
||||
.top-bar h1 {
|
||||
font-size: 16px;
|
||||
font-weight: 600;
|
||||
background: linear-gradient(45deg, #fff, #8e8e93);
|
||||
-webkit-background-clip: text;
|
||||
-webkit-text-fill-color: transparent;
|
||||
}
|
||||
.divider { width: 1px; height: 24px; background: var(--glass-border); margin: 0 4px; }
|
||||
|
||||
/* Stage badge */
|
||||
.stage {
|
||||
padding: 4px 10px;
|
||||
border-radius: 12px;
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
background: rgba(255,255,255,0.1);
|
||||
color: var(--text-secondary);
|
||||
}
|
||||
.stage.DISCONNECTED { background: rgba(142,142,147,0.3); color: #aaa; }
|
||||
.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
|
||||
.stage.REMOTE { background: rgba(48,209,88,0.3); color: #8deda7; }
|
||||
.stage.WEB { background: rgba(0,122,255,0.3); color: #82c4ff; }
|
||||
.stage.NAV { background: rgba(255,214,10,0.3); color: #ffe680; }
|
||||
.stage.DISABLED { background: rgba(142,142,147,0.25); color: #aaa; }
|
||||
.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
|
||||
|
||||
/* Buttons */
|
||||
.btn {
|
||||
background: rgba(255,255,255,0.08);
|
||||
border: 1px solid rgba(255,255,255,0.1);
|
||||
border-radius: 8px;
|
||||
color: var(--text-primary);
|
||||
font-size: 12px;
|
||||
font-weight: 500;
|
||||
padding: 6px 12px;
|
||||
cursor: pointer;
|
||||
transition: all 0.2s var(--spring);
|
||||
font-family: inherit;
|
||||
}
|
||||
.btn:hover:not(:disabled) { background: rgba(255,255,255,0.15); transform: translateY(-1px); }
|
||||
.btn:active:not(:disabled) { transform: translateY(1px); }
|
||||
.btn:disabled { opacity: 0.5; cursor: not-allowed; }
|
||||
.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
|
||||
.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; }
|
||||
.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; }
|
||||
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
|
||||
.btn.active-mode { box-shadow: 0 0 0 2px white; }
|
||||
|
||||
.label { font-size: 11px; color: var(--text-tertiary); }
|
||||
|
||||
/* Side panels */
|
||||
.side-panel {
|
||||
position: fixed;
|
||||
top: 80px;
|
||||
bottom: 20px;
|
||||
width: 340px;
|
||||
border-radius: var(--panel-radius);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
.left-panel { left: 2%; }
|
||||
.right-panel { right: 2%; }
|
||||
.flex-1 { flex: 1; min-height: 0; overflow: hidden; display: flex; flex-direction: column; }
|
||||
|
||||
.panel-section {
|
||||
padding: 14px 16px;
|
||||
border-bottom: 0.5px solid var(--glass-border);
|
||||
}
|
||||
.panel-section:last-child { border-bottom: none; }
|
||||
|
||||
.panel-title {
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
color: var(--text-tertiary);
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
margin-bottom: 10px;
|
||||
}
|
||||
|
||||
/* Diag rows */
|
||||
.diag-row {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
padding: 4px 6px;
|
||||
background: rgba(0,0,0,0.2);
|
||||
border-radius: 4px;
|
||||
margin-bottom: 3px;
|
||||
}
|
||||
.diag-label { font-size: 11px; color: var(--text-tertiary); }
|
||||
.diag-value { font-size: 11px; font-family: monospace; color: var(--text-primary); font-weight: 600; }
|
||||
.diag-value.ok { color: var(--success); }
|
||||
.diag-value.warn { color: var(--warning); }
|
||||
.diag-value.bad { color: var(--danger); }
|
||||
.diag-value.active { color: #82c4ff; }
|
||||
|
||||
/* Joints grid */
|
||||
.motors-grid-list {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 2px;
|
||||
overflow-y: auto;
|
||||
flex: 1;
|
||||
}
|
||||
.motor-row {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 4px;
|
||||
padding: 3px 6px;
|
||||
background: rgba(0,0,0,0.25);
|
||||
border-radius: 5px;
|
||||
}
|
||||
.motor-row .name { font-size: 10px; color: var(--text-secondary); width: 72px; font-family: monospace; flex-shrink: 0; }
|
||||
.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; flex: 1; }
|
||||
.motor-row .val.pos { color: #0a84ff; }
|
||||
.motor-row .val.vel { color: #30d158; }
|
||||
.motor-row .val.tau { color: #ff9f0a; }
|
||||
.motor-row .stale { font-size: 9px; width: 8px; flex-shrink: 0; }
|
||||
|
||||
/* Joystick */
|
||||
.joystick-area { display: flex; flex-direction: column; align-items: center; gap: 8px; margin-bottom: 12px; }
|
||||
.joystick {
|
||||
position: relative;
|
||||
width: 180px;
|
||||
height: 180px;
|
||||
border-radius: 50%;
|
||||
background: radial-gradient(circle, rgba(10,132,255,0.2), rgba(10,132,255,0.05));
|
||||
border: 1px solid rgba(10,132,255,0.3);
|
||||
touch-action: none;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
#stick {
|
||||
position: absolute;
|
||||
left: 50%; top: 50%;
|
||||
width: 56px; height: 56px;
|
||||
border-radius: 50%;
|
||||
background: linear-gradient(135deg, #30d158, #0a84ff);
|
||||
transform: translate(-50%, -50%);
|
||||
box-shadow: 0 8px 24px rgba(0,0,0,0.4);
|
||||
}
|
||||
|
||||
/* Sliders */
|
||||
.slider-group { display: flex; flex-direction: column; gap: 8px; }
|
||||
.slider-row { display: flex; align-items: center; gap: 8px; }
|
||||
.slider-label { font-size: 11px; color: var(--text-tertiary); width: 28px; font-family: monospace; }
|
||||
.slider-val { font-size: 11px; color: var(--accent); font-family: monospace; width: 38px; text-align: right; }
|
||||
.glass-slider {
|
||||
flex: 1;
|
||||
-webkit-appearance: none;
|
||||
height: 4px;
|
||||
border-radius: 2px;
|
||||
background: rgba(255,255,255,0.2);
|
||||
outline: none;
|
||||
}
|
||||
.glass-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
width: 14px; height: 14px;
|
||||
border-radius: 50%;
|
||||
background: white;
|
||||
cursor: pointer;
|
||||
box-shadow: 0 2px 4px rgba(0,0,0,0.5);
|
||||
}
|
||||
.cmd-display {
|
||||
margin-top: 8px;
|
||||
padding: 6px 10px;
|
||||
background: rgba(0,0,0,0.3);
|
||||
border-radius: 6px;
|
||||
font-family: monospace;
|
||||
font-size: 12px;
|
||||
color: var(--accent);
|
||||
text-align: center;
|
||||
}
|
||||
.hint { font-size: 11px; color: var(--text-tertiary); text-align: center; }
|
||||
|
||||
/* Log */
|
||||
.log-section { flex: 1; overflow: hidden; display: flex; flex-direction: column; }
|
||||
.log {
|
||||
flex: 1;
|
||||
background: rgba(0,0,0,0.4);
|
||||
border: 1px solid rgba(255,255,255,0.05);
|
||||
border-radius: 6px;
|
||||
padding: 8px;
|
||||
font-family: monospace;
|
||||
font-size: 11px;
|
||||
color: var(--text-secondary);
|
||||
overflow-y: auto;
|
||||
}
|
||||
.log div { margin-bottom: 2px; line-height: 1.4; }
|
||||
.ev-t { color: var(--text-tertiary); margin-right: 4px; }
|
||||
.ev-ok { color: var(--success); }
|
||||
.ev-warn { color: var(--warning); }
|
||||
.ev-bad { color: var(--danger); }
|
||||
|
||||
@media (max-width: 960px) {
|
||||
.side-panel { position: relative; top: auto; width: 100%; left: 0; right: 0; border-radius: 0; height: auto; }
|
||||
.top-bar { width: 100%; border-radius: 0; top: 0; }
|
||||
body { padding-top: 56px; }
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
.colcon/
|
||||
.vscode/
|
||||
compile_commands.json
|
||||
@@ -0,0 +1,178 @@
|
||||
# ROS 2/C++ Sim2Real v2 部署指南
|
||||
|
||||
本工作区保存 `v0.12.0` 里程计联调快照，包含 ROS 2 Humble/C++ 主体和 Odin 驱动。系统依赖、设备标定和 TensorRT engine 仍与目标机环境绑定，不能仅凭源码声明在新设备上已复现。
|
||||
|
||||
---
|
||||
|
||||
## 1. 前提条件与环境
|
||||
|
||||
### 硬件
|
||||
* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。
|
||||
* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。
|
||||
* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。
|
||||
|
||||
### 主机依赖
|
||||
* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。
|
||||
* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。
|
||||
* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。
|
||||
* **库与 ROS2 包**：
|
||||
* `libyaml-cpp-dev`
|
||||
* `libeigen3-dev`
|
||||
* `libusb-1.0-0-dev`（Odin USB 传感器通信）
|
||||
* `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理）
|
||||
* `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器）
|
||||
* `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用）
|
||||
* `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理）
|
||||
* `can-utils`（SocketCAN 验证工具）
|
||||
|
||||
---
|
||||
|
||||
## 2. 本地编译与部署
|
||||
|
||||
按以下步骤在主机系统上编译运行整个栈：
|
||||
|
||||
### 步骤 1：安装系统依赖
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \
|
||||
libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge ros-humble-pcl-conversions
|
||||
```
|
||||
|
||||
### 步骤 2：下载 ONNXRuntime C++ SDK
|
||||
策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录：
|
||||
|
||||
```bash
|
||||
# 创建目录
|
||||
sudo mkdir -p /opt/onnxruntime
|
||||
cd /opt
|
||||
|
||||
# 针对 Jetson Orin (ARM64 / aarch64)：
|
||||
sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz
|
||||
sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
|
||||
# 或标准桌面仿真 (x86_64 / amd64)：
|
||||
# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz
|
||||
# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
```
|
||||
|
||||
导出 CMake 辅助变量：
|
||||
```bash
|
||||
export ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
```
|
||||
|
||||
### 步骤 3：构建工作区
|
||||
进入包含 `src/` 的本包根目录，运行 `colcon`：
|
||||
```bash
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
### 步骤 4：配置 SocketCAN 接口
|
||||
启动前，以 1 Mbps 波特率激活 CAN 接口：
|
||||
```bash
|
||||
sudo ip link set can0 up type can bitrate 1000000
|
||||
sudo ip link set can1 up type can bitrate 1000000
|
||||
```
|
||||
使用 `ifconfig` 或 `ip link` 验证接口已启动。
|
||||
|
||||
### 步骤 5：启动节点
|
||||
使启动脚本可执行并运行：
|
||||
```bash
|
||||
chmod +x start_sim2real.sh
|
||||
./start_sim2real.sh
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. Docker 部署（推荐）
|
||||
|
||||
强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。
|
||||
|
||||
### 步骤 1：构建镜像
|
||||
确保在 `sim2real_ros2_v2` 目录中（包含 `Dockerfile`）：
|
||||
```bash
|
||||
# 使用标准 docker build：
|
||||
docker build -t sim2real_ros2:latest .
|
||||
|
||||
# 或使用 Docker Compose：
|
||||
docker compose build
|
||||
```
|
||||
|
||||
### 步骤 2：运行容器
|
||||
对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度：
|
||||
|
||||
```bash
|
||||
# 选项 A：手动运行
|
||||
docker run -it \
|
||||
--network host \
|
||||
--privileged \
|
||||
--cap-add=sys_nice \
|
||||
--volume=/dev:/dev \
|
||||
--shm-size=2g \
|
||||
--name sim2real_ros2_run \
|
||||
sim2real_ros2:latest
|
||||
|
||||
# 选项 B：通过 Docker Compose 运行（最简单）
|
||||
docker compose up -d
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 系统拓扑与话题
|
||||
|
||||
控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互：
|
||||
|
||||
* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。
|
||||
* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。
|
||||
* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。
|
||||
* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。
|
||||
* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。
|
||||
* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。
|
||||
|
||||
### TF 树
|
||||
```
|
||||
odom ──→ base_link （由 odom_relay_node 广播）
|
||||
map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式）
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 集成 ROS 2 导航与传感器驱动
|
||||
|
||||
### USB 设备权限（Odin 传感器）
|
||||
要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则：
|
||||
|
||||
```bash
|
||||
# 1. 添加 udev 规则
|
||||
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules
|
||||
|
||||
# 2. 重新加载 udev 规则并重新插拔传感器
|
||||
sudo udevadm control --reload
|
||||
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。
|
||||
|
||||
#### 1. 完整真实硬件闭环（默认）
|
||||
启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 2. Dry-Run / 仿真航点测试
|
||||
在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 3. 仅运动控制（无导航）
|
||||
禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
|
||||
```
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
# 使用 ROS2 官方 Humble 基础镜像
|
||||
FROM ros:humble-ros-base-jammy
|
||||
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
|
||||
# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
can-utils \
|
||||
libyaml-cpp-dev \
|
||||
libeigen3-dev \
|
||||
libusb-1.0-0-dev \
|
||||
libpcl-dev \
|
||||
libopencv-dev \
|
||||
ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup \
|
||||
ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge \
|
||||
ros-humble-pcl-conversions \
|
||||
wget \
|
||||
tar \
|
||||
python3-pip \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# ============================================================================
|
||||
# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
|
||||
# ============================================================================
|
||||
# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
|
||||
# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
|
||||
WORKDIR /opt
|
||||
RUN ARCH=$(uname -m) && \
|
||||
if [ "$ARCH" = "aarch64" ]; then \
|
||||
echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
|
||||
pip3 install --no-cache-dir onnxruntime-gpu && \
|
||||
SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
|
||||
mkdir -p onnxruntime/include onnxruntime/lib && \
|
||||
cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
|
||||
cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
|
||||
echo "[ONNX] CUDA ONNX Runtime installed."; \
|
||||
else \
|
||||
echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
|
||||
wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
|
||||
rm onnxruntime-linux-x64-1.16.3.tgz; \
|
||||
fi
|
||||
|
||||
ENV ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
|
||||
# 创建工作空间，将所有 C++ 源码包拷入
|
||||
WORKDIR /sim2real_ws/src
|
||||
COPY src/sim2real_bringup sim2real_bringup
|
||||
COPY src/sim2real_common sim2real_common
|
||||
COPY src/sim2real_hw sim2real_hw
|
||||
COPY src/sim2real_interfaces sim2real_interfaces
|
||||
COPY src/sim2real_runtime sim2real_runtime
|
||||
COPY src/odin_ros_driver odin_ros_driver
|
||||
COPY src/sim2real_nav2 sim2real_nav2
|
||||
|
||||
# 拷贝策略文件与运行脚本
|
||||
WORKDIR /sim2real_ws
|
||||
COPY policies policies
|
||||
COPY map map
|
||||
COPY tools tools
|
||||
COPY start_sim2real.sh start_sim2real.sh
|
||||
RUN chmod +x start_sim2real.sh
|
||||
|
||||
# 编译 ROS2 工作空间
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN source /opt/ros/humble/setup.bash && \
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
|
||||
# 拷贝 Docker 入口脚本并设置
|
||||
COPY docker_entrypoint.sh /docker_entrypoint.sh
|
||||
RUN chmod +x /docker_entrypoint.sh
|
||||
|
||||
ENTRYPOINT ["/docker_entrypoint.sh"]
|
||||
CMD ["./start_sim2real.sh"]
|
||||
@@ -0,0 +1,76 @@
|
||||
# ROS 2 Sim2Real v2：里程计导航联调
|
||||
|
||||
本目录归档原始 `real/sim2real_ros2_v2(odom)`，对应 `v0.12.0`。该阶段在 `v0.11.1` 的 Odin/TensorRT 与站姿调参基础上，固定纯里程计模式，增加 odom fallback 的 TF 冲突保护、A_min 路线和地图工具。
|
||||
|
||||
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||
|
||||
- `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
|
||||
| | |
|
||||
v v v
|
||||
RuntimeState RuntimeTarget 电机 CAN 指令
|
||||
| |
|
||||
+-------> 诊断 / 遥测
|
||||
|
||||
Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||
(经 odom_relay_node 提供 odom→base_link TF)
|
||||
```
|
||||
|
||||
## 当前状态
|
||||
|
||||
已完成 Phase 0-5 的全部迁移：
|
||||
|
||||
1. ✅ 冻结部署契约（deployment_contract.hpp）
|
||||
2. ✅ ROS 2 包结构搭建
|
||||
3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
|
||||
4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
|
||||
5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
|
||||
|
||||
## 契约来源
|
||||
|
||||
迁移过程中以下文件被视为真值源：
|
||||
|
||||
- `sim2real/deployment_manifest.yaml`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
## 注意事项
|
||||
|
||||
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
|
||||
- Windows 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- Odin `custom_map_mode` 固定为 `0`（纯里程计），避免没有 `map_a.bin` 时产生 map/odom TF 冲突。
|
||||
- `A_min.pcd`、`C.pcd` 和 `map_b.pcd` 均为确定性抽样预览，点数和哈希见 [`map/README.md`](map/README.md)。
|
||||
- TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关，其他机器应从 ONNX 重新生成。
|
||||
@@ -0,0 +1,22 @@
|
||||
version: '3.8'
|
||||
|
||||
services:
|
||||
sim2real_ros2:
|
||||
build:
|
||||
context: .
|
||||
dockerfile: Dockerfile
|
||||
container_name: sim2real_ros2_node
|
||||
runtime: nvidia
|
||||
network_mode: host
|
||||
privileged: true
|
||||
stdin_open: true
|
||||
tty: true
|
||||
environment:
|
||||
- NVIDIA_VISIBLE_DEVICES=all
|
||||
- NVIDIA_DRIVER_CAPABILITIES=compute,utility
|
||||
cap_add:
|
||||
- SYS_NICE
|
||||
shm_size: '2gb'
|
||||
volumes:
|
||||
- /dev:/dev
|
||||
restart: unless-stopped
|
||||
@@ -0,0 +1,12 @@
|
||||
#!/bin/bash
|
||||
set -e
|
||||
|
||||
# Source ROS2 Humble environment
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source workspace install setup if compiled
|
||||
if [ -f "/sim2real_ws/install/setup.bash" ]; then
|
||||
source /sim2real_ws/install/setup.bash
|
||||
fi
|
||||
|
||||
exec "$@"
|
||||
@@ -0,0 +1,102 @@
|
||||
# sim2real_ros2_v2 架构说明
|
||||
|
||||
## 设计目标
|
||||
|
||||
- 保留已验证的 RL 部署契约不变
|
||||
- 将低延迟循环从 Python 迁移至 C++
|
||||
- 暴露标准 ROS 2 接口用于导航和系统集成
|
||||
- 保持安全边界独立于策略正确性
|
||||
|
||||
## 各包职责
|
||||
|
||||
### `sim2real_interfaces`（接口消息）
|
||||
|
||||
定义最小化的运行时消息：
|
||||
|
||||
- `RuntimeState`
|
||||
硬件桥接发布的归一化运行时状态快照
|
||||
- `RuntimeTarget`
|
||||
策略运行时发送至硬件桥接的最新策略目标
|
||||
|
||||
### `sim2real_common`（共享常量）
|
||||
|
||||
存储编译期常量和部署契约辅助：
|
||||
|
||||
- 观测维度和字段布局
|
||||
- 动作维度和轮子索引
|
||||
- 关节顺序和默认站姿
|
||||
- 动作缩放因子和默认循环频率
|
||||
- Mahony 姿态滤波器
|
||||
- 站立平衡控制器
|
||||
- 安全监控器（SafetyMonitor / RuntimeGuard）
|
||||
|
||||
### `sim2real_hw`（硬件桥接）
|
||||
|
||||
拥有硬件侧执行循环和安全边界：
|
||||
|
||||
- RobStride CAN 收发
|
||||
- IMU 与 Odin 状态采集
|
||||
- 电机丢帧检测与保活逻辑（holdover）
|
||||
- 看门狗与阻尼刹车
|
||||
- 发布 `RuntimeState`
|
||||
- 订阅 `RuntimeTarget`
|
||||
- 订阅 `/odom` 里程计数据
|
||||
|
||||
目标热路径：
|
||||
|
||||
- 以 `200Hz` 频率读取状态
|
||||
- 应用最新安全目标
|
||||
- 超时或安全违规时立即停机
|
||||
|
||||
### `sim2real_runtime`（策略运行时）
|
||||
|
||||
拥有策略侧执行：
|
||||
|
||||
- 订阅 `RuntimeState`
|
||||
- 按当前部署契约精确构建 `53D` 观测
|
||||
- 以 `50Hz` 运行 ONNXRuntime 推理
|
||||
- 对 raw_action 做 `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
- 仲裁命令来源：estop > safety_hold > startup > navigation > web
|
||||
|
||||
同时包含：
|
||||
- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
|
||||
|
||||
### `sim2real_nav2`（导航配置）
|
||||
|
||||
拥有：
|
||||
|
||||
- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
|
||||
- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
|
||||
|
||||
### `sim2real_bringup`（启动管理）
|
||||
|
||||
拥有：
|
||||
|
||||
- 参数文件
|
||||
- 启动组合
|
||||
- 运行时模式选择
|
||||
- 集成 odin_ros_driver、sim2real_nav2 的条件启动
|
||||
|
||||
## 迁移规则
|
||||
|
||||
1. 优化之前先冻结当前契约
|
||||
2. 先迁移传输和循环结构，再调整控制算法
|
||||
3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
|
||||
4. 按段测量延迟：
|
||||
- 观测延迟
|
||||
- 策略推理延迟
|
||||
- 目标传输延迟
|
||||
- 执行器响应延迟
|
||||
|
||||
## 首个里程碑
|
||||
|
||||
首个里程碑不是"机器人在 ROS 2 下行走"，而是：
|
||||
|
||||
1. `sim2real_hw` 发布稳定的 `RuntimeState`
|
||||
2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
|
||||
3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
|
||||
4. `sim2real_hw` 消费最新目标并执行超时刹车
|
||||
5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
|
||||
|
||||
> ✅ 以上里程碑已全部完成。
|
||||
@@ -0,0 +1,78 @@
|
||||
# 迁移计划
|
||||
|
||||
> 版本范围：ROS 2 v2 里程计联调快照（`v0.12.0`）。完成标记表示该快照中的代码迁移状态，不等于已在任意新硬件环境复现。
|
||||
|
||||
## Phase 1: 硬件核心迁移 ✅ 已完成
|
||||
|
||||
将当前高频热路径从 Python 迁出。
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/interface/motor_driver.py`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/imu_client.py`
|
||||
- `sim2real/safety/runtime_guard.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- C++ SocketCAN 电机总线封装
|
||||
- C++ 状态缓存
|
||||
- target 超时保活（timeout hold）
|
||||
- 阻尼刹车 / 急停通路
|
||||
- 发布 `RuntimeState`
|
||||
|
||||
## Phase 2: 策略运行时迁移 ✅ 已完成
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- 精确的 `53D` 观测构造器
|
||||
- ONNXRuntime C++ 推理封装
|
||||
- `50Hz` 策略定时器
|
||||
- 命令平滑与来源仲裁
|
||||
- raw_action `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
|
||||
## Phase 3: ROS 2 系统集成 ✅ 已完成
|
||||
|
||||
参考过的外部项目包括 Odin ROS 驱动、EDULITE A3 ROS 工程和 RL-SAR；外部参考目录不属于本仓库公开内容。
|
||||
|
||||
交付物：
|
||||
|
||||
- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
|
||||
- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
|
||||
- 诊断话题
|
||||
- rosbag/foxglove 可观测性
|
||||
|
||||
## Phase 4: 导航集成 ✅ 已完成
|
||||
|
||||
目标：
|
||||
|
||||
- 导航通过 ROS 2 发送身体速度指令
|
||||
- RL 运行时保持为 locomotion 控制器
|
||||
- 看门狗和安全边界始终在导航之下
|
||||
|
||||
规则：
|
||||
|
||||
- 导航绝不直接写电机指令
|
||||
- 策略契约在重新训练前保持不变
|
||||
- 任何新增历史项或里程计项必须版本化
|
||||
|
||||
## 当前状态
|
||||
|
||||
所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
|
||||
|
||||
| 组件 | 节点 | 说明 |
|
||||
|------|------|------|
|
||||
| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
|
||||
| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
|
||||
| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
|
||||
| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
|
||||
| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
|
||||
| 点云转换 | `pointcloud_to_laserscan` | 外部 ROS 包；/odin1/cloud_slam → /scan（供 AMCL 使用） |
|
||||
@@ -0,0 +1,495 @@
|
||||
# sim2real_ros2_v2 遥控器调用说明
|
||||
|
||||
本文档说明如何在 `sim2real_ros2_v2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
|
||||
|
||||
## 1. 当前接入关系
|
||||
|
||||
遥控器节点位于：
|
||||
|
||||
```text
|
||||
src/sim2real_runtime/src/remote_uart_node.py
|
||||
```
|
||||
|
||||
该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
|
||||
|
||||
| 输入 | 输出 | 作用 |
|
||||
|---|---|---|
|
||||
| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
|
||||
| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
|
||||
|
||||
策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
|
||||
|
||||
## 2. 通道映射
|
||||
|
||||
通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
|
||||
|
||||
| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
|
||||
|---|---|---|---:|
|
||||
| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
|
||||
| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
|
||||
| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
|
||||
| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
|
||||
|
||||
默认方向反转配置：
|
||||
|
||||
| 参数 | 默认值 | 含义 |
|
||||
|---|---:|---|
|
||||
| `remote_invert_vx` | `true` | 反转前后方向 |
|
||||
| `remote_invert_vy` | `false` | 不反转横移方向 |
|
||||
| `remote_invert_yaw` | `true` | 反转转向方向 |
|
||||
|
||||
## 3. 参数位置
|
||||
|
||||
遥控器参数在：
|
||||
|
||||
```text
|
||||
src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
当前默认参数：
|
||||
|
||||
```yaml
|
||||
remote_enabled: true
|
||||
remote_port: "/dev/ttyACM0"
|
||||
remote_baudrate: 100000
|
||||
remote_timeout: 0.02
|
||||
remote_axis_deadzone: 50
|
||||
remote_active_threshold: 50
|
||||
remote_axis_full_scale: 660.0
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
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
|
||||
```
|
||||
|
||||
如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
或改成实际设备路径。
|
||||
|
||||
## 4. 启动前检查
|
||||
|
||||
### 4.1 确认串口存在
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
如果使用默认配置，应能看到：
|
||||
|
||||
```bash
|
||||
/dev/ttyACM0
|
||||
```
|
||||
|
||||
### 4.2 确认串口权限
|
||||
|
||||
如果节点提示串口权限不足，可以临时执行：
|
||||
|
||||
```bash
|
||||
sudo chmod 666 /dev/ttyACM0
|
||||
```
|
||||
|
||||
更推荐的长期方式是把当前用户加入 `dialout` 组：
|
||||
|
||||
```bash
|
||||
sudo usermod -aG dialout $USER
|
||||
```
|
||||
|
||||
然后重新登录。
|
||||
|
||||
### 4.3 确认 Python serial 依赖
|
||||
|
||||
节点依赖 `pyserial`。如果系统没有安装：
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install -y python3-serial
|
||||
```
|
||||
|
||||
## 5. 构建
|
||||
|
||||
如果刚修改过代码或参数，建议重新构建相关包：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
|
||||
```
|
||||
|
||||
构建完成后 source 环境：
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
确认可执行节点存在：
|
||||
|
||||
```bash
|
||||
ros2 pkg executables sim2real_runtime
|
||||
```
|
||||
|
||||
应包含：
|
||||
|
||||
```text
|
||||
sim2real_runtime remote_uart_node.py
|
||||
```
|
||||
|
||||
## 6. 推荐启动方式
|
||||
|
||||
### 6.1 启动完整系统，不启动 Nav2
|
||||
|
||||
这是你当前常用方式：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
|
||||
|
||||
等价完整写法：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
|
||||
```
|
||||
|
||||
### 6.2 不启动遥控器
|
||||
|
||||
如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
|
||||
```
|
||||
|
||||
## 7. 单独启动遥控器节点
|
||||
|
||||
如果系统已经在运行，只想单独测试遥控器节点：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
如果要临时指定串口：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args \
|
||||
--params-file src/sim2real_bringup/config/runtime.yaml \
|
||||
-p remote_port:=/dev/ttyUSB0
|
||||
```
|
||||
|
||||
## 8. 执行后会产生什么效果
|
||||
|
||||
启动以下命令后：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
系统会产生以下效果。
|
||||
|
||||
### 8.1 启动硬件桥接节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开 `can0` 和 `can1`。
|
||||
2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
|
||||
3. 设置电机 MIT 模式。
|
||||
4. 设置电机速度限制和力矩限制。
|
||||
5. 以 `200Hz` 运行硬件读写循环。
|
||||
6. 发布 `/runtime/state`。
|
||||
7. 订阅 `/runtime/target` 执行策略目标。
|
||||
|
||||
### 8.2 启动策略运行节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_runtime_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 加载 ONNX 策略模型。
|
||||
2. 订阅 `/runtime/state`。
|
||||
3. 订阅 `/cmd_vel`。
|
||||
4. 订阅 `/safety/estop`。
|
||||
5. 执行启动站立流程：
|
||||
- `boot_hold`
|
||||
- `startup_soft_hold`
|
||||
- `startup_hold`
|
||||
- `runtime_zero_hold`
|
||||
- `runtime_policy`
|
||||
6. 以 `50Hz` 发布 `/runtime/target`。
|
||||
|
||||
### 8.3 启动遥控器节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开默认串口 `/dev/ttyACM0`。
|
||||
2. 以 `50Hz` 轮询 SBUS 数据。
|
||||
3. 遥控器摇杆居中时持续发布零速度：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = 0.0
|
||||
linear.y = 0.0
|
||||
angular.z = 0.0
|
||||
```
|
||||
|
||||
4. 推动遥控器时发布非零速度，例如：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = vx
|
||||
linear.y = vy
|
||||
angular.z = yaw
|
||||
```
|
||||
|
||||
5. 当 CH7 打到高位时发布：
|
||||
|
||||
```text
|
||||
/safety/estop: true
|
||||
```
|
||||
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
|
||||
### 8.4 机器人行为效果
|
||||
|
||||
正常启动后，机器人不会立即按策略行走，而是按阶段执行：
|
||||
|
||||
1. 电机使能。
|
||||
2. 读取当前关节位置。
|
||||
3. 软保持当前姿态。
|
||||
4. 平滑过渡到默认站立姿态。
|
||||
5. 稳定后进入 runtime。
|
||||
6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
|
||||
7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
|
||||
|
||||
也就是说：
|
||||
|
||||
| 遥控器状态 | 机器人效果 |
|
||||
|---|---|
|
||||
| 摇杆居中 | 站立保持，不主动行走 |
|
||||
| CH2 前后推动 | 前进/后退 |
|
||||
| CH4 左右推动 | 横向移动 |
|
||||
| CH1 左右推动 | 原地转向 |
|
||||
| CH7 高位 | 软件急停，进入安全刹车 |
|
||||
|
||||
## 9. 如何确认遥控器已经生效
|
||||
|
||||
### 9.1 查看节点是否存在
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
```
|
||||
|
||||
应看到：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
/sim2real_runtime_node
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
### 9.2 查看 `/cmd_vel`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
|
||||
|
||||
### 9.3 查看 `/safety/estop`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
|
||||
CH7 高位时应看到：
|
||||
|
||||
```yaml
|
||||
data: true
|
||||
```
|
||||
|
||||
### 9.4 查看策略目标阶段
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
常见输出含义：
|
||||
|
||||
| `target_source` | 含义 |
|
||||
|---|---|
|
||||
| `boot_hold` | 刚启动，保持初始姿态 |
|
||||
| `startup_soft_hold` | 启动软保持 |
|
||||
| `startup_hold` | 正在站立或站立后保持 |
|
||||
| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
|
||||
| `runtime_policy` | 遥控器有输入，策略已经介入 |
|
||||
| `safety_brake` | 安全刹车 |
|
||||
| `timeout_hold` | 目标超时，硬件保持默认姿态 |
|
||||
|
||||
### 9.5 查看完整目标状态
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /runtime/target
|
||||
```
|
||||
|
||||
重点关注字段：
|
||||
|
||||
```yaml
|
||||
target_source:
|
||||
zero_command:
|
||||
runtime_released:
|
||||
release_alpha:
|
||||
command:
|
||||
raw_command:
|
||||
```
|
||||
|
||||
如果遥控器摇杆有输入，通常会看到：
|
||||
|
||||
```yaml
|
||||
target_source: runtime_policy
|
||||
zero_command: false
|
||||
runtime_released: true
|
||||
release_alpha: 1.0
|
||||
```
|
||||
|
||||
## 10. 常见问题
|
||||
|
||||
### 10.1 启动后提示无法打开串口
|
||||
|
||||
可能原因：
|
||||
|
||||
1. 串口路径不对。
|
||||
2. 权限不足。
|
||||
3. 设备没有插好。
|
||||
4. 设备被其他程序占用。
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
修改 `runtime.yaml`：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
### 10.2 `/cmd_vel` 没有变化
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
如果节点存在但无变化，可能是：
|
||||
|
||||
1. 遥控器没有输出 SBUS。
|
||||
2. 串口波特率不对。
|
||||
3. SBUS 接线错误。
|
||||
4. 遥控器通道未校准。
|
||||
5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
|
||||
|
||||
### 10.3 摇杆方向反了
|
||||
|
||||
修改：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
```
|
||||
|
||||
例如前后方向反了，就切换：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: false
|
||||
```
|
||||
|
||||
### 10.4 急停后不恢复
|
||||
|
||||
当前配置：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: true
|
||||
```
|
||||
|
||||
这表示急停锁存。触发后建议：
|
||||
|
||||
1. 先确认机器人物理安全。
|
||||
2. 停止 launch。
|
||||
3. 将 CH7 打回安全位置。
|
||||
4. 重新启动系统。
|
||||
|
||||
如果需要非锁存模式，可以改为：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: false
|
||||
```
|
||||
|
||||
但实机调试时更建议使用锁存模式。
|
||||
|
||||
## 11. 快速验证命令清单
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
如果只测遥控器，不启动电机系统：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
@@ -0,0 +1,246 @@
|
||||
# sim2real_ros2_v2 Web UDP 调试说明
|
||||
|
||||
本文档说明本次新增的最小 Web 调试链路。
|
||||
|
||||
## 1. 架构
|
||||
|
||||
```text
|
||||
Windows 本地浏览器/HTTP 服务
|
||||
|
|
||||
| UDP JSON
|
||||
v
|
||||
Nano: sim2real_web_udp_bridge_node.py
|
||||
|
|
||||
| ROS 2 topics
|
||||
v
|
||||
sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
|
||||
```
|
||||
|
||||
Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
|
||||
|
||||
## 2. 新增 ROS2 节点
|
||||
|
||||
### `remote_uart_node.py`
|
||||
|
||||
遥控器节点现在发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
```
|
||||
|
||||
不再直接发布 `/cmd_vel`。
|
||||
|
||||
通道触发阈值改为：
|
||||
|
||||
```yaml
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
```
|
||||
|
||||
只有通道归一化值绝对值大于 `40` 才认为是有效输入。
|
||||
|
||||
### `cmd_mux_node.py`
|
||||
|
||||
输入：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
/cmd_vel_web
|
||||
/cmd_vel_nav
|
||||
/control/mode
|
||||
/remote/enabled
|
||||
/web/enabled
|
||||
/nav/enabled
|
||||
/safety/estop
|
||||
```
|
||||
|
||||
输出：
|
||||
|
||||
```text
|
||||
/cmd_vel
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
控制模式：
|
||||
|
||||
```text
|
||||
DISABLED
|
||||
REMOTE
|
||||
WEB
|
||||
NAV
|
||||
```
|
||||
|
||||
急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
|
||||
|
||||
### `web_udp_bridge_node.py`
|
||||
|
||||
Nano 端 UDP 监听：
|
||||
|
||||
```text
|
||||
0.0.0.0:15000
|
||||
```
|
||||
|
||||
发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_web
|
||||
/safety/estop
|
||||
/control/mode
|
||||
/web/enabled
|
||||
/remote/enabled
|
||||
/nav/enabled
|
||||
```
|
||||
|
||||
订阅并回传状态：
|
||||
|
||||
```text
|
||||
/runtime/state
|
||||
/runtime/target
|
||||
/cmd_vel
|
||||
/safety/estop
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
## 3. Nano 启动
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2_v2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认会启动：
|
||||
|
||||
```text
|
||||
sim2real_remote_uart_node
|
||||
sim2real_cmd_mux_node
|
||||
sim2real_web_udp_bridge_node
|
||||
```
|
||||
|
||||
如果不想启动 Web UDP bridge：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
|
||||
```
|
||||
|
||||
## 4. Windows 本地 Web 启动
|
||||
|
||||
把目录复制到 Windows 或通过共享目录访问：
|
||||
|
||||
```text
|
||||
tools/win_web_debug
|
||||
```
|
||||
|
||||
在 Windows 上安装 Python 3 后运行：
|
||||
|
||||
```bash
|
||||
python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
|
||||
```
|
||||
|
||||
浏览器打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
|
||||
## 5. UDP 命令格式
|
||||
|
||||
### 切换模式
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"REMOTE"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"WEB"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"DISABLED"}
|
||||
```
|
||||
|
||||
### Web 速度控制
|
||||
|
||||
```json
|
||||
{
|
||||
"type": "cmd_vel",
|
||||
"linear": {"x": 0.2, "y": 0.0, "z": 0.0},
|
||||
"angular": {"x": 0.0, "y": 0.0, "z": 0.1}
|
||||
}
|
||||
```
|
||||
|
||||
Nano 端会再次限幅：
|
||||
|
||||
```text
|
||||
vx <= ±0.8 m/s
|
||||
vy <= ±0.3 m/s
|
||||
yaw <= ±0.5 rad/s
|
||||
```
|
||||
|
||||
### 零速度
|
||||
|
||||
```json
|
||||
{"type":"zero"}
|
||||
```
|
||||
|
||||
### 软急停
|
||||
|
||||
```json
|
||||
{"type":"estop","data":true}
|
||||
```
|
||||
|
||||
## 6. 安全保护
|
||||
|
||||
当前最小版本已经包含：
|
||||
|
||||
1. 遥控器误触发阈值：`40`。
|
||||
2. 遥控器/Web/Nav 互斥控制模式。
|
||||
3. `cmd_mux` 二次限幅。
|
||||
4. `cmd_mux` 加速度限制。
|
||||
5. Web UDP 超时自动发布零速度。
|
||||
6. 急停优先级最高。
|
||||
7. Web 页面切换到 `WEB` 模式需要确认。
|
||||
8. Web 松开虚拟摇杆会自动发送零速度。
|
||||
|
||||
建议实机调试流程：
|
||||
|
||||
1. 先点击 `DISABLED`。
|
||||
2. 确认 `/cmd_vel` 为零。
|
||||
3. 如果使用遥控器，点击 `REMOTE`。
|
||||
4. 如果使用 Web，点击 `WEB` 并确认周围安全。
|
||||
5. 一旦异常，立即点击 `软急停`。
|
||||
|
||||
## 7. 验证命令
|
||||
|
||||
查看最终输出速度：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
查看遥控器输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_remote
|
||||
```
|
||||
|
||||
查看 Web 输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_web
|
||||
```
|
||||
|
||||
查看当前仲裁模式：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /control/mode_state
|
||||
```
|
||||
|
||||
查看策略状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
@@ -0,0 +1,20 @@
|
||||
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
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,11 @@
|
||||
# `v0.12.0` 里程计导航点云
|
||||
|
||||
本阶段使用三份 ASCII PCD 进行路线和里程计导航联调。原始文件均使用 `tools/nav_tools/downsample_ascii_pcd.py` 确定性等步长抽样，抽样文件仅用于可视化、路线编辑和软件回归，不等价于原始高密度地图。
|
||||
|
||||
| 文件 | 原始点数 | 步长 | 抽样点数 | 抽样大小 | 原始 SHA-256 | 抽样 SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- | --- |
|
||||
| `map_b.pcd` | 1,080,047 | 4 | 270,012 | 8,680,283 B | `F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7` | `F3052431F217DE8FFFE87726F8E547A59C65048B0285F02AF4803C5F4E4C1B42` |
|
||||
| `A_min.pcd` | 683,908 | 3 | 227,969 | 7,300,173 B | `77C884B17D78C4D157F84093BFEA0729FA25382C70E706C4BA12913A3C90ED20` | `CC4556D472BE6E5B232B9BFEE43F5F337D710F88E88D9B90994368C580327DC1` |
|
||||
| `C.pcd` | 3,186,653 | 11 | 289,696 | 9,040,786 B | `C5C7E60B06A0FFEA15E55C14B8CFAFBC8525371C8C3AC9C72821AB736AED8A6C` | `1FC4124642D934460811DBF62EBE9C5C9CF30668BC42E4782B18F3C002ED1A25` |
|
||||
|
||||
大小和 SHA-256 均按 Git 中保存的字节计算；根目录 `.gitattributes` 禁止对 `*.pcd` 做跨平台换行转换。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,276 @@
|
||||
{
|
||||
"name": "A_min_route",
|
||||
"map": "A_min",
|
||||
"frame_id": "map",
|
||||
"createdAt": "2026-07-04T00:40:56",
|
||||
"segments": [
|
||||
{
|
||||
"name": "segment_1",
|
||||
"obstacle": "slalom",
|
||||
"waypoints": [
|
||||
{
|
||||
"id": 1,
|
||||
"x": 9.3549,
|
||||
"y": 0.9079,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
"x": 11.1136,
|
||||
"y": 0.8621,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
"x": 13.0,
|
||||
"y": 0.75,
|
||||
"yawDeg": 88.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
"x": 12.9102,
|
||||
"y": 2.611,
|
||||
"yawDeg": 90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
"x": 12.9102,
|
||||
"y": 4.88,
|
||||
"yawDeg": 190.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 6,
|
||||
"x": 11.94,
|
||||
"y": 4.6203,
|
||||
"yawDeg": 178.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 7,
|
||||
"x": 10.65,
|
||||
"y": 4.65,
|
||||
"yawDeg": 178.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 8,
|
||||
"x": 9.3997,
|
||||
"y": 4.649,
|
||||
"yawDeg": -95.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 9,
|
||||
"x": 9.3092,
|
||||
"y": 3.8864,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 10,
|
||||
"x": 7.9263,
|
||||
"y": 4.0028,
|
||||
"yawDeg": 150.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 11,
|
||||
"x": 6.8148,
|
||||
"y": 4.6619,
|
||||
"yawDeg": 170.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 12,
|
||||
"x": 5.0054,
|
||||
"y": 5.0,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 13,
|
||||
"x": 3.2994,
|
||||
"y": 5.04,
|
||||
"yawDeg": 185.0,
|
||||
"speed": 0.7,
|
||||
"policy": "crawl",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 14,
|
||||
"x": 1.8838,
|
||||
"y": 4.8455,
|
||||
"yawDeg": 190.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 15,
|
||||
"x": 1.06,
|
||||
"y": 4.45,
|
||||
"yawDeg": 270.0,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 16,
|
||||
"x": 1.0561,
|
||||
"y": 3.9,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 17,
|
||||
"x": 1.04,
|
||||
"y": 2.79,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 18,
|
||||
"x": 1.552,
|
||||
"y": 2.77,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 19,
|
||||
"x": 1.519,
|
||||
"y": 1.6677,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 20,
|
||||
"x": 0.4708,
|
||||
"y": 1.6677,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 21,
|
||||
"x": 0.4552,
|
||||
"y": 0.5,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 22,
|
||||
"x": 1.2783,
|
||||
"y": 0.5,
|
||||
"yawDeg": 90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 23,
|
||||
"x": 1.2628,
|
||||
"y": 1.6211,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 24,
|
||||
"x": 2.3576,
|
||||
"y": 1.5978,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 25,
|
||||
"x": 2.3265,
|
||||
"y": 0.5,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 26,
|
||||
"x": 3.12,
|
||||
"y": 0.5,
|
||||
"yawDeg": 45.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 27,
|
||||
"x": 3.8639,
|
||||
"y": 0.9844,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 28,
|
||||
"x": 6.8222,
|
||||
"y": 0.9145,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 29,
|
||||
"x": 9.5321,
|
||||
"y": 0.9223,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"yawToleranceDegDefault": 30.0
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
# `v0.12.0` 策略文件
|
||||
|
||||
本阶段运行配置使用 `model_9600` 作为 Rough 策略，并保留 Crawl ONNX 和部署契约参考模型。
|
||||
|
||||
| 文件 | 用途 |
|
||||
| --- | --- |
|
||||
| `model_9600.onnx` | 里程计版本 Rough ONNX |
|
||||
| `model_9600_fp16.engine` | 里程计版本 Rough TensorRT |
|
||||
| `model_crawl.onnx` | Crawl 候选；当前配置使用 IK 后端 |
|
||||
| `model_rough.onnx` | 原部署契约参考基线 |
|
||||
|
||||
其余 `model_6800`、`model_8400`、`model_10200` 等候选策略属于相邻实验或后续比赛版本，不在本 Tag 重复归档。
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,8 @@
|
||||
v0.10.5 2026_0525
|
||||
1. 修改 recorddata 数据格式，新增 device_id、algorithm_version 字段
|
||||
2. 修复 SLAM 模式下下载地图失败的问题(USB2.0)
|
||||
3. 修复重定位模式下上传地图失败的问题(USB2.0)
|
||||
|
||||
v0.10.4 2026_0522
|
||||
1. 修复 USB2.0 心跳超时导致软断开的问题
|
||||
2. control_command.yaml 新增 custom_init_pose_search_radius 和 custom_init_pose_max_rot_deg 参数
|
||||
@@ -0,0 +1,447 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(odin_ros_driver)
|
||||
|
||||
|
||||
if(DEFINED BUILD_SYSTEM)
|
||||
set(ROS_VERSION ${BUILD_SYSTEM})
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
elseif(DEFINED ENV{ROS_DISTRO})
|
||||
if("$ENV{ROS_DISTRO}" MATCHES "foxy|galactic|humble|iron|rolling")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
elseif(DEFINED ENV{ROS_VERSION})
|
||||
if("$ENV{ROS_VERSION}" EQUAL "2")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
else()
|
||||
# Attempt automatic detection
|
||||
if(COMMAND catkin_package)
|
||||
set(ROS_VERSION "ROS1")
|
||||
elseif(COMMAND ament_package)
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
# Default to ROS2
|
||||
set(ROS_VERSION "ROS2")
|
||||
message(WARNING "Unable to determine ROS version, defaulting to ROS2")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Add compile definitions after detecting ROS version
|
||||
if(ROS_VERSION STREQUAL "ROS2")
|
||||
add_definitions(-DROS2)
|
||||
message(STATUS "Defining ROS2")
|
||||
else()
|
||||
add_definitions(-DROS1)
|
||||
message(STATUS "Defining ROS1")
|
||||
endif()
|
||||
|
||||
message(STATUS "Build system: ${ROS_VERSION}")
|
||||
|
||||
# Platform detection
|
||||
execute_process(
|
||||
COMMAND uname -m
|
||||
OUTPUT_VARIABLE ARCH
|
||||
OUTPUT_STRIP_TRAILING_WHITESPACE
|
||||
)
|
||||
|
||||
if(ARCH STREQUAL "x86_64")
|
||||
set(TARGET_PLATFORM "x86")
|
||||
message(STATUS "Detected x86_64 architecture")
|
||||
elseif(ARCH MATCHES "arm|aarch64")
|
||||
set(TARGET_PLATFORM "arm")
|
||||
message(STATUS "Detected ARM architecture: ${ARCH}")
|
||||
else()
|
||||
message(WARNING "Unsupported architecture: ${ARCH}. Using default settings")
|
||||
set(TARGET_PLATFORM "unknown")
|
||||
endif()
|
||||
|
||||
# Set library path
|
||||
set(LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib")
|
||||
message(STATUS "Library directory: ${LIB_DIR}")
|
||||
|
||||
# Set library name based on platform
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_arm")
|
||||
else()
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_amd")
|
||||
endif()
|
||||
|
||||
# Find precompiled lydHostApi library
|
||||
find_library(LYD_HOST_API_LIB
|
||||
NAMES
|
||||
${LYD_HOST_API_LIB_NAME}
|
||||
lib${LYD_HOST_API_LIB_NAME}.a
|
||||
lib${LYD_HOST_API_LIB_NAME}.so
|
||||
PATHS ${LIB_DIR}
|
||||
NO_DEFAULT_PATH
|
||||
)
|
||||
|
||||
if(LYD_HOST_API_LIB)
|
||||
message(STATUS "Found lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
else()
|
||||
file(GLOB LIB_FILES "${LIB_DIR}/lib${LYD_HOST_API_LIB_NAME}.*")
|
||||
if(LIB_FILES)
|
||||
message(STATUS "Found library files: ${LIB_FILES}")
|
||||
set(LYD_HOST_API_LIB ${LIB_FILES})
|
||||
else()
|
||||
message(FATAL_ERROR "Could not find precompiled lydHostApi library in ${LIB_DIR}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Set common compile options
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# Set optimization flags
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")
|
||||
|
||||
# Find common dependencies
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(OpenSSL REQUIRED)
|
||||
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
|
||||
|
||||
# Shared include directories
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${yaml-cpp_INCLUDE_DIR}
|
||||
${LIBUSB_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
# Shared library list
|
||||
set(COMMON_LIBS
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
${yaml-cpp_LIBRARIES}
|
||||
${OPENSSL_LIBRARIES}
|
||||
${LIBUSB_LIBRARIES}
|
||||
pthread
|
||||
rt
|
||||
${CMAKE_DL_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
)
|
||||
|
||||
# ===== ROS1 Configuration =====
|
||||
if(ROS_VERSION STREQUAL "ROS1")
|
||||
message(STATUS "Configuring for ROS1 build")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
tf
|
||||
image_transport
|
||||
)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
# Set output directories
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME})
|
||||
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
target_link_libraries(host_sdk_sample
|
||||
${catkin_LIBRARIES}
|
||||
${COMMON_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
${LIBUSB_LIBRARIES}
|
||||
yaml-cpp
|
||||
${OpenCV_LIBS}
|
||||
pthread
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_library(depth_image_ros_node src/depth_image_ros_node.cpp)
|
||||
target_link_libraries(depth_image_ros_node
|
||||
pointcloud_depth_converter
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_executable(pcd2depth_node src/pcd2depth_ros.cpp)
|
||||
target_link_libraries(pcd2depth_node
|
||||
depth_image_ros_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector src/cloud_reprojector.cpp)
|
||||
target_link_libraries(cloud_reprojector
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_node src/cloud_reprojection_ros.cpp)
|
||||
target_link_libraries(cloud_reprojection_node
|
||||
cloud_reprojector
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_link_libraries(image_overlay_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
# Installation rules
|
||||
install(TARGETS host_sdk_sample
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# ===== ROS2 Configuration =====
|
||||
elseif(ROS_VERSION STREQUAL "ROS2")
|
||||
message(STATUS "Configuring for ROS2 build")
|
||||
|
||||
# Find all necessary ROS2 packages
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_transport REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
|
||||
# Create executable
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
|
||||
# Link libraries
|
||||
target_link_libraries(host_sdk_sample
|
||||
${COMMON_LIBS}
|
||||
yaml-cpp
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
# Add ROS2 dependencies
|
||||
ament_target_dependencies(host_sdk_sample
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(depth_image_ros2_node_lib src/depth_image_ros2_node.cpp)
|
||||
target_link_libraries(depth_image_ros2_node_lib
|
||||
pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
ament_target_dependencies(depth_image_ros2_node_lib
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(pcd2depth_ros2_node src/pcd2depth_ros2.cpp)
|
||||
target_link_libraries(pcd2depth_ros2_node
|
||||
depth_image_ros2_node_lib
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(pcd2depth_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector_ros2 src/cloud_reprojector.cpp)
|
||||
target_compile_definitions(cloud_reprojector_ros2 PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_ros2_node src/cloud_reprojection_ros.cpp)
|
||||
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojection_ros2_node
|
||||
cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
ament_target_dependencies(cloud_reprojection_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_compile_definitions(image_overlay_node PRIVATE ROS2)
|
||||
target_link_libraries(image_overlay_node
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
ament_target_dependencies(image_overlay_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
message_filters
|
||||
)
|
||||
|
||||
# Installation rules - ensure all install targets are defined before ament_package()
|
||||
# Install executable
|
||||
install(TARGETS
|
||||
host_sdk_sample
|
||||
pcd2depth_ros2_node
|
||||
cloud_reprojection_ros2_node
|
||||
image_overlay_node
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install package.xml
|
||||
install(FILES package.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install headers
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
# Install launch_ROS2 directory
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch_ROS2")
|
||||
install(DIRECTORY launch_ROS2/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
message(STATUS "Installing launch_ROS2 directory to share/${PROJECT_NAME}/launch")
|
||||
else()
|
||||
message(WARNING "launch_ROS2 directory not found")
|
||||
endif()
|
||||
# Install config files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION share/${PROJECT_NAME}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# Install launch files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
endif()
|
||||
ament_export_targets(export_${PROJECT_NAME})
|
||||
# Declare dependencies
|
||||
ament_export_dependencies(
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
ament_package()
|
||||
message(STATUS "Install targets added")
|
||||
|
||||
else()
|
||||
message(FATAL_ERROR "Invalid ROS_VERSION: ${ROS_VERSION}")
|
||||
endif()
|
||||
|
||||
# ARM platform specific link options
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set_target_properties(host_sdk_sample PROPERTIES
|
||||
LINK_FLAGS "-Wl,--no-as-needed -Wl,--rpath=${LIB_DIR}"
|
||||
)
|
||||
message(STATUS "Adding ARM-specific link options and RPATH")
|
||||
endif()
|
||||
|
||||
# Add debug information
|
||||
message(STATUS "=======================================")
|
||||
message(STATUS "Project: ${PROJECT_NAME}")
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
message(STATUS "Target platform: ${TARGET_PLATFORM}")
|
||||
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
|
||||
message(STATUS "=======================================")
|
||||
|
||||
@@ -0,0 +1,567 @@
|
||||
# Odin_ROS_Driver 说明文档
|
||||
|
||||
Odin 传感器模块 ROS 驱动套件（Manifold Tech Ltd.）
|
||||
|
||||
Odin1 Wiki：https://manifoldtechltd.github.io/wiki/Odin1/Cover.html
|
||||
|
||||
## Odin_ROS_Driver
|
||||
|
||||
兼容性：
|
||||
|
||||
● ROS 1（推荐 LTS 版本：Noetic）
|
||||
|
||||
● ROS 2（推荐 LTS 版本：Humble）
|
||||
|
||||
## 重要提示：
|
||||
|
||||
本驱动包提供点云 SLAM 应用的核心功能，面向特定使用场景。仅供专业技术人员进行二次开发使用。最终用户需根据实际部署环境进行场景优化和定制开发，以满足运行需求。
|
||||
|
||||
## 1. 版本
|
||||
|
||||
当前版本：v0.10.2
|
||||
|
||||
所需设备固件版本：v0.10.0
|
||||
|
||||
## 2. 准备工作
|
||||
|
||||
### 2.1 操作系统要求
|
||||
|
||||
● ROS Noetic 和 ROS2 Foxy 需 Ubuntu 20.04；
|
||||
|
||||
● ROS2 Humble 需 Ubuntu 22.04；
|
||||
|
||||
● 当前不支持 Ubuntu 18.04；
|
||||
|
||||
● Ubuntu 24.04 尚未官方支持，但可能经过一定修改后运行。
|
||||
|
||||
### 2.2 依赖项
|
||||
|
||||
● OpenCV >= 4.2.0（推荐 4.5.5/4.8.0，请确保仅安装一个 OpenCV 版本）
|
||||
|
||||
● yaml-cpp
|
||||
|
||||
● thread
|
||||
|
||||
● OpenSSL
|
||||
|
||||
● Eigen3
|
||||
|
||||
### 2.3 依赖安装
|
||||
|
||||
#### 2.3.1 系统基础
|
||||
```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.5 ROS 安装
|
||||
|
||||
ROS Noetic 安装请参考：
|
||||
[ROS Noetic 安装指南](https://wiki.ros.org/noetic/Installation)
|
||||
|
||||
ROS2 Foxy 安装请参考：
|
||||
[ROS Foxy 安装指南](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
ROS2 Humble 安装请参考：
|
||||
[ROS Humble 安装指南](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
## 3. 准备
|
||||
|
||||
### 3.1 创建 Udev 规则
|
||||
```shell
|
||||
sudo vim /etc/udev/rules.d/99-odin-usb.rules
|
||||
```
|
||||
在 99-odin-usb.rules 文件中添加以下内容：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
重新加载规则并重新插拔设备：
|
||||
```shell
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
### 3.2 克隆源码
|
||||
```shell
|
||||
git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
|
||||
```
|
||||
注意：
|
||||
请将源码克隆到 "[ros_workspace]/src/" 目录下，否则会导致编译错误。
|
||||
|
||||
### 3.3 编译
|
||||
|
||||
#### 3.3.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/noetic/setup.bash
|
||||
./script/build_ros.sh
|
||||
```
|
||||
|
||||
#### 3.3.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/foxy/setup.bash
|
||||
./script/build_ros2.sh
|
||||
```
|
||||
|
||||
### 3.4 运行：
|
||||
|
||||
#### 3.4.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source [ros_workspace]/devel/setup.bash
|
||||
roslaunch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros_workspace：用户的 ROS 环境工作区；
|
||||
```shell
|
||||
roslaunch odin_ros_driver odin1_ros1.launch
|
||||
```
|
||||
|
||||
#### 3.4.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source [ros2_workspace]/install/setup.bash
|
||||
ros2 launch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros2_workspace：用户的 ROS2 环境工作区；
|
||||
|
||||
ROS2 Demo 启动命令：
|
||||
```shell
|
||||
ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
```
|
||||
|
||||
### 3.5 运行模式：
|
||||
|
||||
可通过 `config/control_command.yaml` 中的 `custom_map_mode` 参数配置运行模式。
|
||||
|
||||
#### 里程计模式
|
||||
|
||||
设置 `custom_map_mode = 0` 启用里程计模式。此模式下，map 坐标系与 odom 坐标系共享同一位姿。
|
||||
|
||||
若发现里程计数据漂移，可使用脚本命令 `./set_param.sh algo_reset 1` 动态复位算法。
|
||||
|
||||
#### SLAM 建图模式
|
||||
|
||||
设置 `custom_map_mode = 1` 启用 SLAM 模式。此模式在里程计模式基础上，提供**回环检测**和**地图保存**功能。
|
||||
|
||||
启动驱动后，odin1 将自动进行建图并缓存地图数据。场景采集完成后，需在驱动源码目录下执行 `./set_param.sh save_map 1` 以保存自程序启动以来采集的所有地图数据。地图将保存至 `config/control_command.yaml` 中 `mapping_result_dest_dir` 和 `mapping_result_file_name` 参数指定的路径。若未指定这些参数，将使用默认值。
|
||||
|
||||
首次保存后，可再次执行该命令保存新地图。每次保存操作都会生成一个新的地图文件。（连续保存操作之间请间隔至少 5 秒）
|
||||
|
||||
地图原点对应程序启动时 odom 坐标系的起点。
|
||||
|
||||
##### 重定位模式
|
||||
|
||||
要启用重定位，设置 `custom_map_mode = 2`，并通过 `config/control_command.yaml` 中的 `relocalization_map_abs_path` 参数指定预建地图的绝对路径。
|
||||
|
||||
启动后，odin1 将基于当前视点和指定地图启动重定位过程。为保障高成功率，建议在距 SLAM 轨迹原始位置 1 米、±10° 范围内启动。
|
||||
|
||||
注意，重定位性能高度依赖环境条件。在特征丰富的场景中，成功匹配可能发生在 1m/10° 范围之外，而其他环境可能需要更严格的条件。建议在实际部署环境中测试，以确定实际容忍范围。
|
||||
|
||||
若初始重定位失败，系统将临时以降级 SLAM 模式运行（此状态下地图保存功能禁用）。在此过程中可自由移动 odin1，它将在后台持续尝试重定位。一旦成功，将发布 map 与 odom 坐标系之间的 TF。（提示：初始化后轻轻晃动或移动设备有助于提高重定位准确率。）
|
||||
|
||||
以下话题在 odom 坐标系下发布：`/odin1/cloud_slam`、`/odin1/odom`、`/odin1/highodom` 和 `/odin1/path`。若需在 map 坐标系下获取这些数据，请应用从 odom 坐标系到 map 坐标系的 TF 变换。
|
||||
|
||||
## 4. 文件结构与数据格式
|
||||
### 4.1 文件结构
|
||||
```shell
|
||||
Odin_ROS_Driver/ // ROS1/ROS2 驱动包
|
||||
3rdparty/ // 第三方库
|
||||
src/
|
||||
host_sdk_sample.cpp // 示例源码
|
||||
yaml_parser.cpp // YAML 参数读取源码
|
||||
rawCloudRender.cpp // RenderCloud 渲染源码
|
||||
depth_image_ros_node.cpp // depth_image_ros_node 节点
|
||||
depth_image_ros2_node.cpp // depth_image_ros2_node 节点
|
||||
pcd2depth_ros.cpp // pcd2depth_ros 源码
|
||||
pcd2depth_ros2.cpp // pcd2depth_ros2 源码
|
||||
pointcloud_depth_converter.cpp // pointcloud_depth_converter 源码
|
||||
cloud_reprojection_ros.cpp // 云重投影节点源码 (ROS1/ROS2)
|
||||
cloud_reprojector.cpp // 云重投影核心逻辑
|
||||
lib/
|
||||
liblydHostApi_amd.a // AMD 平台静态库
|
||||
liblydHostApi_arm.a // ARM 平台静态库
|
||||
include/
|
||||
host_sdk_sample.h // 示例头文件
|
||||
lidar_api_type.h // API 数据结构头文件
|
||||
lidar_api.h // API 函数声明
|
||||
yaml_parser.h // 参数文件读取头文件
|
||||
rawCloudRender.h // RenderCloud 相关 API
|
||||
data_logger.h // 数据保存日志
|
||||
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 // 云重投影核心类
|
||||
config/
|
||||
control_command.yaml // 驱动控制参数文件
|
||||
calib.yaml // 设备标定参数 yaml，每个设备独一无二。每次连接 ROS 驱动时从设备读取
|
||||
launch_ROS1/
|
||||
odin1_ros1.launch // ROS1 启动文件
|
||||
launch_ROS2/
|
||||
odin1_ros2.launch.py // ROS2 启动文件
|
||||
script/
|
||||
build_ros1.sh // ROS1 安装脚本
|
||||
build_ros2.sh // ROS2 安装脚本
|
||||
recorddata/ // 存放可导入 MindCloud 的录制数据
|
||||
log/ // 存放日志文件
|
||||
Driver_{timestamp}/ // 每次启动驱动时生成的日志文件夹
|
||||
Conn_{timestamp}/ // 每次 odin1 设备连接时生成的日志文件
|
||||
dev_status.csv // 设备状态日志
|
||||
README.md // 使用说明
|
||||
CMakeLists.txt // CMake 构建文件
|
||||
License // 许可证文件
|
||||
```
|
||||
### 4.2 启动文件
|
||||
| 启动文件名 | 说明 |
|
||||
|--------------------------|-------------|
|
||||
| odin1_ros1.launch | ROS1 启动文件 - Odin1 基础操作演示 |
|
||||
| odin1_ros2.launch.py | ROS2 启动文件 - Odin1 基础操作演示 |
|
||||
|
||||
|
||||
### 4.3 ROS 话题
|
||||
Odin ROS 驱动的内部参数定义在 config/control_command.yaml 中。以下是常用参数说明：
|
||||
|
||||
| 话题 | control_command.yaml | 详细说明 |
|
||||
|---------------------------|----------------------|----------------------|
|
||||
| odin1/imu | sendimu | IMU 话题 |
|
||||
| odin1/image | sendrgb | RGB 相机话题，由设备原始 JPEG 数据解码，bgr8 格式 |
|
||||
| odin1/image_undistort | sendrgbundistort | 去畸变 RGB 相机话题，经设备 calib.yaml 标定参数处理 |
|
||||
| odin1/image/compressed | sendrgbcompressed | RGB 相机压缩话题，设备原始 JPEG 数据 |
|
||||
| odin1/cloud_raw | senddtof | 原始点云话题 |
|
||||
| odin1/cloud_render | sendcloudrender | 渲染点云话题，经原始点云、RGB 图像及设备 calib.yaml 处理 |
|
||||
| odin1/cloud_slam | sendcloudslam | SLAM 点云话题 |
|
||||
| odin1/odometry | sendodom | 里程计话题 |
|
||||
| odin1/odometry_high | sendodom | 高频里程计话题 |
|
||||
| odin1/path | showpath | 里程计路径话题 |
|
||||
| tf | sendodom | TF 树话题 |
|
||||
| odin1/depth_img_competetion | senddepth | 稠密深度图话题。需较高算力，仅作演示。与 odin1/image_undistort 一一对应。使用时请直接订阅本话题而非 echo。原始值即为深度数据，无需额外转换。 |
|
||||
| odin1/depth_img_competetion_cloud | senddepth | 稠密深度点云话题。需较高算力，仅作演示 |
|
||||
| odin1/reprojected_image | sendreprojection | 重投影像素话题。利用里程计将 cloud_slam 投影至相机图像。在主机端处理。 |
|
||||
|
||||
### 4.4 数据格式
|
||||
|
||||
1. 原始点云（cloud_raw）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
uint8 intensity // 反射率，范围 0–255
|
||||
uint16 confidence // 点置信度，典型场景下取值范围约 0–1300，数值越高可靠性越强。推荐过滤阈值 30-35，应结合实际环境调整。
|
||||
float32 offset_time // 相对基准时间戳的时间偏移量，单位：秒
|
||||
```
|
||||
|
||||
要在 PCL 中使用此自定义格式，首先定义点类型：
|
||||
```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)
|
||||
)
|
||||
```
|
||||
然后即可轻松将 ROS sensor_msgs::PointCloud2 消息转换为 PCL 点云：
|
||||
```
|
||||
pcl::PointCloud<ls_ros::Point> ls_cloud;
|
||||
pcl::fromROSMsg(*msg, ls_cloud);
|
||||
```
|
||||
|
||||
2. SLAM 点云（cloud_slam）与直接渲染点云（cloud_render）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
float32 rgb // RGB 颜色值
|
||||
```
|
||||
|
||||
### 4.5 其他功能
|
||||
|
||||
| control_command.yaml 参数 | 详细说明 |
|
||||
|----------------------------|----------------------|
|
||||
| use_host_ros_time | 时间同步模式：0 - 使用 odin 内部系统时间作为数据时间戳（典型用法，推荐）；1 - 接收时使用主机 ROS 时间（不推荐大多数用户使用）；2 - 通过类 NTP 同步将 odin1 时间对齐至主机时间，时间戳为传感器数据在主机时间轴上的接收时间。 |
|
||||
| strict_usb3.0_check | 严格 USB3.0 检查，关闭后即使 USB 连接低于 3.0 标准也允许连接 |
|
||||
| recorddata | 以特定格式记录数据，可导入 MindCloud(TM) 进行后处理。请注意这将消耗大量存储空间，测试显示 10 分钟数据约占 9.5GB。 |
|
||||
| devstatuslog | 设备状态日志记录，当前将设备状态（SoC 温度、CPU 占用率、RAM 占用率、dToF 传感器温度等）及数据发送/接收速率保存至 log 目录下的 devstatus.csv。每次启动驱动时创建新文件。 |
|
||||
| showcamerapose | 显示相机位姿及视野范围。 |
|
||||
| custom_map_mode | 运行模式：模式 0 - 里程计模式：map 坐标系与 odom 坐标系共享同一位姿。模式 1 - 建图模式（带回环检测）：该模式支持地图保存。模式 2 - 重定位模式：需指定地图文件绝对路径，重定位成功后将输出 map 与 odom 坐标系之间的 TF 关系。|
|
||||
| custom_init_pos | 初始化位置（当前未启用）。 |
|
||||
| relocalization_map_abs_path | 地图文件绝对路径：用于重定位模式。 |
|
||||
| mapping_result_dest_dir 和 mapping_result_file_name | 建图模式下地图保存路径与文件名：若未指定，将使用默认值。 |
|
||||
|
||||
## 5. 常见问题
|
||||
### 5.1 重新启动宿主 SDK 时出现段错误
|
||||
**错误信息**
|
||||
60 秒内未连接任何设备
|
||||
|
||||
**解决方案**
|
||||
1. 请重新为 Odin 模块上电 # 断开并重新连接 odin 电源
|
||||
|
||||
2. 重新初始化 Odin SDK # 设备重启后执行 SDK
|
||||
|
||||
|
||||
### 5.2 编译时出现库链接失败
|
||||
|
||||
**错误信息**
|
||||
ld: cannot find -llydHostApi 或符号查找错误
|
||||
|
||||
**解决方案**
|
||||
|
||||
1. 清理之前的构建产物
|
||||
|
||||
ROS1
|
||||
```shell
|
||||
rm -rf devel/ build/
|
||||
```
|
||||
ROS2
|
||||
```shell
|
||||
rm -rf devel/ install/ log/
|
||||
```
|
||||
2. 重新运行脚本安装
|
||||
|
||||
### 5.3 Docker GUI 透传失败
|
||||
|
||||
**错误信息**
|
||||
Unable to open X display 或 No protocol specified
|
||||
|
||||
**解决方案**
|
||||
```shell
|
||||
xhost + # 此命令启用 Docker 容器的图形透传
|
||||
```
|
||||
|
||||
### 5.4 ROS 驱动以"获取版本失败"错误退出
|
||||
|
||||
**错误信息**
|
||||
```shell
|
||||
<ERROR><api.cpp:lidar_get_version:672>: get device version fail.
|
||||
get version failed.
|
||||
```
|
||||
|
||||
**解决方案**
|
||||
|
||||
设备固件版本过低，请升级至最新版本。
|
||||
|
||||
|
||||
### 5.5 RVIZ 长时间无响应
|
||||
|
||||
**错误信息**
|
||||
Rviz 无响应，稍后终端打印"Device disconnected, waiting for reconnection..."
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新为 Odin 模块上电
|
||||
|
||||
### 5.6 设备无响应
|
||||
|
||||
**错误信息**
|
||||
Missed ok response from device, probably wrong interaction procedure.
|
||||
|
||||
**解决方案**
|
||||
|
||||
请采用 5.1 所述的解决方案
|
||||
|
||||
### 5.7 设备无外部标定文件
|
||||
|
||||
**错误信息**
|
||||
ERROR：Missing camera node 'cam_0'
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新插拔 USB
|
||||
|
||||
### 5.8 ROS 驱动在数据流启动后立即报设备断开
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
Device detaching...
|
||||
Wating for device reconnection...
|
||||
Device disconnected, waiting for reconnection...
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
多见于 ROS2 环境且连接到复杂网络环境（如办公 WiFi 和以太网）的情况。ROS2 默认为广播模式，复杂网络环境可能导致 ROS2 发布阻塞，从而引发设备断开。
|
||||
|
||||
**解决方案**
|
||||
|
||||
若不需要跨设备通信，请将 ROS2 限制为仅本地通信：
|
||||
```shell
|
||||
export ROS_LOCALHOST_ONLY=1
|
||||
```
|
||||
|
||||
若需要跨设备通信，请尽量简化网络环境。建议使用仅包含必要设备的小型局域网。
|
||||
|
||||
### 5.9 ROS 驱动在数据流启动后立即崩溃
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
[host_sdk_sample-2] process has died ......
|
||||
```
|
||||
|
||||
**测试**
|
||||
|
||||
在 control_command.yaml 中设置 sendrgb = 0 禁用 odin1/image，然后重试。若驱动此时正常工作，则问题很可能与系统安装了多个 OpenCV 版本有关。
|
||||
|
||||
**解决方案**
|
||||
|
||||
卸载多余的 OpenCV 版本，仅保留单一完整版本，然后重新编译驱动并重试。
|
||||
|
||||
### 5.10 ROS 驱动打印"TF_OLD_DATA ignoring data"警告
|
||||
|
||||
**错误信息**
|
||||
|
||||
```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
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是 ROS 和 rviz 的一项功能，用于警告用户某些 TF 数据因时间戳冲突而被忽略。常见于用户保持 ROS 驱动运行的同时对 odin 设备断电重启，导致 odin 内部系统时间被重置，新数据时间戳与 rviz 上次运行期间接收的旧数据产生冲突。
|
||||
|
||||
**解决方案**
|
||||
|
||||
rviz GUI 底部有一个重置按钮。点击此按钮将重置 rviz 内部状态并停止警告。
|
||||
|
||||
### 5.11 ROS 驱动打印"unknown cmd code: xx"错误
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
<ERROR><api.cpp:cmd_data_deal:418>: unknow command code 21.
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是由于 ROS 驱动版本与设备固件版本不匹配，导致 ROS 驱动无法解码新版固件新增的数据。
|
||||
|
||||
**解决方案**
|
||||
|
||||
请确保使用最新版本的 ROS 驱动和设备固件。
|
||||
|
||||
### 5.12 USB 设备访问错误（LIBUSB_ERROR_BUSY 或 LIBUSB_ERROR_ACCESS）
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
libusb: error [udev_hotplug_event] ignoring udev action bind
|
||||
LIBUSB_ERROR_BUSY
|
||||
```
|
||||
|
||||
或
|
||||
|
||||
```shell
|
||||
libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13
|
||||
LIBUSB_ERROR_ACCESS
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
- **LIBUSB_ERROR_BUSY**：另一个进程正在使用该 USB 设备。常见于多个 ROS 驱动实例正在运行，或其他应用程序（如之前崩溃的实例）仍持有设备句柄。
|
||||
|
||||
- **LIBUSB_ERROR_ACCESS**：当前用户无权访问 USB 设备。通常因缺少 udev 规则或用户权限不足导致。
|
||||
|
||||
**解决方案**
|
||||
|
||||
针对 **LIBUSB_ERROR_BUSY**：
|
||||
|
||||
1. 检查是否有其他驱动实例正在运行：
|
||||
```shell
|
||||
ps aux | grep host_sdk_sample
|
||||
```
|
||||
|
||||
2. 终止所有现存实例：
|
||||
```shell
|
||||
killall host_sdk_sample
|
||||
```
|
||||
|
||||
3. 若问题仍然存在，请拔插 USB 设备以重置设备状态。
|
||||
|
||||
针对 **LIBUSB_ERROR_ACCESS**：
|
||||
|
||||
1. 添加设备 udev 规则。创建文件 `/etc/udev/rules.d/99-odin.rules`，内容如下：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
|
||||
2. 重新加载 udev 规则：
|
||||
```shell
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
3. 或者，使用 sudo 运行驱动（不推荐用于生产环境）：
|
||||
```shell
|
||||
sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py
|
||||
```
|
||||
|
||||
4. 确保当前用户属于 `plugdev` 用户组：
|
||||
```shell
|
||||
sudo usermod -aG plugdev $USER
|
||||
```
|
||||
然后注销并重新登录，使组变更生效。
|
||||
|
||||
## 6. 联系方式
|
||||
|
||||
您可通过 support@manifoldtech.cn 联系我们的技术支持。
|
||||
|
||||
为帮助诊断问题，请向我们的 FAE 工程师提供以下信息：
|
||||
|
||||
1. 当前固件版本
|
||||
```shell
|
||||
[device_version_capture]: ros_driver_version: [版本号]
|
||||
```
|
||||
2. 正在使用的电源适配器和转换线缆照片。
|
||||
|
||||
3. 问题是偶发性还是持续性的？
|
||||
|
||||
4. 提供问题场景的图像。
|
||||
|
||||
5. **第 V 节**中的故障排除方法是否解决了问题？
|
||||
|
||||
6. 问题解决的预期时间线。
|
||||
@@ -0,0 +1,106 @@
|
||||
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
|
||||
|
||||
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_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
|
||||
|
||||
# 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
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#endif
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include "cloud_reprojector.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class CloudReprojectionRosNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using PointCloud2 = sensor_msgs::msg::PointCloud2;
|
||||
using Odometry = nav_msgs::msg::Odometry;
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<Odometry> odom_sub_;
|
||||
message_filters::Subscriber<Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
|
||||
const Odometry::ConstSharedPtr& odom_msg,
|
||||
const Odometry::ConstSharedPtr& wiwc_msg);
|
||||
};
|
||||
#else
|
||||
class CloudReprojectionRosNode
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
ros::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
||||
const nav_msgs::OdometryConstPtr& odom_msg,
|
||||
const nav_msgs::OdometryConstPtr& wiwc_msg);
|
||||
};
|
||||
#endif
|
||||
+85
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
class DepthImageRos2Node : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit DepthImageRos2Node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
void initialize();
|
||||
|
||||
private:
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CompressedImage> color_compressed_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::Image> color_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::msg::PointCloud2,
|
||||
// sensor_msgs::msg::CompressedImage,
|
||||
sensor_msgs::msg::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
std::shared_ptr<image_transport::ImageTransport> it_;
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
void syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
|
||||
// const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr color_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::msg::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::msg::Header &header);
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <mutex>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#endif
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class ImageOverlayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
|
||||
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
|
||||
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::msg::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(Image::ConstSharedPtr msg);
|
||||
void cameraCallback(Image::ConstSharedPtr msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#else
|
||||
#include <mutex>
|
||||
class ImageOverlayNode
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
ros::Subscriber reproj_sub_;
|
||||
ros::Subscriber camera_sub_;
|
||||
ros::Publisher overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,532 @@
|
||||
#ifndef LIDAR_API_H
|
||||
#define LIDAR_API_H
|
||||
|
||||
/**
|
||||
* @file lidar_api.h
|
||||
* @brief LiDAR device API for controlling and accessing LiDAR sensor data
|
||||
*
|
||||
* This header provides the public interface for interacting with LiDAR devices.
|
||||
* It includes functions for device management, data streaming control, and
|
||||
* device configuration.
|
||||
*
|
||||
* @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
|
||||
* @version 1.0
|
||||
*
|
||||
* ┌──────────────────────────────────────────────────────────────────────────────┐
|
||||
* │ QUICK START │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 1. API Call Sequence (typical usage): │
|
||||
* │ │
|
||||
* │ lidar_system_init(device_cb) // Initialize system, register device cb│
|
||||
* │ ↓ │
|
||||
* │ [Wait for device_cb with attach=true to get device info] │
|
||||
* │ ↓ │
|
||||
* │ lidar_create_device(&dev_info, &handle) // Create device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_register_stream_callback(handle, cb_info) // Register data callback│
|
||||
* │ ↓ │
|
||||
* │ lidar_open_device(handle) // Connect to device │
|
||||
* │ ↓ │
|
||||
* │ lidar_set_mode(handle, mode) // Set RAW or SLAM mode │
|
||||
* │ ↓ │
|
||||
* │ lidar_start_stream(handle, type, odr) // Start specific data stream │
|
||||
* │ ↓ │
|
||||
* │ [Data arrives via registered callback] │
|
||||
* │ ↓ │
|
||||
* │ lidar_stop_stream(handle, type) // Stop data stream │
|
||||
* │ ↓ │
|
||||
* │ lidar_close_device(handle) // Disconnect │
|
||||
* │ ↓ │
|
||||
* │ lidar_destory_device(handle) // Release device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_system_deinit() // Cleanup system resources │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 2. Operating Modes and Available Data Types: │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_RAW: │
|
||||
* │ - LIDAR_DT_RAW_RGB (RGB camera image, NV12 format) │
|
||||
* │ - LIDAR_DT_RAW_IMU (IMU data at 400Hz) │
|
||||
* │ - LIDAR_DT_RAW_DTOF (DTOF depth + point cloud + confidence) │
|
||||
* │ - LIDAR_DT_DEV_STATUS (Device status info) │
|
||||
* │ - LIDAR_DT_NTP (PTP/NTP sync data) │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_SLAM: │
|
||||
* │ - All RAW mode types, plus: │
|
||||
* │ - LIDAR_DT_SLAM_CLOUD (SLAM point cloud, XYZRGBA) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY (SLAM odometry at ~10Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ (Odometry at IMU rate ~400Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_TF (Map-Odom transform) │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 3. activate_stream_type vs start_stream: │
|
||||
* │ │
|
||||
* │ lidar_activate_stream_type(): │
|
||||
* │ - Configure which stream types are enabled in device │
|
||||
* │ - Can enable multiple types before starting │
|
||||
* │ - Does NOT start data transmission │
|
||||
* │ │
|
||||
* │ lidar_start_stream(): │
|
||||
* │ - Actually starts data transmission for the specified type │
|
||||
* │ - Callback will begin receiving data after this call │
|
||||
* │ │
|
||||
* │ Typical flow: │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_IMU); // Enable IMU │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_DTOF); // Enable DTOF │
|
||||
* │ start_stream(LIDAR_DT_RAW_IMU, ...); // Start IMU stream │
|
||||
* │ start_stream(LIDAR_DT_RAW_DTOF, ...); // Start DTOF stream │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 4. Callback Notes (IMPORTANT): │
|
||||
* │ │
|
||||
* │ Thread Safety: │
|
||||
* │ - Callbacks are invoked from internal SDK threads │
|
||||
* │ - Different data types may use different threads │
|
||||
* │ - User callback code must be thread-safe │
|
||||
* │ │
|
||||
* │ Data Lifetime: │
|
||||
* │ - Data pointers (pAddr) are ONLY valid during callback execution │
|
||||
* │ - If you need to keep data, COPY it before callback returns │
|
||||
* │ - Do NOT store or dereference pAddr after callback returns │
|
||||
* │ │
|
||||
* │ Performance: │
|
||||
* │ - Avoid blocking or time-consuming operations in callback │
|
||||
* │ - Long callback execution may cause data loss or jitter │
|
||||
* │ - For heavy processing, copy data and process in separate thread │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 5. Error Codes: │
|
||||
* │ 0 : Success │
|
||||
* │ -1 : General failure │
|
||||
* │ -2 : Invalid parameter │
|
||||
* │ -3 : Device not found / not connected │
|
||||
* │ -4 : Operation timeout │
|
||||
* │ -5 : Resource allocation failed │
|
||||
* │ │
|
||||
* └──────────────────────────────────────────────────────────────────────────────┘
|
||||
*/
|
||||
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialize the LiDAR system
|
||||
*
|
||||
* Must be called before any other lidar function to set up the system resources.
|
||||
* This function starts device discovery and will invoke the callback when devices
|
||||
* are found or disconnected.
|
||||
*
|
||||
* @param cb Callback function for device events:
|
||||
* - Called with attach=true when a new device is discovered
|
||||
* - Called with attach=false when a device is disconnected
|
||||
* - The lidar_device_info_t contains serial number to identify the device
|
||||
*
|
||||
* Example callback:
|
||||
* void device_callback(const lidar_device_info_t* info, bool attach) {
|
||||
* if (attach) {
|
||||
* printf("Device connected: %s\n", info->serial);
|
||||
* // Save info for lidar_create_device()
|
||||
* } else {
|
||||
* printf("Device disconnected: %s\n", info->serial);
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_init(lidar_device_callback_t cb);
|
||||
|
||||
/**
|
||||
* @brief Deinitialize the LiDAR system
|
||||
*
|
||||
* Releases all resources allocated by the system. Should be called when
|
||||
* application is shutting down.
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_deinit(void);
|
||||
|
||||
/**
|
||||
* @brief Create a handle for a LiDAR device
|
||||
*
|
||||
* Creates a device handle using the device info received from the device callback.
|
||||
* The handle is used for all subsequent operations on the device.
|
||||
*
|
||||
* @param dev_info Information about the LiDAR device (from lidar_system_init callback)
|
||||
* - serial: Device serial number (required, used to identify device)
|
||||
* - model: Device model string
|
||||
* - online: Device connection status
|
||||
* @param device [OUTPUT] Pointer to receive the device handle upon success
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note The dev_info should be the same structure received from the device callback
|
||||
*/
|
||||
int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
|
||||
|
||||
/**
|
||||
* @brief Destroy a LiDAR device handle
|
||||
*
|
||||
* Releases resources associated with the device handle. Must be called
|
||||
* when the device is no longer needed.
|
||||
*
|
||||
* @param device Handle to the device to destroy
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_destory_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Register callback function for receiving LiDAR data streams
|
||||
*
|
||||
* Sets up a callback function that will be called when new data is available.
|
||||
* All data types use the same callback; use lidar_data_t.type to distinguish.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param cb Callback info structure:
|
||||
* - data_callback: Function pointer, signature: void(const lidar_data_t*, void*)
|
||||
* - user_data: User context pointer passed to callback (can be NULL)
|
||||
*
|
||||
* Example:
|
||||
* void data_callback(const lidar_data_t* data, void* user_data) {
|
||||
* switch (data->type) {
|
||||
* case LIDAR_DT_RAW_IMU:
|
||||
* imu_convert_data_t* imu = (imu_convert_data_t*)data->stream.imageList[0].pAddr;
|
||||
* // Process IMU data (COPY if needed, pointer invalid after return)
|
||||
* break;
|
||||
* case LIDAR_DT_RAW_DTOF:
|
||||
* // data->stream.imageList[0]: depth
|
||||
* // data->stream.imageList[1]: point cloud XYZ
|
||||
* // data->stream.imageList[2]: confidence
|
||||
* // data->stream.imageList[3]: intensity
|
||||
* break;
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @warning Callback is invoked from SDK internal threads. Avoid blocking operations.
|
||||
* @warning Data pointers are only valid during callback execution. Copy if needed.
|
||||
*/
|
||||
int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
|
||||
|
||||
/**
|
||||
* @brief Unregister stream callback for a device
|
||||
*
|
||||
* Stops the device from calling back when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_unregister_stream_callback(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Open a LiDAR device for communication
|
||||
*
|
||||
* Establishes a connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to open
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_open_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Close a LiDAR device
|
||||
*
|
||||
* Closes the connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to close
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_close_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Set the operating mode of the LiDAR device
|
||||
*
|
||||
* Must be called after lidar_open_device() and before lidar_start_stream().
|
||||
* Mode determines which data types are available for streaming.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param mode Operating mode to set:
|
||||
* - LIDAR_MODE_RAW: Raw sensor data (RGB, IMU, DTOF)
|
||||
* - LIDAR_MODE_SLAM: SLAM processing enabled (adds odometry, point cloud)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_mode(device_handle device, int mode);
|
||||
|
||||
/**
|
||||
* @brief Start data streaming from the device
|
||||
*
|
||||
* Begins the flow of data from the device for the specified type.
|
||||
* After calling this function, registered callbacks will start receiving data.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to start (lidar_data_type_e):
|
||||
* - LIDAR_DT_RAW_RGB: RGB camera frames
|
||||
* - LIDAR_DT_RAW_IMU: IMU data at 400Hz
|
||||
* - LIDAR_DT_RAW_DTOF: Depth sensor data
|
||||
* - LIDAR_DT_SLAM_CLOUD: SLAM point cloud (requires SLAM mode)
|
||||
* - LIDAR_DT_SLAM_ODOMETRY: SLAM odometry (requires SLAM mode)
|
||||
* - etc. (see lidar_data_type_e in lidar_api_type.h)
|
||||
* @param dtof_subframe_odr [OUTPUT] Returns DTOF subframe interval in microseconds.
|
||||
* Only meaningful when type=LIDAR_DT_RAW_DTOF.
|
||||
* For other types, this value can be ignored.
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note You can start multiple stream types simultaneously by calling this
|
||||
* function multiple times with different types.
|
||||
*/
|
||||
int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
|
||||
|
||||
/**
|
||||
* @brief Stop data streaming from the device
|
||||
*
|
||||
* Stops the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to stop
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_stop_stream(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Activate a specific stream type on the device
|
||||
*
|
||||
* Enables a specific data stream type in the device configuration.
|
||||
* This configures the device to be ready for the stream type, but does NOT
|
||||
* start actual data transmission. Call lidar_start_stream() to begin streaming.
|
||||
*
|
||||
* Use this to pre-configure multiple stream types before starting them:
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_IMU);
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_DTOF);
|
||||
* start_stream(handle, LIDAR_DT_RAW_IMU, odr);
|
||||
* start_stream(handle, LIDAR_DT_RAW_DTOF, odr);
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to activate (lidar_data_type_e)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @see lidar_start_stream() to actually begin data transmission
|
||||
* @see lidar_deactivate_stream_type() to disable a stream type
|
||||
*/
|
||||
int lidar_activate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Deactivate a specific stream type on the device
|
||||
*
|
||||
* Disables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to deactivate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_deactivate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Get the calibration file from the device
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calib_file(device_handle device, const char* path);
|
||||
|
||||
/**
|
||||
* @brief Get device calibration parameters
|
||||
*
|
||||
* Retrieves the current calibration parameters from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to receive the calibration parameters
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calibration(device_handle device, lidar_calibration_t* param);
|
||||
|
||||
/**
|
||||
* @brief Set device calibration parameters
|
||||
*
|
||||
* Applies new calibration parameters to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to the calibration parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_calibration(device_handle device, const lidar_calibration_t *param);
|
||||
|
||||
/**
|
||||
* @brief Set log verbosity level
|
||||
*
|
||||
* Controls the amount of log information generated by the LiDAR API.
|
||||
*
|
||||
* @param level Log level to set (see level definitions in lidar_api_type.h)
|
||||
*/
|
||||
void lidar_log_set_level(lidar_log_level_e level);
|
||||
|
||||
/**
|
||||
* @brief Get the version information of the LiDAR device
|
||||
*
|
||||
* Retrieves version information including firmware, system, and application versions.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param version struct Pointer to receive the version information
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
|
||||
|
||||
/**
|
||||
* @brief Set custom algorithm parameters for the device
|
||||
*
|
||||
* Sends custom parameter settings to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to set
|
||||
* @param value_data Pointer to the value data to set for the parameter
|
||||
* @param value_length Length of the value data in bytes
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Set the map file used for relocalization
|
||||
*
|
||||
* Read & send specified map file to device for relocalization
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the map file
|
||||
* @return int 0 on success, otherwise on failure
|
||||
*/
|
||||
int lidar_set_relocalization_map(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief Get the mapping result file from device
|
||||
*
|
||||
* Read & send specified map file from device to host
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the map file
|
||||
* @param file_name File name to save the map file
|
||||
* @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
|
||||
*/
|
||||
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
|
||||
|
||||
/**
|
||||
* @brief Save the current map to a file on the host. Synchronous, all-in-one API.
|
||||
*
|
||||
* Internally drives the complete save-map state machine, so callers do not need
|
||||
* to coordinate the multi-step protocol themselves:
|
||||
* 1) Set custom parameter save_map = 1 on the device (kicks off generation).
|
||||
* 2) Poll save_map until the device resets it to 0 (map generation finished).
|
||||
* 3) Pull the resulting file via the standard mapping-result transfer (same as
|
||||
* lidar_get_mapping_result).
|
||||
* 4) Return the final status to the caller.
|
||||
*
|
||||
* This is the recommended entry point for saving a map. The lower-level pair
|
||||
* (lidar_set_custom_parameter("save_map") + lidar_get_mapping_result) remains
|
||||
* available for advanced use cases.
|
||||
*
|
||||
* 同步保存地图到主机上的文件。一站式 API，内部完成「触发生成 → 等待设备完成 →
|
||||
* 拉取文件」的完整流程，调用方无需自行轮询和处理异步状态。
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param dest_dir Host directory to save the file into (must already exist).
|
||||
* @param file_name File name (e.g. "map.bin").
|
||||
* @param gen_timeout_ms Maximum time (ms) to wait for the device to finish
|
||||
* generating the map. Pass 0 to use the default (120000 ms).
|
||||
* @return int
|
||||
* 0 success, file saved at dest_dir/file_name
|
||||
* -1 invalid arguments (null device/dir, SDK not initialized, etc.)
|
||||
* -2 device is busy with another file transfer
|
||||
* -3 timed out waiting for the device to finish map generation
|
||||
* -4 file transfer stalled or failed (see logs for details)
|
||||
* other negative values are propagated from the underlying transfer.
|
||||
*/
|
||||
int lidar_save_map(device_handle device,
|
||||
const char *dest_dir,
|
||||
const char *file_name,
|
||||
uint32_t gen_timeout_ms);
|
||||
|
||||
/**
|
||||
* @brief Set the image mask file for the device
|
||||
*
|
||||
* Read & send specified image mask file to device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
|
||||
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_image_mask(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief enable encrypted device log
|
||||
*
|
||||
* enable encrypted device log, save to specified directory
|
||||
* Please send to our support when needed
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the encrypted logs
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the depth parameters for the device
|
||||
*
|
||||
* This function must be called before starting data stream.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param params Pointer to the depth parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
|
||||
|
||||
/**
|
||||
* @brief Enable or disable IMU smooth sending feature
|
||||
*
|
||||
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
|
||||
* using a dedicated high-priority thread to reduce jitter and timing variance.
|
||||
* When disabled, IMU data will be sent immediately upon reception.
|
||||
*
|
||||
* @param enable 1 to enable smooth sending, 0 to disable
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_imu_smooth_sending(int enable);
|
||||
|
||||
/**
|
||||
* @brief Set IMU smooth sending frequency
|
||||
*
|
||||
* Set the target frequency for IMU smooth sending. Only effective when
|
||||
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
|
||||
*
|
||||
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#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;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0,
|
||||
LIDAR_DT_RAW_RGB,
|
||||
LIDAR_DT_RAW_IMU,
|
||||
LIDAR_DT_RAW_DTOF,
|
||||
LIDAR_DT_SLAM_CLOUD,
|
||||
LIDAR_DT_SLAM_ODOMETRY,
|
||||
LIDAR_DT_DEV_STATUS,
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF,
|
||||
LIDAR_DT_SLAM_WIWC,
|
||||
LIDAR_DT_NTP
|
||||
} 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];
|
||||
int64_t orient[4];
|
||||
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 {
|
||||
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 {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_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 configured output data rate */
|
||||
int tx_odr; /* rgb image sensor tx output data rate */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* dtof lidar sensor configured output data rate */
|
||||
int tx_odr; /* dtof lidar sensor tx output data rate */
|
||||
int subframe_odr; /* dtof lidar sensor subframe output data rate */
|
||||
short tx_temp; /* dtof lidar tx module temp */
|
||||
short rx_temp; /* dtof lidar rx module temp */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* imu sensor configured output data rate */
|
||||
int tx_odr; /* imu sensor tx output data rate */
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp; /* soc package temp */
|
||||
int cpu_temp; /* cpu temp */
|
||||
int center_temp; /* center temp */
|
||||
int gpu_temp; /* gpu temp */
|
||||
int npu_temp; /* npu temp */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal;
|
||||
|
||||
int cpu_use_rate[8]; /* cpu usage rate */
|
||||
int ram_use_rate; /* ram usage rate */
|
||||
|
||||
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; /* slam cloud tx output data rate */
|
||||
int slam_odom_tx_odr; /* slam odom tx output data rate */
|
||||
int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */
|
||||
|
||||
} 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,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
+111
@@ -0,0 +1,111 @@
|
||||
|
||||
# USAGE: ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
import os
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directory
|
||||
package_dir = get_package_share_directory('odin_ros_driver')
|
||||
|
||||
# Declare configuration parameter
|
||||
config_file_arg = DeclareLaunchArgument(
|
||||
'config_file',
|
||||
default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
|
||||
description='Path to the control config YAML file'
|
||||
)
|
||||
|
||||
# Add RViz2 configuration file parameter
|
||||
rviz_config_arg = DeclareLaunchArgument(
|
||||
'rviz_config',
|
||||
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(
|
||||
package='odin_ros_driver',
|
||||
executable='host_sdk_sample',
|
||||
name='host_sdk_sample',
|
||||
output='screen',
|
||||
# arguments=['--ros-args', '--log-level', 'debug'],
|
||||
parameters=[{
|
||||
'config_file': LaunchConfiguration('config_file')
|
||||
}]
|
||||
)
|
||||
|
||||
pcd2depth_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(pcd2depth_config_path, 'r') as f:
|
||||
pcd2depth_params = yaml.safe_load(f)
|
||||
pcd2depth_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
pcd2depth_params['calib_file_path'] = pcd2depth_calib_path
|
||||
pcd2depth_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='pcd2depth_ros2_node',
|
||||
name='pcd2depth_ros2_node',
|
||||
output='screen',
|
||||
parameters=[pcd2depth_params]
|
||||
)
|
||||
|
||||
# Cloud reprojection node
|
||||
reprojection_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(reprojection_config_path, 'r') as f:
|
||||
reprojection_params = yaml.safe_load(f)
|
||||
reprojection_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
reprojection_params['calib_file_path'] = reprojection_calib_path
|
||||
cloud_reprojection_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='cloud_reprojection_ros2_node',
|
||||
name='cloud_reprojection_ros2_node',
|
||||
output='screen',
|
||||
parameters=[reprojection_params]
|
||||
)
|
||||
|
||||
# Image overlay node - overlays reprojected points on camera image
|
||||
overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(overlay_config_path, 'r') as f:
|
||||
overlay_params = yaml.safe_load(f)
|
||||
image_overlay_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='image_overlay_node',
|
||||
name='image_overlay_node',
|
||||
output='screen',
|
||||
parameters=[overlay_params]
|
||||
)
|
||||
|
||||
# Create RViz2 node - loads specified configuration file
|
||||
rviz_node = Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', LaunchConfiguration('rviz_config')],
|
||||
condition=IfCondition(LaunchConfiguration('launch_rviz'))
|
||||
)
|
||||
|
||||
# Create 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(host_sdk_node)
|
||||
ld.add_action(pcd2depth_node)
|
||||
ld.add_action(cloud_reprojection_node)
|
||||
ld.add_action(image_overlay_node)
|
||||
ld.add_action(rviz_node) # Add RViz node
|
||||
|
||||
return ld
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<!-- ROS1 uses catkin as the build tool -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- ROS1 dependencies -->
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
|
||||
<!-- System dependencies -->
|
||||
<depend>eigen</depend>
|
||||
<depend>opencv</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
|
||||
<!-- Specify build type as catkin -->
|
||||
<export>
|
||||
<build_type>catkin</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user