Compare commits
12 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
765173676f | ||
|
zeitvex
|
dc7a2c2a67 | ||
|
zeitvex
|
c8aa490b37 | ||
|
zeitvex
|
302b6acc64 | ||
|
zeitvex
|
3bf6eb0c4c | ||
|
zeitvex
|
55094bf682 | ||
|
zeitvex
|
4ee4af028c | ||
|
zeitvex
|
c05c1cb162 | ||
|
zeitvex
|
d8c5d34091 | ||
|
zeitvex
|
60f7a08e91 | ||
|
zeitvex
|
b08956aec7 | ||
|
zeitvex
|
dfd646cf47 |
@@ -29,12 +29,18 @@ log/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
|
||||
!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
checkpoints/
|
||||
wandb/
|
||||
sim2sim_log_*.txt
|
||||
**/sim2sim_temp.xml
|
||||
**/route_check_runs/
|
||||
**/route_experiments/suite_*/
|
||||
**/tools/nav_tools/points/auto_candidates/
|
||||
|
||||
# IDE and operating system files
|
||||
.idea/
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
# 训练代码演进
|
||||
|
||||
本项目将“训练代码架构”和“训练产生的模型 checkpoint”分别管理。代码、奖励、课程或观测契约发生变化时形成新的 Git 版本；同一架构下继续训练产生的模型编号作为实验和部署工件记录。
|
||||
|
||||
| 版本 | 来源快照 | 主要目的 |
|
||||
| --- | --- | --- |
|
||||
| `v0.4.0` | `uni_mjlab(1)` | 第一份完整的新 MJCF 与新 mjlab 训练工程 |
|
||||
| `v0.5.0` | `uni_mjlab_new` | 扩大观测、延迟和动力学随机化，探索更强 Sim2Real 鲁棒性 |
|
||||
| `v0.6.0` | `best` | 面向比赛越障重新设计奖励、课程、站姿和诊断体系 |
|
||||
|
||||
## 从随机化增强到比赛训练
|
||||
|
||||
`v0.6.0` 不是简单地继续增大 `v0.5.0` 的随机范围。实践中重新降低了部分噪声、延迟和动力学随机化强度，并把训练重点转向可控的比赛任务课程：
|
||||
|
||||
- 投影重力噪声由 `±0.08` 调回 `±0.05`。
|
||||
- 腿和轮动作最大随机延迟由 4 步调回 2 步。
|
||||
- 摩擦、刚度和阻尼随机化调回较窄范围。
|
||||
- 移除该阶段的连续机身外力扰动与腿部质量随机化。
|
||||
- 将 x、y 和 yaw 跟踪拆成独立奖励和独立指令课程。
|
||||
- 障碍由课程逐步释放，高墙训练地形改为五道重复横墙。
|
||||
- 增加楼梯侧向漂移和偏航漂移惩罚。
|
||||
- 增加大量只用于训练评估的误差、轮速、姿态和接触指标。
|
||||
|
||||
这种调整反映的是从“广泛鲁棒性探索”转向“比赛场景定向优化”，不表示 `v0.5.0` 被删除；它仍由对应 Tag 完整保留。
|
||||
|
||||
## 模型与比赛部署
|
||||
|
||||
比赛训练可能先获得基模，再修改参数继续训练和筛选 checkpoint。最终关系为：
|
||||
|
||||
```text
|
||||
训练代码架构：v0.6.0 / best
|
||||
比赛 Rough 策略：model_6800.onnx
|
||||
比赛真机工程：last_not_slalom_1050
|
||||
比赛得分：1050
|
||||
```
|
||||
|
||||
`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。
|
||||
@@ -0,0 +1,120 @@
|
||||
# 版本演进
|
||||
|
||||
本项目使用同一条 `16dof` 主线和里程碑 Tag 保存线性演进，不在源码目录中复制历史版本。
|
||||
|
||||
| Tag | 阶段 | 核心内容 |
|
||||
| --- | --- | --- |
|
||||
| `v0.1.0` | 8DOF 中期检查 | 8DOF 串联足机械与大疆 A 板实机版本 |
|
||||
| `v0.2.0` | 16DOF 机械 | 16DOF 串联轮足机械 CAD 与 STEP |
|
||||
| `v0.3.0` | 第一代软件闭环 | 早期训练、MJCF、MuJoCo、Sim2Sim、IK 与 Python Sim2Real |
|
||||
| `v0.3.1` | 实机记录 | 补充第一代 Sim2Real 实机视频 |
|
||||
| `v0.4.0` | 新训练基线 | 第一份完整的新 MJCF、新 mjlab 框架和 Rough 策略工程 |
|
||||
| `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
|
||||
| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
|
||||
| `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
|
||||
| `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 |
|
||||
| `v0.12.0` | 里程计导航联调 | 纯里程计 fallback、A_min 路线、TF 冲突保护和 model_9600 |
|
||||
|
||||
> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
|
||||
|
||||
## `v0.9.0` 的 Python Sim2Real v2
|
||||
|
||||
- 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。
|
||||
- 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。
|
||||
- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
|
||||
- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
|
||||
|
||||
## `v0.10.0` 的 ROS 2/C++ Sim2Real 初版
|
||||
|
||||
- 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。
|
||||
- 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。
|
||||
- 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。
|
||||
- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
|
||||
- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
|
||||
|
||||
## `v0.11.0` 的 ROS 2 Sim2Real v2
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2`，保持 `v0.10.0` 的 ROS 2/C++ 控制契约。
|
||||
- 增加 `simple_nav_node.py`、PCD 点击工具、任务点/任务序列配置和 Web 导航控制入口。
|
||||
- 默认命令源从遥控切换为 `NAV`，加入简单导航状态、PCD 位姿和地图显示链路。
|
||||
- 原始 `map1.pcd`、`map6.pcd` 分别约 49.05 MiB、44.24 MiB，归档时确定性抽样到 10 MB 以下并记录哈希。
|
||||
- 原始快照中的 Odin 驱动仍为空目录；排除设备运行日志和开发草稿。
|
||||
|
||||
## `v0.11.1` 的 Odin、TensorRT 与站姿调参
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，在同一 `sim2real_ros2_v2` 目录中记录真实差异。
|
||||
- 首次随部署工程保留完整 Odin ROS 驱动、Apache-2.0 许可证、设备标定参数和预编译 SDK 静态库。
|
||||
- 策略运行时增加 TensorRT、ONNX 回退、Rough/Crawl 模式切换、事件日志和更完整的电机失效诊断。
|
||||
- 默认 Rough 策略为 `NEWmodel_1900`，默认站姿为髋俯仰 `0.670`、膝关节 `-1.390`；Crawl 配置使用 IK 后端。
|
||||
- `map_b.pcd` 从 1,080,047 点确定性抽样为 270,012 点，并保留原始和抽样哈希。
|
||||
- 排除嵌套 Git、Odin 运行日志、缓存、开发草稿和未被配置引用的候选策略。
|
||||
|
||||
## `v0.12.0` 的里程计导航联调
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2(odom)`，继续沿用 `sim2real_ros2_v2` 目录的线性演进。
|
||||
- 将 Rough 策略切换为 `model_9600`，默认站姿恢复为髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- Odin `custom_map_mode` 固定为纯里程计，加入 odom 新鲜度、外部 map/odom TF 冲突和任务结束交接保护。
|
||||
- 增加 A_min 路线、PCD 地图编辑工具和三份抽样点云；原始大 PCD 不直接进入 Git。
|
||||
- 保留完整 Odin 驱动、标定参数和 SDK 静态库；未找到的 `map_a.bin` 仍不伪造，重定位闭环不在本 Tag 声称已复现。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
- 机械 CAD 不变。
|
||||
- MJCF 更新整机质量和惯性参数，旧、新 `wheelleg.xml` 的 SHA-256 不同。
|
||||
- mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。
|
||||
- 保留轮腿分组执行器随机化所需的本地补丁。
|
||||
- 本阶段归档 `model_rough.pt`，不将生成日志、缓存和临时 XML 纳入版本库。
|
||||
|
||||
## `v0.5.0` 的训练变化
|
||||
|
||||
- MJCF、mjlab 基准和已有模型文件保持不变。
|
||||
- 投影重力噪声由 `±0.05` 扩大到 `±0.08`。
|
||||
- 腿与轮动作的最大随机延迟由 2 步增加到 4 步。
|
||||
- 地面摩擦随机范围由 `0.3–1.0` 扩大到 `0.15–1.25`。
|
||||
- 执行器刚度和阻尼缩放由 `0.9–1.1` 扩大到 `0.5–1.5`。
|
||||
- 增加膝部和轮部质量的 `0.7–1.3` 随机缩放。
|
||||
- 增加作用于机身的连续随机外力和力矩扰动。
|
||||
|
||||
## `v0.6.0` 的比赛训练架构
|
||||
|
||||
- 保持 `v0.4.0` 引入的新 MJCF 和 mjlab 框架不变。
|
||||
- 将线速度奖励拆分为 x/y 两轴，并独立配置偏航角速度奖励。
|
||||
- 增加自适应 x/y/yaw 指令范围课程。
|
||||
- 增加障碍地形逐步释放与更严格的地形晋级逻辑。
|
||||
- 增加楼梯横向速度和偏航漂移约束。
|
||||
- 默认站姿调整为髋俯仰 `0.550`、膝关节 `-1.125`，初始机身高度为 `0.42 m`。
|
||||
- 增加速度误差、轮速跟踪、动作和姿态等训练诊断指标。
|
||||
- 该 Tag 保存比赛训练代码架构，不把每次继续训练产生的 checkpoint 误记为新的软件版本。
|
||||
|
||||
训练阶段的详细关系见 [`training_evolution.md`](training_evolution.md)。
|
||||
|
||||
## `v0.7.0` 的独立 MuJoCo 工具
|
||||
|
||||
- 比赛训练架构、MJCF、模型和依赖锁文件保持 `v0.6.0` 状态不变。
|
||||
- 增加解析姿态表、RL 友好姿态筛选和 MuJoCo 静态姿态优化。
|
||||
- 增加 IK/差速轮参数扫描，可导出 JSON 结果。
|
||||
- 增加 Robot、Controller、Dynamics、MPCController 和 GUI 调试链路。
|
||||
- 记录历史工具常量与新版 MJCF 质量、比赛默认站姿之间的参数边界，避免将分析结果直接当作已校准真机参数。
|
||||
|
||||
## `v0.8.0` 的后期 Sim2Sim
|
||||
|
||||
- 比赛训练任务、MJCF 和 `v0.7.0` 的 MuJoCo 工具保持不变。
|
||||
- 策略运行器增加 ONNX 加载，并允许在缺少 `pynput` 时关闭后台键盘监听继续运行。
|
||||
- MuJoCo 执行器重建同时兼容新旧 Spec 删除接口。
|
||||
- 增加 PT→ONNX 导出、IK 补偿扫描、纯 IK 绕桩和 ONNX 批量路线检查入口。
|
||||
- 归档比赛最终 Rough 策略 `model_6800.onnx`；其 SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
|
||||
- Crawl 权重、运行日志、临时 XML 和大量重复路线实验不在本阶段归档。
|
||||
|
||||
## `v0.8.1` 的导航打点工具
|
||||
|
||||
- 补充 Pygame 地图/PCD/航点综合编辑器、避障区域编辑器和坐标变换工具。
|
||||
- 补充路线安全检查、候选航点优化、XML/航点镜像和批量 Sim2Sim 实验入口。
|
||||
- 按源文件时间保留 14 份比赛路线 JSON，不将开发期文件名误解释为正式版本号。
|
||||
- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
|
||||
- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
|
||||
- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。
|
||||
+10
-5
@@ -1,6 +1,6 @@
|
||||
# 软件
|
||||
|
||||
本目录当前保存 16DOF 轮足机器人的第一代完整软件闭环。
|
||||
本目录保存 16DOF 轮足机器人的训练、仿真和真机软件演进。
|
||||
|
||||
```text
|
||||
05_software/
|
||||
@@ -8,7 +8,10 @@
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
└─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
└─ sim2real_ros2_v2/ # ROS 2 导航原型及后续演进
|
||||
```
|
||||
|
||||
## 数据流
|
||||
@@ -19,16 +22,18 @@ MJCF + mjlab task
|
||||
v
|
||||
PPO 训练策略
|
||||
|
|
||||
+----> MuJoCo 独立模型调试
|
||||
+----> MuJoCo 姿态 / IK / MPC 调试
|
||||
|
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
+----> Python Sim2Real ----> 电机 / IMU
|
||||
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
||||
|
|
||||
+----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
|
||||
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 在早期版本中是自包含工程。训练、MJCF、独立 MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此本次保留其原始内部布局，没有为了目录外观拆散。
|
||||
`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`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# 第一代真机控制
|
||||
# 真机控制版本演进
|
||||
|
||||
本目录保存 16DOF 轮足机器人的早期真机控制实现。
|
||||
本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
@@ -19,3 +19,29 @@
|
||||
- 对齐、标定和独立检查工具
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_v2`
|
||||
|
||||
Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。
|
||||
|
||||
部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_ros2`
|
||||
|
||||
ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。
|
||||
|
||||
原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
## `sim2real_ros2_v2`
|
||||
|
||||
ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。该版本对应重排主线的 `v0.11.0`。
|
||||
|
||||
`v0.11.1` 在同一目录继续演进，首次随工程归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。各 Tag 可恢复对应阶段，当前目录说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
|
||||
`v0.12.0` 继续在同一目录保存 odom 快照，固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。
|
||||
|
||||
## 实机记录
|
||||
|
||||
[](../../06_assets/videos/early_sim2real.mp4)
|
||||
|
||||
该视频记录了这一阶段的早期真机测试，用于对应本目录中的第一代控制与部署实现。
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
.colcon/
|
||||
.vscode/
|
||||
compile_commands.json
|
||||
@@ -0,0 +1,178 @@
|
||||
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||
|
||||
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||
|
||||
---
|
||||
|
||||
## 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` 目录中（包含 `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,78 @@
|
||||
# 使用 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 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,74 @@
|
||||
# ROS 2/C++ Sim2Real 初版
|
||||
|
||||
本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
|
||||
|
||||
本工程保留当前 `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 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
|
||||
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
|
||||
@@ -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 架构说明
|
||||
|
||||
## 设计目标
|
||||
|
||||
- 保留已验证的 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,80 @@
|
||||
# 迁移计划
|
||||
|
||||
## 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 系统集成 ✅ 已完成
|
||||
|
||||
参考的源项目：
|
||||
|
||||
- `00_ reference/odin_ros_driver`
|
||||
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||
- `00_ reference/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` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||
@@ -0,0 +1,495 @@
|
||||
# sim2real_ros2 遥控器调用说明
|
||||
|
||||
本文档说明如何在 `sim2real_ros2` 中调用已接入的 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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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 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
|
||||
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
|
||||
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.
@@ -0,0 +1,5 @@
|
||||
# Odin 驱动依赖占位
|
||||
|
||||
`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
|
||||
|
||||
因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
|
||||
@@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,103 @@
|
||||
# =============================================================================
|
||||
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||
# =============================================================================
|
||||
#
|
||||
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||
#
|
||||
# =============================================================================
|
||||
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||
- {name: projected_gravity, dim: 3}
|
||||
- {name: command, dim: 3}
|
||||
- {name: joint_pos_rel, dim: 12}
|
||||
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||
- {name: last_actions, dim: 16}
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
fl_hip_abduction: [1, 1]
|
||||
fl_hip_pitch: [1, 2]
|
||||
fl_knee: [1, 3]
|
||||
fl_wheel: [1, 4]
|
||||
fr_hip_abduction: [1, 5]
|
||||
fr_hip_pitch: [1, 6]
|
||||
fr_knee: [1, 7]
|
||||
fr_wheel: [1, 8]
|
||||
rl_hip_abduction: [2, 1]
|
||||
rl_hip_pitch: [2, 2]
|
||||
rl_knee: [2, 3]
|
||||
rl_wheel: [2, 4]
|
||||
rr_hip_abduction: [2, 5]
|
||||
rr_hip_pitch: [2, 6]
|
||||
rr_knee: [2, 7]
|
||||
rr_wheel: [2, 8]
|
||||
direction_map:
|
||||
fl_hip_abduction: -1
|
||||
fl_hip_pitch: -1
|
||||
fl_knee: -1
|
||||
fl_wheel: -1
|
||||
fr_hip_abduction: -1
|
||||
fr_hip_pitch: 1
|
||||
fr_knee: 1
|
||||
fr_wheel: 1
|
||||
rl_hip_abduction: 1
|
||||
rl_hip_pitch: -1
|
||||
rl_knee: -1
|
||||
rl_wheel: -1
|
||||
rr_hip_abduction: 1
|
||||
rr_hip_pitch: 1
|
||||
rr_knee: 1
|
||||
rr_wheel: 1
|
||||
zero_offset_map:
|
||||
fl_hip_abduction: 0.003
|
||||
fl_hip_pitch: 0.030
|
||||
fl_knee: 0.028
|
||||
fl_wheel: 0.0
|
||||
fr_hip_abduction: 0.004
|
||||
fr_hip_pitch: 0.038
|
||||
fr_knee: 0.011
|
||||
fr_wheel: 0.0
|
||||
rl_hip_abduction: 0.019
|
||||
rl_hip_pitch: -0.034
|
||||
rl_knee: 0.025
|
||||
rl_wheel: 0.0
|
||||
rr_hip_abduction: -0.001
|
||||
rr_hip_pitch: 0.039
|
||||
rr_knee: 0.018
|
||||
rr_wheel: 0.0
|
||||
@@ -0,0 +1,80 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
policy_hz: 50.0
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
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"
|
||||
can1_name: "can1"
|
||||
imu_topic: "/odin1/imu"
|
||||
odom_topic: "/odom"
|
||||
|
||||
# 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.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
|
||||
|
||||
# Command mux parameters
|
||||
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.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
|
||||
|
||||
# Windows/Nano Web UDP bridge parameters
|
||||
web_bridge_enabled: true
|
||||
web_udp_listen_host: "0.0.0.0"
|
||||
web_udp_listen_port: 15000
|
||||
web_udp_remote_host: ""
|
||||
web_udp_remote_port: 15001
|
||||
web_udp_state_hz: 20.0
|
||||
web_udp_cmd_timeout_ms: 300.0
|
||||
web_udp_max_packet_bytes: 8192
|
||||
web_udp_max_vx: 0.8
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.5
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
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: 200.0
|
||||
|
||||
# Policy alignment with the Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: true
|
||||
@@ -0,0 +1,129 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
runtime_params = ParameterFile(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("sim2real_bringup"),
|
||||
"config",
|
||||
"runtime.yaml",
|
||||
]),
|
||||
allow_substs=True,
|
||||
)
|
||||
|
||||
# Declare launch configurations
|
||||
launch_driver_arg = DeclareLaunchArgument(
|
||||
'launch_driver',
|
||||
default_value='true',
|
||||
description='Whether to launch the odin_ros_driver sensor node'
|
||||
)
|
||||
|
||||
launch_nav2_arg = DeclareLaunchArgument(
|
||||
'launch_nav2',
|
||||
default_value='true',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
launch_remote_arg = DeclareLaunchArgument(
|
||||
'launch_remote',
|
||||
default_value='true',
|
||||
description='Whether to launch the SBUS UART remote control node'
|
||||
)
|
||||
|
||||
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||
'launch_web_bridge',
|
||||
default_value='true',
|
||||
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||
)
|
||||
|
||||
# Include odin_ros_driver launch
|
||||
driver_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('odin_ros_driver'),
|
||||
'launch',
|
||||
'odin1_ros2.launch.py'
|
||||
])
|
||||
),
|
||||
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||
)
|
||||
|
||||
# Include sim2real_nav2 launch
|
||||
nav2_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('sim2real_nav2'),
|
||||
'launch',
|
||||
'nav2.launch.py'
|
||||
])
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('launch_nav2'))
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
launch_driver_arg,
|
||||
launch_nav2_arg,
|
||||
launch_remote_arg,
|
||||
launch_web_bridge_arg,
|
||||
Node(
|
||||
package="sim2real_hw",
|
||||
executable="sim2real_hw_node",
|
||||
name="sim2real_hw_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="cmd_mux_node.py",
|
||||
name="sim2real_cmd_mux_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="web_udp_bridge_node.py",
|
||||
name="sim2real_web_udp_bridge_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="remote_uart_node.py",
|
||||
name="sim2real_remote_uart_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_remote')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="odom_relay_node",
|
||||
name="odom_relay_node",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"odom_input_topic": "/odin1/odometry",
|
||||
"odom_output_topic": "/odom",
|
||||
"base_frame": "base_link",
|
||||
"publish_tf": True,
|
||||
}],
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||
),
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Launch and configuration package for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>sim2real_common</exec_depend>
|
||||
<exec_depend>sim2real_hw</exec_depend>
|
||||
<exec_depend>sim2real_interfaces</exec_depend>
|
||||
<exec_depend>sim2real_runtime</exec_depend>
|
||||
<exec_depend>sim2real_nav2</exec_depend>
|
||||
<exec_depend>odin_ros_driver</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_common)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} INTERFACE)
|
||||
target_include_directories(${PROJECT_NAME} INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_package()
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
struct DeploymentContract
|
||||
{
|
||||
static constexpr std::size_t kObsDim = 53;
|
||||
static constexpr std::size_t kActionDim = 16;
|
||||
static constexpr std::size_t kLegJointCount = 12;
|
||||
static constexpr std::size_t kWheelCount = 4;
|
||||
static constexpr double kPolicyHz = 50.0;
|
||||
static constexpr double kMotorHz = 200.0;
|
||||
static constexpr double kStatusHz = 10.0;
|
||||
|
||||
static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
|
||||
static constexpr float kLegKp = 50.0f;
|
||||
static constexpr float kLegKd = 1.5f;
|
||||
static constexpr float kLegHoldKp = 80.0f;
|
||||
static constexpr float kLegHoldKd = 4.0f;
|
||||
static constexpr float kWheelKd = 1.0f;
|
||||
|
||||
static constexpr std::array<int, 16> kCanBusMap = {
|
||||
1, 1, 1, // fl legs
|
||||
1, 1, 1, // fr legs
|
||||
2, 2, 2, // rl legs
|
||||
2, 2, 2, // rr legs
|
||||
1, 1, 2, 2 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<int, 16> kCanIdMap = {
|
||||
1, 2, 3, // fl legs
|
||||
5, 6, 7, // fr legs
|
||||
1, 2, 3, // rl legs
|
||||
5, 6, 7, // rr legs
|
||||
4, 8, 4, 8 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDirectionMap = {
|
||||
-1.0f, -1.0f, -1.0f, // fl
|
||||
-1.0f, 1.0f, 1.0f, // fr
|
||||
1.0f, -1.0f, -1.0f, // rl
|
||||
1.0f, 1.0f, 1.0f, // rr
|
||||
-1.0f, 1.0f, -1.0f, 1.0f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kZeroOffsetMap = {
|
||||
0.003f, 0.030f, 0.028f, // fl
|
||||
0.004f, 0.038f, 0.011f, // fr
|
||||
0.019f, -0.034f, 0.025f, // rl
|
||||
-0.001f, 0.039f, 0.018f, // rr
|
||||
0.000f, 0.000f, 0.000f, 0.000f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kActionScale = {
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
5.0f, 5.0f, 5.0f, 5.0f
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||
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
|
||||
};
|
||||
};
|
||||
|
||||
static constexpr std::array<const char *, 16> kJointLabels = {
|
||||
"fl_hip_abduction",
|
||||
"fl_hip_pitch",
|
||||
"fl_knee",
|
||||
"fr_hip_abduction",
|
||||
"fr_hip_pitch",
|
||||
"fr_knee",
|
||||
"rl_hip_abduction",
|
||||
"rl_hip_pitch",
|
||||
"rl_knee",
|
||||
"rr_hip_abduction",
|
||||
"rr_hip_pitch",
|
||||
"rr_knee",
|
||||
"fl_wheel",
|
||||
"fr_wheel",
|
||||
"rl_wheel",
|
||||
"rr_wheel"
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+55
@@ -0,0 +1,55 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class LowPassFilter
|
||||
{
|
||||
public:
|
||||
LowPassFilter(double cutoff_freq, double dt, std::size_t dim)
|
||||
: dim_(dim), initialized_(false)
|
||||
{
|
||||
alpha_ = static_cast<float>(1.0 - std::exp(-2.0 * M_PI * cutoff_freq * dt));
|
||||
y_prev_.resize(dim, 0.0f);
|
||||
}
|
||||
|
||||
void filter(const float* x, float* y)
|
||||
{
|
||||
if (!initialized_) {
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y_prev_[i] = x[i];
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y[i] = alpha_ * x[i] + (1.0f - alpha_) * y_prev_[i];
|
||||
y_prev_[i] = y[i];
|
||||
}
|
||||
}
|
||||
|
||||
void filter(const std::vector<float>& x, std::vector<float>& y)
|
||||
{
|
||||
filter(x.data(), y.data());
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
float alpha_;
|
||||
std::size_t dim_;
|
||||
bool initialized_;
|
||||
std::vector<float> y_prev_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
// Helper to calculate gravity orientation from quaternion [w, x, y, z]
|
||||
inline std::array<float, 3> get_gravity_orientation(const std::array<float, 4>& quat_wxyz)
|
||||
{
|
||||
float qw = quat_wxyz[0];
|
||||
float qx = quat_wxyz[1];
|
||||
float qy = quat_wxyz[2];
|
||||
float qz = quat_wxyz[3];
|
||||
|
||||
float gx = 2.0f * (-qz * qx + qw * qy);
|
||||
float gy = -2.0f * (qz * qy + qw * qx);
|
||||
float gz = 1.0f - 2.0f * (qw * qw + qz * qz);
|
||||
return {gx, gy, gz};
|
||||
}
|
||||
|
||||
// Helper to create quaternion from acceleration vector
|
||||
inline std::array<float, 4> quat_from_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
if (norm_a < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
// Ref gravity vector is [0.0, 0.0, 1.0]
|
||||
float cross_x = -ay;
|
||||
float cross_y = ax;
|
||||
float cross_z = 0.0f;
|
||||
float dot = az;
|
||||
|
||||
if (dot < -0.999999f) {
|
||||
return {0.0f, 1.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float s = std::sqrt((1.0f + dot) * 2.0f);
|
||||
std::array<float, 4> q = {
|
||||
s * 0.5f,
|
||||
cross_x / s,
|
||||
cross_y / s,
|
||||
cross_z / s
|
||||
};
|
||||
|
||||
float norm_q = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
if (norm_q < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
q[0] /= norm_q;
|
||||
q[1] /= norm_q;
|
||||
q[2] /= norm_q;
|
||||
q[3] /= norm_q;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
class MahonyFilter
|
||||
{
|
||||
public:
|
||||
MahonyFilter(float kp = 2.0f, float ki = 0.0f)
|
||||
: kp_(kp), ki_(ki)
|
||||
{
|
||||
q_ = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
void reset_with_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
q_ = quat_from_accel(accel);
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
std::array<float, 4> update(const std::array<float, 3>& accel, const std::array<float, 3>& gyro, float dt)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
std::array<float, 3> gyro_corr = gyro;
|
||||
|
||||
if (norm_a > 1e-6f) {
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float vx = 2.0f * (qx * qz - qw * qy);
|
||||
float vy = 2.0f * (qw * qx + qy * qz);
|
||||
float vz = qw * qw - qx * qx - qy * qy + qz * qz;
|
||||
|
||||
// Error = cross(a, v)
|
||||
float ex = ay * vz - az * vy;
|
||||
float ey = az * vx - ax * vz;
|
||||
float ez = ax * vy - ay * vx;
|
||||
|
||||
if (ki_ > 0.0f) {
|
||||
e_int_[0] += ex * dt;
|
||||
e_int_[1] += ey * dt;
|
||||
e_int_[2] += ez * dt;
|
||||
} else {
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
gyro_corr[0] += kp_ * ex + ki_ * e_int_[0];
|
||||
gyro_corr[1] += kp_ * ey + ki_ * e_int_[1];
|
||||
gyro_corr[2] += kp_ * ez + ki_ * e_int_[2];
|
||||
}
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float q_dot_w = 0.5f * (-qx * gyro_corr[0] - qy * gyro_corr[1] - qz * gyro_corr[2]);
|
||||
float q_dot_x = 0.5f * ( qw * gyro_corr[0] + qy * gyro_corr[2] - qz * gyro_corr[1]);
|
||||
float q_dot_y = 0.5f * ( qw * gyro_corr[1] - qx * gyro_corr[2] + qz * gyro_corr[0]);
|
||||
float q_dot_z = 0.5f * ( qw * gyro_corr[2] + qx * gyro_corr[1] - qy * gyro_corr[0]);
|
||||
|
||||
q_[0] += q_dot_w * dt;
|
||||
q_[1] += q_dot_x * dt;
|
||||
q_[2] += q_dot_y * dt;
|
||||
q_[3] += q_dot_z * dt;
|
||||
|
||||
float norm_q = std::sqrt(q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3]) + 1e-9f;
|
||||
q_[0] /= norm_q;
|
||||
q_[1] /= norm_q;
|
||||
q_[2] /= norm_q;
|
||||
q_[3] /= norm_q;
|
||||
|
||||
return q_;
|
||||
}
|
||||
|
||||
const std::array<float, 4>& get_q() const { return q_; }
|
||||
|
||||
private:
|
||||
float kp_;
|
||||
float ki_;
|
||||
std::array<float, 4> q_;
|
||||
std::array<float, 3> e_int_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class GuardLevel : int {
|
||||
OK = 0,
|
||||
WARN = 1,
|
||||
STOP = 2
|
||||
};
|
||||
|
||||
struct GuardDecision {
|
||||
GuardLevel level{GuardLevel::OK};
|
||||
std::string reason;
|
||||
};
|
||||
|
||||
class RuntimeGuard {
|
||||
public:
|
||||
RuntimeGuard(
|
||||
float max_ang_vel = 12.0f,
|
||||
float max_tilt_z = -0.30f,
|
||||
float imu_age_warn_ms = 60.0f,
|
||||
float imu_age_stop_ms = 200.0f)
|
||||
: max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
imu_age_warn_ms_(imu_age_warn_ms),
|
||||
imu_age_stop_ms_(imu_age_stop_ms)
|
||||
{}
|
||||
|
||||
GuardDecision check(
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
float imu_age_ms,
|
||||
bool estop_triggered,
|
||||
const std::vector<float>& extra_vals = {})
|
||||
{
|
||||
GuardDecision decision;
|
||||
|
||||
// 1) user E-stop
|
||||
if (estop_triggered) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 2) NaN/Inf check
|
||||
for (float v : imu_gyro) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in imu_gyro";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : projected_gravity) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in projected_gravity";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : extra_vals) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in checked values";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
|
||||
// 3) IMU stale
|
||||
if (imu_age_ms > imu_age_stop_ms_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "IMU stale " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
bool warned_imu = (imu_age_ms > imu_age_warn_ms_);
|
||||
|
||||
// 4) Tilt check
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "tilt: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 5) Angular velocity check
|
||||
float ang_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_norm > max_ang_vel_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "ang_vel overflow: |w|=" + std::to_string(ang_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
if (warned_imu) {
|
||||
decision.level = GuardLevel::WARN;
|
||||
decision.reason = "IMU age " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
|
||||
decision.level = GuardLevel::OK;
|
||||
return decision;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
float imu_age_warn_ms_;
|
||||
float imu_age_stop_ms_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+119
@@ -0,0 +1,119 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class SafetyLevel : int {
|
||||
NORMAL = 0,
|
||||
CLIP = 1,
|
||||
BRAKE = 2,
|
||||
ESTOP = 3
|
||||
};
|
||||
|
||||
struct SafetyDecision {
|
||||
SafetyLevel level{SafetyLevel::NORMAL};
|
||||
std::string message;
|
||||
std::array<float, 16> clipped_target{};
|
||||
};
|
||||
|
||||
class SafetyMonitor {
|
||||
public:
|
||||
SafetyMonitor(
|
||||
float max_target_offset = 0.6f,
|
||||
float max_ang_vel = 10.0f,
|
||||
float max_tilt_z = -0.3f,
|
||||
int clip_to_brake = 0,
|
||||
float hard_target_offset = 1.2f)
|
||||
: max_target_offset_(max_target_offset),
|
||||
max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
clip_to_brake_(clip_to_brake),
|
||||
hard_target_offset_(hard_target_offset),
|
||||
consecutive_clips_(0)
|
||||
{}
|
||||
|
||||
SafetyDecision check(
|
||||
const std::array<float, 16>& target_pose,
|
||||
const std::array<float, 16>& default_pose,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
bool estop_triggered)
|
||||
{
|
||||
SafetyDecision decision;
|
||||
decision.clipped_target = target_pose;
|
||||
|
||||
if (estop_triggered) {
|
||||
decision.level = SafetyLevel::ESTOP;
|
||||
decision.message = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Tilt check (g_z should be ~ -1.0, if it is > max_tilt_z e.g. -0.3, it is tilted)
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "tilt detected: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Angular velocity norm check
|
||||
float ang_vel_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_vel_norm > max_ang_vel_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "angular velocity overflow: |w|=" + std::to_string(ang_vel_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Offset check
|
||||
bool needs_clip = false;
|
||||
float max_offset = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) { // check leg joint offsets from default pose
|
||||
float offset = target_pose[i] - default_pose[i];
|
||||
max_offset = std::max(max_offset, std::abs(offset));
|
||||
if (std::abs(offset) > max_target_offset_) {
|
||||
needs_clip = true;
|
||||
float clipped_val = std::clamp(offset, -max_target_offset_, max_target_offset_);
|
||||
decision.clipped_target[i] = default_pose[i] + clipped_val;
|
||||
}
|
||||
}
|
||||
|
||||
if (needs_clip) {
|
||||
consecutive_clips_++;
|
||||
if (hard_target_offset_ > 0.0f && max_offset > hard_target_offset_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "target leg offset exceeds hard limit: " + std::to_string(max_offset);
|
||||
return decision;
|
||||
}
|
||||
if (clip_to_brake_ > 0 && consecutive_clips_ >= clip_to_brake_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "clipped " + std::to_string(consecutive_clips_) + " frames in a row";
|
||||
return decision;
|
||||
}
|
||||
decision.level = SafetyLevel::CLIP;
|
||||
decision.message = "target leg offset out of range";
|
||||
return decision;
|
||||
}
|
||||
|
||||
consecutive_clips_ = 0;
|
||||
decision.level = SafetyLevel::NORMAL;
|
||||
return decision;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
consecutive_clips_ = 0;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_target_offset_;
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
int clip_to_brake_;
|
||||
float hard_target_offset_;
|
||||
int consecutive_clips_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class StandBalanceController
|
||||
{
|
||||
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 reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
}
|
||||
|
||||
std::array<float, 16> computeTarget(
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
float hip_base = 0.9f;
|
||||
float knee_base = -1.8f;
|
||||
estimateBaseLegPose(hip_base, knee_base);
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
estimateRollPitch(projected_gravity, roll, pitch);
|
||||
|
||||
float roll_rate = imu_gyro[0];
|
||||
float pitch_rate = imu_gyro[1];
|
||||
|
||||
float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate;
|
||||
|
||||
float lateral_lean = lateral_lean_gain_ * cmd[1];
|
||||
|
||||
std::array<float, 16> target{};
|
||||
for (int leg_idx = 0; leg_idx < 4; ++leg_idx) {
|
||||
float side = (leg_idx == 0 || leg_idx == 2) ? 1.0f : -1.0f;
|
||||
|
||||
target[leg_idx * 3 + 0] = std::clamp(side * roll_corr + lateral_lean, -hip_abduction_clip_, hip_abduction_clip_);
|
||||
target[leg_idx * 3 + 1] = std::clamp(hip_base, hip_pitch_clip_[0], hip_pitch_clip_[1]);
|
||||
target[leg_idx * 3 + 2] = std::clamp(knee_base, knee_clip_[0], knee_clip_[1]);
|
||||
}
|
||||
// wheels 0
|
||||
target[12] = target[13] = target[14] = target[15] = 0.0f;
|
||||
|
||||
bool stable = (std::abs(roll * 180.0f / static_cast<float>(M_PI)) <= stable_roll_deg_) &&
|
||||
(std::abs(pitch * 180.0f / static_cast<float>(M_PI)) <= stable_pitch_deg_) &&
|
||||
(std::max(std::abs(roll_rate * 180.0f / static_cast<float>(M_PI)), std::abs(pitch_rate * 180.0f / static_cast<float>(M_PI))) <= stable_gyro_deg_s_);
|
||||
|
||||
stable_time_ = stable ? (stable_time_ + static_cast<float>(control_dt_)) : 0.0f;
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
bool isStable() const
|
||||
{
|
||||
return stable_time_ >= enter_hold_s_;
|
||||
}
|
||||
|
||||
private:
|
||||
void estimateRollPitch(const std::array<float, 3>& projected_gravity, float& roll, float& pitch)
|
||||
{
|
||||
float gx = projected_gravity[0];
|
||||
float gy = projected_gravity[1];
|
||||
float gz = projected_gravity[2];
|
||||
roll = std::atan2(-gy, std::max(1e-6f, -gz));
|
||||
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 height_{0.33f};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
float hip_abduction_clip_{0.45f};
|
||||
std::array<float, 2> hip_pitch_clip_{-1.0f, 2.5f};
|
||||
std::array<float, 2> knee_clip_{-2.6f, -0.3f};
|
||||
float stable_roll_deg_{6.0f};
|
||||
float stable_pitch_deg_{8.0f};
|
||||
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};
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
@@ -0,0 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_common</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Shared constants and deployment contract helpers for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
</package>
|
||||
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_hw)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
add_executable(sim2real_hw_node
|
||||
src/hardware_bridge_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_hw_node PRIVATE include)
|
||||
target_compile_features(sim2real_hw_node PRIVATE cxx_std_17)
|
||||
ament_target_dependencies(sim2real_hw_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
nav_msgs
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_hw_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+156
@@ -0,0 +1,156 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.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"
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
int id; // motor CAN id
|
||||
float direction;
|
||||
float offset;
|
||||
};
|
||||
|
||||
struct MotorStateInternal
|
||||
{
|
||||
float position{0.0f};
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{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};
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
HardwareBridgeNode();
|
||||
~HardwareBridgeNode();
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
bool initCan(const std::string& ifname, int& fd);
|
||||
bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc);
|
||||
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
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);
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_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_;
|
||||
|
||||
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::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
|
||||
// SocketCAN file descriptors
|
||||
int can0_fd_{-1};
|
||||
int can1_fd_{-1};
|
||||
std::string can0_name_{"can0"};
|
||||
std::string can1_name_{"can1"};
|
||||
|
||||
// CAN error recovery
|
||||
static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit
|
||||
int can0_error_count_{0};
|
||||
int can1_error_count_{0};
|
||||
bool reinitCan(const std::string& ifname, int& fd, int& error_count);
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<MotorConfig, 16> motors_;
|
||||
std::array<MotorStateInternal, 16> motor_states_;
|
||||
|
||||
// IMU state
|
||||
std::mutex imu_mutex_;
|
||||
std::array<float, 3> imu_gyro_{};
|
||||
std::array<float, 3> imu_accel_{};
|
||||
std::array<float, 3> projected_gravity_{0.0f, 0.0f, -1.0f};
|
||||
bool imu_fresh_{false};
|
||||
rclcpp::Time last_imu_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::chrono::steady_clock::time_point last_imu_recv_time_{};
|
||||
bool has_received_imu_{false};
|
||||
std::array<float, 3> imu_gravity_sum_{0.0f, 0.0f, 0.0f};
|
||||
std::uint32_t imu_gravity_sample_count_{0};
|
||||
static constexpr std::uint32_t kImuGravityAlignSamples = 50;
|
||||
|
||||
// Odom state
|
||||
std::mutex odom_mutex_;
|
||||
rclcpp::Time last_odom_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::array<float, 3> odom_pos_{};
|
||||
std::array<float, 4> odom_quat_wxyz_{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
std::array<float, 3> odom_linear_vel_{};
|
||||
std::array<float, 3> odom_angular_vel_{};
|
||||
bool odom_fresh_{false};
|
||||
|
||||
// Filters and Estimators
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_legs_;
|
||||
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::RuntimeGuard> runtime_guard_;
|
||||
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};
|
||||
|
||||
// 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};
|
||||
|
||||
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_{""};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_hw</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Hardware bridge and safety boundary for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
</package>
|
||||
@@ -0,0 +1,852 @@
|
||||
#include "sim2real_hw/hardware_bridge_node.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <net/if.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
// Protocol constants
|
||||
const std::uint32_t COMM_ENABLE = 3;
|
||||
const std::uint32_t COMM_DISABLE = 4;
|
||||
const std::uint32_t COMM_WRITE_PARAMETER = 18;
|
||||
const std::uint32_t COMM_OPERATION_CONTROL = 1;
|
||||
const std::uint32_t COMM_SET_ZERO_POSITION = 6;
|
||||
const std::uint16_t PARAM_MODE = 0x7005;
|
||||
const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017;
|
||||
const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B;
|
||||
const std::uint8_t HOST_ID = 0xFD;
|
||||
|
||||
inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val)
|
||||
{
|
||||
buf[0] = (val >> 8) & 0xFF;
|
||||
buf[1] = val & 0xFF;
|
||||
}
|
||||
|
||||
inline float nearest_periodic(float val, float ref)
|
||||
{
|
||||
float diff = val - ref;
|
||||
float wrapped = diff - 2.0f * static_cast<float>(M_PI) * std::floor((diff + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
return ref + wrapped;
|
||||
}
|
||||
|
||||
HardwareBridgeNode::HardwareBridgeNode()
|
||||
: Node("sim2real_hw_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
target_timeout_ms_ = declare_parameter<double>("target_timeout_ms", 150.0);
|
||||
can0_name_ = declare_parameter<std::string>("can0_name", "can0");
|
||||
can1_name_ = declare_parameter<std::string>("can1_name", "can1");
|
||||
dry_run_ = declare_parameter<bool>("dry_run", true); // Default to dry-run for safety
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false");
|
||||
if (safety_enabled_) {
|
||||
RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel);
|
||||
} else {
|
||||
RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!");
|
||||
}
|
||||
|
||||
// 2. Set up logical motors mapping matching contract
|
||||
// Mapping index in array matches joint ordering in kJointLabels
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i];
|
||||
motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i];
|
||||
motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i];
|
||||
motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i];
|
||||
}
|
||||
|
||||
// 3. Initialize filters & safety monitors
|
||||
lpf_legs_ = std::make_unique<sim2real_common::LowPassFilter>(5.0, 0.005, 12);
|
||||
lpf_wheels_ = std::make_unique<sim2real_common::LowPassFilter>(15.0, 0.005, 4);
|
||||
mahony_filter_ = std::make_unique<sim2real_common::MahonyFilter>(2.0f, 0.0f);
|
||||
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 4. Initialize CAN sockets if not in dry-run
|
||||
if (!dry_run_) {
|
||||
if (!initCan(can0_name_, can0_fd_) || !initCan(can1_name_, can1_fd_)) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN initialization failed! Falling back to dry-run.");
|
||||
dry_run_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 5. Initialize motor target states
|
||||
latest_target_ = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
latest_raw_action_.fill(0.0f);
|
||||
|
||||
// 6. Set up ROS publishers & subscriptions
|
||||
state_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeState>("runtime/state", 10);
|
||||
target_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeTarget>(
|
||||
"runtime/target", 10,
|
||||
std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1));
|
||||
std::string imu_topic = declare_parameter<std::string>("imu_topic", "/odin1/imu");
|
||||
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
|
||||
imu_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// Odom subscription
|
||||
std::string odom_topic = declare_parameter<std::string>("odom_topic", "/odom");
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
// 7. Enable motors on total startup
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Enabling RobStride motors...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
enableMotor(fd, motors_[i].id);
|
||||
setModeRaw(fd, motors_[i].id, 0); // MIT Mode
|
||||
writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f);
|
||||
writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// 8. Timers at 200Hz (5ms)
|
||||
read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this));
|
||||
write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this));
|
||||
}
|
||||
|
||||
HardwareBridgeNode::~HardwareBridgeNode()
|
||||
{
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
disableMotor(fd, motors_[i].id);
|
||||
}
|
||||
if (can0_fd_ >= 0) {
|
||||
if (::close(can0_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
if (can1_fd_ >= 0) {
|
||||
if (::close(can1_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
latest_target_ = msg->target;
|
||||
latest_raw_action_ = msg->raw_action;
|
||||
latest_target_source_ = msg->target_source;
|
||||
latest_target_stamp_ = rclcpp::Time(msg->stamp);
|
||||
target_sequence_ = msg->sequence;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
|
||||
const float gyro_x = static_cast<float>(msg->angular_velocity.x);
|
||||
const float gyro_y = static_cast<float>(msg->angular_velocity.y);
|
||||
const float gyro_z = static_cast<float>(msg->angular_velocity.z);
|
||||
|
||||
const float accel_x = static_cast<float>(msg->linear_acceleration.x);
|
||||
const float accel_y = static_cast<float>(msg->linear_acceleration.y);
|
||||
const float accel_z = static_cast<float>(msg->linear_acceleration.z);
|
||||
|
||||
imu_gyro_ = {gyro_x, gyro_y, gyro_z};
|
||||
imu_accel_ = {accel_x, accel_y, accel_z};
|
||||
if (!mahony_initialized_ && imu_gravity_sample_count_ < kImuGravityAlignSamples) {
|
||||
imu_gravity_sum_[0] += accel_x;
|
||||
imu_gravity_sum_[1] += accel_y;
|
||||
imu_gravity_sum_[2] += accel_z;
|
||||
imu_gravity_sample_count_++;
|
||||
}
|
||||
// Track both ROS header time and local receive time. The local steady clock
|
||||
// is used for stale detection so scheduler jitter or device timestamp quirks
|
||||
// don't falsely trip the runtime guard.
|
||||
last_imu_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
last_imu_recv_time_ = std::chrono::steady_clock::now();
|
||||
has_received_imu_ = true;
|
||||
imu_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "Physical E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
last_odom_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
|
||||
odom_pos_[0] = static_cast<float>(msg->pose.pose.position.x);
|
||||
odom_pos_[1] = static_cast<float>(msg->pose.pose.position.y);
|
||||
odom_pos_[2] = static_cast<float>(msg->pose.pose.position.z);
|
||||
|
||||
odom_quat_wxyz_[0] = static_cast<float>(msg->pose.pose.orientation.w);
|
||||
odom_quat_wxyz_[1] = static_cast<float>(msg->pose.pose.orientation.x);
|
||||
odom_quat_wxyz_[2] = static_cast<float>(msg->pose.pose.orientation.y);
|
||||
odom_quat_wxyz_[3] = static_cast<float>(msg->pose.pose.orientation.z);
|
||||
|
||||
odom_linear_vel_[0] = static_cast<float>(msg->twist.twist.linear.x);
|
||||
odom_linear_vel_[1] = static_cast<float>(msg->twist.twist.linear.y);
|
||||
odom_linear_vel_[2] = static_cast<float>(msg->twist.twist.linear.z);
|
||||
|
||||
odom_angular_vel_[0] = static_cast<float>(msg->twist.twist.angular.x);
|
||||
odom_angular_vel_[1] = static_cast<float>(msg->twist.twist.angular.y);
|
||||
odom_angular_vel_[2] = static_cast<float>(msg->twist.twist.angular.z);
|
||||
|
||||
odom_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onReadLoop()
|
||||
{
|
||||
// 1. Process CAN messages (only if CAN is open)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motor_states_[i].stale_count++;
|
||||
}
|
||||
|
||||
struct can_frame frame;
|
||||
// Process can0 (bus 1)
|
||||
while (readCanFrame(can0_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) { // Status Frame
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 1 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
// Apply motor mapping: real_to_sim
|
||||
// real = sign * sim + offset -> sim = (real - offset) / sign
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Process can1 (bus 2)
|
||||
while (readCanFrame(can1_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) {
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 2 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Hold-over: apply last valid data for stale motors
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) {
|
||||
motor_states_[i].position = motor_states_[i].last_valid_pos;
|
||||
motor_states_[i].velocity = motor_states_[i].last_valid_vel;
|
||||
motor_states_[i].torque = motor_states_[i].last_valid_torque;
|
||||
holdover_events_total_++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Fetch IMU data & update MahonyFilter
|
||||
auto now_time = now();
|
||||
double dt = 0.005;
|
||||
if (last_read_time_.nanoseconds() > 0) {
|
||||
dt = (now_time - last_read_time_).seconds();
|
||||
if (dt <= 0.0 || dt > 0.5) {
|
||||
dt = 0.005;
|
||||
}
|
||||
}
|
||||
last_read_time_ = now_time;
|
||||
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> accel{0.0f, 0.0f, 9.81f};
|
||||
bool imu_fresh = false;
|
||||
double imu_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
accel = imu_accel_;
|
||||
imu_fresh = imu_fresh_;
|
||||
imu_fresh_ = false;
|
||||
|
||||
if (has_received_imu_) {
|
||||
const auto age = std::chrono::steady_clock::now() - last_imu_recv_time_;
|
||||
imu_age_ms = std::chrono::duration<double, std::milli>(age).count();
|
||||
} else if (last_imu_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - last_imu_stamp_).nanoseconds();
|
||||
imu_age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 4> quat{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
if (imu_fresh || mahony_initialized_) {
|
||||
if (!mahony_initialized_) {
|
||||
std::array<float, 3> gravity_init = accel;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
if (imu_gravity_sample_count_ >= kImuGravityAlignSamples) {
|
||||
gravity_init = {
|
||||
imu_gravity_sum_[0] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[1] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[2] / static_cast<float>(imu_gravity_sample_count_)
|
||||
};
|
||||
}
|
||||
}
|
||||
mahony_filter_->reset_with_accel(gravity_init);
|
||||
mahony_initialized_ = true;
|
||||
}
|
||||
quat = mahony_filter_->update(accel, gyro, static_cast<float>(dt));
|
||||
}
|
||||
std::array<float, 3> projected_gravity = sim2real_common::get_gravity_orientation(quat);
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
projected_gravity_ = projected_gravity;
|
||||
}
|
||||
|
||||
// Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(32);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
extra_vals.push_back(motor_states_[i].position);
|
||||
extra_vals.push_back(motor_states_[i].velocity);
|
||||
}
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
auto guard_decision = runtime_guard_->check(gyro, projected_gravity, imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
// 3. Populate RuntimeState message
|
||||
sim2real_interfaces::msg::RuntimeState msg;
|
||||
msg.stamp = now_time;
|
||||
msg.sequence = state_sequence_++;
|
||||
msg.source = dry_run_ ? "stub_hw" : "socket_can_hw";
|
||||
|
||||
fresh_count_ = 0;
|
||||
holdover_count_ = 0;
|
||||
stale_max_ = 0;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Stale frames detection & holdover count
|
||||
if (!dry_run_) {
|
||||
if (motor_states_[i].stale_count > 0) {
|
||||
holdover_count_++;
|
||||
stale_max_ = std::max(stale_max_, motor_states_[i].stale_count);
|
||||
} else {
|
||||
fresh_count_++;
|
||||
}
|
||||
}
|
||||
|
||||
if (dry_run_) {
|
||||
// Mock motor positions tracking target
|
||||
msg.joint_pos[i] = latest_target_[i];
|
||||
msg.joint_vel[i] = 0.0f;
|
||||
msg.joint_torque[i] = 0.0f;
|
||||
msg.update_counts[i] = target_sequence_;
|
||||
} else {
|
||||
msg.joint_pos[i] = motor_states_[i].position;
|
||||
msg.joint_vel[i] = motor_states_[i].velocity;
|
||||
msg.joint_torque[i] = motor_states_[i].torque;
|
||||
msg.update_counts[i] = motor_states_[i].update_count;
|
||||
}
|
||||
}
|
||||
|
||||
msg.imu_gyro = gyro;
|
||||
msg.imu_accel = accel;
|
||||
msg.quat_wxyz = quat;
|
||||
msg.projected_gravity = projected_gravity;
|
||||
msg.imu_age_ms = imu_age_ms;
|
||||
msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness
|
||||
|
||||
msg.odom_age_ms = 0.0f;
|
||||
msg.odom_fresh = false;
|
||||
msg.odom_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
msg.odom_linear_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_angular_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_yaw = 0.0f;
|
||||
|
||||
// Populate odom fields from subscriber data
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
if (odom_fresh_) {
|
||||
double odom_age = (now_time - last_odom_stamp_).seconds() * 1000.0;
|
||||
msg.odom_age_ms = static_cast<float>(odom_age);
|
||||
msg.odom_fresh = (odom_age < 200.0); // 200ms threshold
|
||||
msg.odom_pos = odom_pos_;
|
||||
msg.odom_quat_wxyz = odom_quat_wxyz_;
|
||||
msg.odom_linear_vel = odom_linear_vel_;
|
||||
msg.odom_angular_vel = odom_angular_vel_;
|
||||
msg.odom_local_pos = odom_pos_;
|
||||
// Compute yaw from quaternion
|
||||
float qw = odom_quat_wxyz_[0], qx = odom_quat_wxyz_[1];
|
||||
float qy = odom_quat_wxyz_[2], qz = odom_quat_wxyz_[3];
|
||||
float siny_c = 2.0f * (qw * qz + qx * qy);
|
||||
float cosy_c = 1.0f - 2.0f * (qy * qy + qz * qz);
|
||||
msg.odom_local_yaw = std::atan2(siny_c, cosy_c);
|
||||
}
|
||||
}
|
||||
|
||||
msg.fresh_count = dry_run_ ? 16 : fresh_count_;
|
||||
msg.holdover_count = dry_run_ ? 0 : holdover_count_;
|
||||
msg.stale_max = dry_run_ ? 0 : stale_max_;
|
||||
|
||||
state_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onWriteLoop()
|
||||
{
|
||||
const auto now_time = now();
|
||||
std::array<float, 16> target{};
|
||||
std::string target_source;
|
||||
double age_ms = 0.0;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
target = latest_target_;
|
||||
target_source = latest_target_source_;
|
||||
|
||||
if (latest_target_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - latest_target_stamp_).nanoseconds();
|
||||
age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Timeout guard: default stand pose if target is stale
|
||||
if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) {
|
||||
target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target_source = "timeout_hold";
|
||||
}
|
||||
|
||||
// Run SafetyMonitor check on incoming target commands
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> proj_grav{};
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
proj_grav = projected_gravity_;
|
||||
}
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
auto safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target = safety_decision.clipped_target;
|
||||
target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped.");
|
||||
}
|
||||
}
|
||||
|
||||
// Override to safety_brake if safety is triggered locally or by estop
|
||||
if (safety_triggered_) {
|
||||
target_source = "safety_brake";
|
||||
}
|
||||
|
||||
// 1. Joint LPF command filtering (200Hz, dt=0.005s)
|
||||
std::array<float, 12> legs_in{};
|
||||
std::array<float, 12> legs_out{};
|
||||
std::array<float, 4> wheels_in{};
|
||||
std::array<float, 4> wheels_out{};
|
||||
|
||||
std::copy(target.begin(), target.begin() + 12, legs_in.begin());
|
||||
std::copy(target.begin() + 12, target.end(), wheels_in.begin());
|
||||
|
||||
lpf_legs_->filter(legs_in.data(), legs_out.data());
|
||||
lpf_wheels_->filter(wheels_in.data(), wheels_out.data());
|
||||
|
||||
std::array<float, 16> filtered_target{};
|
||||
std::copy(legs_out.begin(), legs_out.end(), filtered_target.begin());
|
||||
std::copy(wheels_out.begin(), wheels_out.end(), filtered_target.begin() + 12);
|
||||
|
||||
// 2. Control execution (MIT mode write over CAN)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Coordinate transform: sim_to_real
|
||||
// real = sign * sim + offset
|
||||
float sim_val = filtered_target[i];
|
||||
float real_val = motors_[i].direction * sim_val + motors_[i].offset;
|
||||
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
|
||||
if (i < 12) {
|
||||
// Leg joints: MIT position control
|
||||
// Kp & Kd depend on whether we are holding pose, running policy, or in safety damping mode
|
||||
double kp_val = sim2real_common::DeploymentContract::kLegKp;
|
||||
double kd_val = sim2real_common::DeploymentContract::kLegKd;
|
||||
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
kp_val = 0.0;
|
||||
kd_val = 2.5; // Leg damping Kd
|
||||
real_val = 0.0; // Set to zero position (sign/offset will be ignored anyway under kp=0)
|
||||
} else if (target_source == "startup_soft_hold") {
|
||||
if (startup_soft_hold_start_time_.nanoseconds() == 0) {
|
||||
startup_soft_hold_start_time_ = now_time;
|
||||
}
|
||||
double elapsed = (now_time - startup_soft_hold_start_time_).seconds();
|
||||
double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp * kp_scale;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
} else {
|
||||
startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
|
||||
if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") {
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
}
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0);
|
||||
} else {
|
||||
// Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0)
|
||||
double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign
|
||||
double kd_val = sim2real_common::DeploymentContract::kWheelKd;
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
vel_real = 0.0;
|
||||
kd_val = 2.0; // Wheel damping Kd
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::enableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, nullptr, 0);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::disableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = PARAM_MODE & 0xFF;
|
||||
data[1] = (PARAM_MODE >> 8) & 0xFF;
|
||||
data[4] = static_cast<std::uint8_t>(mode);
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = param_id & 0xFF;
|
||||
data[1] = (param_id >> 8) & 0xFF;
|
||||
std::memcpy(&data[4], &limit, sizeof(float));
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp_val, double kd_val, double torque)
|
||||
{
|
||||
const double P_LIMIT = 4.0 * M_PI;
|
||||
const double V_LIMIT = 44.0;
|
||||
const double T_LIMIT = 17.0;
|
||||
const double KP_LIMIT = 500.0;
|
||||
const double KD_LIMIT = 5.0;
|
||||
|
||||
double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos));
|
||||
double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel));
|
||||
double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val));
|
||||
double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val));
|
||||
double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque));
|
||||
|
||||
std::uint16_t pos_u16 = static_cast<std::uint16_t>(((pos_clamped / P_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t vel_u16 = static_cast<std::uint16_t>(((vel_clamped / V_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t kp_u16 = static_cast<std::uint16_t>((kp_clamped / KP_LIMIT) * 65535.0);
|
||||
std::uint16_t kd_u16 = static_cast<std::uint16_t>((kd_clamped / KD_LIMIT) * 65535.0);
|
||||
std::uint16_t torque_u16 = static_cast<std::uint16_t>(((torque_clamped / T_LIMIT) + 1.0) * 32767.0);
|
||||
|
||||
std::uint8_t data[8];
|
||||
pack_u16_be(&data[0], pos_u16);
|
||||
pack_u16_be(&data[2], vel_u16);
|
||||
pack_u16_be(&data[4], kp_u16);
|
||||
pack_u16_be(&data[6], kd_u16);
|
||||
|
||||
std::uint32_t ext_id = (COMM_OPERATION_CONTROL << 24) | (torque_u16 << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
|
||||
{
|
||||
struct sockaddr_can addr;
|
||||
struct ifreq ifr;
|
||||
|
||||
if ((fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to create SocketCAN socket for %s", ifname.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set non-blocking mode
|
||||
int flags = ::fcntl(fd, F_GETFL, 0);
|
||||
if (flags < 0 || ::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to set socket to non-blocking for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
|
||||
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if (::bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to bind SocketCAN socket for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully bound to SocketCAN interface %s", ifname.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
struct can_frame frame;
|
||||
frame.can_id = can_id | CAN_EFF_FLAG; // Extended frame format (29-bit CAN ID)
|
||||
frame.can_dlc = dlc;
|
||||
if (data) {
|
||||
std::memcpy(frame.data, data, dlc);
|
||||
} else {
|
||||
std::memset(frame.data, 0, 8);
|
||||
}
|
||||
|
||||
ssize_t bytes_written = ::write(fd, &frame, sizeof(struct can_frame));
|
||||
if (bytes_written != sizeof(struct can_frame)) {
|
||||
int err = errno;
|
||||
// Track errors per bus for recovery logic
|
||||
if (fd == can0_fd_) {
|
||||
can0_error_count_++;
|
||||
if (can0_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN0 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can0_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can0_name_, can0_fd_, can0_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN0 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
} else if (fd == can1_fd_) {
|
||||
can1_error_count_++;
|
||||
if (can1_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can1_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN1 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// Reset error count on success
|
||||
if (fd == can0_fd_) can0_error_count_ = 0;
|
||||
else if (fd == can1_fd_) can1_error_count_ = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
auto* frame = static_cast<struct can_frame*>(frame_ptr);
|
||||
|
||||
if (timeout_us > 0) {
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = timeout_us;
|
||||
fd_set rdfs;
|
||||
FD_ZERO(&rdfs);
|
||||
FD_SET(fd, &rdfs);
|
||||
|
||||
int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv);
|
||||
if (ret < 0) {
|
||||
int err = errno;
|
||||
if (err != EINTR) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN select error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (ret == 0) {
|
||||
return false; // timeout, normal
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame));
|
||||
if (bytes_read < 0) {
|
||||
int err = errno;
|
||||
if (err != EAGAIN && err != EWOULDBLOCK) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN read error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return (bytes_read == sizeof(struct can_frame));
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count)
|
||||
{
|
||||
RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str());
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
bool success = initCan(ifname, fd);
|
||||
if (success) {
|
||||
error_count = 0;
|
||||
RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str());
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
} // namespace sim2real_hw
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_hw::HardwareBridgeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_interfaces)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/RuntimeState.msg"
|
||||
"msg/RuntimeTarget.msg"
|
||||
DEPENDENCIES builtin_interfaces std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@@ -0,0 +1,30 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] joint_pos
|
||||
float32[16] joint_vel
|
||||
float32[16] joint_torque
|
||||
|
||||
float32[3] imu_gyro
|
||||
float32[3] imu_accel
|
||||
float32[4] quat_wxyz
|
||||
float32[3] projected_gravity
|
||||
|
||||
float32 imu_age_ms
|
||||
bool imu_fresh
|
||||
|
||||
float32 odom_age_ms
|
||||
bool odom_fresh
|
||||
float32[3] odom_pos
|
||||
float32[4] odom_quat_wxyz
|
||||
float32[3] odom_linear_vel
|
||||
float32[3] odom_angular_vel
|
||||
float32[3] odom_local_pos
|
||||
float32 odom_local_yaw
|
||||
|
||||
uint32 fresh_count
|
||||
uint32 holdover_count
|
||||
uint32 stale_max
|
||||
uint32[16] update_counts
|
||||
|
||||
string source
|
||||
@@ -0,0 +1,15 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] target
|
||||
float32[16] raw_action
|
||||
float32[16] scaled_action
|
||||
float32[3] command
|
||||
float32[3] raw_command
|
||||
|
||||
bool zero_command
|
||||
bool runtime_released
|
||||
float32 release_alpha
|
||||
float32 target_age_ms
|
||||
|
||||
string target_source
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_interfaces</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS 2 interfaces for the sim2real wheel-leg runtime.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(sim2real_nav2)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
config
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,232 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
odom_frame_id: "odom"
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: -1.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
transform_tolerance: 1.0
|
||||
update_min_d: 0.25
|
||||
update_min_a: 0.2
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: "scan"
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_frame: odom
|
||||
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
|
||||
plugin_lib_names:
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_recompute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node
|
||||
- nav2_recovery_node
|
||||
- nav2_pipeline_sequence_node
|
||||
- nav2_round_robin_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 10.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.001
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
prune_plan: true
|
||||
prune_distance: 1.0
|
||||
debug_trajectory_details: false
|
||||
trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
|
||||
velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator"
|
||||
critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
|
||||
|
||||
# DWB Velocity parameters matching Units
|
||||
min_vel_x: 0.0
|
||||
max_vel_x: 0.6
|
||||
min_vel_y: 0.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 2.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.6
|
||||
min_speed_theta: 0.0
|
||||
|
||||
# DWB Acceleration parameters matching Units
|
||||
acc_lim_x: 15.0
|
||||
acc_lim_y: 15.0
|
||||
acc_lim_theta: 12.0
|
||||
decel_lim_x: -15.0
|
||||
decel_lim_y: -15.0
|
||||
decel_lim_theta: -12.0
|
||||
|
||||
# Critics tuning
|
||||
ObstacleFootprint.scale: 0.2
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: false
|
||||
planner_plugins: ["GridTransition"]
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors::Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors::Backup"
|
||||
wait:
|
||||
plugin: "nav2_behaviors::Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 1.0
|
||||
update_frequency: 1.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 30
|
||||
height: 30
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.6
|
||||
cost_scaling_factor: 4.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 5.0
|
||||
update_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 4
|
||||
height: 4
|
||||
resolution: 0.05
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.4
|
||||
cost_scaling_factor: 4.0
|
||||
@@ -0,0 +1,146 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directories
|
||||
my_share_dir = get_package_share_directory('sim2real_nav2')
|
||||
|
||||
# Declare launch configuration variables
|
||||
params_file_arg = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(my_share_dir, 'config', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes'
|
||||
)
|
||||
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
|
||||
# Define Nav2 lifecycle nodes to run
|
||||
lifecycle_nodes = ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator',
|
||||
'global_costmap', 'local_costmap', 'amcl']
|
||||
|
||||
# Controller server node
|
||||
controller_server_node = Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
name='controller_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Planner server node
|
||||
planner_server_node = Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Behavior server node (called recovery_server in Galactic, behavior_server in Humble)
|
||||
behavior_server_node = Node(
|
||||
package='nav2_behaviors',
|
||||
executable='behavior_server',
|
||||
name='behavior_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# BT Navigator node
|
||||
bt_navigator_node = Node(
|
||||
package='nav2_bt_navigator',
|
||||
executable='bt_navigator',
|
||||
name='bt_navigator',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Global costmap node
|
||||
global_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='global_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Local costmap node
|
||||
local_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='local_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# AMCL node (Adaptive Monte Carlo Localization), now receives /scan from pointcloud_to_laserscan
|
||||
amcl_node = Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
name='amcl',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# PointCloud2 to LaserScan converter (AMCL needs LaserScan, LiDAR publishes PointCloud2)
|
||||
pointcloud_to_laserscan_node = Node(
|
||||
package='pointcloud_to_laserscan',
|
||||
executable='pointcloud_to_laserscan_node',
|
||||
name='pointcloud_to_laserscan',
|
||||
output='screen',
|
||||
remappings=[
|
||||
('cloud_in', '/odin1/cloud_slam'),
|
||||
('scan', '/scan')
|
||||
],
|
||||
parameters=[{
|
||||
'target_frame': 'base_link',
|
||||
'transform_tolerance': 0.01,
|
||||
'min_height': 0.05,
|
||||
'max_height': 2.0,
|
||||
'angle_min': -3.14159,
|
||||
'angle_max': 3.14159,
|
||||
'angle_increment': 0.0087, # ~0.5 degrees
|
||||
'scan_time': 0.1,
|
||||
'range_min': 0.1,
|
||||
'range_max': 10.0,
|
||||
'use_inf': True,
|
||||
'inf_epsilon': 1.0,
|
||||
'concurrency_level': 1
|
||||
}]
|
||||
)
|
||||
|
||||
# Lifecycle manager node to transition Nav2 nodes to ACTIVE state
|
||||
lifecycle_manager_node = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'autostart': True,
|
||||
'node_names': lifecycle_nodes
|
||||
}]
|
||||
)
|
||||
|
||||
# Create launch description
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set stdout line buffering
|
||||
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'))
|
||||
|
||||
# Add actions
|
||||
ld.add_action(params_file_arg)
|
||||
ld.add_action(controller_server_node)
|
||||
ld.add_action(planner_server_node)
|
||||
ld.add_action(behavior_server_node)
|
||||
ld.add_action(bt_navigator_node)
|
||||
ld.add_action(global_costmap_node)
|
||||
ld.add_action(local_costmap_node)
|
||||
ld.add_action(amcl_node)
|
||||
ld.add_action(pointcloud_to_laserscan_node)
|
||||
ld.add_action(lifecycle_manager_node)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_nav2</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 Nav2 configuration package for legged-wheeled quadruped</description>
|
||||
<maintainer email="31560@todo.todo">USER</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>navigation2</exec_depend>
|
||||
<exec_depend>pointcloud_to_laserscan</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,99 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_runtime)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
# Search for ONNX Runtime headers and library
|
||||
find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h
|
||||
PATHS
|
||||
/usr/include
|
||||
/usr/include/onnxruntime
|
||||
/usr/local/include
|
||||
/usr/local/include/onnxruntime
|
||||
/opt/onnxruntime/include
|
||||
)
|
||||
|
||||
find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/lib/aarch64-linux-gnu
|
||||
/usr/local/lib
|
||||
/opt/onnxruntime/lib
|
||||
)
|
||||
|
||||
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||
|
||||
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()
|
||||
|
||||
add_executable(sim2real_runtime_node
|
||||
src/policy_runtime_node.cpp
|
||||
)
|
||||
|
||||
add_executable(odom_relay_node
|
||||
src/odom_relay_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
include
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
target_include_directories(odom_relay_node PRIVATE include)
|
||||
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
)
|
||||
|
||||
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)
|
||||
|
||||
ament_target_dependencies(sim2real_runtime_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
std_msgs
|
||||
tf2_ros
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
ament_target_dependencies(odom_relay_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_runtime_node odom_relay_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
PROGRAMS
|
||||
src/remote_uart_node.py
|
||||
src/cmd_mux_node.py
|
||||
src/web_udp_bridge_node.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
/// Subscribes to odin_ros_driver's odometry (e.g. /odin1/odometry),
|
||||
/// remaps child_frame_id to "base_link", republishes on /odom,
|
||||
/// and broadcasts the odom → base_link TF.
|
||||
class OdomRelayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
OdomRelayNode();
|
||||
|
||||
private:
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
std::string odom_input_topic_;
|
||||
std::string odom_output_topic_;
|
||||
std::string base_frame_;
|
||||
bool publish_tf_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
+124
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/stand_balance_controller.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
// ONNXRuntime C++ API
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
class PolicyRuntimeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PolicyRuntimeNode();
|
||||
|
||||
private:
|
||||
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 applyCmdVel(float vx, float vy, float vyaw);
|
||||
void onPolicyLoop();
|
||||
|
||||
std::array<float, 53> buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const;
|
||||
|
||||
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;
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_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::TimerBase::SharedPtr policy_timer_;
|
||||
|
||||
std::mutex mutex_;
|
||||
sim2real_interfaces::msg::RuntimeState latest_state_;
|
||||
bool has_state_{false};
|
||||
std::chrono::steady_clock::time_point last_state_recv_time_{};
|
||||
std::array<float, 3> cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 3> raw_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 16> last_actions_{};
|
||||
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_{};
|
||||
rclcpp::Time state_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double transition_time_{4.0};
|
||||
double hold_time_{1.0};
|
||||
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||
|
||||
// ONNX Runtime members
|
||||
std::string model_path_{"policies/model_rough.onnx"};
|
||||
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||
std::unique_ptr<Ort::Env> env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
|
||||
std::vector<std::string> input_names_str_;
|
||||
std::vector<std::string> output_names_str_;
|
||||
std::vector<const char*> input_names_char_;
|
||||
std::vector<const char*> output_names_char_;
|
||||
|
||||
std::vector<std::int64_t> input_shape_;
|
||||
std::vector<std::int64_t> output_shape_;
|
||||
|
||||
// Command filter and release states
|
||||
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.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 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};
|
||||
float release_active_time_{0.0f};
|
||||
float zero_cmd_lin_thresh_{0.05f};
|
||||
float zero_cmd_yaw_thresh_{0.05f};
|
||||
float zero_yaw_rate_thresh_{0.10f};
|
||||
|
||||
// E-stop and Safety variables
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_runtime</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Policy runtime node for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
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
|
||||
|
||||
|
||||
class ControlMode(str, Enum):
|
||||
DISABLED = "DISABLED"
|
||||
REMOTE = "REMOTE"
|
||||
WEB = "WEB"
|
||||
NAV = "NAV"
|
||||
|
||||
|
||||
class CmdMuxNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper()
|
||||
self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value)
|
||||
self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value)
|
||||
self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value)
|
||||
self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value)
|
||||
self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value)
|
||||
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.mode = self.parse_mode(self.default_mode)
|
||||
self.estop = False
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
|
||||
self.latest_remote = Twist()
|
||||
self.latest_web = Twist()
|
||||
self.latest_nav = Twist()
|
||||
self.remote_stamp: Optional[rclpy.time.Time] = None
|
||||
self.web_stamp: Optional[rclpy.time.Time] = None
|
||||
self.nav_stamp: Optional[rclpy.time.Time] = None
|
||||
self.last_output = Twist()
|
||||
self.last_pub_time = self.get_clock().now()
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode_state", 10)
|
||||
self.status_pub = self.create_publisher(String, "control/mux_status", 10)
|
||||
|
||||
self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10)
|
||||
self.create_subscription(String, "control/mode", self.on_mode, 10)
|
||||
self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10)
|
||||
self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10)
|
||||
self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
|
||||
period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
self.get_logger().info(f"Command mux started in mode {self.mode.value}")
|
||||
|
||||
def parse_mode(self, value: str) -> ControlMode:
|
||||
try:
|
||||
return ControlMode(value.upper())
|
||||
except ValueError:
|
||||
self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED")
|
||||
return ControlMode.DISABLED
|
||||
|
||||
def on_remote(self, msg: Twist) -> None:
|
||||
self.latest_remote = msg
|
||||
self.remote_stamp = self.get_clock().now()
|
||||
|
||||
def on_web(self, msg: Twist) -> None:
|
||||
self.latest_web = msg
|
||||
self.web_stamp = self.get_clock().now()
|
||||
|
||||
def on_nav(self, msg: Twist) -> None:
|
||||
self.latest_nav = msg
|
||||
self.nav_stamp = self.get_clock().now()
|
||||
|
||||
def on_mode(self, msg: String) -> None:
|
||||
new_mode = self.parse_mode(msg.data)
|
||||
if new_mode != self.mode:
|
||||
self.mode = new_mode
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
self.get_logger().info(f"Control mode changed to {self.mode.value}")
|
||||
|
||||
def on_remote_enabled(self, msg: Bool) -> None:
|
||||
self.remote_enabled = bool(msg.data)
|
||||
if self.remote_enabled:
|
||||
self.mode = ControlMode.REMOTE
|
||||
|
||||
def on_web_enabled(self, msg: Bool) -> None:
|
||||
self.web_enabled = bool(msg.data)
|
||||
if self.web_enabled:
|
||||
self.mode = ControlMode.WEB
|
||||
|
||||
def on_nav_enabled(self, msg: Bool) -> None:
|
||||
self.nav_enabled = bool(msg.data)
|
||||
if self.nav_enabled:
|
||||
self.mode = ControlMode.NAV
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
if self.estop:
|
||||
self.mode = ControlMode.DISABLED
|
||||
|
||||
def on_timer(self) -> None:
|
||||
now = self.get_clock().now()
|
||||
target = Twist()
|
||||
source = "zero"
|
||||
|
||||
if not self.estop:
|
||||
if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now):
|
||||
target = self.latest_remote
|
||||
source = "remote"
|
||||
elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now):
|
||||
target = self.latest_web
|
||||
source = "web"
|
||||
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"
|
||||
|
||||
target = self.limit_twist(target)
|
||||
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}"))
|
||||
|
||||
def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool:
|
||||
if stamp is None:
|
||||
return False
|
||||
age_ms = (now - stamp).nanoseconds / 1.0e6
|
||||
return age_ms <= timeout_ms
|
||||
|
||||
def limit_twist(self, msg: Twist) -> Twist:
|
||||
out = Twist()
|
||||
out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx)
|
||||
out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy)
|
||||
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) -> Twist:
|
||||
dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3)
|
||||
out = Twist()
|
||||
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
|
||||
|
||||
@staticmethod
|
||||
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()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,60 @@
|
||||
#include "sim2real_runtime/odom_relay_node.hpp"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
OdomRelayNode::OdomRelayNode()
|
||||
: Node("odom_relay_node")
|
||||
{
|
||||
odom_input_topic_ = declare_parameter<std::string>("odom_input_topic", "/odin1/odometry");
|
||||
odom_output_topic_ = declare_parameter<std::string>("odom_output_topic", "/odom");
|
||||
base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
|
||||
publish_tf_ = declare_parameter<bool>("publish_tf", true);
|
||||
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_input_topic_, 10,
|
||||
std::bind(&OdomRelayNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(odom_output_topic_, 10);
|
||||
|
||||
if (publish_tf_) {
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"Odom relay: %s -> %s (base_frame=%s, publish_tf=%s)",
|
||||
odom_input_topic_.c_str(), odom_output_topic_.c_str(),
|
||||
base_frame_.c_str(), publish_tf_ ? "true" : "false");
|
||||
}
|
||||
|
||||
void OdomRelayNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
// Remap child_frame_id and republish
|
||||
auto out_msg = *msg;
|
||||
out_msg.header.frame_id = "odom";
|
||||
out_msg.child_frame_id = base_frame_;
|
||||
odom_pub_->publish(out_msg);
|
||||
|
||||
// Broadcast TF: odom → base_link
|
||||
if (publish_tf_ && tf_broadcaster_) {
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp = msg->header.stamp;
|
||||
tf.header.frame_id = "odom";
|
||||
tf.child_frame_id = base_frame_;
|
||||
tf.transform.translation.x = msg->pose.pose.position.x;
|
||||
tf.transform.translation.y = msg->pose.pose.position.y;
|
||||
tf.transform.translation.z = msg->pose.pose.position.z;
|
||||
tf.transform.rotation = msg->pose.pose.orientation;
|
||||
tf_broadcaster_->sendTransform(tf);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::OdomRelayNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,583 @@
|
||||
#include "sim2real_runtime/policy_runtime_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
// Named constants for timing and command filtering
|
||||
constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz)
|
||||
constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz)
|
||||
constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz)
|
||||
|
||||
PolicyRuntimeNode::PolicyRuntimeNode()
|
||||
: Node("sim2real_runtime_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
model_path_ = declare_parameter<std::string>("model_path", "policies/model_rough.onnx");
|
||||
use_cuda_ = declare_parameter<bool>("use_cuda", false); // enable CUDA EP on Orin Nano
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
command_release_s_ = static_cast<float>(declare_parameter<double>("command_release_s", 0.35));
|
||||
release_command_hold_s_ = static_cast<float>(declare_parameter<double>("release_command_hold_s", 0.12));
|
||||
release_posture_max_err_ = static_cast<float>(declare_parameter<double>("release_posture_max_err", 0.35));
|
||||
release_target_blend_s_ = static_cast<float>(declare_parameter<double>("release_target_blend_s", 0.30));
|
||||
clip_obs_ = static_cast<float>(declare_parameter<double>("clip_obs", 100.0));
|
||||
hold_zero_command_pose_ = declare_parameter<bool>("hold_zero_command_pose", true);
|
||||
enable_zero_cmd_suppression_ = declare_parameter<bool>("enable_zero_cmd_suppression", true);
|
||||
require_active_command_to_release_ = declare_parameter<bool>("require_active_command_to_release", true);
|
||||
zero_cmd_use_yaw_rate_ = declare_parameter<bool>("zero_cmd_use_yaw_rate", true);
|
||||
runtime_released_ = !require_active_command_to_release_;
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str());
|
||||
|
||||
// Initialize StandBalanceController
|
||||
stand_balance_ = std::make_unique<sim2real_common::StandBalanceController>(0.02);
|
||||
|
||||
// Initialize SafetyMonitor and RuntimeGuard
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0),
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 2. Initialize Ort C++ environment
|
||||
try {
|
||||
env_ = std::make_unique<Ort::Env>(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env");
|
||||
|
||||
Ort::SessionOptions session_options;
|
||||
// single-thread ORIN optimization to prevent thread scheduling jitter
|
||||
session_options.SetIntraOpNumThreads(1);
|
||||
session_options.SetInterOpNumThreads(1);
|
||||
session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
||||
|
||||
// CUDA Execution Provider (Orin Nano GPU acceleration)
|
||||
if (use_cuda_) {
|
||||
try {
|
||||
OrtCUDAProviderOptions cuda_opts{};
|
||||
cuda_opts.device_id = 0;
|
||||
// enable_cuda_graph: false for single-inference RL policy (avoids overhead)
|
||||
session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
||||
RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)");
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_WARN(get_logger(),
|
||||
"CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.",
|
||||
e.what());
|
||||
use_cuda_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
session_ = std::make_unique<Ort::Session>(*env_, model_path_.c_str(), session_options);
|
||||
memory_info_ = std::make_unique<Ort::MemoryInfo>(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU));
|
||||
|
||||
// Get input/output nodes names and shapes
|
||||
Ort::AllocatorWithDefaultOptions allocator;
|
||||
|
||||
std::size_t num_inputs = session_->GetInputCount();
|
||||
for (std::size_t i = 0; i < num_inputs; ++i) {
|
||||
auto name = session_->GetInputNameAllocated(i, allocator);
|
||||
input_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : input_names_str_) {
|
||||
input_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
std::size_t num_outputs = session_->GetOutputCount();
|
||||
for (std::size_t i = 0; i < num_outputs; ++i) {
|
||||
auto name = session_->GetOutputNameAllocated(i, allocator);
|
||||
output_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : output_names_str_) {
|
||||
output_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
auto input_type_info = session_->GetInputTypeInfo(0);
|
||||
auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo();
|
||||
input_shape_ = input_tensor_info.GetShape();
|
||||
if (input_shape_[0] < 0) {
|
||||
input_shape_[0] = 1;
|
||||
}
|
||||
|
||||
auto output_type_info = session_->GetOutputTypeInfo(0);
|
||||
auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo();
|
||||
output_shape_ = output_tensor_info.GetShape();
|
||||
if (output_shape_[0] < 0) {
|
||||
output_shape_[0] = 1;
|
||||
}
|
||||
|
||||
// Validate output shape matches expected action dimension
|
||||
if (output_shape_.size() < 2 || output_shape_[1] != static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim)) {
|
||||
RCLCPP_FATAL(get_logger(),
|
||||
"ONNX model output dimension mismatch! Expected %ld, got %ld. Wrong model?",
|
||||
static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim),
|
||||
output_shape_.size() >= 2 ? output_shape_[1] : -1);
|
||||
throw std::runtime_error("ONNX model output shape mismatch");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]",
|
||||
input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what());
|
||||
throw;
|
||||
}
|
||||
|
||||
// 3. Create publishers and subscriptions
|
||||
target_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeTarget>("runtime/target", 10);
|
||||
state_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeState>(
|
||||
"runtime/state", 10,
|
||||
std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1));
|
||||
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1));
|
||||
cmd_stamped_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
|
||||
"cmd_vel_stamped", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// 4. Timer at 50Hz (20ms)
|
||||
policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this));
|
||||
|
||||
last_actions_.fill(0.0f);
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
latest_state_ = *msg;
|
||||
has_state_ = true;
|
||||
last_state_recv_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw)
|
||||
{
|
||||
// Velocity saturation limits (consistent with training domain)
|
||||
constexpr float kMaxLinVelX = 0.8f; // m/s
|
||||
constexpr float kMaxLinVelY = 0.3f; // m/s
|
||||
constexpr float kMaxAngVelZ = 0.5f; // rad/s
|
||||
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX);
|
||||
raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY);
|
||||
raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ);
|
||||
cmd_ = raw_cmd_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->linear.x),
|
||||
static_cast<float>(msg->linear.y),
|
||||
static_cast<float>(msg->angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->twist.linear.x),
|
||||
static_cast<float>(msg->twist.linear.y),
|
||||
static_cast<float>(msg->twist.angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 53> PolicyRuntimeNode::buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const
|
||||
{
|
||||
std::array<float, 53> obs{};
|
||||
std::size_t cursor = 0;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.imu_gyro[i] * 0.25f;
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.projected_gravity[i];
|
||||
}
|
||||
for (float v : cmd) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_pos[i] - sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (float v : last_actions) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
|
||||
// Clip observations values to ±clip_obs_
|
||||
if (clip_obs_ > 0.0f) {
|
||||
for (float & v : obs) {
|
||||
v = std::clamp(v, -clip_obs_, clip_obs_);
|
||||
}
|
||||
}
|
||||
|
||||
return obs;
|
||||
}
|
||||
|
||||
std::array<float, 16> PolicyRuntimeNode::runPolicy(const std::array<float, 53> & obs)
|
||||
{
|
||||
std::array<float, 16> action{};
|
||||
try {
|
||||
auto input_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
const_cast<float*>(obs.data()),
|
||||
obs.size(),
|
||||
input_shape_.data(),
|
||||
input_shape_.size()
|
||||
);
|
||||
|
||||
auto output_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
action.data(),
|
||||
action.size(),
|
||||
output_shape_.data(),
|
||||
output_shape_.size()
|
||||
);
|
||||
|
||||
session_->Run(
|
||||
Ort::RunOptions{nullptr},
|
||||
input_names_char_.data(),
|
||||
&input_tensor,
|
||||
1,
|
||||
output_names_char_.data(),
|
||||
&output_tensor,
|
||||
1
|
||||
);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what());
|
||||
action.fill(0.0f);
|
||||
}
|
||||
|
||||
for (float& v : action) {
|
||||
v = std::clamp(v, -10.0f, 10.0f);
|
||||
}
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
const bool cmd_is_zero = planar_cmd < zero_cmd_lin_thresh_ && std::abs(cmd[2]) < zero_cmd_yaw_thresh_;
|
||||
if (!zero_cmd_use_yaw_rate_) {
|
||||
return cmd_is_zero;
|
||||
}
|
||||
return cmd_is_zero && std::abs(imu_gyro[2]) < zero_yaw_rate_thresh_;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isCommandActive(const std::array<float, 3> & cmd) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onPolicyLoop()
|
||||
{
|
||||
sim2real_interfaces::msg::RuntimeState state;
|
||||
std::array<float, 3> cmd{};
|
||||
std::array<float, 3> raw_cmd{};
|
||||
std::array<float, 16> last_actions{};
|
||||
bool estop_active = false;
|
||||
bool safety_active = false;
|
||||
double state_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!has_state_) {
|
||||
return;
|
||||
}
|
||||
state = latest_state_;
|
||||
cmd = cmd_;
|
||||
raw_cmd = raw_cmd_;
|
||||
last_actions = last_actions_;
|
||||
estop_active = estop_triggered_;
|
||||
safety_active = safety_triggered_;
|
||||
if (last_state_recv_time_.time_since_epoch().count() != 0) {
|
||||
state_age_ms = std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - last_state_recv_time_).count();
|
||||
}
|
||||
}
|
||||
|
||||
// 1) Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_active) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(48);
|
||||
for (float v : state.joint_pos) extra_vals.push_back(v);
|
||||
for (float v : state.joint_vel) extra_vals.push_back(v);
|
||||
for (float v : last_actions) extra_vals.push_back(v);
|
||||
|
||||
const float effective_imu_age_ms = static_cast<float>(std::max(
|
||||
static_cast<double>(state.imu_age_ms), state_age_ms));
|
||||
|
||||
auto guard_decision = runtime_guard_->check(
|
||||
state.imu_gyro, state.projected_gravity, effective_imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_active = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
if (safety_active) {
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.command = cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
target.target_age_ms = 0.0f;
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.command = cmd;
|
||||
|
||||
const auto now_time = rclcpp::Time(target.stamp);
|
||||
|
||||
if (startup_state_ == StartupState::BOOT_HOLD) {
|
||||
// 1. Initial State Read
|
||||
start_pose_ = state.joint_pos;
|
||||
start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0
|
||||
|
||||
// 2. Shortest periodic delta to stand pose
|
||||
float max_dev = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
startup_delta_[i] = delta;
|
||||
max_dev = std::max(max_dev, std::abs(delta));
|
||||
}
|
||||
startup_delta_[12] = startup_delta_[13] = startup_delta_[14] = startup_delta_[15] = 0.0f;
|
||||
|
||||
if (max_dev > 3.0f) {
|
||||
RCLCPP_WARN(get_logger(), "Measured joint dev too large (%f rad > 3.0 rad). Aborting standup transition.", max_dev);
|
||||
target.target = start_pose_;
|
||||
target.target_source = "boot_hold";
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
// Adapt transition time: min 2s, max 6s, 1.5s per rad
|
||||
transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0);
|
||||
startup_state_ = StartupState::STARTUP_SOFT_HOLD;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_);
|
||||
}
|
||||
|
||||
if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
target.target = start_pose_;
|
||||
target.target_source = "startup_soft_hold";
|
||||
|
||||
if (elapsed >= 1.0) { // 1s soft hold
|
||||
startup_state_ = StartupState::STARTUP_TRANSITION;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Transitioning to stand pose...");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_TRANSITION) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
double phase = std::min(1.0, elapsed / transition_time_);
|
||||
|
||||
// Cosine blend interpolation
|
||||
double blend = 0.5 - 0.5 * std::cos(M_PI * phase);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
target.target[i] = start_pose_[i] + blend * startup_delta_[i];
|
||||
}
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (phase >= 1.0) {
|
||||
// Settle check
|
||||
float max_pos_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
max_pos_err = std::max(max_pos_err, std::abs(delta));
|
||||
}
|
||||
float max_vel_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i]));
|
||||
}
|
||||
|
||||
if (max_pos_err <= 0.30f && max_vel_err <= 0.6f) {
|
||||
startup_state_ = StartupState::STARTUP_HOLD_AFTER;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Pose settled. Holding for 1.0s...");
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
|
||||
// Run stand balance controller during holding phase
|
||||
target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd);
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (elapsed >= 1.0 && stand_balance_->isStable()) {
|
||||
startup_state_ = StartupState::RUNTIME;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::RUNTIME) {
|
||||
// Python template uses the command directly in policy obs/release logic.
|
||||
// Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here.
|
||||
filtered_cmd_ = cmd;
|
||||
|
||||
const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array<float, 3>{0.0f, 0.0f, 0.0f});
|
||||
const bool zero_command = isZeroCommand(cmd, state.imu_gyro);
|
||||
|
||||
if (!runtime_released_) {
|
||||
if (require_active_command_to_release_) {
|
||||
if (isCommandActive(cmd)) {
|
||||
release_active_time_ += kPolicyDt;
|
||||
} else {
|
||||
release_active_time_ = 0.0f;
|
||||
}
|
||||
|
||||
float max_hold_err = 0.0f;
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i]));
|
||||
}
|
||||
|
||||
const bool active_ready = release_active_time_ >= release_command_hold_s_;
|
||||
const bool posture_ready = max_hold_err <= release_posture_max_err_;
|
||||
if (active_ready && posture_ready) {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
} else {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!runtime_released_ || zero_command) {
|
||||
release_alpha_ = 0.0f;
|
||||
target.runtime_released = false;
|
||||
target.release_alpha = 0.0f;
|
||||
target.zero_command = zero_command;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
last_actions.fill(0.0f);
|
||||
target.target_source = "runtime_zero_hold";
|
||||
target.target = target_hold;
|
||||
if (!runtime_released_) {
|
||||
target.target_source = "runtime_hold";
|
||||
}
|
||||
} else {
|
||||
release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f));
|
||||
target.runtime_released = (release_alpha_ >= 1.0f);
|
||||
target.release_alpha = release_alpha_;
|
||||
target.zero_command = false;
|
||||
target.command = cmd;
|
||||
|
||||
auto raw = runPolicy(buildObservation(state, cmd, last_actions));
|
||||
for (float & v : raw) {
|
||||
v *= release_alpha_;
|
||||
}
|
||||
target.raw_action = raw;
|
||||
|
||||
const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt)));
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i];
|
||||
const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target;
|
||||
last_actions[i] = raw[i];
|
||||
}
|
||||
target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy";
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Run SafetyMonitor check on computed target
|
||||
if (safety_enabled_) {
|
||||
auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
|
||||
// Override target to safety_brake damping pose
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target.target = safety_decision.clipped_target;
|
||||
target.target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime.");
|
||||
}
|
||||
}
|
||||
|
||||
target.target_age_ms = 0.0f;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
last_actions_ = last_actions;
|
||||
}
|
||||
|
||||
target_pub_->publish(target);
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::PolicyRuntimeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,250 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
|
||||
import serial
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
SBUS_AXIS_SCALE = 660.0
|
||||
|
||||
SWITCH_LOW = -1
|
||||
SWITCH_MID = 0
|
||||
SWITCH_HIGH = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch7: int = SWITCH_MID
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteControlState:
|
||||
ch1: int = 0
|
||||
ch2: int = 0
|
||||
ch3: int = 0
|
||||
ch4: int = 0
|
||||
switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
|
||||
frame_ok: bool = False
|
||||
|
||||
@property
|
||||
def estop_requested(self) -> bool:
|
||||
return self.switches.ch7 == SWITCH_HIGH
|
||||
|
||||
|
||||
class SbusUartReceiver:
|
||||
def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int):
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.timeout = float(timeout)
|
||||
self.axis_deadzone = int(axis_deadzone)
|
||||
self.serial: Optional[serial.Serial] = None
|
||||
self.buffer = bytearray()
|
||||
self.state = RemoteControlState()
|
||||
|
||||
def open(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
return
|
||||
self.serial = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_EVEN,
|
||||
stopbits=serial.STOPBITS_TWO,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
self.serial.close()
|
||||
|
||||
def poll(self) -> RemoteControlState:
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise RuntimeError("remote uart is not open")
|
||||
|
||||
waiting = self.serial.in_waiting
|
||||
if waiting:
|
||||
self.buffer.extend(self.serial.read(waiting))
|
||||
|
||||
while len(self.buffer) >= SBUS_FRAME_SIZE:
|
||||
start_idx = self.buffer.find(0x0F)
|
||||
if start_idx < 0:
|
||||
self.buffer.clear()
|
||||
break
|
||||
if start_idx > 0:
|
||||
del self.buffer[:start_idx]
|
||||
if len(self.buffer) < SBUS_FRAME_SIZE:
|
||||
break
|
||||
frame = bytes(self.buffer[:SBUS_FRAME_SIZE])
|
||||
del self.buffer[:SBUS_FRAME_SIZE]
|
||||
parsed = self._parse_frame(frame)
|
||||
if parsed is not None:
|
||||
self.state = parsed
|
||||
return self.state
|
||||
|
||||
def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
|
||||
if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
|
||||
return None
|
||||
|
||||
channels = [0] * 16
|
||||
channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
|
||||
channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
|
||||
channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
|
||||
channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
|
||||
channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
|
||||
channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
|
||||
channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
|
||||
channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
|
||||
channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
|
||||
channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
|
||||
|
||||
if channels[0] < 100:
|
||||
return None
|
||||
|
||||
state = RemoteControlState(
|
||||
ch1=self._normalize_axis(channels[0]),
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
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)):
|
||||
return None
|
||||
return state
|
||||
|
||||
def _normalize_axis(self, value: int) -> int:
|
||||
mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
|
||||
return 0 if abs(mapped) <= self.axis_deadzone else mapped
|
||||
|
||||
@staticmethod
|
||||
def _decode_switch(value: int) -> int:
|
||||
if value < 500:
|
||||
return SWITCH_LOW
|
||||
if value > 1500:
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
|
||||
class RemoteUartNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("remote_enabled", True).value)
|
||||
self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value)
|
||||
self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value)
|
||||
self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value)
|
||||
self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value)
|
||||
self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value)
|
||||
self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0)
|
||||
self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value)
|
||||
self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value)
|
||||
self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value)
|
||||
self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value)
|
||||
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.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.receiver: Optional[SbusUartReceiver] = None
|
||||
self.estop_published = False
|
||||
self.open_error_logged = False
|
||||
|
||||
if self.enabled:
|
||||
self.receiver = SbusUartReceiver(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
axis_deadzone=self.axis_deadzone,
|
||||
)
|
||||
try:
|
||||
self.receiver.open()
|
||||
self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud")
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}")
|
||||
self.open_error_logged = True
|
||||
else:
|
||||
self.get_logger().warn("Remote UART node is disabled by parameter")
|
||||
|
||||
period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
if self.receiver is not None:
|
||||
self.receiver.close()
|
||||
return super().destroy_node()
|
||||
|
||||
def on_timer(self) -> None:
|
||||
if not self.enabled or self.receiver is None:
|
||||
return
|
||||
|
||||
try:
|
||||
if not self.receiver.serial or not self.receiver.serial.is_open:
|
||||
self.receiver.open()
|
||||
state = self.receiver.poll()
|
||||
except Exception as exc:
|
||||
if not self.open_error_logged:
|
||||
self.get_logger().error(f"Remote UART poll failed: {exc}")
|
||||
self.open_error_logged = True
|
||||
return
|
||||
|
||||
self.open_error_logged = False
|
||||
|
||||
if state.estop_requested:
|
||||
if not self.estop_published or not self.estop_latch:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.get_logger().warn("Remote E-stop requested by CH7 high")
|
||||
self.estop_published = True
|
||||
self.publish_zero_cmd()
|
||||
return
|
||||
|
||||
if not self.estop_latch and self.estop_published:
|
||||
self.estop_pub.publish(Bool(data=False))
|
||||
self.estop_published = False
|
||||
|
||||
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()
|
||||
cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
|
||||
cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
|
||||
cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
|
||||
self.cmd_pub.publish(cmd)
|
||||
|
||||
def publish_zero_cmd(self) -> None:
|
||||
self.cmd_pub.publish(Twist())
|
||||
|
||||
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
|
||||
if invert:
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RemoteUartNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import socket
|
||||
from typing import Any, Optional
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from sim2real_interfaces.msg import RuntimeState, RuntimeTarget
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
|
||||
class WebUdpBridgeNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value)
|
||||
self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value)
|
||||
self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value)
|
||||
self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value)
|
||||
self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value)
|
||||
self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value)
|
||||
self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value)
|
||||
self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value)
|
||||
self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value)
|
||||
self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value)
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.sock.setblocking(False)
|
||||
self.sock.bind((self.listen_host, self.listen_port))
|
||||
|
||||
self.client_addr: Optional[tuple[str, int]] = None
|
||||
if self.remote_host:
|
||||
self.client_addr = (self.remote_host, self.remote_port)
|
||||
|
||||
self.latest_target: Optional[RuntimeTarget] = None
|
||||
self.latest_state: Optional[RuntimeState] = None
|
||||
self.latest_cmd = Twist()
|
||||
self.latest_mode = "UNKNOWN"
|
||||
self.latest_mux_status = ""
|
||||
self.estop = False
|
||||
self.web_enabled = False
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10)
|
||||
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||
|
||||
self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10)
|
||||
self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10)
|
||||
self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
|
||||
|
||||
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||
|
||||
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||
|
||||
def on_target(self, msg: RuntimeTarget) -> None:
|
||||
self.latest_target = msg
|
||||
|
||||
def on_state(self, msg: RuntimeState) -> None:
|
||||
self.latest_state = msg
|
||||
|
||||
def on_cmd_vel(self, msg: Twist) -> None:
|
||||
self.latest_cmd = msg
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
|
||||
def on_mode_state(self, msg: String) -> None:
|
||||
self.latest_mode = msg.data
|
||||
|
||||
def on_mux_status(self, msg: String) -> None:
|
||||
self.latest_mux_status = msg.data
|
||||
|
||||
def on_rx_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
while True:
|
||||
try:
|
||||
data, addr = self.sock.recvfrom(self.max_packet_bytes)
|
||||
except BlockingIOError:
|
||||
break
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP receive failed: {exc}")
|
||||
break
|
||||
self.client_addr = addr
|
||||
try:
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
self.handle_packet(payload)
|
||||
except Exception as exc:
|
||||
self.send_packet({"type": "error", "message": str(exc)})
|
||||
|
||||
def handle_packet(self, payload: dict[str, Any]) -> None:
|
||||
msg_type = str(payload.get("type", "")).lower()
|
||||
if msg_type == "cmd_vel":
|
||||
cmd = self.parse_twist(payload)
|
||||
self.cmd_pub.publish(cmd)
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
elif msg_type == "zero":
|
||||
self.cmd_pub.publish(Twist())
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
elif msg_type == "estop":
|
||||
self.estop_pub.publish(Bool(data=bool(payload.get("data", True))))
|
||||
elif msg_type == "mode":
|
||||
mode = str(payload.get("mode", "DISABLED")).upper()
|
||||
self.mode_pub.publish(String(data=mode))
|
||||
self.web_enabled = mode == "WEB"
|
||||
self.web_enabled_pub.publish(Bool(data=mode == "WEB"))
|
||||
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
|
||||
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
|
||||
elif msg_type == "web_enable":
|
||||
self.web_enabled = bool(payload.get("data", False))
|
||||
self.web_enabled_pub.publish(Bool(data=self.web_enabled))
|
||||
if self.web_enabled:
|
||||
self.mode_pub.publish(String(data="WEB"))
|
||||
elif msg_type == "remote_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.remote_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="REMOTE"))
|
||||
elif msg_type == "nav_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.nav_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="NAV"))
|
||||
elif msg_type == "ping":
|
||||
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||
else:
|
||||
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||
|
||||
def parse_twist(self, payload: dict[str, Any]) -> Twist:
|
||||
cmd = Twist()
|
||||
linear = payload.get("linear", {}) or {}
|
||||
angular = payload.get("angular", {}) or {}
|
||||
cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx)
|
||||
cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy)
|
||||
cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw)
|
||||
return cmd
|
||||
|
||||
def on_guard_timer(self) -> None:
|
||||
age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6
|
||||
if age_ms > self.cmd_timeout_ms:
|
||||
self.cmd_pub.publish(Twist())
|
||||
if self.estop_on_timeout and not self.timeout_estop_sent:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.timeout_estop_sent = True
|
||||
|
||||
def on_state_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
self.send_packet(self.build_state_packet())
|
||||
|
||||
def build_state_packet(self) -> dict[str, Any]:
|
||||
target = self.latest_target
|
||||
state = self.latest_state
|
||||
packet: dict[str, Any] = {
|
||||
"type": "state",
|
||||
"stamp": self.now_sec(),
|
||||
"mode": self.latest_mode,
|
||||
"mux_status": self.latest_mux_status,
|
||||
"estop": self.estop,
|
||||
"web_enabled": self.web_enabled,
|
||||
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||
"runtime": {},
|
||||
"robot": {},
|
||||
}
|
||||
if target is not None:
|
||||
packet["runtime"] = {
|
||||
"target_source": str(target.target_source),
|
||||
"zero_command": bool(target.zero_command),
|
||||
"runtime_released": bool(target.runtime_released),
|
||||
"release_alpha": self._f(target.release_alpha),
|
||||
"command": [self._f(v) for v in target.command],
|
||||
"raw_command": [self._f(v) for v in target.raw_command],
|
||||
}
|
||||
if state is not None:
|
||||
packet["robot"] = {
|
||||
"joint_pos": [self._f(v) for v in state.joint_pos],
|
||||
"joint_vel": [self._f(v) for v in state.joint_vel],
|
||||
"joint_torque": [self._f(v) for v in state.joint_torque],
|
||||
"imu_gyro": [self._f(v) for v in state.imu_gyro],
|
||||
"imu_accel": [self._f(v) for v in state.imu_accel],
|
||||
"projected_gravity": [self._f(v) for v in state.projected_gravity],
|
||||
"quat_wxyz": [self._f(v) for v in state.quat_wxyz],
|
||||
"imu_age_ms": self._f(state.imu_age_ms),
|
||||
"imu_fresh": bool(state.imu_fresh),
|
||||
"odom_age_ms": self._f(state.odom_age_ms),
|
||||
"odom_fresh": bool(state.odom_fresh),
|
||||
"odom_local_pos": [self._f(v) for v in state.odom_local_pos],
|
||||
"odom_local_yaw": self._f(state.odom_local_yaw),
|
||||
"fresh_count": int(state.fresh_count),
|
||||
"holdover_count": int(state.holdover_count),
|
||||
"stale_max": int(state.stale_max),
|
||||
"update_counts": [int(v) for v in state.update_counts],
|
||||
}
|
||||
return packet
|
||||
|
||||
def send_packet(self, payload: dict[str, Any]) -> None:
|
||||
if self.client_addr is None:
|
||||
return
|
||||
try:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
self.sock.sendto(data, self.client_addr)
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP send failed: {exc}")
|
||||
|
||||
@staticmethod
|
||||
def _f(v: Any) -> float:
|
||||
try:
|
||||
f = float(v)
|
||||
if f != f:
|
||||
return 0.0
|
||||
return round(f, 6)
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
|
||||
def now_sec(self) -> float:
|
||||
return self.get_clock().now().nanoseconds / 1.0e9
|
||||
|
||||
@staticmethod
|
||||
def twist_to_dict(msg: Twist) -> dict[str, Any]:
|
||||
return {
|
||||
"linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z},
|
||||
"angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z},
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, value))
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = WebUdpBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Exit immediately if a command exits with a non-zero status
|
||||
set -e
|
||||
|
||||
# Define color codes for pretty output
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
RED='\033[0;31m'
|
||||
NC='\033[0m' # No Color
|
||||
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
echo -e "${GREEN} Starting Sim2Real Locomotion ROS2 Stack ${NC}"
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
|
||||
# 1. Source ROS2 Humble environment
|
||||
if [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
echo -e "[System] Sourcing ROS2 Humble..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] ROS2 Humble not found. Please install ROS2 Humble first.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# 2. Check if local workspace is compiled and source it
|
||||
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||
cd "$SCRIPT_DIR"
|
||||
|
||||
if [ -f "install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing local workspace..."
|
||||
source install/setup.bash
|
||||
elif [ -f "../../install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing parent install/setup.bash..."
|
||||
source ../../install/setup.bash
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] install/setup.bash not found. Attempting to build the workspace first...${NC}"
|
||||
if command -v colcon &> /dev/null; then
|
||||
echo -e "[Build] Running colcon build..."
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
source install/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] 'colcon' tool not found. Please compile the workspace manually before running.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
# 3. Check for SocketCAN interfaces (in non-dry-run mode)
|
||||
# Reading dry_run parameter from yaml config
|
||||
if [ -f "src/sim2real_bringup/config/runtime.yaml" ]; then
|
||||
# Use sed for portability (busybox-compatible, avoids GNU grep -oP dependency)
|
||||
DRY_RUN=$(sed -n 's/^[[:space:]]*dry_run:[[:space:]]*//p' src/sim2real_bringup/config/runtime.yaml | head -n 1 || echo "true")
|
||||
# Trim trailing whitespace/newlines
|
||||
DRY_RUN=$(echo "$DRY_RUN" | tr -d '[:space:]')
|
||||
else
|
||||
DRY_RUN="true"
|
||||
fi
|
||||
|
||||
if [ "$DRY_RUN" = "false" ]; then
|
||||
echo -e "[Network] Checking CAN interfaces..."
|
||||
if ip link show can0 &> /dev/null && ip link show can1 &> /dev/null; then
|
||||
echo -e "[Network] can0 and can1 interfaces detected."
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] CAN interfaces (can0/can1) not fully active.${NC}"
|
||||
echo -e "To configure CAN interfaces, run:"
|
||||
echo -e " sudo ip link set can0 up type can bitrate 1000000"
|
||||
echo -e " sudo ip link set can1 up type can bitrate 1000000"
|
||||
fi
|
||||
else
|
||||
echo -e "${YELLOW}[Dry-Run] Running in Dry-Run mode. SocketCAN will not be accessed.${NC}"
|
||||
fi
|
||||
|
||||
# 4. Run the ROS2 Launch file
|
||||
echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}"
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py "$@"
|
||||
|
||||
@@ -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 @@
|
||||
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||
|
||||
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||
|
||||
---
|
||||
|
||||
## 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` 目录中（包含 `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 架构说明
|
||||
|
||||
## 设计目标
|
||||
|
||||
- 保留已验证的 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,80 @@
|
||||
# 迁移计划
|
||||
|
||||
## 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 系统集成 ✅ 已完成
|
||||
|
||||
参考的源项目：
|
||||
|
||||
- `00_ reference/odin_ros_driver`
|
||||
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||
- `00_ reference/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` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||
@@ -0,0 +1,495 @@
|
||||
# sim2real_ros2 遥控器调用说明
|
||||
|
||||
本文档说明如何在 `sim2real_ros2` 中调用已接入的 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 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,9 @@
|
||||
# `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` |
|
||||
BIN
Binary file not shown.
@@ -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,5 @@
|
||||
recorddata/
|
||||
/config/calib.yaml
|
||||
!/config/calib.yaml
|
||||
/log
|
||||
/map
|
||||
@@ -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,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright 2025 Manifold Tech Ltd.
|
||||
|
||||
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.
|
||||
@@ -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,642 @@
|
||||
# Relocalization Guide / 重定位使用指南
|
||||
|
||||
This guide explains how to use the relocalization feature in Odin ROS Driver, including automatic relocalization and init position relocalization modes.
|
||||
|
||||
本指南介绍如何使用 Odin ROS Driver 的重定位功能，包括自动重定位和指定初始位置重定位两种模式。
|
||||
|
||||
---
|
||||
|
||||
## Table of Contents / 目录
|
||||
|
||||
1. [Overview / 概述](#overview--概述)
|
||||
2. [Prerequisites / 前提条件](#prerequisites--前提条件)
|
||||
3. [Mode 1: Auto Relocalization / 自动重定位](#mode-1-auto-relocalization--自动重定位)
|
||||
4. [Mode 2: Init Position Relocalization / 指定初始位置重定位](#mode-2-init-position-relocalization--指定初始位置重定位)
|
||||
5. [init_pos Format / init_pos 格式说明](#init_pos-format--init_pos-格式说明)
|
||||
6. [Configuration Examples / 配置示例](#configuration-examples--配置示例)
|
||||
7. [Programmatic API / 编程接口](#programmatic-api--编程接口)
|
||||
8. [Troubleshooting / 故障排除](#troubleshooting--故障排除)
|
||||
|
||||
---
|
||||
|
||||
## Overview / 概述
|
||||
|
||||
### English
|
||||
|
||||
Relocalization mode (`custom_map_mode: 2`) allows Odin to localize itself within a pre-built map. There are two approaches:
|
||||
|
||||
| Mode | Description | Use Case |
|
||||
|------|-------------|----------|
|
||||
| **Auto Relocalization** | Algorithm automatically searches for position in the map | Starting position is unknown or within recommended range |
|
||||
| **Init Position Relocalization** | User provides an initial pose estimate via `init_pos` | Starting position is known, faster convergence needed |
|
||||
|
||||
### 中文
|
||||
|
||||
重定位模式（`custom_map_mode: 2`）允许 Odin 在预先构建的地图中进行自我定位。有两种方式：
|
||||
|
||||
| 模式 | 描述 | 适用场景 |
|
||||
|------|------|----------|
|
||||
| **自动重定位** | 算法自动在地图中搜索位置 | 起始位置未知，或在推荐范围内 |
|
||||
| **指定初始位置重定位** | 用户通过 `init_pos` 提供初始位姿估计 | 起始位置已知，需要更快收敛 |
|
||||
|
||||
---
|
||||
|
||||
## Prerequisites / 前提条件
|
||||
|
||||
### English
|
||||
|
||||
1. **Pre-built map file**: A `.bin` map file created in SLAM mode (`custom_map_mode: 1`)
|
||||
2. **Map file path**: Know the absolute path to your map file
|
||||
3. **Starting position**: For init position mode, know the approximate starting pose in map coordinates
|
||||
|
||||
### 中文
|
||||
|
||||
1. **预构建的地图文件**：在 SLAM 模式（`custom_map_mode: 1`）下创建的 `.bin` 地图文件
|
||||
2. **地图文件路径**：知道地图文件的绝对路径
|
||||
3. **起始位置**：对于指定初始位置模式，需要知道在地图坐标系中的大致起始位姿
|
||||
|
||||
---
|
||||
|
||||
## Mode 1: Auto Relocalization / 自动重定位
|
||||
|
||||
### English
|
||||
|
||||
In auto relocalization mode, the algorithm automatically searches for the device's position within the map based on current sensor observations.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos is NOT set or uses default [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**Recommended Starting Conditions**:
|
||||
- Within **1 meter** of a position on the original SLAM trajectory
|
||||
- Within **±10 degrees** of the original orientation
|
||||
- In a visually distinctive area of the map
|
||||
|
||||
**Behavior**:
|
||||
1. On startup, Odin attempts to match current observations with the map
|
||||
2. If successful, TF between `map` and `odom` frames is published
|
||||
3. If unsuccessful, system operates in fallback SLAM mode (map saving disabled)
|
||||
4. Relocalization attempts continue in background until successful
|
||||
|
||||
**Tips**:
|
||||
- Gently shaking or moving the device after startup can improve relocalization accuracy
|
||||
- Highly distinctive scenes may allow successful matching beyond the 1m/10° range
|
||||
|
||||
### 中文
|
||||
|
||||
在自动重定位模式下，算法根据当前传感器观测自动在地图中搜索设备位置。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos 不设置或使用默认值 [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**推荐起始条件**：
|
||||
- 距离原始 SLAM 轨迹上某点 **1 米**以内
|
||||
- 朝向与原始方向偏差在 **±10 度**以内
|
||||
- 位于地图中视觉特征明显的区域
|
||||
|
||||
**行为**：
|
||||
1. 启动时，Odin 尝试将当前观测与地图匹配
|
||||
2. 如果成功，发布 `map` 和 `odom` 坐标系之间的 TF
|
||||
3. 如果失败，系统进入后备 SLAM 模式（地图保存功能禁用）
|
||||
4. 后台持续尝试重定位直到成功
|
||||
|
||||
**提示**：
|
||||
- 启动后轻轻晃动或移动设备可以提高重定位精度
|
||||
- 在特征明显的场景中，可能在超出 1m/10° 范围时也能成功匹配
|
||||
|
||||
---
|
||||
|
||||
## Mode 2: Init Position Relocalization / 指定初始位置重定位
|
||||
|
||||
### English
|
||||
|
||||
In init position relocalization mode, you provide an initial pose estimate to help the algorithm converge faster.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**When to Use**:
|
||||
- You know the approximate starting position (e.g., from external localization system)
|
||||
- Starting position is far from the recommended 1m/10° range
|
||||
- You need faster relocalization convergence
|
||||
- Deploying in a fixed docking station with known pose
|
||||
|
||||
**Behavior**:
|
||||
1. Algorithm uses provided `init_pos` as initial pose estimate
|
||||
2. Searches for matches in the vicinity of the provided position
|
||||
3. Faster convergence compared to auto mode when estimate is accurate
|
||||
|
||||
### 中文
|
||||
|
||||
在指定初始位置重定位模式下，您提供初始位姿估计以帮助算法更快收敛。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**适用场景**：
|
||||
- 您知道大致的起始位置（例如，来自外部定位系统）
|
||||
- 起始位置远离推荐的 1m/10° 范围
|
||||
- 需要更快的重定位收敛速度
|
||||
- 部署在已知位姿的固定充电桩
|
||||
|
||||
**行为**：
|
||||
1. 算法使用提供的 `init_pos` 作为初始位姿估计
|
||||
2. 在提供位置的附近搜索匹配
|
||||
3. 当估计准确时，比自动模式收敛更快
|
||||
|
||||
---
|
||||
|
||||
## init_pos Format / init_pos 格式说明
|
||||
|
||||
### English
|
||||
|
||||
`init_pos` is an array of **7 float values** representing position and orientation:
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| Index | Parameter | Description | Unit |
|
||||
|-------|-----------|-------------|------|
|
||||
| 0 | x | X position in map frame | meters |
|
||||
| 1 | y | Y position in map frame | meters |
|
||||
| 2 | z | Z position in map frame | meters |
|
||||
| 3 | qx | Quaternion X component | - |
|
||||
| 4 | qy | Quaternion Y component | - |
|
||||
| 5 | qz | Quaternion Z component | - |
|
||||
| 6 | qw | Quaternion W component | - |
|
||||
|
||||
**Important Notes**:
|
||||
- The quaternion must be normalized: `sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- Coordinates are relative to the **map frame** (world frame at SLAM start)
|
||||
- Default value `[0, 0, 0, 0, 0, 0, 1]` represents origin with no rotation
|
||||
|
||||
**Common Quaternion Values**:
|
||||
|
||||
| Orientation | qx | qy | qz | qw |
|
||||
|-------------|----|----|----|----|
|
||||
| No rotation (identity) | 0 | 0 | 0 | 1 |
|
||||
| 90° around Z-axis | 0 | 0 | 0.707 | 0.707 |
|
||||
| 180° around Z-axis | 0 | 0 | 1 | 0 |
|
||||
| -90° around Z-axis | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
### 中文
|
||||
|
||||
`init_pos` 是一个包含 **7 个 float 值**的数组，表示位置和朝向：
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| 索引 | 参数 | 描述 | 单位 |
|
||||
|------|------|------|------|
|
||||
| 0 | x | 地图坐标系中的 X 位置 | 米 |
|
||||
| 1 | y | 地图坐标系中的 Y 位置 | 米 |
|
||||
| 2 | z | 地图坐标系中的 Z 位置 | 米 |
|
||||
| 3 | qx | 四元数 X 分量 | - |
|
||||
| 4 | qy | 四元数 Y 分量 | - |
|
||||
| 5 | qz | 四元数 Z 分量 | - |
|
||||
| 6 | qw | 四元数 W 分量 | - |
|
||||
|
||||
**重要说明**：
|
||||
- 四元数必须归一化：`sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- 坐标相对于**地图坐标系**（SLAM 启动时的世界坐标系）
|
||||
- 默认值 `[0, 0, 0, 0, 0, 0, 1]` 表示原点且无旋转
|
||||
|
||||
**常用四元数值**：
|
||||
|
||||
| 朝向 | qx | qy | qz | qw |
|
||||
|------|----|----|----|----|
|
||||
| 无旋转（单位四元数） | 0 | 0 | 0 | 1 |
|
||||
| 绕 Z 轴旋转 90° | 0 | 0 | 0.707 | 0.707 |
|
||||
| 绕 Z 轴旋转 180° | 0 | 0 | 1 | 0 |
|
||||
| 绕 Z 轴旋转 -90° | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
---
|
||||
|
||||
## Configuration Examples / 配置示例
|
||||
|
||||
### Example 1: Auto Relocalization / 自动重定位示例
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
```
|
||||
|
||||
### Example 2: Init Position at Origin / 在原点指定初始位置
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
```
|
||||
|
||||
### Example 3: Init Position with Offset / 带偏移的初始位置
|
||||
|
||||
Position at (5.2, -3.1, 0) with 90° rotation around Z-axis:
|
||||
|
||||
位置在 (5.2, -3.1, 0)，绕 Z 轴旋转 90°：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/warehouse_map.bin"
|
||||
custom_init_pos: [5.2, -3.1, 0.0, 0.0, 0.0, 0.707, 0.707]
|
||||
```
|
||||
|
||||
### Example 4: Docking Station Pose / 充电桩位置
|
||||
|
||||
Known docking station at (10.5, 2.3, 0) facing -X direction (180° rotation):
|
||||
|
||||
已知充电桩位置在 (10.5, 2.3, 0)，朝向 -X 方向（旋转 180°）：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/factory_map.bin"
|
||||
custom_init_pos: [10.5, 2.3, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Programmatic API / 编程接口
|
||||
|
||||
### English
|
||||
|
||||
You can also set `init_pos` programmatically using the `lidar_set_custom_parameter` API. This is useful for:
|
||||
- Dynamic relocalization during runtime
|
||||
- Integration with external localization systems
|
||||
- Setting initial pose from robot's last known position
|
||||
|
||||
#### API Function
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief Set a custom parameter on the device
|
||||
* @param device Device handle obtained from lidar_open_device()
|
||||
* @param param_name Parameter name (e.g., "init_pos")
|
||||
* @param value_data Pointer to the parameter data
|
||||
* @param value_length Size of the data in bytes
|
||||
* @return 0 on success, -1 on error, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### Complete Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// Helper function to create quaternion from yaw angle (rotation around Z-axis)
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos format: [x, y, z, qx, qy, qz, qw] - 7 floats
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // Parameter name
|
||||
init_pos, // Data pointer
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 bytes
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("Successfully set init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("Failed to set init_pos, error code: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Usage examples:
|
||||
|
||||
// Example 1: Set position at origin with no rotation
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw (identity)
|
||||
);
|
||||
}
|
||||
|
||||
// Example 2: Set position with 90° yaw rotation
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // quaternion from yaw
|
||||
);
|
||||
}
|
||||
|
||||
// Example 3: Set position from external localization system
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS Integration Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// or
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// Callback for /initialpose topic (from RViz "2D Pose Estimate" tool)
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("Set init_pos from RViz: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("Failed to set init_pos: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### Important Notes
|
||||
|
||||
1. **Call timing**: Set `init_pos` **before** starting the stream with `lidar_start_stream()`
|
||||
2. **Map mode**: Ensure `custom_map_mode` is set to `2` (relocalization mode)
|
||||
3. **Map file**: The relocalization map must be set via `lidar_set_relocalization_map()` or YAML config
|
||||
4. **Thread safety**: `lidar_set_custom_parameter` is thread-safe but blocks until response received
|
||||
|
||||
### 中文
|
||||
|
||||
您也可以使用 `lidar_set_custom_parameter` API 以编程方式设置 `init_pos`。适用于：
|
||||
- 运行时动态重定位
|
||||
- 与外部定位系统集成
|
||||
- 从机器人上次已知位置设置初始位姿
|
||||
|
||||
#### API 函数
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief 在设备上设置自定义参数
|
||||
* @param device 从 lidar_open_device() 获取的设备句柄
|
||||
* @param param_name 参数名称（如 "init_pos"）
|
||||
* @param value_data 指向参数数据的指针
|
||||
* @param value_length 数据大小（字节）
|
||||
* @return 成功返回 0，错误返回 -1，文件传输中返回 -2
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### 完整示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// 辅助函数：从偏航角（绕 Z 轴旋转）创建四元数
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos 格式: [x, y, z, qx, qy, qz, qw] - 7 个 float
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // 参数名
|
||||
init_pos, // 数据指针
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 字节
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("成功设置 init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("设置 init_pos 失败，错误码: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 使用示例：
|
||||
|
||||
// 示例 1：在原点设置位置，无旋转
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw（单位四元数）
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 2：设置带 90° 偏航旋转的位置
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // 从偏航角计算的四元数
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 3：从外部定位系统设置位置
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS 集成示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// 或
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// /initialpose 话题的回调函数（来自 RViz 的 "2D Pose Estimate" 工具）
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("从 RViz 设置 init_pos: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("设置 init_pos 失败: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### 重要说明
|
||||
|
||||
1. **调用时机**：在调用 `lidar_start_stream()` 启动数据流**之前**设置 `init_pos`
|
||||
2. **地图模式**：确保 `custom_map_mode` 设置为 `2`（重定位模式）
|
||||
3. **地图文件**：必须通过 `lidar_set_relocalization_map()` 或 YAML 配置设置重定位地图
|
||||
4. **线程安全**：`lidar_set_custom_parameter` 是线程安全的，但会阻塞直到收到响应
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting / 故障排除
|
||||
|
||||
### Relocalization Fails / 重定位失败
|
||||
|
||||
**English**:
|
||||
- Ensure starting position is within recommended range (1m/10°)
|
||||
- Check that the map file path is correct and file exists
|
||||
- Verify the environment hasn't changed significantly since mapping
|
||||
- Try gently moving the device to provide more observations
|
||||
|
||||
**中文**：
|
||||
- 确保起始位置在推荐范围内（1m/10°）
|
||||
- 检查地图文件路径是否正确且文件存在
|
||||
- 验证环境自建图以来没有显著变化
|
||||
- 尝试轻轻移动设备以提供更多观测
|
||||
|
||||
### init_pos Not Taking Effect / init_pos 未生效
|
||||
|
||||
**English**:
|
||||
- Verify `custom_map_mode` is set to `2`
|
||||
- Check that `custom_init_pos` has exactly 7 values
|
||||
- Ensure quaternion is normalized (sum of squares ≈ 1)
|
||||
- Restart the driver after modifying configuration
|
||||
|
||||
**中文**：
|
||||
- 验证 `custom_map_mode` 设置为 `2`
|
||||
- 检查 `custom_init_pos` 是否恰好有 7 个值
|
||||
- 确保四元数已归一化（平方和 ≈ 1）
|
||||
- 修改配置后重启驱动程序
|
||||
|
||||
### TF Not Published / TF 未发布
|
||||
|
||||
**English**:
|
||||
- Relocalization may still be in progress
|
||||
- Check ROS logs for relocalization status messages
|
||||
- System operates in fallback mode until relocalization succeeds
|
||||
|
||||
**中文**：
|
||||
- 重定位可能仍在进行中
|
||||
- 检查 ROS 日志中的重定位状态消息
|
||||
- 系统在重定位成功前以后备模式运行
|
||||
|
||||
### Map File Not Found / 地图文件未找到
|
||||
|
||||
**English**:
|
||||
- Use absolute path (starting with `/`)
|
||||
- Check file permissions
|
||||
- Verify file extension is `.bin`
|
||||
|
||||
**中文**：
|
||||
- 使用绝对路径（以 `/` 开头）
|
||||
- 检查文件权限
|
||||
- 验证文件扩展名为 `.bin`
|
||||
|
||||
---
|
||||
|
||||
## Related Topics / 相关话题
|
||||
|
||||
| Topic | Description |
|
||||
|-------|-------------|
|
||||
| `/odin1/odometry` | Odometry in odom frame |
|
||||
| `/odin1/odometry_highfreq` | High-frequency odometry |
|
||||
| `/odin1/cloud_slam` | SLAM point cloud in odom frame |
|
||||
| `/tf` | Transform tree (includes map→odom after successful relocalization) |
|
||||
|
||||
---
|
||||
|
||||
## See Also / 参见
|
||||
|
||||
- [README.md](README.md) - Main documentation
|
||||
- [config/control_command.yaml](config/control_command.yaml) - Configuration file
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
# O1-P040100042
|
||||
cam_num: 1
|
||||
img_topic_0: /camera/rgb
|
||||
Tcl_0: [
|
||||
-0.00916, -0.99996, 0.00032, 0.03215,
|
||||
0.00239, -0.00034, -1.00000, -0.01185,
|
||||
0.99996, -0.00916, 0.00239, 0.00536,
|
||||
0.00000, 0.00000, 0.00000, 1.00000
|
||||
]
|
||||
|
||||
cam_0:
|
||||
cam_model: FishPoly
|
||||
image_width: 1600
|
||||
image_height: 1296
|
||||
k2: 6.5597407531742908e-04
|
||||
k3: -2.8960653254664001e-02
|
||||
k4: 4.5390330144604515e-02
|
||||
k5: -6.4513398102889072e-02
|
||||
k6: 3.8735200610848508e-02
|
||||
k7: -9.9027101865577991e-03
|
||||
p1: 0.
|
||||
p2: 0.
|
||||
A11: 7.3689426240691989e+02
|
||||
A12: -1.6114977429994648e-01
|
||||
A22: 7.3661135398452438e+02
|
||||
u0: 8.0612553451014685e+02
|
||||
v0: 6.3965071026991302e+02
|
||||
isFast: 0
|
||||
numDiff: 3000
|
||||
maxIncidentAngle: 120
|
||||
@@ -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
|
||||
@@ -0,0 +1,438 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.4993045926094055
|
||||
Tree Height: 495
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/image/undistorted
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/reprojected_image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_raw
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8493094444274902
|
||||
Min Value: -0.13891705870628357
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_render
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 1
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic: /odin1/odometry
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 100
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 10
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 52; 101; 164
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic: /odin1/odometry_highfreq
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/camera_pose_visual
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 5
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_decay
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/path
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/depth_img_competetion
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/depth_img_competetion_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Filter (blacklist): ""
|
||||
Filter (whitelist): ""
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 16.25591278076172
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.010398030281067
|
||||
Target Frame: odom
|
||||
Yaw: 0.8753980994224548
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001e70000033afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026f000001080000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a0065006300740065006400000002b2000000c50000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
@@ -0,0 +1,499 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Image1/Topic1
|
||||
- /cloudslam_reprojected1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /dense_depth_demo1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 593
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image/undistorted
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/reprojected_image
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 239
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_raw
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8730175495147705
|
||||
Min Value: -0.12252448499202728
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 2.3509885615147286e-38
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_render
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 10
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 98; 160; 234
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry_highfreq
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/camera_pose_visual
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 10
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_deacy
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/path
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 10.9336576461792
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.2568470537662506
|
||||
Y: 2.1451337337493896
|
||||
Z: 0.3774382472038269
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.3953983187675476
|
||||
Target Frame: odom
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.230407953262329
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1920
|
||||
X: 540
|
||||
Y: 124
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
+78
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
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 <std_msgs/msg/color_rgba.hpp>
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
#include <std_msgs/Header.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#endif
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
class camera_pose_visualization {
|
||||
public:
|
||||
std::string m_marker_ns;
|
||||
|
||||
camera_pose_visualization(float r, float g, float b, float a);
|
||||
|
||||
void setImageBoundaryColor(float r, float g, float b, float a = 1.0);
|
||||
void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);
|
||||
void setScale(double s);
|
||||
void setLineWidth(double width);
|
||||
|
||||
void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
|
||||
void reset();
|
||||
|
||||
#ifdef ROS2
|
||||
using ColorRGBA = std_msgs::msg::ColorRGBA;
|
||||
using Marker = visualization_msgs::msg::Marker;
|
||||
using MarkerArray = visualization_msgs::msg::MarkerArray;
|
||||
using Header = std_msgs::msg::Header;
|
||||
using Publisher = rclcpp::Publisher<MarkerArray>;
|
||||
#else
|
||||
using ColorRGBA = std_msgs::ColorRGBA;
|
||||
using Marker = visualization_msgs::Marker;
|
||||
using MarkerArray = visualization_msgs::MarkerArray;
|
||||
using Header = std_msgs::Header;
|
||||
using Publisher = ros::Publisher;
|
||||
#endif
|
||||
|
||||
void publish_by(Publisher& pub, const Header& header);
|
||||
void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
private:
|
||||
std::vector<Marker> m_markers;
|
||||
ColorRGBA m_image_boundary_color;
|
||||
ColorRGBA m_optical_center_connector_color;
|
||||
double m_scale;
|
||||
double m_line_width;
|
||||
|
||||
static const Eigen::Vector3d imlt;
|
||||
static const Eigen::Vector3d imlb;
|
||||
static const Eigen::Vector3d imrt;
|
||||
static const Eigen::Vector3d imrb;
|
||||
static const Eigen::Vector3d oc ;
|
||||
static const Eigen::Vector3d lt0 ;
|
||||
static const Eigen::Vector3d lt1 ;
|
||||
static const Eigen::Vector3d lt2 ;
|
||||
};
|
||||
+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
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
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 <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
|
||||
#include "polynomial_camera.hpp"
|
||||
|
||||
#include <memory>
|
||||
|
||||
class CloudReprojector
|
||||
{
|
||||
public:
|
||||
struct CameraParams
|
||||
{
|
||||
int image_width = 1600;
|
||||
int image_height = 1296;
|
||||
double A11 = 0.0, A12 = 0.0, A22 = 0.0;
|
||||
double u0 = 0.0, v0 = 0.0;
|
||||
double k2 = 0.0, k3 = 0.0, k4 = 0.0, k5 = 0.0, k6 = 0.0, k7 = 0.0;
|
||||
};
|
||||
|
||||
struct ExtrinsicParams
|
||||
{
|
||||
Eigen::Matrix4d Tcl = Eigen::Matrix4d::Identity(); // camera to lidar
|
||||
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity(); // lidar to imu (fixed)
|
||||
Eigen::Matrix4d Tic = Eigen::Matrix4d::Identity(); // camera to imu (calculated)
|
||||
};
|
||||
|
||||
struct OdomPose
|
||||
{
|
||||
Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity();
|
||||
Eigen::Vector3d position = Eigen::Vector3d::Zero();
|
||||
};
|
||||
|
||||
CloudReprojector();
|
||||
~CloudReprojector() = default;
|
||||
|
||||
bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params);
|
||||
|
||||
cv::Mat reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
|
||||
const OdomPose& odom_pose);
|
||||
|
||||
void setPointRadius(int radius) { point_radius_ = radius; }
|
||||
int getPointRadius() const { return point_radius_; }
|
||||
|
||||
const CameraParams& getCameraParams() const { return camera_params_; }
|
||||
const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
|
||||
|
||||
// Update extrinsic parameters at runtime with real-time values from module
|
||||
void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
|
||||
extrinsic_params_.Tcl = Tcl;
|
||||
extrinsic_params_.Til = Til;
|
||||
extrinsic_params_.Tic = calculateTic(Tcl, Til);
|
||||
}
|
||||
|
||||
static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
|
||||
|
||||
private:
|
||||
Eigen::Matrix4d odomPoseToMatrix(const OdomPose& odom) const;
|
||||
|
||||
cv::Mat projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const;
|
||||
|
||||
CameraParams camera_params_;
|
||||
ExtrinsicParams extrinsic_params_;
|
||||
std::unique_ptr<mini_vikit::PolynomialCamera> camera_model_;
|
||||
|
||||
int point_radius_ = 4;
|
||||
bool initialized_ = false;
|
||||
};
|
||||
@@ -0,0 +1,240 @@
|
||||
/*
|
||||
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 <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <atomic>
|
||||
#include <deque>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <sstream>
|
||||
|
||||
class BinaryDataLogger {
|
||||
public:
|
||||
struct Options {
|
||||
public:
|
||||
// Number of frames per batch before flushing to disk
|
||||
size_t batch_size = 20;
|
||||
// Base directory to place time-stamped run directory under
|
||||
// If empty, will use $ODIN_DATA_DIR or default to $HOME/OdinData
|
||||
std::filesystem::path base_dir;
|
||||
};
|
||||
|
||||
explicit BinaryDataLogger(const Options& opts) {
|
||||
namespace fs = std::filesystem;
|
||||
// Determine base dir
|
||||
fs::path base = opts.base_dir;
|
||||
if (base.empty()) {
|
||||
const char* env_dir = std::getenv("ODIN_DATA_DIR");
|
||||
if (env_dir && *env_dir) {
|
||||
base = fs::path(env_dir);
|
||||
} else {
|
||||
const char* home = std::getenv("HOME");
|
||||
base = home && *home ? fs::path(home) / "OdinData" : fs::current_path() / "OdinData";
|
||||
}
|
||||
}
|
||||
// Make timestamp folder name: YYYYMMDD_HHMMSS
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t t = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm tm{};
|
||||
#ifdef _WIN32
|
||||
localtime_s(&tm, &t);
|
||||
#else
|
||||
localtime_r(&t, &tm);
|
||||
#endif
|
||||
char buf[32];
|
||||
std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
|
||||
root_dir_ = base / buf;
|
||||
fs::create_directories(root_dir_);
|
||||
fs::create_directories(root_dir_ / "image");
|
||||
// Convert to std::string
|
||||
std::string timestamp(buf);
|
||||
created_at_ = timestamp;
|
||||
// Compose file name
|
||||
std::filesystem::path pcFile = root_dir_ / ("MT" + timestamp + ".olx");
|
||||
// Create placeholder files (device_id / firmware / algorithm filled later via update_info_file)
|
||||
write_info_file_unlocked();
|
||||
write_text_file(root_dir_ / "image" / "cam_in_ex.txt", "# camera intrinsics/extrinsics TBD\n");
|
||||
|
||||
// Init writers
|
||||
pose_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPose.bin", opts.batch_size);
|
||||
cloud_writer_ = std::make_unique<Writer>(pcFile, opts.batch_size);
|
||||
//cloud_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPointCloud.olx", opts.batch_size);
|
||||
image_writer_ = std::make_unique<Writer>(root_dir_ / "OdinImage.bin", opts.batch_size);
|
||||
roatation_writer_ = std::make_unique<Writer>(root_dir_ / "OdinRotate.bin", opts.batch_size);
|
||||
imu_writer_ = std::make_unique<Writer>(root_dir_ / "OdinIMU.bin", opts.batch_size);
|
||||
}
|
||||
|
||||
~BinaryDataLogger() {
|
||||
// Ensure all writers flush on destruction
|
||||
if (pose_writer_) pose_writer_->shutdown();
|
||||
if (cloud_writer_) cloud_writer_->shutdown();
|
||||
if (image_writer_) image_writer_->shutdown();
|
||||
if (roatation_writer_) roatation_writer_->shutdown();
|
||||
if (imu_writer_) imu_writer_->shutdown();
|
||||
}
|
||||
|
||||
const std::filesystem::path& root_dir() const { return root_dir_; }
|
||||
|
||||
// Update info.txt with device_id (SN) / firmware (SoC) / algorithm version.
|
||||
// Safe to call multiple times; latest values win.
|
||||
void update_info_file(const std::string& device_id,
|
||||
const std::string& firmware_version,
|
||||
const std::string& algorithm_version) {
|
||||
std::lock_guard<std::mutex> lk(info_mtx_);
|
||||
device_id_ = device_id;
|
||||
firmware_version_ = firmware_version;
|
||||
algorithm_version_ = algorithm_version;
|
||||
write_info_file_unlocked();
|
||||
}
|
||||
|
||||
// Enqueue ready-to-write frame blobs (already formatted as per spec)
|
||||
void enqueuePoseFrame(std::vector<uint8_t>&& blob) {
|
||||
if (pose_writer_) pose_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueuePointCloudFrame(std::vector<uint8_t>&& blob) {
|
||||
if (cloud_writer_) cloud_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueImageFrame(std::vector<uint8_t>&& blob) {
|
||||
if (image_writer_) image_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueRotateFrame(std::vector<uint8_t>&& blob) {
|
||||
if (roatation_writer_) roatation_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueIMUFrame(std::vector<uint8_t>&& blob) {
|
||||
if (imu_writer_) imu_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
|
||||
private:
|
||||
struct Writer {
|
||||
explicit Writer(const std::filesystem::path& filepath, size_t batch)
|
||||
: file_path(filepath), batch_size(batch), stop(false) {
|
||||
worker = std::thread([this]() { this->run(); });
|
||||
}
|
||||
~Writer() {
|
||||
shutdown();
|
||||
}
|
||||
void enqueue(std::vector<uint8_t>&& frame) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
pending.emplace_back(std::move(frame));
|
||||
if (pending.size() >= batch_size) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
}
|
||||
cv.notify_one();
|
||||
}
|
||||
void shutdown() {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
if (stop) return;
|
||||
// Move leftovers to write buffer
|
||||
if (!pending.empty()) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
stop = true;
|
||||
}
|
||||
cv.notify_one();
|
||||
if (worker.joinable()) worker.join();
|
||||
}
|
||||
private:
|
||||
void swap_pending_unlocked() {
|
||||
if (!pending.empty()) {
|
||||
write_queue.emplace_back(std::move(pending));
|
||||
pending.clear();
|
||||
}
|
||||
}
|
||||
void run() {
|
||||
std::ofstream out(file_path, std::ios::binary | std::ios::app);
|
||||
if (!out.is_open()) {
|
||||
// If file can't be opened, silently drop (or could add logging hook)
|
||||
return;
|
||||
}
|
||||
for (;;) {
|
||||
std::vector<std::vector<uint8_t>> batch;
|
||||
{
|
||||
std::unique_lock<std::mutex> lk(mtx);
|
||||
cv.wait(lk, [&]{ return stop || !write_queue.empty(); });
|
||||
if (!write_queue.empty()) {
|
||||
batch = std::move(write_queue.front());
|
||||
write_queue.pop_front();
|
||||
} else if (stop) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!batch.empty()) {
|
||||
for (auto& frame : batch) {
|
||||
if (!frame.empty()) {
|
||||
out.write(reinterpret_cast<const char*>(frame.data()), static_cast<std::streamsize>(frame.size()));
|
||||
}
|
||||
}
|
||||
out.flush();
|
||||
}
|
||||
}
|
||||
}
|
||||
std::filesystem::path file_path;
|
||||
size_t batch_size;
|
||||
std::mutex mtx;
|
||||
std::condition_variable cv;
|
||||
std::vector<std::vector<uint8_t>> pending; // accumulate frames
|
||||
std::deque<std::vector<std::vector<uint8_t>>> write_queue; // queued batches
|
||||
std::thread worker;
|
||||
bool stop;
|
||||
};
|
||||
|
||||
static void write_text_file(const std::filesystem::path& p, const std::string& content) {
|
||||
std::ofstream f(p, std::ios::out | std::ios::trunc);
|
||||
if (f.is_open()) {
|
||||
f << content;
|
||||
}
|
||||
}
|
||||
|
||||
// Render info.txt from current member fields. Caller must hold info_mtx_.
|
||||
void write_info_file_unlocked() {
|
||||
std::ostringstream oss;
|
||||
oss << "device=OdinOne\n"
|
||||
<< "pointcloud=xyzrgbi\n";
|
||||
if (!device_id_.empty()) {
|
||||
oss << "device_id=" << device_id_ << "\n";
|
||||
}
|
||||
if (!firmware_version_.empty()) {
|
||||
oss << "firmware_version=" << firmware_version_ << "\n";
|
||||
}
|
||||
if (!algorithm_version_.empty()) {
|
||||
oss << "algorithm_version=" << algorithm_version_ << "\n";
|
||||
}
|
||||
oss << "created_at=" << created_at_ << "\n";
|
||||
write_text_file(root_dir_ / "image" / "info.txt", oss.str());
|
||||
}
|
||||
|
||||
std::filesystem::path root_dir_;
|
||||
std::string created_at_;
|
||||
std::string device_id_;
|
||||
std::string firmware_version_;
|
||||
std::string algorithm_version_;
|
||||
std::mutex info_mtx_;
|
||||
std::unique_ptr<Writer> pose_writer_;
|
||||
std::unique_ptr<Writer> cloud_writer_;
|
||||
std::unique_ptr<Writer> image_writer_;
|
||||
std::unique_ptr<Writer> roatation_writer_;
|
||||
std::unique_ptr<Writer> imu_writer_;
|
||||
};
|
||||
+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);
|
||||
};
|
||||
+83
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
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 <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CompressedImage.h>
|
||||
|
||||
#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.h>
|
||||
|
||||
#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 DepthImageRosNode
|
||||
{
|
||||
public:
|
||||
DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
image_transport::ImageTransport it_;
|
||||
|
||||
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::Image> color_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::CompressedImage> color_compressed_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
ros::Publisher depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
||||
const sensor_msgs::ImageConstPtr &image_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::Header &header);
|
||||
};
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user