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

01_doc/version_history.md

@@ -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

@@ -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

@@ -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

@@ -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 策略为 `model_9600`，默认站姿回到比赛站姿髋俯仰 `0.550`、膝关节 `-1.125`。
-- 默认 Rough 站姿为髋俯仰 `0.670`、膝关节 `-1.390`，源目录名中的 `z=0.380` 记录该阶段的机身高度配置意图。
+- Odin `custom_map_mode` 固定为 `0`（纯里程计），避免没有 `map_a.bin` 时产生 map/odom TF 冲突。
-- `map_b.pcd` 已确定性抽样到 10 MB 以下，点数和哈希见 [`map/README.md`](map/README.md)。
+- `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/README.md

@@ -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
+| 文件 | 原始点数 | 步长 | 抽样点数 | 抽样大小 | 原始 SHA-256 | 抽样 SHA-256 |
-- 抽样大小：8,680,283 字节
+| --- | ---: | ---: | ---: | ---: | --- | --- |
-- 原始 SHA-256：`F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7`
+| `map_b.pcd` | 1,080,047 | 4 | 270,012 | 8,680,283 B | `F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7` | `F3052431F217DE8FFFE87726F8E547A59C65048B0285F02AF4803C5F4E4C1B42` |
-- 抽样 SHA-256：`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` |
-抽样文件适合路线编辑和代码演示，不等价于原始高密度点云；需要复现实机导航精度时应恢复原始 PCD 并核对哈希。

05_software/real/sim2real_ros2_v2/map/routes/A_min/A_min_route.json

@@ -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/README.md

@@ -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` |
+| `model_9600.onnx` | 里程计版本 Rough ONNX |
-| `NEWmodel_1900_fp16.engine` | Rough 比赛机 TensorRT | `DBC73AC9B3780029EC08BE19B9972B9840A067EBD146DEB860698F4F7066CC03` |
+| `model_9600_fp16.engine` | 里程计版本 Rough TensorRT |
-| `model_crawl.onnx` | Crawl 策略候选；当前配置使用 IK 后端 | `633AB2F3C3737FDDFD657F4A006A414BFA3950DD60D708B101E6744DF9E6DF67` |
+| `model_crawl.onnx` | Crawl 候选；当前配置使用 IK 后端 |
-| `model_rough.onnx` | 原部署契约参考基线 | `4149BE180A197E70495A7A6D186C62BE43271A4CE9035A8EEE56CD293D869A27` |
+| `model_rough.onnx` | 原部署契约参考基线 |
 
-TensorRT engine 不保证跨 JetPack、TensorRT 版本或 GPU 架构兼容。
+其余 `model_6800`、`model_8400`、`model_10200` 等候选策略属于相邻实验或后续比赛版本，不在本 Tag 重复归档。

05_software/real/sim2real_ros2_v2/src/odin_ros_driver/config/control_command.yaml

@@ -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

@@ -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

@@ -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
+    model_path: policies/model_9600.onnx
-    rough_model_engine_path: policies/NEWmodel_1900_fp16.engine
+    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

@@ -64,10 +64,10 @@ struct DeploymentContract
   };
 
   static constexpr std::array<float, 16> kDefaultDofPos = {
-    0.0f, 0.670f, -1.390f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.670f, -1.390f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.670f, -1.390f,
+    0.0f, 0.550f, -1.125f,
-    0.0f, 0.670f, -1.390f,
+    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

@@ -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

@@ -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;
+  return writeParameterInt(fd, motor_id, PARAM_MODE, static_cast<std::uint8_t>(mode));
-  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)
@@ -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

@@ -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

@@ -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

@@ -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 = self.get_default_mission_goals()
-        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
         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/run_a_min_route_editor.bat

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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;
+  const runEnabled = latestConnected && currentMode === 'NAV' && hasMission;
-  btn.disabled = !enabled;
+  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;
+    } else {
-  }
+      btn.title = runEnabled
-  btn.title = enabled
         ? `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

@@ -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

@@ -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

@@ -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 和比赛版本