Robocon/RC_WheelLeg
4
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[real] 整理 ROS 2 v2 里程计导航联调

Browse Source
This commit is contained in:
eastzio
2026-07-27 17:38:59 +08:00
parent dc7a2c2a67
commit 765173676f
31 changed files with 520049 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
01_doc/version_history.md
+9
View File
@@ -18,6 +18,7 @@
| `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`。
@@ -53,6 +54,14 @@
- `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 不变。
05_software/README.md
+1 -1
View File
@@ -33,7 +33,7 @@ MJCF + mjlab task
IK real --------------------------------> 电机
```
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`。
`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`。
详细说明见：
05_software/real/README.md
+2
View File
@@ -38,6 +38,8 @@ ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PC
`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`，默认站姿回到比赛站姿。
## 实机记录
[![第一代 Sim2Real 真机验证](../../06_assets/images/early_sim2real_preview.jpg)](../../06_assets/videos/early_sim2real.mp4)
05_software/real/sim2real_ros2_v2/README.md
+5 -6
View File
@@ -1,6 +1,6 @@
# ROS 2 Sim2Real v2：Odin、TensorRT 与站姿调参
# ROS 2 Sim2Real v2：里程计导航联调
本目录在 `v0.11.0` 导航原型基础上归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，对应重排主线的 `v0.11.1`。虽然源目录以站姿参数命名，但该阶段同时首次完整加入 Odin 驱动、TensorRT、多策略切换、事件日志和更完整的硬件诊断。
本目录归档 `real/sim2real_ros2_v2(odom)`，对应重排主线的 `v0.12.0`。该阶段在 `v0.11.1` 的 Odin/TensorRT 与站姿调参基础上，固定纯里程计模式，增加 odom fallback 的 TF 冲突保护、A_min 路线和地图工具。
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
@@ -70,8 +70,7 @@ Nav2 / cmd_vel ------------------------------> sim2real_runtime
- Windows 仅作为编辑环境使用。
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
除非训练与部署同步更新。
- Rough 默认策略为 `NEWmodel_1900`，优先使用比赛机生成的 TensorRT engine；Crawl 保留 ONNX，但运行配置使用 IK 后端。
- 默认 Rough 站姿为髋俯仰 `0.670`、膝关节 `-1.390`，源目录名中的 `z=0.380` 记录该阶段的机身高度配置意图。
- `map_b.pcd` 已确定性抽样到 10 MB 以下，点数和哈希见 [`map/README.md`](map/README.md)。
- 默认 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 重新生成。
- 源配置引用的 Odin `map_a.bin` 不在该备份中；使用 Relocalization 前必须从设备或建图备份补回真实文件并填写绝对路径，缺失时只能验证软件链路或使用后续 odom 配置。
05_software/real/sim2real_ros2_v2/map/A_min.pcd
+227980
View File
File diff suppressed because it is too large Load Diff
05_software/real/sim2real_ros2_v2/map/C.pcd
+289707
View File
File diff suppressed because it is too large Load Diff
05_software/real/sim2real_ros2_v2/map/README.md
+7 -8
View File
@@ -1,10 +1,9 @@
# `v0.11.1` 导航点云
# `v0.12.0` 里程计导航点云
原始 `map_b.pcd` 为 ASCII PCD，共 1,080,047 点、34,720,771 字节。为避免大文件进入 Git 历史，使用 `tools/nav_tools/downsample_ascii_pcd.py` 以步长 4 做确定性抽样。
本阶段使用三份 ASCII PCD 进行路线和里程计导航联调。原始文件均使用 `tools/nav_tools/downsample_ascii_pcd.py` 确定性等步长抽样，抽样文件仅用于可视化、路线编辑和软件回归，不等价于原始高密度地图。
- 抽样点数：270,012
- 抽样大小：8,680,283 字节
- 原始 SHA-256：`F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7`
- 抽样 SHA-256：`F3052431F217DE8FFFE87726F8E547A59C65048B0285F02AF4803C5F4E4C1B42`
抽样文件适合路线编辑和代码演示，不等价于原始高密度点云；需要复现实机导航精度时应恢复原始 PCD 并核对哈希。
| 文件 | 原始点数 | 步长 | 抽样点数 | 抽样大小 | 原始 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` |
05_software/real/sim2real_ros2_v2/map/routes/A_min/A_min_route.json
+276
View File
@@ -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
}
05_software/real/sim2real_ros2_v2/policies/NEWmodel_1900.onnx
BIN
View File
Binary file not shown.
05_software/real/sim2real_ros2_v2/policies/NEWmodel_1900_fp16.engine
BIN
View File
Binary file not shown.
05_software/real/sim2real_ros2_v2/policies/README.md
+9 -9
View File
@@ -1,12 +1,12 @@
# `v0.11.1` 策略文件
# `v0.12.0` 策略文件
本阶段只归档运行配置引用或作为部署契约基线的策略，未纳入其余候选 checkpoint 和 engine。
本阶段运行配置使用 `model_9600` 作为 Rough 策略，并保留 Crawl ONNX 和部署契约参考模型。
| 文件 | 用途 | SHA-256 |
| --- | --- | --- |
| `NEWmodel_1900.onnx` | Rough 默认 ONNX | `A9E2F0D117225874C8B4F986787C3E6C8E7EA45D1CDA3AED58AC38F581442B00` |
| `NEWmodel_1900_fp16.engine` | Rough 比赛机 TensorRT | `DBC73AC9B3780029EC08BE19B9972B9840A067EBD146DEB860698F4F7066CC03` |
| `model_crawl.onnx` | Crawl 策略候选；当前配置使用 IK 后端 | `633AB2F3C3737FDDFD657F4A006A414BFA3950DD60D708B101E6744DF9E6DF67` |
| `model_rough.onnx` | 原部署契约参考基线 | `4149BE180A197E70495A7A6D186C62BE43271A4CE9035A8EEE56CD293D869A27` |
| 文件 | 用途 |
| --- | --- |
| `model_9600.onnx` | 里程计版本 Rough ONNX |
| `model_9600_fp16.engine` | 里程计版本 Rough TensorRT |
| `model_crawl.onnx` | Crawl 候选；当前配置使用 IK 后端 |
| `model_rough.onnx` | 原部署契约参考基线 |
TensorRT engine 不保证跨 JetPack、TensorRT 版本或 GPU 架构兼容。
其余 `model_6800`、`model_8400`、`model_10200` 等候选策略属于相邻实验或后续比赛版本，不在本 Tag 重复归档。
05_software/real/sim2real_ros2_v2/policies/model_9600.onnx
BIN
View File
Binary file not shown.
05_software/real/sim2real_ros2_v2/policies/model_9600_fp16.engine
BIN
View File
Binary file not shown.
05_software/real/sim2real_ros2_v2/src/odin_ros_driver/config/control_command.yaml
+2 -2
View File
@@ -90,9 +90,9 @@ register_keys:
showpath: 0 # 0: off; 1: on
showcamerapose: 0 # 0: off; 1: on
custom_map_mode: 2 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode
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" # supply the external Odin map before using Relocalization mode
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}/
05_software/real/sim2real_ros2_v2/src/sim2real_bringup/config/deployment_contract.yaml
+1 -1
View File
@@ -47,7 +47,7 @@ action:
- rr_wheel
wheel_indices: [12, 13, 14, 15]
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
default_dof_pos: [0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.0, 0.0, 0.0]
default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
motor_mapping:
can_id_map:
05_software/real/sim2real_ros2_v2/src/sim2real_bringup/config/runtime.yaml
+17 -7
View File
@@ -5,13 +5,13 @@
status_hz: 10.0
target_timeout_ms: 150.0
event_log_dir: "logs_v2_web"
model_engine_path: policies/NEWmodel_1900_fp16.engine
model_engine_path: policies/model_9600_fp16.engine
prefer_tensorrt: true
model_path: policies/NEWmodel_1900.onnx
rough_model_engine_path: policies/NEWmodel_1900_fp16.engine
model_path: policies/model_9600.onnx
rough_model_engine_path: policies/model_9600_fp16.engine
crawl_model_path: policies/model_crawl.onnx
crawl_model_engine_path: ""
rough_default_dof_pos: [0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.670, -1.390, 0.0, 0.0, 0.0, 0.0]
rough_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
crawl_backend: "ik"
crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0]
crawl_ik_wheel_linear_gain: 6.25
@@ -43,7 +43,7 @@
remote_axis_deadzone: 40
remote_active_threshold: 40
remote_axis_full_scale: 660.0
remote_max_vx: 0.8
remote_max_vx: 0.85
remote_max_vy: 0.3
remote_max_yaw_rate: 0.5
remote_invert_vx: true
@@ -136,6 +136,7 @@
# Simple navigation parameters
nav_map_frame: "map"
nav_odom_frame: "odom"
nav_base_frame: "base_link"
nav_control_hz: 20.0
nav_goal_tolerance: 0.20
@@ -173,13 +174,22 @@
# Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below.
nav_route_file: src/sim2real_nav2/config/maps/a_min/test_route.yaml
# Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes.
nav_route_task_file: tools/test_route.json
# The web "odom" button uses the first waypoint of this route as the fixed odom fallback start pose.
nav_route_task_file: map/routes/A_min/A_min_route.json
nav_route_auto_align_enabled: false
nav_route_rotation_offset_deg: 0.0
nav_route_align_max_angle_deg: 6.0
nav_route_align_angle_step_deg: 0.5
nav_route_align_search_radius: 0.35
pcd_nav_file: map/map_b.pcd
odom_fallback_require_odom_fresh: true
odom_fallback_max_odom_age_ms: 500.0
odom_fallback_block_existing_map_odom_tf: true
odom_fallback_tf_conflict_window_s: 1.0
odom_fallback_tf_conflict_xy_tolerance: 0.05
odom_fallback_tf_conflict_yaw_tolerance_deg: 2.0
# Keep odom fallback running if Odin relocalizes mid-task; hand off after mission end or Exit odom.
odom_fallback_stop_on_external_tf: false
pcd_nav_file: map/A_min.pcd
pcd_floor_z_min: -1.6
pcd_floor_z_max: 0.4
pcd_sample_step: 25
05_software/real/sim2real_ros2_v2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp
+4 -4
View File
@@ -64,10 +64,10 @@ struct DeploymentContract
};
static constexpr std::array<float, 16> kDefaultDofPos = {
0.0f, 0.670f, -1.390f,
0.0f, 0.670f, -1.390f,
0.0f, 0.670f, -1.390f,
0.0f, 0.670f, -1.390f,
0.0f, 0.550f, -1.125f,
0.0f, 0.550f, -1.125f,
0.0f, 0.550f, -1.125f,
0.0f, 0.550f, -1.125f,
0.0f, 0.0f, 0.0f, 0.0f
};
};
05_software/real/sim2real_ros2_v2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp
+2 -1
View File
@@ -124,7 +124,8 @@ private:
bool readCanFrame(int fd, void* frame, int timeout_us);
bool enableMotor(int fd, int motor_id);
bool disableMotor(int fd, int motor_id);
bool disableMotor(int fd, int motor_id, bool clear_fault = false);
bool writeParameterInt(int fd, int motor_id, std::uint16_t param_id, std::uint32_t value);
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
bool readParameter(int fd, int motor_id, std::uint16_t param_id);
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
05_software/real/sim2real_ros2_v2/src/sim2real_hw/src/hardware_bridge_node.cpp
+41 -15
View File
@@ -35,7 +35,7 @@ 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::uint16_t PARAM_CAN_TIMEOUT = 0x200C;
const std::uint16_t PARAM_CAN_TIMEOUT = 0x7028;
const std::uint16_t PARAM_VBUS = 0x3007;
const std::uint16_t PARAM_DRV_FAULT = 0x3022;
const std::uint16_t PARAM_DRV_FAULT_DETAIL_1 = 0x3024;
@@ -852,10 +852,21 @@ bool HardwareBridgeNode::startMotorRecoverySequence(
}
bool step_ok = true;
step_ok = enableMotor(fd, motors_[index].id) && step_ok;
logMotorPowerEvent(index, "reset_before_enable",
"trigger=" + trigger + ", recovery_attempt=" + std::to_string(attempt_number));
step_ok = disableMotor(fd, motors_[index].id, true) && step_ok;
state.fault_code = 0;
state.fault_detail_1 = 0;
state.fault_detail_2 = 0;
state.fault_code_reported = false;
std::this_thread::sleep_for(30ms);
step_ok = setModeRaw(fd, motors_[index].id, 0) && step_ok;
std::this_thread::sleep_for(30ms);
step_ok = enableMotor(fd, motors_[index].id) && step_ok;
std::this_thread::sleep_for(20ms);
step_ok = writeLimit(fd, motors_[index].id, PARAM_VELOCITY_LIMIT, 20.0f) && step_ok;
step_ok = writeLimit(fd, motors_[index].id, PARAM_TORQUE_LIMIT, 17.0f) && step_ok;
step_ok = writeParameterInt(fd, motors_[index].id, PARAM_CAN_TIMEOUT, 0) && step_ok;
if (!step_ok) {
logMotorDiagnosticEvent(index, "motor_init_retry",
@@ -979,10 +990,21 @@ bool HardwareBridgeNode::initializeMotor(std::size_t index, const std::string &
for (int attempt = 1; attempt <= max_attempts; ++attempt) {
state.init_attempt_count++;
bool step_ok = true;
step_ok = enableMotor(fd, motors_[index].id) && step_ok;
logMotorPowerEvent(index, "reset_before_enable",
"reason=" + reason + ", attempt=" + std::to_string(attempt));
step_ok = disableMotor(fd, motors_[index].id, true) && step_ok;
state.fault_code = 0;
state.fault_detail_1 = 0;
state.fault_detail_2 = 0;
state.fault_code_reported = false;
std::this_thread::sleep_for(30ms);
step_ok = setModeRaw(fd, motors_[index].id, 0) && step_ok;
std::this_thread::sleep_for(30ms);
step_ok = enableMotor(fd, motors_[index].id) && step_ok;
std::this_thread::sleep_for(20ms);
step_ok = writeLimit(fd, motors_[index].id, PARAM_VELOCITY_LIMIT, 20.0f) && step_ok;
step_ok = writeLimit(fd, motors_[index].id, PARAM_TORQUE_LIMIT, 17.0f) && step_ok;
step_ok = writeParameterInt(fd, motors_[index].id, PARAM_CAN_TIMEOUT, 0) && step_ok;
if (step_ok) {
success = waitForMotorFeedback(index, std::chrono::milliseconds(kMotorInitConfirmTimeoutMs));
@@ -1766,13 +1788,15 @@ void HardwareBridgeNode::onWriteLoop()
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);
std::uint8_t data[8] = {0};
return sendCanFrame(fd, ext_id, data, 8);
}
bool HardwareBridgeNode::disableMotor(int fd, int motor_id)
bool HardwareBridgeNode::disableMotor(int fd, int motor_id, bool clear_fault)
{
std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id;
std::uint8_t data[8] = {0};
data[0] = clear_fault ? 1 : 0;
return sendCanFrame(fd, ext_id, data, 8);
}
@@ -1787,12 +1811,7 @@ bool HardwareBridgeNode::readParameter(int fd, int motor_id, std::uint16_t param
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);
return writeParameterInt(fd, motor_id, PARAM_MODE, static_cast<std::uint8_t>(mode));
}
bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit)
@@ -1805,6 +1824,16 @@ bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id
return sendCanFrame(fd, ext_id, data, 8);
}
bool HardwareBridgeNode::writeParameterInt(int fd, int motor_id, std::uint16_t param_id, std::uint32_t value)
{
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], &value, sizeof(std::uint32_t));
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;
@@ -1839,8 +1868,6 @@ bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
{
struct sockaddr_can addr;
struct ifreq ifr;
std::memset(&addr, 0, sizeof(addr));
std::memset(&ifr, 0, sizeof(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());
@@ -1857,7 +1884,6 @@ bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
}
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
ifr.ifr_name[IFNAMSIZ - 1] = '\0';
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str());
::close(fd);
@@ -1868,7 +1894,7 @@ bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (::bind(fd, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) {
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;
05_software/real/sim2real_ros2_v2/src/sim2real_runtime/package.xml
+1
View File
@@ -12,6 +12,7 @@
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<depend>sim2real_common</depend>
<depend>sim2real_interfaces</depend>
05_software/real/sim2real_ros2_v2/src/sim2real_runtime/src/policy_runtime_node.cpp
-1
View File
@@ -235,7 +235,6 @@ PolicyRuntimeNode::PolicyRuntimeNode()
run_log_dir_.c_str());
logEvent("INFO", "node_start", "Policy runtime node started.");
initializeDebugTrace();
// Initialize StandBalanceController
stand_balance_ = std::make_unique<sim2real_common::StandBalanceController>(0.02);
const float rough_hip_mean =
05_software/real/sim2real_ros2_v2/src/sim2real_runtime/src/web_udp_bridge_node.py
+311 -14
View File
@@ -13,12 +13,13 @@ from urllib.parse import urlparse
import rclpy
import yaml
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TransformStamped, 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
from tf2_ros import Buffer, TransformException, TransformListener
from tf2_msgs.msg import TFMessage
from tf2_ros import Buffer, TransformBroadcaster, TransformException, TransformListener
class WebUdpBridgeNode(Node):
@@ -42,6 +43,7 @@ class WebUdpBridgeNode(Node):
self.http_port = int(self.declare_parameter("web_http_port", 18080).value)
self.web_static_dir = str(self.declare_parameter("web_static_dir", "").value)
self.nav_map_frame = str(self.declare_parameter("nav_map_frame", "map").value)
self.nav_odom_frame = str(self.declare_parameter("nav_odom_frame", "odom").value)
self.nav_base_frame = str(self.declare_parameter("nav_base_frame", "base_link").value)
self.nav_goals_file = str(self.declare_parameter("nav_goals_file", "").value)
self.nav_missions_file = str(self.declare_parameter("nav_missions_file", "").value)
@@ -68,9 +70,40 @@ class WebUdpBridgeNode(Node):
0.05,
float(self.declare_parameter("nav_route_align_search_radius", 0.35).value),
)
self.odom_fallback_require_odom_fresh = bool(
self.declare_parameter("odom_fallback_require_odom_fresh", True).value
)
self.odom_fallback_max_odom_age_ms = max(
0.0,
float(self.declare_parameter("odom_fallback_max_odom_age_ms", 500.0).value),
)
self.odom_fallback_block_existing_map_odom_tf = bool(
self.declare_parameter("odom_fallback_block_existing_map_odom_tf", True).value
)
self.odom_fallback_tf_conflict_window_s = max(
0.0,
float(self.declare_parameter("odom_fallback_tf_conflict_window_s", 1.0).value),
)
self.odom_fallback_tf_conflict_xy_tolerance = max(
0.0,
float(self.declare_parameter("odom_fallback_tf_conflict_xy_tolerance", 0.05).value),
)
self.odom_fallback_tf_conflict_yaw_tolerance = math.radians(
max(0.0, float(self.declare_parameter("odom_fallback_tf_conflict_yaw_tolerance_deg", 2.0).value))
)
self.odom_fallback_stop_on_external_tf = bool(
self.declare_parameter("odom_fallback_stop_on_external_tf", False).value
)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.tf_broadcaster = TransformBroadcaster(self)
self.odom_fallback_active = False
self.odom_fallback_transform: Optional[TransformStamped] = None
self.odom_fallback_anchor: dict[str, Any] = {}
self.odom_fallback_handoff_pending = False
self.odom_fallback_handoff_info: dict[str, Any] = {}
self.last_external_map_odom_tf: Optional[dict[str, Any]] = None
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.setblocking(False)
@@ -122,6 +155,7 @@ class WebUdpBridgeNode(Node):
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
self.create_subscription(String, "simple_nav/status", self.on_nav_status, 10)
self.create_subscription(String, "simple_nav/path", self.on_nav_path, 10)
self.create_subscription(TFMessage, "/tf", self.on_tf, 50)
self.map_points = self.load_filtered_pcd(Path(self.pcd_nav_file)) if self.pcd_nav_file else []
self.goal_specs: list[dict[str, Any]] = []
@@ -145,6 +179,7 @@ class WebUdpBridgeNode(Node):
self.on_state_timer,
)
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
self.odom_fallback_timer = self.create_timer(0.05, self.on_odom_fallback_timer)
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
self.get_logger().info(f"Web HTTP UI serving on http://{self.http_host}:{self.http_port}")
@@ -673,22 +708,12 @@ class WebUdpBridgeNode(Node):
self.http_thread.start()
def build_map_packet(self) -> dict[str, Any]:
goal_lookup = {goal["name"]: goal for goal in self.goal_specs if goal.get("name")}
default_goals: list[dict[str, Any]] = []
if self.default_mission_name:
for mission in self.mission_specs:
if mission["name"] != self.default_mission_name:
continue
default_goals = [
goal_lookup[goal_name]
for goal_name in mission.get("goals", [])
if goal_name in goal_lookup
]
break
default_goals = self.get_default_mission_goals()
return {
"points": self.map_points,
"map_frame": self.nav_map_frame,
"base_frame": self.nav_base_frame,
"odom_frame": self.nav_odom_frame,
"pose": self.lookup_pose(),
"route_source_file": self.get_route_source_file(),
"goal_specs": self.goal_specs,
@@ -696,6 +721,12 @@ class WebUdpBridgeNode(Node):
"default_mission_name": self.default_mission_name,
"default_mission_goals": default_goals,
"route_alignment": self.route_alignment_info,
"odom_fallback": {
"active": self.odom_fallback_active,
"handoff_pending": self.odom_fallback_handoff_pending,
"handoff": dict(self.odom_fallback_handoff_info),
**self.odom_fallback_anchor,
},
"nav_path": dict(self.latest_nav_path),
}
@@ -713,6 +744,8 @@ class WebUdpBridgeNode(Node):
"nav_cmd",
"go_to",
"go_rel",
"odom_task",
"odom_stop",
"model_toggle",
"model_cmd",
}:
@@ -758,6 +791,61 @@ class WebUdpBridgeNode(Node):
self.latest_nav_status = msg.data
if msg.data.startswith("reloaded goals="):
self.reload_nav_task_config()
elif msg.data.startswith("navigation stopped:") and self.odom_fallback_active:
self.stop_odom_fallback(msg.data.removeprefix("navigation stopped:").strip(), stop_nav=False)
def on_tf(self, msg: TFMessage) -> None:
for transform in msg.transforms:
parent = transform.header.frame_id.strip("/")
child = transform.child_frame_id.strip("/")
map_frame = self.nav_map_frame.strip("/")
odom_frame = self.nav_odom_frame.strip("/")
if parent == map_frame and child == odom_frame:
self.handle_map_odom_tf(transform, "map_to_odom")
elif parent == odom_frame and child == map_frame:
self.handle_map_odom_tf(transform, "odom_to_map")
def handle_map_odom_tf(self, transform: TransformStamped, direction: str) -> None:
if not self.odom_fallback_active:
self.last_external_map_odom_tf = {
"direction": direction,
"stamp": self.stamp_to_sec(transform.header.stamp),
}
return
if direction == "odom_to_map":
self.on_external_tf_during_odom_fallback(
direction,
"external odom->map TF detected, likely Odin relocalization recovered",
)
return
if not self.is_own_odom_fallback_tf(transform):
self.on_external_tf_during_odom_fallback(
direction,
"external map->odom TF conflict detected",
)
def on_external_tf_during_odom_fallback(self, direction: str, reason: str) -> None:
if self.odom_fallback_stop_on_external_tf:
self.stop_odom_fallback(reason, stop_nav=True)
return
self.mark_odom_fallback_handoff_pending(direction, reason)
def mark_odom_fallback_handoff_pending(self, direction: str, reason: str) -> None:
first_notice = not self.odom_fallback_handoff_pending
self.odom_fallback_handoff_pending = True
self.odom_fallback_handoff_info = {
"pending": True,
"direction": direction,
"reason": reason,
"stamp": round(self.now_sec(), 3),
}
if first_notice:
self.latest_nav_status = f"odom fallback handoff pending: {reason}"
self.get_logger().warn(
f"{self.latest_nav_status}; keeping pure odom task active until mission ends or Exit odom"
)
def on_nav_path(self, msg: String) -> None:
try:
@@ -842,6 +930,8 @@ class WebUdpBridgeNode(Node):
command = str(payload.get("command", "")).strip()
if command:
self.set_control_mode("NAV")
if command.lower() == "stop" and self.odom_fallback_active:
self.stop_odom_fallback("nav stop requested", stop_nav=False)
self.nav_cmd_pub.publish(String(data=command))
elif msg_type == "go_to":
x = float(payload.get("x", 0.0))
@@ -853,6 +943,10 @@ class WebUdpBridgeNode(Node):
dy = float(payload.get("dy", 0.0))
self.set_control_mode("NAV")
self.nav_cmd_pub.publish(String(data=f"go_rel {dx:.3f} {dy:.3f}"))
elif msg_type == "odom_task":
self.start_odom_fallback_task()
elif msg_type == "odom_stop":
self.stop_odom_fallback("manual stop", stop_nav=True)
elif msg_type == "model_toggle":
self.model_cmd_pub.publish(String(data="toggle"))
elif msg_type == "model_cmd":
@@ -876,6 +970,195 @@ class WebUdpBridgeNode(Node):
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
def get_default_mission_goals(self) -> list[dict[str, Any]]:
goal_lookup = {goal["name"]: goal for goal in self.goal_specs if goal.get("name")}
if not self.default_mission_name:
return []
for mission in self.mission_specs:
if mission["name"] != self.default_mission_name:
continue
return [
goal_lookup[goal_name]
for goal_name in mission.get("goals", [])
if goal_name in goal_lookup
]
return []
def start_odom_fallback_task(self) -> None:
self.reload_nav_task_config()
default_goals = self.get_default_mission_goals()
if not self.default_mission_name or not default_goals:
raise ValueError("odom task unavailable: no default mission loaded")
anchor_goal = default_goals[0]
yaw_deg = anchor_goal.get("yaw_deg")
if yaw_deg is None:
yaw_deg = 0.0
self.activate_odom_fallback(
float(anchor_goal["x"]),
float(anchor_goal["y"]),
math.radians(float(yaw_deg)),
str(anchor_goal.get("name", "route_p01")),
)
self.set_control_mode("NAV")
self.nav_cmd_pub.publish(String(data="reload"))
self.nav_cmd_pub.publish(String(data=f"run {self.default_mission_name}"))
def activate_odom_fallback(self, map_x: float, map_y: float, map_yaw: float, anchor_name: str) -> None:
self.validate_odom_fallback_preconditions()
try:
odom_to_base = self.tf_buffer.lookup_transform(
self.nav_odom_frame,
self.nav_base_frame,
rclpy.time.Time(),
)
except TransformException as exc:
raise ValueError(
f"odom fallback failed: missing TF {self.nav_odom_frame}->{self.nav_base_frame}: {exc}"
) from exc
t = odom_to_base.transform.translation
q = odom_to_base.transform.rotation
odom_x = float(t.x)
odom_y = float(t.y)
odom_yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w)
map_to_odom_yaw = self.normalize_angle(map_yaw - odom_yaw)
cos_yaw = math.cos(map_to_odom_yaw)
sin_yaw = math.sin(map_to_odom_yaw)
map_to_odom_x = map_x - (cos_yaw * odom_x - sin_yaw * odom_y)
map_to_odom_y = map_y - (sin_yaw * odom_x + cos_yaw * odom_y)
transform = TransformStamped()
transform.header.frame_id = self.nav_map_frame
transform.child_frame_id = self.nav_odom_frame
transform.transform.translation.x = map_to_odom_x
transform.transform.translation.y = map_to_odom_y
transform.transform.translation.z = 0.0
transform.transform.rotation.z = math.sin(map_to_odom_yaw * 0.5)
transform.transform.rotation.w = math.cos(map_to_odom_yaw * 0.5)
self.odom_fallback_transform = transform
self.odom_fallback_active = True
self.odom_fallback_handoff_pending = False
self.odom_fallback_handoff_info = {}
self.odom_fallback_anchor = {
"anchor": anchor_name,
"map_pose": {
"x": round(map_x, 3),
"y": round(map_y, 3),
"yaw_deg": round(math.degrees(map_yaw), 3),
},
"odom_pose_at_init": {
"x": round(odom_x, 3),
"y": round(odom_y, 3),
"yaw_deg": round(math.degrees(odom_yaw), 3),
},
"map_to_odom": {
"x": round(map_to_odom_x, 3),
"y": round(map_to_odom_y, 3),
"yaw_deg": round(math.degrees(map_to_odom_yaw), 3),
},
}
self.broadcast_odom_fallback()
self.latest_nav_status = (
f"odom fallback active: {anchor_name} -> "
f"map({map_x:.2f},{map_y:.2f},{math.degrees(map_yaw):.1f}deg)"
)
self.get_logger().info(self.latest_nav_status)
def validate_odom_fallback_preconditions(self) -> None:
if self.odom_fallback_require_odom_fresh:
state = self.latest_state
if state is None:
raise ValueError("odom fallback failed: runtime/state unavailable, cannot verify odom freshness")
odom_age_ms = float(state.odom_age_ms)
if not bool(state.odom_fresh):
raise ValueError(f"odom fallback failed: odom is not fresh (age={odom_age_ms:.1f}ms)")
if self.odom_fallback_max_odom_age_ms > 0.0 and odom_age_ms > self.odom_fallback_max_odom_age_ms:
raise ValueError(
"odom fallback failed: "
f"odom age {odom_age_ms:.1f}ms > {self.odom_fallback_max_odom_age_ms:.1f}ms"
)
if self.odom_fallback_block_existing_map_odom_tf and not self.odom_fallback_active:
if self.recent_external_map_odom_tf_exists():
direction = str(self.last_external_map_odom_tf.get("direction", "map<->odom"))
raise ValueError(
f"odom fallback failed: recent external {direction} TF detected. "
"Disable Odin relocalization/map TF before starting pure odom fallback."
)
try:
existing = self.tf_buffer.lookup_transform(
self.nav_map_frame,
self.nav_odom_frame,
rclpy.time.Time(),
)
except TransformException:
return
age_s = self.transform_age_s(existing)
if age_s is not None and age_s > self.odom_fallback_tf_conflict_window_s:
return
raise ValueError(
"odom fallback failed: existing map<->odom TF detected. "
"Disable Odin relocalization/map TF before starting pure odom fallback."
)
def recent_external_map_odom_tf_exists(self) -> bool:
if not self.last_external_map_odom_tf:
return False
stamp = self.last_external_map_odom_tf.get("stamp")
if not isinstance(stamp, (int, float)):
return False
return self.now_sec() - float(stamp) <= self.odom_fallback_tf_conflict_window_s
def transform_age_s(self, transform: TransformStamped) -> Optional[float]:
stamp = self.stamp_to_sec(transform.header.stamp)
if stamp <= 0.0:
return None
return max(0.0, self.now_sec() - stamp)
def is_own_odom_fallback_tf(self, transform: TransformStamped) -> bool:
expected = self.odom_fallback_transform
if expected is None:
return False
t = transform.transform.translation
e = expected.transform.translation
xy_error = math.hypot(float(t.x) - float(e.x), float(t.y) - float(e.y))
q = transform.transform.rotation
eq = expected.transform.rotation
yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w)
expected_yaw = self.quaternion_to_yaw(eq.x, eq.y, eq.z, eq.w)
yaw_error = abs(self.normalize_angle(yaw - expected_yaw))
return (
xy_error <= self.odom_fallback_tf_conflict_xy_tolerance
and yaw_error <= self.odom_fallback_tf_conflict_yaw_tolerance
)
def stop_odom_fallback(self, reason: str, stop_nav: bool = True) -> None:
was_active = self.odom_fallback_active
self.odom_fallback_active = False
self.odom_fallback_transform = None
self.odom_fallback_anchor = {}
self.odom_fallback_handoff_pending = False
self.odom_fallback_handoff_info = {}
if stop_nav:
self.nav_cmd_pub.publish(String(data="stop"))
status = f"odom fallback stopped: {reason}" if was_active else f"odom fallback already inactive: {reason}"
self.latest_nav_status = status
self.get_logger().info(status)
def on_odom_fallback_timer(self) -> None:
if self.odom_fallback_active:
self.broadcast_odom_fallback()
def broadcast_odom_fallback(self) -> None:
if self.odom_fallback_transform is None:
return
transform = self.odom_fallback_transform
transform.header.stamp = self.get_clock().now().to_msg()
self.tf_broadcaster.sendTransform(transform)
def parse_twist(self, payload: dict[str, Any]) -> Twist:
cmd = Twist()
linear = payload.get("linear", {}) or {}
@@ -918,7 +1201,14 @@ class WebUdpBridgeNode(Node):
"pose": self.lookup_pose(),
"map_frame": self.nav_map_frame,
"base_frame": self.nav_base_frame,
"odom_frame": self.nav_odom_frame,
"path": dict(self.latest_nav_path),
"odom_fallback": {
"active": self.odom_fallback_active,
"handoff_pending": self.odom_fallback_handoff_pending,
"handoff": dict(self.odom_fallback_handoff_info),
**self.odom_fallback_anchor,
},
},
"model": dict(self.latest_model_status),
}
@@ -975,6 +1265,13 @@ class WebUdpBridgeNode(Node):
def now_sec(self) -> float:
return self.get_clock().now().nanoseconds / 1.0e9
@staticmethod
def stamp_to_sec(stamp: Any) -> float:
try:
return float(stamp.sec) + float(stamp.nanosec) / 1.0e9
except Exception:
return 0.0
@staticmethod
def twist_to_dict(msg: Twist) -> dict[str, Any]:
return {
05_software/real/sim2real_ros2_v2/tools/pcd_nav_pygame.py
+1502
View File
File diff suppressed because it is too large Load Diff
05_software/real/sim2real_ros2_v2/tools/run_a_min_route_editor.bat
+7
View File
@@ -0,0 +1,7 @@
@echo off
setlocal
cd /d "%~dp0\.."
python .\tools\pcd_nav_pygame.py --pcd .\map\A_min.pcd --route-name A_min_route --route-map A_min --route-obstacle slalom --save-route .\map\routes\A_min\A_min_route.json --default-speed 0.35 --default-policy rough --default-tolerance 0.15
endlocal
05_software/real/sim2real_ros2_v2/tools/run_pcd_nav_map.bat
+7
View File
@@ -0,0 +1,7 @@
@echo off
setlocal
cd /d "%~dp0\.."
python .\tools\pcd_nav_pygame.py --pcd .\map\map_b.pcd --overlay-json .\tools\semantic_overlay_example.json
endlocal
05_software/real/sim2real_ros2_v2/tools/semantic_overlay_example.json
+52
View File
@@ -0,0 +1,52 @@
{
"elements": [
{
"name": "wall_01",
"label": "wall",
"type": "rect",
"x": 1.8,
"y": -7.0,
"length": 1.0,
"width": 0.05,
"yawDeg": 90.0
},
{
"name": "low_bar_01",
"label": "low_bar",
"type": "rect",
"x": 5.7,
"y": -9.0,
"length": 1.0,
"width": 0.03,
"yawDeg": 0.0
},
{
"name": "slalom_pole_01",
"label": "slalom_pole",
"type": "circle",
"x": 1.8,
"y": -10.5,
"radius": 0.05
},
{
"name": "stairs_area",
"label": "stairs",
"type": "rect",
"x": 2.4,
"y": -3.5,
"length": 2.4,
"width": 1.2,
"yawDeg": 0.0
},
{
"name": "ramp_area",
"label": "ramp",
"type": "rect",
"x": 5.2,
"y": -5.2,
"length": 2.0,
"width": 3.0,
"yawDeg": 0.0
}
]
}
05_software/real/sim2real_ros2_v2/tools/win_web_debug/server.py
+3
View File
@@ -25,6 +25,9 @@ class Handler(SimpleHTTPRequestHandler):
self._json(200, data)
return
if self.path == "/api/map":
global LATEST_MAP
with STATE_LOCK:
LATEST_MAP = None
send_udp({"type": "map_request", "stamp": time.time()})
deadline = time.time() + 1.0
while time.time() < deadline:
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/app.js
+92 -6
View File
@@ -25,6 +25,7 @@ let goalPointsByName = {};
let defaultMissionName = '';
let defaultMissionGoals = [];
let routeAlignmentInfo = {};
let odomFallbackInfo = { active: false };
let activeNavPath = { goal_name: '', stage: 'idle', path_index: 0, points: [] };
const mapCanvas = $('map-canvas');
@@ -89,19 +90,38 @@ function highlightMode(mode) {
function updateTaskButtonState() {
const btn = $('btn-run-task');
if (!btn) return;
const odomBtn = $('btn-odom-task');
const odomStopBtn = $('btn-odom-stop');
const hasMission = Boolean(defaultMissionName);
const enabled = latestConnected && currentMode === 'NAV' && hasMission;
btn.disabled = !enabled;
const runEnabled = latestConnected && currentMode === 'NAV' && hasMission;
const odomEnabled = latestConnected && hasMission;
if (btn) {
btn.disabled = !runEnabled;
btn.textContent = hasMission ? `Run ${defaultMissionName}` : 'Run Mission';
if (!hasMission) {
btn.title = 'No default mission loaded from backend';
return;
}
btn.title = enabled
} else {
btn.title = runEnabled
? `Run default mission ${defaultMissionName}`
: 'Available only in NAV mode while connected';
}
}
if (odomBtn) {
odomBtn.disabled = !odomEnabled;
odomBtn.title = hasMission
? 'Initialize map->odom from waypoint id1, switch to NAV, then run the current route'
: 'No default mission loaded from backend';
}
if (odomStopBtn) {
odomStopBtn.disabled = !(latestConnected && odomFallbackInfo.active);
odomStopBtn.title = odomFallbackInfo.active
? 'Stop pure odom fallback and stop simple_nav'
: 'Odom fallback is not active';
}
}
function setText(id, text, cls) {
const el = $(id);
@@ -355,6 +375,17 @@ function formatRouteAlignment(info) {
return `${applied} hits=${hits} ${reason}`;
}
function formatOdomFallback(info) {
if (!info || !info.active) return 'inactive';
const anchor = info.anchor || 'id1';
const pose = info.map_pose || {};
const x = Number.isFinite(pose.x) ? Number(pose.x).toFixed(2) : '--';
const y = Number.isFinite(pose.y) ? Number(pose.y).toFixed(2) : '--';
const yaw = Number.isFinite(pose.yaw_deg) ? Number(pose.yaw_deg).toFixed(1) : '--';
const handoff = info.handoff_pending ? ' handoff pending' : '';
return `${anchor} x=${x} y=${y} yaw=${yaw}deg${handoff}`;
}
function renderTaskGoals() {
const list = $('task-goals-list');
if (!list) return;
@@ -392,6 +423,7 @@ function applyMapMetadata(data) {
defaultMissionName = data.default_mission_name || '';
defaultMissionGoals = Array.isArray(data.default_mission_goals) ? data.default_mission_goals : [];
routeAlignmentInfo = data.route_alignment || {};
odomFallbackInfo = data.odom_fallback || odomFallbackInfo;
activeNavPath = data.nav_path || activeNavPath;
currentGoalIndex = currentGoalName
? defaultMissionGoals.findIndex(goal => goal.name === currentGoalName)
@@ -401,6 +433,11 @@ function applyMapMetadata(data) {
setText('d-task-mission', defaultMissionName || '--', defaultMissionName ? 'ok' : '');
setText('d-task-goals', String(defaultMissionGoals.length || 0), defaultMissionGoals.length ? 'ok' : '');
setText('d-route-align', formatRouteAlignment(routeAlignmentInfo), routeAlignmentInfo.enabled ? 'ok' : 'warn');
setText(
'd-odom-fallback',
formatOdomFallback(odomFallbackInfo),
odomFallbackInfo.handoff_pending ? 'warn' : odomFallbackInfo.active ? 'active' : '',
);
updateTaskButtonState();
renderTaskGoals();
}
@@ -527,6 +564,14 @@ function applyState(data) {
syncGoalMarkerFromNavStatus(data.nav_status || '');
const nav = data.nav || {};
if (nav.odom_fallback) {
odomFallbackInfo = nav.odom_fallback;
setText(
'd-odom-fallback',
formatOdomFallback(odomFallbackInfo),
odomFallbackInfo.handoff_pending ? 'warn' : odomFallbackInfo.active ? 'active' : '',
);
}
if (nav.pose) {
latestPose = nav.pose;
setText('d-nav-pose', `x=${nav.pose.x.toFixed(2)} y=${nav.pose.y.toFixed(2)} yaw=${(nav.pose.yaw * 57.2958).toFixed(1)}deg`);
@@ -605,6 +650,47 @@ $('btn-estop').onclick = async () => {
}
};
$('btn-refresh-map').onclick = fetchMap;
$('btn-odom-task').onclick = async () => {
if (!latestConnected) {
appendEvent('ODOM', 'bridge not connected', 'warn');
updateTaskButtonState();
return;
}
if (!defaultMissionName || !defaultMissionGoals.length) {
appendEvent('ODOM', 'no route mission loaded', 'warn');
return;
}
const start = defaultMissionGoals[0];
const ok = confirm(
`Run odom fallback mission ${defaultMissionName}?\n\n` +
`Place the robot at waypoint id1 before confirming:\n` +
`x=${Number(start.x).toFixed(3)} y=${Number(start.y).toFixed(3)} yaw=${Number(start.yaw_deg || 0).toFixed(1)}deg`
);
if (!ok) return;
setLastGoalByName(start.name);
drawMap();
const result = await post({ type: 'odom_task' });
if (result.ok) {
highlightMode('NAV');
appendEvent('ODOM', `init at ${start.name}, run ${defaultMissionName}`, 'ok');
setTimeout(fetchMap, 400);
}
};
$('btn-odom-stop').onclick = async () => {
if (!latestConnected) {
appendEvent('ODOM', 'bridge not connected', 'warn');
updateTaskButtonState();
return;
}
const result = await post({ type: 'odom_stop' });
if (result.ok) {
odomFallbackInfo = { active: false };
setText('d-odom-fallback', formatOdomFallback(odomFallbackInfo), '');
updateTaskButtonState();
appendEvent('ODOM', 'fallback stopped', 'ok');
setTimeout(fetchMap, 400);
}
};
$('btn-run-task').onclick = async () => {
if (!latestConnected) {
appendEvent('TASK', 'bridge not connected', 'warn');
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/index.html
+5 -2
View File
@@ -4,7 +4,7 @@
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>sim2real ROS2 Console</title>
<link rel="stylesheet" href="style.css?v=mission-sync-2">
<link rel="stylesheet" href="style.css?v=odom-fallback-1">
</head>
<body>
<header class="glass-panel top-bar">
@@ -56,6 +56,7 @@
<div class="diag-row"><span class="diag-label">active goal</span><span class="diag-value" id="d-active-goal">--</span></div>
<div class="diag-row"><span class="diag-label">goal count</span><span class="diag-value" id="d-task-goals">--</span></div>
<div class="diag-row"><span class="diag-label">route align</span><span class="diag-value" id="d-route-align">--</span></div>
<div class="diag-row"><span class="diag-label">odom fallback</span><span class="diag-value" id="d-odom-fallback">--</span></div>
</div>
<div class="panel-section flex-1">
@@ -70,6 +71,8 @@
<h2 class="panel-title">PCD 2D Nav</h2>
<div class="map-actions">
<button class="btn" id="btn-refresh-map">Refresh Map</button>
<button class="btn btn-odom" id="btn-odom-task" disabled>odom</button>
<button class="btn" id="btn-odom-stop" disabled>Exit odom</button>
<button class="btn btn-nav" id="btn-run-task" disabled>Run Mission</button>
<button class="btn" id="btn-record">Record Pose</button>
<button class="btn" id="btn-stop-nav">Stop Nav</button>
@@ -121,6 +124,6 @@
</div>
</div>
<script src="app.js?v=mission-sync-2"></script>
<script src="app.js?v=odom-fallback-1"></script>
</body>
</html>
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/style.css
+1
View File
@@ -105,6 +105,7 @@ body {
.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-odom { background: rgba(255,159,10,0.22); border-color: rgba(255,159,10,0.45); color: #ffd199; }
.btn.active-mode { box-shadow: 0 0 0 2px white; }
.route-select {
README.md
+1 -1
View File
@@ -39,7 +39,7 @@ RC_WheelLeg/
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
- [x] 整理 Python Sim2Real v2、ROS 2/C++ 初版、导航原型与 Odin/TensorRT 调参版
- [x] 整理 Python Sim2Real v2、ROS 2/C++ 初版、导航原型、Odin/TensorRT 调参版与 odom 联调版
- [ ] 核对比赛机械与仿真模型参数
- [ ] 整理 URDF/MJCF 机器人描述
- [ ] 整理后续统一训练、ROS 2 和比赛版本