diff --git a/.gitignore b/.gitignore index ab3083c..6d9b0e2 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,7 @@ log/ !05_software/real/sim2real/vendored/odin1_imu/lib/*.a !05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a !05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a +!05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/*.a # Training outputs logs/ diff --git a/01_doc/version_history.md b/01_doc/version_history.md index c0a392d..1c3d778 100644 --- a/01_doc/version_history.md +++ b/01_doc/version_history.md @@ -19,8 +19,10 @@ | `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 | | `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 | | `v0.12.0` | 里程计导航联调 | 纯里程计 fallback、A_min 路线、TF 冲突保护和 model_9600 | -| `v1.0.0` | 比赛最终部署 | last_not_slalom_1050、model_6800/model_84、最终路线和触控屏 | +| `v1.0.0` | ROS 2 v3 比赛最终部署 | last_not_slalom_1050、model_6800/model_84、最终路线和触控屏 | | `v1.0.1` | 比赛成果媒体补充 | 最终机器人图片与 1050 分比赛视频 | +| `v1.0.2` | 文档一致性修正 | 统一历史 Tag、当前快照和成果媒体的描述 | +| `v1.1.0` | 三代目录规范化 | 恢复无后缀初版、保留 v2 里程计版、明确最终比赛 v3，并校准训练 README | > 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排后已正式归入 `v1.0.0`。 @@ -66,7 +68,7 @@ ## `v1.0.0` 的比赛最终部署 -- 归档 `last_not_slalom_1050` 最终 ROS 2/C++ 真机工程；`1050` 是比赛成绩，不是模型编号。 +- 将原始 `sim2real_ros2_v2(last_not_slalom_1050)` 正式整理为 `real/sim2real_ros2_v3`；这是第三版、最终 ROS 2/C++ 真机工程，`1050` 是比赛成绩，不是模型编号。 - Rough 使用 `model_6800`，Wall 使用 `model_84`，Crawl 按比赛配置使用 IK 后端。 - 保留最终五份路线、1 号场地抽样 PCD、Odin 驱动、CAN 硬件桥、命令仲裁、导航和 Orin 触控屏 UI。 - 最终配置默认命令源为 `NAV`、定位模式为 `relocal`，但真实 Odin `1hao.bin` 不在备份中，重定位闭环需要从比赛设备补回。 @@ -76,7 +78,19 @@ - 保持 `v1.0.0` 的比赛最终代码和部署内容不变。 - 补充最终机器人图片和比赛视频，成绩为 1050 分、第七名（前 5%）。 -- `main` 当前指向该补丁版本；代码复现仍应优先查看 `v1.0.0`，完整成果展示可查看 `v1.0.1`。 +- 代码复现可查看 `v1.0.0`，包含成果媒体的对应快照可查看 `v1.0.1`。 + +## `v1.0.2` 的文档一致性修正 + +- 统一 ROS 2 历史 Tag、当前工作树和媒体补丁的说明。 +- 该版本仅修正文档，没有改变训练或真机运行代码。 + +## `v1.1.0` 的目录与说明规范化 + +- 从 `v0.10.0` 恢复无后缀 `sim2real_ros2` 初版快照。 +- `sim2real_ros2_v2` 保持 `v0.12.0` 里程计联调快照。 +- 将 `last_not_slalom_1050` 最终比赛部署正式命名为 `sim2real_ros2_v3`。 +- 依据当前源码重新校准 `rc_mjlab` README 中的物理步长、控制频率、环境数、执行器、地形、奖励和随机化说明。 ## `v0.4.0` 的模型变化 diff --git a/05_software/README.md b/05_software/README.md index 7740dbd..38b9942 100644 --- a/05_software/README.md +++ b/05_software/README.md @@ -11,10 +11,11 @@ ├─ sim2real/ # 第一代 Python 策略真机部署 ├─ sim2real_v2/ # Python Sim2Real v2 ├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版 - └─ sim2real_ros2_v2/ # ROS 2 导航原型及后续演进 + ├─ sim2real_ros2_v2/ # ROS 2 导航原型及里程计演进 + └─ sim2real_ros2_v3/ # 最终比赛 ROS 2/C++ 部署 ``` -上图是软件版本线的目录入口，不表示每个目录仍停留在其最初阶段。当前 `main` 工作树中，`sim2real_ros2` 保存比赛最终部署快照（代码里程碑 `v1.0.0`，媒体补充为 `v1.0.1`），`sim2real_ros2_v2` 保存 `v0.12.0` 里程计联调快照；历史初版请通过对应 Tag 恢复。 +当前工作树按架构大版本同时保留三个 ROS 2 目录：无后缀目录是初版，`_v2` 是第二版演进的最终里程计快照，`_v3` 是 `last_not_slalom_1050` 最终比赛部署。各目录内部的小阶段仍可通过对应 Tag 恢复。 ## 数据流 diff --git a/05_software/real/README.md b/05_software/real/README.md index 2603803..365d8ee 100644 --- a/05_software/real/README.md +++ b/05_software/real/README.md @@ -28,13 +28,9 @@ Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新 ## ROS 2/C++ 版本线 -以下目录名表示版本线的演进路径；历史阶段通过对应 Tag 恢复，不能把当前工作树中的目录内容直接当作最初快照。 +### `sim2real_ros2`（初版，`v0.10.0`） -### `sim2real_ros2`（历史 `v0.10.0`，当前为最终部署快照） - -在 `v0.10.0` 中，这是 ROS 2/C++ Sim2Real 初版：将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该历史 Tag 未随工程保存 Odin ROS 2 驱动源码，依赖边界以对应 Tag 的 README 为准。 - -当前 `main`/`v1.0.1` 工作树中的同一路径已经被整理为 `v1.0.0` 比赛最终部署快照，包含完整 Odin 驱动、TensorRT、多策略、最终路线、CAN 和触控屏；因此查看初版请切换到 `v0.10.0`，查看比赛版请使用 `v1.0.0` 或 `v1.0.1`。 +无后缀目录固定表示 ROS 2/C++ Sim2Real 初版：将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。原始快照未随工程保存 Odin ROS 2 驱动源码，依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。 ### `sim2real_ros2_v2`（`v0.11.0`～`v0.12.0`） @@ -44,6 +40,10 @@ Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新 `v0.12.0` 仍在同一路径上形成里程计导航联调快照：固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。当前该目录保持 `v0.12.0` 快照，阶段说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。 +### `sim2real_ros2_v3`（最终比赛版，`v1.0.0`） + +第三版来自原始目录 `sim2real_ros2_v2(last_not_slalom_1050)`，整理时正式命名为 `sim2real_ros2_v3`。它是 1050 分比赛最终部署，包含 `model_6800` Rough、`model_84` Wall、最终路线、完整 Odin 驱动、CAN 和触控屏。部署说明见 [`sim2real_ros2_v3/README.md`](sim2real_ros2_v3/README.md)。 + ## 实机记录 [![第一代 Sim2Real 真机验证](../../06_assets/images/early_sim2real_preview.jpg)](../../06_assets/videos/early_sim2real.mp4) diff --git a/05_software/real/sim2real_ros2/.gitignore b/05_software/real/sim2real_ros2/.gitignore index 2641ed6..8ba3d84 100644 --- a/05_software/real/sim2real_ros2/.gitignore +++ b/05_software/real/sim2real_ros2/.gitignore @@ -1,14 +1,6 @@ build/ install/ log/ -logs_v2_web/ -map/load/ -src/odin_ros_driver/log/ -src/odin_ros_driver/recorddata/ -src/odin_ros_driver/image/ -*.bak_* -__pycache__/ -*.py[cod] .colcon/ .vscode/ compile_commands.json diff --git a/05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md b/05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md index 00ee11b..17b4c70 100644 --- a/05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md +++ b/05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md @@ -156,7 +156,7 @@ sudo udevadm trigger 统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈： * `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。 -* `launch_nav2`（默认：`false`）：按需启动 ROS2 Navigation2；比赛默认使用 `simple_nav_node.py` 的路线跟踪。 +* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。 #### 1. 完整真实硬件闭环（默认） 启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航： @@ -175,3 +175,4 @@ ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driv ```bash ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false ``` + diff --git a/05_software/real/sim2real_ros2/Dockerfile b/05_software/real/sim2real_ros2/Dockerfile index 5753fa4..d7b8907 100644 --- a/05_software/real/sim2real_ros2/Dockerfile +++ b/05_software/real/sim2real_ros2/Dockerfile @@ -62,7 +62,6 @@ COPY src/sim2real_nav2 sim2real_nav2 # 拷贝策略文件与运行脚本 WORKDIR /sim2real_ws COPY policies policies -COPY map map COPY start_sim2real.sh start_sim2real.sh RUN chmod +x start_sim2real.sh diff --git a/05_software/real/sim2real_ros2/README.md b/05_software/real/sim2real_ros2/README.md index 8bec28c..25d4398 100644 --- a/05_software/real/sim2real_ros2/README.md +++ b/05_software/real/sim2real_ros2/README.md @@ -1,106 +1,74 @@ -# ROS 2 最终比赛 Sim2Real +# ROS 2/C++ Sim2Real 初版 -本目录归档 `last_not_slalom_1050` 真机工程，对应 RC_WheelLeg 在 RoboCon 仿生足式障碍赛使用的最终 ROS 2 部署栈。`1050` 是比赛得分，不是模型编号；比赛 Rough 策略为 `model_6800.onnx`。 +本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。 -该里程碑对应重排主线的 `v1.0.0`，是 1050 分比赛最终版本。训练架构和策略来源见 `v0.6.0`，比赛 Rough 模型首次归档见 `v0.8.0`，导航打点与路线演进见 `v0.8.1`，ROS 2 迁移过程见 `v0.10.0`～`v0.12.0`。 +本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++： -当前 `main` 的 `v1.0.1` 只是在 `v1.0.0` 基础上补充比赛机器人图片和比赛视频，本目录代码仍以 `v1.0.0` 为比赛部署基线。 +- `53D` 策略观测契约不变 +- `16D` 动作契约不变 +- `50Hz` 策略循环与训练对齐 +- `200Hz` 电机循环为专用 C++ 热路径 +- ROS 2 作为导航、TF、诊断和启动管理的系统集成层 -## 系统闭环 +## 工作区布局 + +- `src/sim2real_interfaces` + 硬件桥接与策略运行时共享的 ROS 2 消息定义。 +- `src/sim2real_common` + 共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。 +- `src/sim2real_hw` + 面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。 +- `src/sim2real_runtime` + 策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。 + 同时包含 `odom_relay_node`（里程计中继与 TF 广播）。 +- `src/sim2real_nav2` + ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。 +- `src/sim2real_bringup` + 统一启动文件与运行时参数配置。 +- `src/odin_ros_driver` + Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。 +- `docs` + 架构说明与迁移计划。 + +## 目标架构 ```text -Odin IMU / Odom ──> hardware bridge ──> RuntimeState - | -导航 / 遥控 / 屏幕 ──> cmd mux ──> policy runtime (50 Hz) - | - RuntimeTarget - | - hardware bridge / CAN (200 Hz) +Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw + | | | + v v v + RuntimeState RuntimeTarget 电机 CAN 指令 + | | + +-------> 诊断 / 遥测 + +Nav2 / cmd_vel ------------------------------> sim2real_runtime + (经 odom_relay_node 提供 odom→base_link TF) ``` -核心约束： +## 当前状态 -- 53 维策略观测、16 维动作输出。 -- Rough：`model_6800`，优先 TensorRT，失败时回退 ONNX Runtime。 -- Wall：`model_84`，同样保留 TensorRT 与 ONNX 两种文件。 -- Crawl：比赛配置使用解析 IK，不加载 Crawl RL 权重。 -- 默认站姿：髋俯仰 `0.550`、膝关节 `-1.125`。 -- 默认命令源：`NAV`；默认定位模式：`relocal`。 +已完成 Phase 0-5 的全部迁移： -## 目录 +1. ✅ 冻结部署契约（deployment_contract.hpp） +2. ✅ ROS 2 包结构搭建 +3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环） +4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环） +5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF） -```text -sim2real_ros2/ -├─ src/ -│ ├─ sim2real_interfaces/ # RuntimeState / RuntimeTarget 消息 -│ ├─ sim2real_common/ # 部署契约、滤波、平衡和安全监控 -│ ├─ sim2real_hw/ # SocketCAN、IMU 和 200 Hz 电机热路径 -│ ├─ sim2real_runtime/ # 策略、命令仲裁、导航、Web API -│ ├─ sim2real_nav2/ # Nav2 配置入口 -│ ├─ sim2real_bringup/ # 统一参数和启动文件 -│ └─ odin_ros_driver/ # Odin ROS 驱动（Apache-2.0） -├─ policies/ # 比赛实际使用的 Rough / Wall 模型 -├─ map/ # 比赛路线和抽样 PCD -├─ screen/ # Orin 800×600 触控面板 -├─ docs/ # 架构、遥控、Web 和迁移说明 -├─ Dockerfile -└─ start_sim2real.sh -``` +## 契约来源 -## 构建与运行 +迁移过程中以下文件被视为真值源： -目标环境是 Ubuntu 22.04、ROS 2 Humble 和 Jetson Orin。系统依赖和 Docker 流程见 [`DEPLOYMENT_GUIDE.md`](DEPLOYMENT_GUIDE.md)。 +- `sim2real/deployment_manifest.yaml` +- `sim2real/interface/motor_mapping.py` +- `sim2real/interface/real_io.py` +- `sim2real/policy/policy_runner.py` +- `sim2real/web/session.py` -```bash -cd 05_software/real/sim2real_ros2 -colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release -./start_sim2real.sh -``` +## 注意事项 -运行参数和模型/路线均使用工作区根目录相对路径，因此应从本目录启动。常用启动覆盖： - -```bash -# 纯里程计模式，不等待 Odin 重定位地图 -./start_sim2real.sh localization_mode:=odom \ - odin_config_file:=src/odin_ros_driver/config/control_command_odom.yaml - -# 禁止驱动，仅做软件链路检查 -./start_sim2real.sh launch_driver:=false launch_remote:=false -``` - -## 必须补充的部署资产 - -最终源目录配置引用了 Odin `map/1hao.bin`，但工作区备份中不存在这个文件；全盘检索也未找到同名文件。为避免用来源不明的 `.bin` 冒充比赛地图，本仓库不伪造该资产。 - -使用 `relocal` 前必须： - -1. 从比赛 Orin 或 Odin 建图备份取得真实 `1hao.bin`。 -2. 修改 `src/odin_ros_driver/config/control_command_relocal.yaml` 中的 `relocalization_map_abs_path` 为目标机绝对路径。 -3. 核对文件哈希并在发布说明中补充来源。 - -缺少该文件时请使用 `localization_mode:=odom`，不要宣称重定位闭环已复现。地图和路线边界见 [`map/README.md`](map/README.md)。 - -## 归档边界 - -已保留： - -- 最终六个 ROS 2 包、Odin 驱动源码、比赛设备标定参数和预编译 SDK 静态库。 -- 最终 Rough/Wall ONNX 与比赛机 TensorRT engine。 -- 五份最终工程路线、1 号场地抽样 PCD、屏幕 UI 和启动脚本。 -- Odin 驱动 Apache-2.0 许可证。 - -未保留： - -- 嵌套 `.git`、`__pycache__`、日志、备份、构建/安装目录。 -- 未被比赛配置引用的候选模型与候选 TensorRT engine。 -- 开发计划、任务草稿、重复地图工具和运行时轨迹。 -- 原备份中大小为 0 的浏览器静态页面；HTTP JSON API 和屏幕 UI 源码仍保留。 - -TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关；其他机器应从同名 ONNX 重新生成，不应默认复用比赛 engine。模型哈希见 [`policies/README.md`](policies/README.md)。 - -## 安全与开源状态 - -- 真机运行前必须架空轮组验证 CAN 映射、方向、零位、急停和限幅。 -- `deployment_contract.hpp` 是电机映射和动作缩放真值源；参考 YAML 不会自动修改 C++ 契约。 -- 自研 ROS 包的 `package.xml` 仍保留原工程的 `Proprietary` 字段。迁移到 GitHub 公共开源前，需要由项目负责人选择许可证并统一修改；本次整理不代替权利人作许可证决定。 -- 当前 Windows 环境只能做静态检查，不能证明 ROS 2、SocketCAN、Odin SDK 或 TensorRT 真机运行成功。 +- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。 +- Windows 仅作为编辑环境使用。 +- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改， + 除非训练与部署同步更新。 +- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。 +- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。 diff --git a/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md b/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md index 8837954..2c56a6b 100644 --- a/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md +++ b/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md @@ -21,7 +21,7 @@ src/sim2real_runtime/src/remote_uart_node.py ## 2. 通道映射 -通道映射与本仓库第一代 Python Sim2Real 实现中的遥控器配置保持一致。 +通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。 | 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 | |---|---|---|---:| @@ -64,10 +64,6 @@ remote_invert_vy: false remote_invert_yaw: true remote_publish_inactive_zero: true remote_estop_latch: true -remote_estop_channel: 7 -remote_estop_level: "high" -remote_estop_debounce_frames: 3 -remote_estop_require_remote_mode: true remote_poll_hz: 50.0 ``` @@ -280,7 +276,7 @@ ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false /safety/estop: true ``` -由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。 +由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。 ### 8.4 机器人行为效果 diff --git a/05_software/real/sim2real_ros2/image/cam_in_ex.txt b/05_software/real/sim2real_ros2/image/cam_in_ex.txt new file mode 100644 index 0000000..c831cff --- /dev/null +++ b/05_software/real/sim2real_ros2/image/cam_in_ex.txt @@ -0,0 +1,20 @@ +Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150, + 0.002390, -0.000340, -1.000000, -0.011850, + 0.999960, -0.009160, 0.002390, 0.005360, + 0.000000, 0.000000, 0.000000, 1.000000] +cam_0: + image_width: 1600 + image_height: 1296 + k2: 0.000656 + k3: -0.028961 + k4: 0.045390 + k5: -0.064513 + k6: 0.038735 + k7: -0.009903 + p1: 0.000000 + p2: 0.000000 + A11: 736.894262 + A12: -0.161150 + A22: 736.611354 + u0: 806.125535 + v0: 639.650710 diff --git a/05_software/real/sim2real_ros2/policies/model_4700.onnx b/05_software/real/sim2real_ros2/policies/model_4700.onnx new file mode 100644 index 0000000..ac3887c Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_4700.onnx differ diff --git a/05_software/real/sim2real_ros2/policies/model_4700.onnx.data b/05_software/real/sim2real_ros2/policies/model_4700.onnx.data new file mode 100644 index 0000000..1f267ba Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_4700.onnx.data differ diff --git a/05_software/real/sim2real_ros2/policies/model_6000.onnx b/05_software/real/sim2real_ros2/policies/model_6000.onnx new file mode 100644 index 0000000..163a04d Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_6000.onnx differ diff --git a/05_software/real/sim2real_ros2/policies/model_6000.onnx.data b/05_software/real/sim2real_ros2/policies/model_6000.onnx.data new file mode 100644 index 0000000..12ac30b Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_6000.onnx.data differ diff --git a/05_software/real/sim2real_ros2/policies/model_crawl.onnx b/05_software/real/sim2real_ros2/policies/model_crawl.onnx new file mode 100644 index 0000000..c1acf18 Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_crawl.onnx differ diff --git a/05_software/real/sim2real_ros2/policies/model_rough.onnx b/05_software/real/sim2real_ros2/policies/model_rough.onnx new file mode 100644 index 0000000..c22dbf2 Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_rough.onnx differ diff --git a/05_software/real/sim2real_ros2/policies/model_rough_dontkw.onnx b/05_software/real/sim2real_ros2/policies/model_rough_dontkw.onnx new file mode 100644 index 0000000..eadebd8 Binary files /dev/null and b/05_software/real/sim2real_ros2/policies/model_rough_dontkw.onnx differ diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/README.md b/05_software/real/sim2real_ros2/src/odin_ros_driver/README.md index f099caa..b1ee747 100644 --- a/05_software/real/sim2real_ros2/src/odin_ros_driver/README.md +++ b/05_software/real/sim2real_ros2/src/odin_ros_driver/README.md @@ -1,821 +1,5 @@ -# Odin_ROS_Driver Readme +# Odin 驱动依赖占位 -ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) +`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。 -Odin1 wiki: https://manifoldtechltd.github.io/wiki/Odin1/Cover.html - -## Odin_ROS_Driver - -Compatibility: - -● ROS 1(LTS Release: Noetic recommended) - -● ROS 2(LTS Release: Humble recommended) - -## Important Notice: - -This driver package provides core functionality for point cloud SLAM applications and targets specific use cases. It is intended exclusively for technical professionals conducting secondary development. End users must perform scenario-specific optimization and custom development to align with operational requirements in practical deployment environments. - -## 1. Version - -Current version: v0.12.0 - -Required device firmware version: v0.12.0 - -## 2. Preparation - -### 2.1 OS Requirement - -● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; - -● Ubuntu 22.04 for ROS2 Humble; - -● Ubuntu 18.04 is currently not supported; - -● Ubuntu 24.04 is not officially supported but may work with some modifications. - -### 2.2 Dependencies - -● Opencv >= 4.2.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed) - -● yaml-cpp - -● thread - -● OpenSSL - -● Eigen3 - -### 2.3 Dependencies Install - -#### 2.3.1 System -```shell -sudo apt update -sudo apt-get install build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev -``` - -#### 2.3.2 yaml-cpp -```shell -sudo apt update -sudo apt install -y libyaml-cpp-dev -``` - -#### 2.3.3 libusb -```shell -sudo apt update -sudo apt install -y libusb-1.0-0-dev -``` - -#### 2.3.4 opencv -```shell -sudo apt update -sudo apt-get install libopencv-dev -``` - -#### 2.3.4 ROS install - -For ROS Noetic installation, please refer to: -[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation) - -For ROS2 Foxy installation, please refer to: -[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) - -For ROS2 Humble installation, please refer to: -[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) - -## 3. Preparation - -### 3.1 Create Udev rules -```shell -sudo vim /etc/udev/rules.d/99-odin-usb.rules -``` -Add the following content to the 99-odin-usb.rules file -```shell -SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev" -``` -Reload rules and reinsert devices -```shell -sudo udevadm control --reload -sudo udevadm trigger -``` -### 3.2 OS Requirement -```shell -git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver -``` -Note: -Please clone the source code into the "[ros_workspace]/src/" folder, otherwise compilation errors will occur. - -### 3.3 make - -#### 3.3.1 ROS1 (Noetic for example): - -```shell -source /opt/ros/noetic/setup.bash -./script/build_ros.sh -``` - -#### 3.3.2 ROS2 (Foxy for example): - -```shell -source /opt/ros/foxy/setup.bash -./script/build_ros2.sh -``` - -### 3.4 run: - -#### 3.4.1 ROS1 (Noetic for example): - -```shell -source [ros_workspace]/devel/setup.bash -roslaunch odin_ros_driver [launch file] -``` -● odin_ros_driver: package name; - -● launch file: launch file; - -● ros_workspace: User's ROS environment workspace; -```shell -roslaunch odin_ros_driver odin1_ros1.launch -``` -#### 3.4.2 ROS2 (Foxy for example): - -```shell -source [ros2_workspace]/install/setup.bash -ros2 launch odin_ros_driver [launch file] -``` -● odin_ros_driver: package name; - -● launch file: launch file; - -● ros2_workspace: User's ROS2 environment workspace; - -ROS2 Demo Launch Instructions: -```shell -ros2 launch odin_ros_driver odin1_ros2.launch.py -``` - -### 3.5 Operation Mode: - -The operation mode can be configured via the `custom_map_mode` parameter in config/control_command.yaml. - -#### Odometry mode - -Set `custom_map_mode = 0` to enable odometry mode. In this mode, the map frame and odom frame share the same pose. - -If the odom data is found to drift, the script command "./set_param.sh algo_reset 1" can be used to dynamically reset the algorithm. - -#### SLAM mode - -Set `custom_map_mode = 1` to enable slam mode. This mode provides a complete SLAM system that builds upon the Odometry Mode by adding **loop closure detection** and **map saving** capabilities. - -After launching the driver, odin1 will automatically perform mapping and cache map data. When the scene capture is complete, users need to execute `./set_param.sh save_map 1` in the driver's source directory to save all map data collected since the program started. The map will be saved to the location specified by the `mapping_result_dest_dir` and `mapping_result_file_name` parameters in config/control_command.yaml. If these parameters are not specified, default values will be used. - -After the initial save, you can execute the command again to save a new map. Each save operation will generate a new map file. (Please allow at least 5 seconds between consecutive save operations) - -The map origin corresponds to the odom coordinate system's origin at the program's startup. - -##### Relocalization mode - -To enable relocalization, set `custom_map_mode = 2` and specify the absolute path to the pre-built map using the `relocalization_map_abs_path` parameter in config/control_command.yaml. - -Once launched, odin1 will initiate the relocalization process based on the current viewpoint and the specified map. To ensure a high success rate, it is recommended to starting within 1 meter ±10 degrees of the original position and orientation from the SLAM trajectory. - -Note that relocalization performance is highly environment-dependent. In highly distinctive scenes, successful matching may occur even beyond the 1m/10° range, while other environments may require more stringent conditions. We advise testing in your target environment to determine practical tolerances. - -If relocalization fails initially, the system will temporarily operate in a fallback SLAM mode (map saving is disabled in this state). During this time, you can freely move odin1. It will continue relocalization attempts in the background. Once successful, the TF between map and odom frames will be published. (Tip: Gently shaking or moving the device after initialization can help improve relocalization accuracy.) - -The following topics are published in the odom frame: `/odin1/cloud_slam, /odin1/odom, /odin1/highodom and /odin1/path`. To obtain these in the map frame, apply the TF from odom frame to map frame. - -## 4. File structure and data format -### 4.1 File structure -```shell -Odin_ROS_Driver/ // ROS1/ROS2 driver package - 3rdparty/ // Third-party libraries - src/ - host_sdk_sample.cpp // Example source code - yaml_parser.cpp // Source code for reading yaml parameters - rawCloudRender.cpp // Source code for RenderCloud - depth_image_ros_node.cpp //depth_image_ros_node - depth_image_ros2_node.cpp //depth_image_ros2_node - pcd2depth_ros.cpp //Source code for pcd2depth_ros - pcd2depth_ros2.cpp //Source code for pcd2depth_ros2 - pointcloud_depth_converter.cpp //Source code for pointcloud_depth_converter - cloud_reprojection_ros.cpp //Source code for cloud reprojection node (ROS1/ROS2) - cloud_reprojector.cpp //Core logic for cloud reprojection - lib/ - liblydHostApi_amd.a // Static library for AMD platform - liblydHostApi_arm.a // Static library for ARM platform - include/ - host_sdk_sample.h // Example header file - lidar_api_type.h // API data structure header file - lidar_api.h // API function declarations - yaml_parser.h // Parameter file reading header file - rawCloudRender.h // API about RenderCloud - data_logger.h // LOG about save_data - depth_image_ros_node.hpp // depth_image_ros_node - depth_image_ros2_node.hpp // depth_image_ros2_node - pointcloud_depth_converter.hpp // pointcloud_depth_convert - cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node (ROS1/ROS2) - cloud_reprojector.hpp // Core class for cloud reprojection - config/ - control_command.yaml // Control parameter file for driver - calib.yaml // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver - launch_ROS1/ - odin1_ros1.launch // ROS1 launch file - launch_ROS2/ - odin1_ros2.launch.py // ROS2 launch file - script/ - build_ros1.sh // Installation script for ROS1 - build_ros2.sh // Installation script for ROS2 - recorddata/ // holds recorded data that can import into MindCloud - log/ // holds log files - Driver_{timestamp}/ // holds all log folders for each time driver started - Conn_{timestamp}/ // holds all log files for each odin1 device connection - dev_status.csv // device status log file - README.md // Usage instructions - CMakeLists.txt // CMake build file - License // License file -``` -### 4.2 File structure -| Launch File Name | Description | -|--------------------------|-------------| -| odin1_ros1.launch | Launch file for ROS1 - Odin1 Basic Operations Demo | -| odin1_ros2.launch.py | Launch file for ROS2 - Odin1 Basic Operations Demo | - - -### 4.3 ROS topics -Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters: - -| Topic |control_command.yaml | Detailed Description | -|---------------------|----------------------|----------------------| -| odin1/imu | sendimu | Imu Topic | -| odin1/image | sendrgb | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format | -| odin1/image_undistort | sendrgbundistort | undistorted RGB Camera Topic, processed with calib.yaml from device | -| odin1/image/compressed | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device | -| odin1/cloud_raw | senddtof | Raw_Cloud Topic | -| odin1/cloud_render | sendcloudrender | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device | -| odin1/cloud_slam | sendcloudslam | Slam_PointCloud Topic | -| odin1/odometry | sendodom | Odom Topic | -| odin1/odometry_high | sendodom | high frequency Odom Topic | -| odin1/path | showpath | Odom Path Topic | -| tf | sendodom | tf tree Topic | -| odin1/depth_img_competetion | senddepth | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. | -| odin1/depth_img_competetion_cloud | senddepth | Dense Depth_Cloud Topic. Demo, high computing power required | -| odin1/reprojected_image | sendreprojection | Reprojected cloud to image Topic. Projects cloud_slam to camera image using odometry. Processed on host device. | - -### 4.4 Data format - -1. The raw point cloud (cloud_raw) has the following fields: -``` -float32 x // X axis, in meters -float32 y // Y axis, in meters -float32 z // Z axis, in meters -uint8 intensity // Reflectivity, range 0–255 -uint16 confidence // Point confidence, actual value range from 0 to around 1300 in typical scene, higher value means more reliable. Recommanded filtering threshold is 30-35, should be adjusted accordingly. -float32 offset_time // Time offset relative to the base timestamp unit: s -``` - -To work with this custom format in PCL, first define the point type: -```cpp -/*** LS ***/ -namespace ls_ros { - struct EIGEN_ALIGN16 Point { - float x; - float y; - float z; - uint8_t intensity; - uint16_t confidence; - float offset_time; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; -} // namespace ls_ros - -POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point, - (float, x, x) - (float, y, y) - (float, z, z) - (uint8_t, intensity, intensity) - (uint16_t, confidence, confidence) - (float offset_time , offset_time) -) -``` -Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL point cloud: -``` -pcl::PointCloud ls_cloud; -pcl::fromROSMsg(*msg, ls_cloud); -``` - -2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields: -``` -float32 x // X axis, in meters -float32 y // Y axis, in meters -float32 z // Z axis, in meters -float32 rgb // RGB value -``` - -### 4.5 Other functionalities - -|control_command.yaml | Detailed Description | -|-----------------------|----------------------| -| use_host_ros_time | Time synchronization mode: 0 - use odin internal system time as data timestamp (typical and recommended); 1 - use host ROS time upon receive (not recommended for most users); 2 - align odin1 time to host time via NTP-like synchronization, timestamp is the sensor data reception time on host time axis. | -| strict_usb3.0_check | Strict USB3.0 check, if off, allow connection even if usb connection is below usb 3.0 | -| recorddata | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. The per-frame timestamps written into the recorded files (IMU / image / point cloud / pose / rotate) follow the same alignment policy as `use_host_ros_time`, so under NTP mode (`use_host_ros_time=1` or `2`) the recorded timestamps are NTP-aligned host time instead of odin1 boot time.
录制文件 (IMU / 图像 / 点云 / Pose / Rotate) 中每帧的时间戳与 `use_host_ros_time` 采用相同对齐策略：在 NTP 模式 (`use_host_ros_time=1` 或 `2`) 下，录制时间戳为 NTP 对齐后的主机时间，而非 odin1 开机时间。 | -| devstatuslog | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. | -| showcamerapose | Display Camera Pose and Field of View. | -| custom_map_mode | Operation Modes: Mode 0 - Odometry mode: The map frame and odom frame share the same pose. Mode 1 - Mapping (with loop closure) mode: This mode supports map saving. Mode 2 - Relocalization mode: Requires specifying the absolute path to the map file. After successful relocalization, it will output the TF relationship between the map and odom frames.| -| custom_init_pos | Initialization Position (currently unused). | -| relocalization_map_abs_path | Absolute Path to Map File: Used for relocalization mode. | -| mapping_result_dest_dir and mapping_result_file_name| Path and Name for Saving Maps in Mapping Mode: If not specified, default values will be used. | - -### 4.6 Runtime AE/AWB Tuning via ROS Service / 通过 ROS Service 在线调节 AE/AWB - -The driver hosts four ROS services that let a side terminal tune the -camera's auto exposure (AE) and auto white balance (AWB) at runtime, -while the main data streams keep flowing. The same SDK call is shared -with the driver's main control path and serialised by an internal -mutex, so it is safe to invoke these services concurrently with normal -operation. - -驱动启动后会注册 4 个 ROS Service，允许在不重启 driver 的前提下，从另一个终端动态调节 -相机的自动曝光（AE）和自动白平衡（AWB）。底层 SDK 调用与驱动主控制路径共享同一把 -互斥锁，因此可以与正常数据流并发调用。 - -**Service list / Service 一览** - -| Service name | Type / 类型 | Purpose / 用途 | -|---|---|---| -| `/odin1/get_ae` | `odin_ros_driver/srv/GetAe` | Query current AE status / 查询当前 AE 状态 | -| `/odin1/get_awb` | `odin_ros_driver/srv/GetAwb` | Query current AWB status / 查询当前 AWB 状态 | -| `/odin1/set_ae` | `odin_ros_driver/srv/SetAe` | Set AE mode and (manual) exposure / gain / 设置 AE 模式和手动曝光/增益 | -| `/odin1/set_awb` | `odin_ros_driver/srv/SetAwb` | Set AWB mode and (manual) R/B gain / 设置 AWB 模式和手动 R/B 增益 | - -#### 4.6.1 Request fields, ranges, physical meaning / 请求字段、范围与物理含义 - -**`SetAe.Request`** - -| Field | Range / 范围 | Meaning / 含义 | -|---|---|---| -| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AE loop, the two floats below are ignored / 设备自动调 AE，下方参数被忽略
`1` = device locks AE and applies the provided values / 设备锁 AE 并应用提供的值 | -| `exposure_time` | `0.0001` ~ `0.033` s (manual only / 仅手动模式) | Sensor exposure time per frame. Longer = brighter but more motion blur / 每帧传感器曝光时间。越长越亮但运动模糊增大 | -| `gain` | `1.0` ~ `64.0` (manual only / 仅手动模式) | Analog gain. Higher = brighter output but worse SNR / 模拟增益。越大越亮但信噪比越差 | - -**`SetAwb.Request`** - -| Field | Range / 范围 | Meaning / 含义 | -|---|---|---| -| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AWB loop / 设备自动 AWB
`1` = device locks AWB and applies provided gains / 设备锁定 AWB 并应用所给增益 | -| `rgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | R channel gain. Higher `rgain` vs `bgain` shifts the image warm (yellow/red) / R 通道增益，相对 bgain 越大，画面越偏暖 | -| `bgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | B channel gain. Higher `bgain` vs `rgain` shifts the image cool (blue) / B 通道增益，相对 rgain 越大，画面越偏冷 | - -> Gr / Gb channels are fixed to 1.0 by the device and are not adjustable. -> Gr / Gb 通道被设备固定为 1.0，不可调节。 - -#### 4.6.2 Response fields / 响应字段 - -All four services return a `success` (bool) and `rc` (int32). Get -services additionally return the queried state. -4 个 Service 都返回 `success` (bool) 与 `rc` (int32)。Get 类还会返回查询到的状态字段。 - -**`GetAe.Response`** - -| Field | Typical range / 典型范围 | Meaning / 含义 | -|---|---|---| -| `exposure_time` | `0.0001`~`0.033` s | Current exposure / 当前曝光时间 | -| `gain` | `1.0`~`64.0` | Current analog gain / 当前模拟增益 | -| `iso` | `100`~`6400` | Equivalent ISO / 等效 ISO | -| `brightness` | `0`~`255` | Average frame brightness / 平均帧亮度 | -| `is_converged` | `0` or `1` | `1` = AE settled / AE 已收敛 | -| `env_lv` | `0`~`15` | Ambient luminance index, higher = brighter / 环境光强度指数，越大越亮 | -| `fps` | `~10` / `~14.5` / `~29` | Current frame rate / 当前帧率 | - -**`GetAwb.Response`** - -| Field | Typical range / 典型范围 | Meaning / 含义 | -|---|---|---| -| `rgain` / `bgain` | `0.1`~`4.0` | R / B channel gain / R / B 通道增益 | -| `grgain` / `gbgain` | `1.0` (fixed / 固定) | Gr / Gb gain, device-fixed / Gr / Gb 增益，设备固定 | -| `cct` | `2500`~`8000` K | Correlated color temperature / 相关色温 | -| `ccri` | `-50`~`50` | Color temp deviation index, 0 = on Planckian locus / 色温偏离指数，0 表示在普朗克轨迹上 | -| `is_converged` | `0` or `1` | `1` = AWB settled / AWB 已收敛 | - -#### 4.6.3 `rc` return code / `rc` 返回码 - -| `rc` | Meaning / 含义 | -|---|---| -| `0` | Success / 成功 | -| `400` | Device payload too short / 设备载荷过短 | -| `401` | Device opcode not supported / 设备不支持该 opcode | -| `402` | Device parameter length wrong / 参数长度错误 | -| `403` | **Parameter out of range** / 参数越界 — most common when manual values exceed the table above / 手动值超出上表范围时最常见 | -| `404` | Device-side socket error / 设备端 socket 错误 | -| `405` | Device-side `ae_control` did not respond / 设备端 `ae_control` 无应答（确认 lydapp 已运行） | -| `255` (`0xFF`) | Unknown opcode reported by ae_control / ae_control 报未知 opcode | -| `-1` | SDK not initialised / SDK 未初始化 | -| `-2` ~ `-5` | USB transfer / timeout / malformed reply / USB 传输异常、超时、应答畸形 | -| `-100` | **Driver has not opened the device yet** / driver 还未打开设备，请等设备连接成功 | - -#### 4.6.4 Usage examples / 调用示例 - -ROS2 (Humble) — start the driver in one terminal, then in a side terminal: -ROS2（Humble）—— 在一个终端启动 driver，在另一个终端： - -```bash -source install/setup.bash - -# Query current state / 查询当前状态 -ros2 service call /odin1/get_ae odin_ros_driver/srv/GetAe -ros2 service call /odin1/get_awb odin_ros_driver/srv/GetAwb - -# Set AE to AUTO / 设置 AE 为自动 -ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}" - -# Set AE to MANUAL with 10 ms exposure and gain 4.0 -# 设置 AE 为手动，10 毫秒曝光，增益 4.0 -ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe \ - "{mode: 1, exposure_time: 0.010, gain: 4.0}" - -# Set AWB to MANUAL with rgain=1.5, bgain=2.0 -# 设置 AWB 为手动，rgain=1.5、bgain=2.0 -ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb \ - "{mode: 1, rgain: 1.5, bgain: 2.0}" - -# Restore AUTO / 一键回自动 -ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}" -ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb "{mode: 0}" - -# Inspect srv definition / 查看 srv 完整定义 -ros2 interface show odin_ros_driver/srv/SetAe -``` - -ROS1 (Noetic) — start the driver, then in a side terminal: -ROS1（Noetic）—— 启动 driver 后，新开终端： - -```bash -source devel/setup.bash - -# Query / 查询 -rosservice call /odin1/get_ae -rosservice call /odin1/get_awb - -# Set AE manual / 设置 AE 手动 -rosservice call /odin1/set_ae "{mode: 1, exposure_time: 0.010, gain: 4.0}" - -# Set AWB manual / 设置 AWB 手动 -rosservice call /odin1/set_awb "{mode: 1, rgain: 1.5, bgain: 2.0}" - -# Restore AUTO (ROS1 requires all fields to be present) -# 一键回自动（ROS1 要求填齐全部字段） -rosservice call /odin1/set_ae "{mode: 0, exposure_time: 0.0, gain: 0.0}" -rosservice call /odin1/set_awb "{mode: 0, rgain: 0.0, bgain: 0.0}" - -# Inspect srv definition / 查看 srv 完整定义 -rossrv show odin_ros_driver/SetAe -``` - -#### 4.6.5 Recommended starting points by scene / 不同场景推荐起步参数 - -**AE (`exposure_time`, `gain`)** - -| Scene / 场景 | `exposure_time` | `gain` | -|---|---|---| -| Bright outdoor / 明亮室外 | `0.001` ~ `0.005` s | `1.0` ~ `2.0` | -| Normal indoor / 普通室内 | `0.008` ~ `0.015` s | `2.0` ~ `8.0` | -| Dim light / 暗光环境 | `0.020` ~ `0.030` s | `8.0` ~ `32.0` | -| Very dark / 极暗 | `0.033` s | `32.0` ~ `64.0` | - -**AWB (`rgain`, `bgain`)** - -| Target tone / 目标色调 | `rgain` | `bgain` | -|---|---|---| -| Warm (tungsten, sunset) / 暖（钨丝灯、夕阳） | `2.0` ~ `2.5` | `1.0` ~ `1.2` | -| Neutral (D65 daylight) / 中性（D65 日光） | `1.5` ~ `1.7` | `1.8` ~ `2.0` | -| Cool (cloudy, fluorescent) / 冷（阴天、荧光） | `1.2` ~ `1.4` | `2.2` ~ `2.6` | -| Very cool / 极冷 | `1.0` | `3.0` ~ `4.0` | - -#### 4.6.6 Caveats / 注意事项 - -- The service blocks for up to ~10 s waiting for the device to reply; - typical latency is tens of milliseconds. - Service 最长阻塞约 10 秒等设备应答；正常几十毫秒返回。 -- Manual mode is **not** persisted across driver / device restart; - it falls back to AUTO on each new connection. - 手动模式**不会**跨重启保留；每次重连默认回到 AUTO。 -- `rc = -100` means the driver has not yet opened the device. - Wait until the driver logs `device connected` before calling. - 返回 `rc = -100` 表示 driver 还没打开设备，等到 driver 日志显示 `device connected` 再调用。 -- The effective maximum `exposure_time` is bounded by the frame - period `1 / fps`. With `dtof_fps = 290` (29 Hz, period ~34 ms) - the upper limit 0.033 s is already at the frame boundary. - 最大可用 `exposure_time` 受帧周期 `1/fps` 限制。在 `dtof_fps = 290`（29 Hz、周期 ~34 ms）下，上限 0.033 s 已经贴到帧边界。 - -## 5. FAQ -### 5.1 Segmentation fault upon re-launching host SDK -**Error Message** -No device connected after 60 seconds - -**Solution** -1. Please power on Odin module again # Disconnect and reconnect odin power - -2. Reinitialize Odin SDK # Execute SDK after device reboot - - -### 5.2 Library binding failure during compilation - -**Error Message** -ld: cannot find -llydHostApi or symbol lookup errors - -**Resolution** - -1. Clean previous build artifacts - -ROS1 -```shell -rm -rf devel/ build/ -``` -ROS2 -```shell -rm -rf devel/ install/ log/ -``` -2. Re-run script installation - -### 5.3 Docker GUI passthrough failure - -**Error Message** -Unable to open X display or No protocol specified - -**Resolution** -```shell -xhost + #This command enables graphical passthrough to Docker containers -``` - -### 5.4 ROS driver exit with get version failed error - -**Error Message** -```shell -: get device version fail. -get version failed. -``` - -**Resolution** - -Device firmware version is too low, please update to latest version. - - -### 5.5 RVIZ has not responded for a long time - -**Error Message** -Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection... - -**Resolution** - -Please power on Odin module again - -### 5.6 Device not responding - -**Error Message** -Missed ok response from device,probably wrong interaction procedure. - -**Resolution** - -Please adopt the solution mentioned in 5.1 - -### 5.7 Device has no external calibration file - -**Error Message** -ERROR：Missing camera node 'cam_0' - -**Resolution** - -Please plug and unplug the USB again - -### 5.8 ROS Driver report device disconnected immediately after stream started - -**Error Message** - -```shell -Device ready and streams activated -Device detaching... -Wating for device reconnection... -Device disconnected, waiting for reconnection... -``` - -**Reason** - -Mostly common on ros2 environment and connected to complex network environment, such as office wifi & ethernet. ROS2 default to broadcast, and complex network environment will cause ros2 publish to block, leading to device disconnection. - -**Resolution** - -If cross-device communication is not required, please restrict ros2 to localhost only with: -```shell -export ROS_LOCALHOST_ONLY=1 -``` - -If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended. - -### 5.9 ROS Driver died immediately after stream started - -**Error Message** - -```shell -Device ready and streams activated -[host_sdk_sample-2] process has died ...... -``` - -**Test** - -Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system. - -**Resolution** - -Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again. - -### 5.10 ROS Driver printing "TF_OLD_DATA ignoring data" warning - -**Error Message** - -```shell -[rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame odin1_base_link at time 20.547632 according to authority Authority undetectable -[rviz2-3] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained -[rviz2-3] at line 294 in ./src/buffer_core.cpp -``` - -**Reason** - -This is a ros & rviz feature to warn user that some tf data is being ignored due to timestamp conflicts. It happens when user keeps ros driver running and power-cycles odin device, which cause odin's internal system time being reset and now data timestamps conflicts with old data recieved by rviz during last run. - -**Resolution** - -There's a reset button on bottom of rviz gui. Click on this button will reset rviz's internal state and stop the warning. - -### 5.11 ROS Driver printing "unknown cmd code: xx" error - -**Error Message** - -```shell -: unknow command code 21. -``` - -**Reason** - -This is due to ros driver version mismatch with device firmware version, resulting in ros driver unable to decode new data added in newer firmware. - -**Resolution** - -Please make sure you are using most up-to-date ros driver and device firmware. - -### 5.12 USB device access error (LIBUSB_ERROR_BUSY or LIBUSB_ERROR_ACCESS) - -**Error Message** - -```shell -libusb: error [udev_hotplug_event] ignoring udev action bind -LIBUSB_ERROR_BUSY -``` - -or - -```shell -libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13 -LIBUSB_ERROR_ACCESS -``` - -**Reason** - -- **LIBUSB_ERROR_BUSY**: Another process is already using the USB device. This commonly happens when multiple instances of the ROS driver are running, or another application (such as a previous crashed instance) still holds the device handle. - -- **LIBUSB_ERROR_ACCESS**: The current user does not have permission to access the USB device. This is typically caused by missing udev rules or insufficient user privileges. - -**Resolution** - -For **LIBUSB_ERROR_BUSY**: - -1. Check if another instance of the driver is running: -```shell -ps aux | grep host_sdk_sample -``` - -2. Kill any existing instances: -```shell -killall host_sdk_sample -``` - -3. If the issue persists, unplug and replug the USB device to reset the device state. - -For **LIBUSB_ERROR_ACCESS**: - -1. Add udev rules for the device. Create a file `/etc/udev/rules.d/99-odin.rules` with the following content: -```shell -SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev" -``` - -2. Reload udev rules: -```shell -sudo udevadm control --reload-rules -sudo udevadm trigger -``` - -3. Alternatively, run the driver with sudo (not recommended for production): -```shell -sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py -``` - -4. Make sure your user is in the `plugdev` group: -```shell -sudo usermod -aG plugdev $USER -``` -Then log out and log back in for the group change to take effect. - -### 5.13 ros2 bag drops high-frequency topics (IMU / odometry_highfreq) / ros2 bag 录制丢失高频话题（IMU / odometry_highfreq） - -**Symptom / 现象** - -When recording with `ros2 bag record`, low-frequency topics (cloud, image, odometry, wiwc) are intact, but `/odin1/imu` (400 Hz) and `/odin1/odometry_highfreq` (400 Hz) show missing samples — analysis scripts report inter-message intervals that are 2× or more of the expected period, while no drop is reported on the SDK side or by an online subscriber such as `ros2 topic hz`. - -使用 `ros2 bag record` 录制时，低频话题（cloud、image、odometry、wiwc）完整无丢，但 `/odin1/imu`（400 Hz）和 `/odin1/odometry_highfreq`（400 Hz）会出现丢帧——分析脚本上看到消息间隔达到正常周期的 2 倍以上，而 SDK 侧不报丢，独立的 `ros2 topic hz` 订阅者也看不到丢。 - -**Reason / 原因** - -The driver publishes `/odin1/imu` and `/odin1/odometry_highfreq` with `RELIABLE` QoS. By default `ros2 bag record` subscribes with `history = keep_last`, `depth = 10`, which only buffers ~25 ms of samples at 400 Hz. Whenever the recorder is briefly delayed (disk flush, mcap/sqlite chunk write, scheduler jitter), its subscription queue overflows and DDS silently drops the oldest samples on the **subscriber side**. The SDK and publisher are unaffected, which is why no drop appears in the driver logs or in `ros2 topic hz`. - -驱动以 `RELIABLE` QoS 发布 `/odin1/imu` 与 `/odin1/odometry_highfreq`。`ros2 bag record` 默认订阅使用 `history = keep_last`、`depth = 10`，在 400 Hz 下只能缓冲约 25 ms。一旦录制端有短暂阻塞（落盘 flush、mcap/sqlite chunk 写入、调度抖动），订阅队列就会溢出，DDS 在**订阅端**静默丢掉最旧的样本。SDK 与 publisher 不受影响，因此驱动日志和 `ros2 topic hz` 都看不到丢。 - -**Resolution / 解决方案** - -Use the provided QoS override file `script/rosbag2_qos.yaml` to raise the subscriber-side queue depth on the recorder for the two high-rate topics: - -使用本仓库提供的 QoS 配置 `script/rosbag2_qos.yaml`，把高频话题的录制订阅 depth 拉大： - -```yaml -# script/rosbag2_qos.yaml -/odin1/imu: - reliability: reliable - history: keep_last - depth: 4000 - -/odin1/odometry_highfreq: - reliability: reliable - history: keep_last - depth: 4000 -``` - -Apply it when recording / 录制时通过 `--qos-profile-overrides-path` 应用： - -```shell -ros2 bag record -a \ - --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ - -o my_bag -``` - -Or only the high-rate topics / 也可以只录制高频话题： - -```shell -ros2 bag record \ - --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ - -o my_bag \ - /odin1/imu /odin1/odometry_highfreq /odin1/odometry /odin1/wiwc /odin1/cloud_raw -``` - -**Optional further tuning / 可选的进一步优化** - -If drops still occur after applying the override (typically on slower disks), try the following in addition / 套用上述 override 后仍有丢包时（通常发生在慢盘上），可叠加以下措施： - -```shell -# Use mcap backend with a larger internal cache (faster than sqlite3). -# 使用 mcap 后端 + 更大的内部缓存（比 sqlite3 快）。 -ros2 bag record -s mcap --max-cache-size 1073741824 \ - --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ - -o my_bag \ - /odin1/imu /odin1/odometry_highfreq ... - -# Enlarge kernel UDP socket buffers (the most common hidden bottleneck for -# 400 Hz RELIABLE traffic, default is only 208 KB). -# 放大内核 UDP socket buffer（400 Hz RELIABLE 流量最常见的隐藏瓶颈，默认仅 208 KB）。 -sudo sysctl -w net.core.rmem_max=33554432 -sudo sysctl -w net.core.wmem_max=33554432 -``` - -**Does ROS1 have the same problem? / ROS1 是否存在同样的问题？** - -No. ROS1 uses TCP-based publish/subscribe with a single `queue_size` parameter on each side, and has no QoS profile mismatch between publisher and subscriber. The ROS1 publisher path in this driver already sizes the IMU and `odometry_highfreq` publishers to `queue_size = 4000` (`include/host_sdk_sample.h`, see `initialize_publishers` ROS1 branch), and `rosbag record` uses TCP transport which is reliable by construction. As a result this specific drop pattern does not occur under ROS1; no additional configuration is required. - -不存在。ROS1 使用基于 TCP 的发布/订阅，发布端与订阅端各自只有一个 `queue_size` 参数，不存在 ROS2 那种 QoS profile 不匹配的问题。本驱动 ROS1 路径已经把 IMU 与 `odometry_highfreq` 的发布队列设置为 `queue_size = 4000`（见 `include/host_sdk_sample.h` 中 `initialize_publishers` 的 ROS1 分支），并且 `rosbag record` 使用 TCP 传输本身即可靠传递。因此在 ROS1 下不会出现该丢帧现象，也不需要额外配置。 - -## 6. Contact Information​​ - -You can contact our support through support@manifoldtech.cn - -To help diagnose the issue, please provide the following details to our FAE engineer: - -1. Current firmware version​​ -```shell -[device_version_capture]: ros_driver_version: [Version Number] -``` -2. Photos of power adapter and converter cable​​ in use. - -3. Does the issue happen occasionally or consistently? - -4. Provide images of the problem scenario. - -5. Did the troubleshooting methods in ​​Section V​​ resolve the issue? - -6. Expected timeline for issue resolution. +因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。 diff --git a/05_software/real/sim2real_ros2/src/sim2real_bringup/config/deployment_contract.yaml b/05_software/real/sim2real_ros2/src/sim2real_bringup/config/deployment_contract.yaml index cd931ee..45a3290 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_bringup/config/deployment_contract.yaml +++ b/05_software/real/sim2real_ros2/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.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0] + default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0] motor_mapping: can_id_map: diff --git a/05_software/real/sim2real_ros2/src/sim2real_bringup/config/runtime.yaml b/05_software/real/sim2real_ros2/src/sim2real_bringup/config/runtime.yaml index cb6e208..69cd43a 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_bringup/config/runtime.yaml +++ b/05_software/real/sim2real_ros2/src/sim2real_bringup/config/runtime.yaml @@ -4,47 +4,8 @@ motor_hz: 200.0 status_hz: 10.0 target_timeout_ms: 150.0 - event_log_dir: "logs_v2_web" - model_engine_path: policies/model_6800_fp16.engine - prefer_tensorrt: true - model_path: policies/model_6800.onnx - rough_model_engine_path: policies/model_6800_fp16.engine - crawl_model_path: policies/model_crawl.onnx # unused while crawl_backend is "ik" - crawl_model_engine_path: "" - wall_model_path: policies/model_84.onnx - wall_model_engine_path: policies/model_84_fp16.engine - rough_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0] - wall_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0] - crawl_backend: "ik" - crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0] - crawl_ik_wheel_linear_gain: 12.5 - crawl_ik_wheel_yaw_gain: 8.0 - crawl_ik_max_wheel_speed: 12.0 - crawl_ik_abduction_clip: 0.45 - crawl_ik_yaw_rate_kp: 0.5 - crawl_ik_imu_posture: false - crawl_ik_encoder_posture_kp: 0.0 - crawl_ik_encoder_posture_max: 0.03 - crawl_ik_encoder_guard: false - crawl_ik_encoder_guard_start: 0.28 - crawl_ik_encoder_guard_stop: 0.65 - crawl_ik_imu_guard: true - crawl_ik_imu_guard_start_deg: 12.0 - crawl_ik_imu_guard_stop_deg: 28.0 - model_switch_transition_s: 0.9 - model_switch_min_transition_s: 0.4 - model_switch_to_stand_transition_scale: 2.1 - model_switch_to_model_transition_scale: 2.4 - model_switch_stand_hold_s: 0.45 - model_switch_stand_max_err: 0.18 - model_switch_stand_max_vel: 0.8 - model_switch_release_scale: 1.0 - runtime_max_vx: 0.9 - runtime_max_vy: 0.5 - runtime_max_yaw_rate: 0.85 - debug_trace_enabled: true - debug_trace_decimation: 1 - use_cuda: true # enable CUDA Execution Provider on Orin Nano GPU + model_path: policies/model_rough.onnx + use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速） contract_file: deployment_contract.yaml dry_run: false can0_name: "can0" @@ -52,60 +13,39 @@ imu_topic: "/odin1/imu" odom_topic: "/odom" - # Remote UART / SBUS parameters, aligned with the first-generation Python deployment - remote_enabled: false # true + # Remote UART / SBUS parameters, aligned with the Python deployment + remote_enabled: true remote_port: "/dev/ttyACM0" remote_baudrate: 100000 remote_timeout: 0.02 remote_axis_deadzone: 40 remote_active_threshold: 40 remote_axis_full_scale: 660.0 - remote_max_vx: 0.9 - remote_max_vy: 0.5 - remote_max_yaw_rate: 0.85 + remote_max_vx: 0.8 + remote_max_vy: 0.3 + remote_max_yaw_rate: 0.5 remote_invert_vx: true remote_invert_vy: false remote_invert_yaw: true remote_publish_inactive_zero: true remote_estop_latch: true remote_poll_hz: 50.0 - remote_model_switch_enabled: true - remote_model_switch_channel: 10 - remote_model_switch_debounce_frames: 3 - remote_model_switch_rough_level: "low" - remote_model_switch_ik_level: "high" # Command mux parameters - cmd_mux_default_mode: "NAV" + cmd_mux_default_mode: "REMOTE" cmd_mux_output_hz: 50.0 cmd_mux_remote_timeout_ms: 250.0 cmd_mux_web_timeout_ms: 300.0 cmd_mux_nav_timeout_ms: 500.0 - cmd_mux_max_vx: 0.9 - cmd_mux_max_vy: 0.5 - cmd_mux_max_yaw_rate: 0.85 + cmd_mux_max_vx: 0.8 + cmd_mux_max_vy: 0.3 + cmd_mux_max_yaw_rate: 0.5 cmd_mux_max_vx_acc: 1.0 cmd_mux_max_vy_acc: 1.0 cmd_mux_max_yaw_acc: 1.5 - cmd_mux_max_vx_decel: 2.0 - cmd_mux_max_vy_decel: 2.0 - cmd_mux_max_yaw_decel: 2.0 - # The rough locomotion policy has an approximately 0.2 m/s linear command dead zone. - # Skip that ineffective band on start-up, but still allow exact zero for braking/estop. - cmd_mux_linear_deadzone_epsilon: 0.05 - cmd_mux_yaw_deadzone_epsilon: 0.02 - cmd_mux_min_effective_vx: 0.22 - cmd_mux_min_effective_vy: 0.22 - cmd_mux_min_effective_yaw_rate: 0.0 - cmd_mux_deadzone_sources: "nav" # Windows/Nano Web UDP bridge parameters web_bridge_enabled: true - web_http_host: "0.0.0.0" - web_http_port: 18080 - web_static_dir: "" - # Odom task actual path export; files can be opened by nav_tools over the PCD. - odom_trace_export_dir: "map/load" web_udp_listen_host: "0.0.0.0" web_udp_listen_port: 15000 web_udp_remote_host: "" @@ -113,46 +53,22 @@ web_udp_state_hz: 20.0 web_udp_cmd_timeout_ms: 300.0 web_udp_max_packet_bytes: 8192 - web_udp_max_vx: 0.9 + web_udp_max_vx: 0.8 web_udp_max_vy: 0.3 - web_udp_max_yaw_rate: 0.85 + web_udp_max_yaw_rate: 0.5 web_udp_estop_on_timeout: false # Safety parameters safety_enabled: true - max_target_offset: 2.4 - model_switch_max_target_offset: 1.8 - hard_target_offset: 3.0 - max_ang_vel: 30.0 + max_target_offset: 0.6 + hard_target_offset: 2.0 + max_ang_vel: 10.0 max_tilt_z: -0.3 clip_to_brake: 0 imu_age_warn_ms: 60.0 - imu_age_stop_ms: 500.0 - wheel_no_effect_command_threshold: 1.0 - wheel_no_effect_min_response_ratio: 0.20 - wheel_no_effect_velocity_epsilon: 0.25 - wheel_no_effect_max_temperature_c: 90.0 - wheel_no_effect_min_bus_voltage_v: 18.0 - wheel_no_effect_command_warmup_cycles: 12 - wheel_no_effect_trigger_cycles: 30 - wheel_no_effect_attempt_limit: 2 - wheel_no_effect_cooldown_ms: 1200 - wheel_recovery_verify_timeout_ms: 180 - wheel_no_effect_diag_freshness_ms: 350 - wheel_no_effect_diag_request_period_ms: 80 - leg_no_effect_position_error_threshold: 0.18 - leg_no_effect_velocity_epsilon: 0.12 - leg_no_effect_max_estimated_current_arms: 4.0 - leg_no_effect_max_abs_torque_nm: 5.0 - leg_no_effect_max_temperature_c: 100.0 - leg_no_effect_min_bus_voltage_v: 18.0 - leg_no_effect_command_warmup_cycles: 40 - leg_no_effect_trigger_cycles: 25 - leg_no_effect_attempt_limit: 2 - leg_no_effect_cooldown_ms: 1200 - leg_recovery_verify_timeout_ms: 220 + imu_age_stop_ms: 200.0 - # Policy alignment with the first-generation Python deployment + # Policy alignment with the Python deployment command_release_s: 0.35 release_command_hold_s: 0.12 release_posture_max_err: 0.35 @@ -162,143 +78,3 @@ enable_zero_cmd_suppression: true require_active_command_to_release: true zero_cmd_use_yaw_rate: true - - # Simple navigation parameters - localization_mode: "relocal" # relocal: wait for Odin map/odom TF; odom: bridge map->odom fallback - nav_map_frame: "map" - nav_odom_frame: "odom" - nav_base_frame: "base_link" - nav_control_hz: 20.0 - nav_goal_tolerance: 0.20 - nav_yaw_stop_threshold: 0.80 - nav_max_vx: 0.90 - nav_max_vy: 0.50 - nav_max_wz: 0.85 - nav_kp_dist: 0.80 - nav_kp_yaw: 1.80 - nav_goal_exit_tolerance_margin: 0.08 - nav_goal_complete_stable_cycles: 2 - nav_final_align_kp_yaw_scale: 0.60 - nav_final_align_max_wz: 0.45 - nav_final_align_creep_speed: 0.05 - nav_goal_yaw_tolerance_deg: 12.0 - nav_astar_enabled: true - nav_astar_resolution: 0.10 - nav_astar_pcd_sample_step: 5 - nav_astar_allow_diagonal: true - nav_astar_smooth_enabled: true - nav_astar_corner_blend_dist: 0.20 - nav_astar_waypoint_reach_dist: 0.18 - nav_astar_lookahead_dist: 0.35 - nav_astar_snap_radius: 0.60 - nav_astar_max_expansions: 120000 - # Slalom is treated as a continuous path by simple_nav even if the route JSON - # was saved without precisionFollow/stableCycles/lookahead metadata. - nav_slalom_auto_precision_enabled: true - nav_slalom_auto_precision_force: true - nav_slalom_task_names: "slalom" - nav_slalom_stable_cycles: 0 - nav_slalom_lookahead: 0.35 - nav_slalom_yaw_rate_limit: 0.45 - nav_slalom_tolerance: 0.15 - nav_slalom_max_vx: 0.58 - nav_slalom_min_vx: 0.22 - nav_slalom_curvature_slowdown_enabled: true - nav_slalom_min_turn_speed_scale: 0.45 - # Execute waypoints marked slalomStraight as odometry-closed scripted moves. - nav_slalom_script_enabled: true - nav_slalom_script_start_tolerance: 0.22 - nav_slalom_script_pos_tolerance: 0.10 - nav_slalom_script_yaw_tolerance_deg: 5.0 - nav_slalom_script_drive_yaw_deadband_deg: 8.0 - nav_slalom_script_stable_cycles: 1 - nav_slalom_script_rotate_steps_enabled: false - nav_slalom_script_final_rotate_enabled: false - nav_slalom_script_require_yaw_at_step: false - nav_slalom_script_kp_dist: 1.00 - nav_slalom_script_kp_yaw: 1.20 - nav_slalom_script_max_vx: 0.58 - nav_slalom_script_max_vy: 0.50 - nav_slalom_script_max_wz: 0.50 - nav_slalom_script_min_cmd_linear: 0.22 - nav_slalom_script_min_cmd_angular: 0.20 - nav_slalom_script_min_cmd_epsilon: 0.05 - nav_slalom_script_min_step_distance: 0.02 - nav_slalom_script_yaw_gate_deg: 8.0 - nav_slalom_script_lateral_gate: 0.07 - nav_slalom_script_lateral_slow_gate: 0.15 - nav_slalom_script_lateral_creep_vx: 0.22 - nav_slalom_script_drive_yaw_source: "segment" - nav_slalom_script_segment_yaw_min_dist: 0.45 - nav_precision_lateral_control_enabled: true - nav_precision_lateral_kp: 0.80 - nav_precision_lateral_max_vy: 0.22 - # Lightweight DWA-style local safety layer over the route/avoid polygons. - nav_local_planner_enabled: true - nav_local_planner_tasks: "slalom" - nav_local_planner_precision_enabled: true - nav_local_planner_sim_time: 0.9 - nav_local_planner_sim_dt: 0.1 - nav_local_planner_v_samples: 5 - nav_local_planner_w_samples: 7 - nav_local_planner_vy_samples: 3 - nav_local_planner_obstacle_margin: 0.08 - nav_local_planner_recovery_clearance_epsilon: 0.005 - # 0.0 means auto: use the nav_tools body+wheel lateral footprint. - nav_local_planner_robot_radius: 0.0 - nav_local_planner_clearance_weight: 2.0 - nav_local_planner_path_weight: 2.0 - nav_local_planner_heading_weight: 0.7 - nav_local_planner_speed_weight: 0.3 - nav_local_planner_nominal_weight: 1.0 - nav_local_planner_min_vx: 0.22 - nav_slalom_script_safety_filter_enabled: true - nav_local_planner_use_astar_grid: false - nav_turn_in_place_enabled: true - nav_turn_in_place_enter_yaw_deg: 70.0 - nav_turn_in_place_exit_yaw_deg: 18.0 - nav_turn_in_place_max_wz: 0.80 - nav_pre_dock_enabled: true - nav_pre_dock_distance: 0.35 - nav_pre_dock_tolerance: 0.18 - nav_pre_dock_skip_within_goal_dist: 0.45 - nav_goals_file: "" - nav_missions_file: "" - # Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below. - nav_route_file: "" - # Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes. - # The web "odom" button uses the first waypoint of this route as the fixed odom fallback start pose. - nav_route_task_file: map/routes/1hao_reall.json - nav_route_auto_align_enabled: false - nav_route_rotation_offset_deg: 0.0 - nav_route_align_max_angle_deg: 6.0 - nav_route_align_angle_step_deg: 0.5 - nav_route_align_search_radius: 0.35 - nav_avoid_regions_enabled: true - nav_avoid_region_margin: 0.0 - # 0.0 means auto: use the nav_tools body+wheel lateral footprint for avoid-region inflation. - nav_avoid_footprint_radius: 0.0 - nav_robot_body_length: 0.356 - nav_robot_body_width: 0.235 - nav_robot_body_center_x: 0.1518 - nav_robot_origin_from_front: 0.105 - nav_robot_pose_hip: 0.550 - nav_robot_pose_knee: -1.125 - nav_robot_wheel_vis_length: 0.16 - nav_robot_wheel_vis_width: 0.055 - nav_robot_footprint_padding: 0.02 - odom_fallback_require_odom_fresh: true - odom_fallback_max_odom_age_ms: 500.0 - odom_fallback_block_existing_map_odom_tf: true - odom_fallback_tf_conflict_window_s: 1.0 - odom_fallback_tf_conflict_xy_tolerance: 0.05 - odom_fallback_tf_conflict_yaw_tolerance_deg: 2.0 - # Keep odom fallback running if Odin relocalizes mid-task; hand off after mission end or Exit odom. - odom_fallback_stop_on_external_tf: false - pcd_nav_file: map/1hao.pcd - pcd_floor_z_min: -1.6 - pcd_floor_z_max: 0.4 - pcd_sample_step: 25 - pcd_robot_radius: 0.18 - - diff --git a/05_software/real/sim2real_ros2/src/sim2real_bringup/launch/sim2real_system.launch.py b/05_software/real/sim2real_ros2/src/sim2real_bringup/launch/sim2real_system.launch.py index 807d751..e691bd4 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_bringup/launch/sim2real_system.launch.py +++ b/05_software/real/sim2real_ros2/src/sim2real_bringup/launch/sim2real_system.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterFile @@ -27,7 +27,7 @@ def generate_launch_description(): launch_nav2_arg = DeclareLaunchArgument( 'launch_nav2', - default_value='false', + default_value='true', description='Whether to launch the Nav2 navigation stack' ) @@ -43,36 +43,6 @@ def generate_launch_description(): description='Whether to launch the Windows/Nano UDP web debug bridge' ) - launch_simple_nav_arg = DeclareLaunchArgument( - 'launch_simple_nav', - default_value='true', - description='Whether to launch the simple waypoint navigation node' - ) - - localization_mode_arg = DeclareLaunchArgument( - 'localization_mode', - default_value='relocal', - description='Localization profile: odom uses bridge fallback; relocal waits for Odin map/odom TF' - ) - - odin_config_file_arg = DeclareLaunchArgument( - 'odin_config_file', - default_value=PathJoinSubstitution([ - FindPackageShare('odin_ros_driver'), - 'config', - 'control_command_relocal.yaml', - ]), - description='Odin control config YAML for the selected localization profile' - ) - - event_log_dir_arg = DeclareLaunchArgument( - 'event_log_dir', - default_value=PythonExpression([ - "'logs_v2_web/run_' + __import__('datetime').datetime.now().strftime('%Y-%m-%d_%H-%M-%S_%f')[:-3]" - ]), - description='Per-run event log directory' - ) - # Include odin_ros_driver launch driver_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -82,10 +52,7 @@ def generate_launch_description(): 'odin1_ros2.launch.py' ]) ), - launch_arguments={ - 'launch_rviz': 'false', - 'config_file': LaunchConfiguration('odin_config_file'), - }.items(), + launch_arguments={'launch_rviz': 'false'}.items(), condition=IfCondition(LaunchConfiguration('launch_driver')) ) @@ -106,23 +73,19 @@ def generate_launch_description(): launch_nav2_arg, launch_remote_arg, launch_web_bridge_arg, - launch_simple_nav_arg, - localization_mode_arg, - odin_config_file_arg, - event_log_dir_arg, Node( package="sim2real_hw", executable="sim2real_hw_node", name="sim2real_hw_node", output="screen", - parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}], + parameters=[runtime_params], ), Node( package="sim2real_runtime", executable="sim2real_runtime_node", name="sim2real_runtime_node", output="screen", - parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}], + parameters=[runtime_params], ), Node( package="sim2real_runtime", @@ -136,7 +99,7 @@ def generate_launch_description(): executable="web_udp_bridge_node.py", name="sim2real_web_udp_bridge_node", output="screen", - parameters=[runtime_params, {"localization_mode": LaunchConfiguration("localization_mode")}], + parameters=[runtime_params], condition=IfCondition(LaunchConfiguration('launch_web_bridge')), ), Node( @@ -147,14 +110,6 @@ def generate_launch_description(): parameters=[runtime_params], condition=IfCondition(LaunchConfiguration('launch_remote')), ), - Node( - package="sim2real_runtime", - executable="simple_nav_node.py", - name="sim2real_simple_nav_node", - output="screen", - parameters=[runtime_params], - condition=IfCondition(LaunchConfiguration('launch_simple_nav')), - ), Node( package="sim2real_runtime", executable="odom_relay_node", @@ -171,3 +126,4 @@ def generate_launch_description(): driver_launch, nav2_launch, ]) + diff --git a/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp index eb8cee7..35105ce 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp +++ b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp @@ -19,8 +19,8 @@ struct DeploymentContract static constexpr std::array kWheelIndices = {12, 13, 14, 15}; static constexpr float kLegKp = 50.0f; static constexpr float kLegKd = 1.5f; - static constexpr float kLegHoldKp = kLegKp; - static constexpr float kLegHoldKd = kLegKd; + static constexpr float kLegHoldKp = 80.0f; + static constexpr float kLegHoldKd = 4.0f; static constexpr float kWheelKd = 1.0f; static constexpr std::array kCanBusMap = { @@ -64,10 +64,10 @@ struct DeploymentContract }; static constexpr std::array kDefaultDofPos = { - 0.0f, 0.550f, -1.125f, - 0.0f, 0.550f, -1.125f, - 0.0f, 0.550f, -1.125f, - 0.0f, 0.550f, -1.125f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.9f, -1.8f, 0.0f, 0.0f, 0.0f, 0.0f }; }; diff --git a/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp index c696572..e158815 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp +++ b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include -#include "sim2real_common/deployment_contract.hpp" - #ifndef M_PI #define M_PI 3.14159265358979323846 #endif @@ -19,15 +18,12 @@ public: StandBalanceController(double control_dt = 0.02) : control_dt_(control_dt) { + profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f}; + profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f}; + profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f}; reset(); } - void setNominalLegPose(float hip_pitch, float knee) - { - nominal_hip_pitch_ = hip_pitch; - nominal_knee_ = knee; - } - void reset() { stable_time_ = 0.0f; @@ -38,8 +34,9 @@ public: const std::array& imu_gyro, const std::array& cmd) { - const float hip_base = nominal_hip_pitch_; - const float knee_base = nominal_knee_; + float hip_base = 0.9f; + float knee_base = -1.8f; + estimateBaseLegPose(hip_base, knee_base); float roll = 0.0f; float pitch = 0.0f; @@ -87,9 +84,28 @@ private: pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz))); } + void estimateBaseLegPose(float& hip, float& knee) + { + float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back()); + hip = interpolate(h_clamp, profile_h_, profile_hip_); + knee = interpolate(h_clamp, profile_h_, profile_knee_); + } + + float interpolate(float x, const std::vector& xp, const std::vector& fp) + { + if (x <= xp.front()) return fp.front(); + if (x >= xp.back()) return fp.back(); + for (std::size_t i = 0; i < xp.size() - 1; ++i) { + if (x >= xp[i] && x <= xp[i+1]) { + float f = (x - xp[i]) / (xp[i+1] - xp[i]); + return fp[i] + f * (fp[i+1] - fp[i]); + } + } + return fp.back(); + } + double control_dt_; - float nominal_hip_pitch_{DeploymentContract::kDefaultDofPos[1]}; - float nominal_knee_{DeploymentContract::kDefaultDofPos[2]}; + float height_{0.33f}; float kp_roll_{0.85f}; float kd_roll_rate_{0.03f}; float lateral_lean_gain_{0.0f}; @@ -101,6 +117,10 @@ private: float stable_gyro_deg_s_{45.0f}; float enter_hold_s_{1.0f}; + std::vector profile_h_; + std::vector profile_hip_; + std::vector profile_knee_; + float stable_time_{0.0f}; }; diff --git a/05_software/real/sim2real_ros2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp b/05_software/real/sim2real_ros2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp index 7612333..598a499 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp +++ b/05_software/real/sim2real_ros2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp @@ -12,41 +12,16 @@ #include "sensor_msgs/msg/imu.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/string.hpp" #include "sim2real_interfaces/msg/runtime_state.hpp" #include "sim2real_interfaces/msg/runtime_target.hpp" -#include "sim2real_common/event_logger.hpp" #include "sim2real_common/low_pass_filter.hpp" #include "sim2real_common/mahony_filter.hpp" #include "sim2real_common/safety_monitor.hpp" #include "sim2real_common/runtime_guard.hpp" -struct can_frame; - namespace sim2real_hw { -enum class RecoveryKind -{ - None, - Stale, - NoEffect -}; - -enum class RecoveryStage -{ - Idle, - AwaitInitFeedback, - AwaitEffectVerification -}; - -enum class ActiveModelMode -{ - Rough, - Crawl, - Wall -}; - struct MotorConfig { int bus; // 1 or 2 @@ -61,49 +36,14 @@ struct MotorStateInternal float velocity{0.0f}; float torque{0.0f}; float temperature{0.0f}; - float bus_voltage{0.0f}; - float estimated_current_arms{0.0f}; - float last_command_sim{0.0f}; std::uint32_t update_count{0}; std::uint32_t stale_count{0}; - std::uint32_t command_active_count{0}; - std::uint32_t no_effect_count{0}; - std::uint16_t fault_code{0}; - std::uint16_t fault_detail_1{0}; - std::uint16_t fault_detail_2{0}; // Hold-over state float last_valid_pos{0.0f}; float last_valid_vel{0.0f}; float last_valid_torque{0.0f}; std::uint32_t prev_update_count{0}; bool has_valid_data{false}; - bool has_bus_voltage{false}; - bool has_fault_snapshot{false}; - bool stale_reported{false}; - bool recovered_reported{false}; - bool disable_reported{false}; - bool command_effect_monitoring_active{false}; - bool no_effect_reported{false}; - bool high_temp_reported{false}; - bool high_current_reported{false}; - bool high_voltage_reported{false}; - bool low_voltage_reported{false}; - bool fault_code_reported{false}; - bool init_confirmed{false}; - std::uint32_t init_attempt_count{0}; - std::uint32_t recovery_attempt_count{0}; - std::uint32_t no_effect_recovery_attempt_count{0}; - std::chrono::steady_clock::time_point last_recovery_attempt_time_{}; - std::chrono::steady_clock::time_point last_no_effect_recovery_attempt_time_{}; - std::chrono::steady_clock::time_point last_diag_snapshot_time_{}; - std::chrono::steady_clock::time_point last_diag_request_time_{}; - std::chrono::steady_clock::time_point recovery_stage_deadline_{}; - std::uint32_t recovery_start_update_count{0}; - std::uint32_t recovery_active_attempt_number{0}; - RecoveryKind recovery_kind{RecoveryKind::None}; - RecoveryStage recovery_stage{RecoveryStage::Idle}; - std::string recovery_trigger; - std::string last_power_event_reason; }; class HardwareBridgeNode : public rclcpp::Node @@ -114,7 +54,6 @@ public: private: void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg); - void onModelStatus(const std_msgs::msg::String::SharedPtr msg); void onReadLoop(); void onWriteLoop(); void onImu(const sensor_msgs::msg::Imu::SharedPtr msg); @@ -125,83 +64,25 @@ private: bool readCanFrame(int fd, void* frame, int timeout_us); bool enableMotor(int fd, int motor_id); - bool disableMotor(int fd, int motor_id, bool clear_fault = false); - bool writeParameterInt(int fd, int motor_id, std::uint16_t param_id, std::uint32_t value); + bool disableMotor(int fd, int motor_id); bool setModeRaw(int fd, int motor_id, std::int8_t mode); - bool readParameter(int fd, int motor_id, std::uint16_t param_id); bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit); bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque); - bool initializeMotor(std::size_t index, const std::string & reason, int max_attempts = 3); - bool initializeMotorsOnBus(int bus_id, const std::string & reason); - bool waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout); - void processCanFrame(const struct can_frame & frame, int bus_id); - void drainCanFrames(int fd, int bus_id, int timeout_us); - bool isLegMotor(std::size_t index) const; - bool isWheelMotor(std::size_t index) const; - bool motorHasBlockingFault(std::size_t index) const; - bool isNoEffectConditionPresent(std::size_t index) const; - std::uint32_t noEffectCommandWarmupCycles(std::size_t index) const; - std::uint32_t noEffectTriggerCycles(std::size_t index) const; - std::uint32_t noEffectAttemptLimit(std::size_t index) const; - std::uint32_t noEffectCooldownMs(std::size_t index) const; - std::uint32_t noEffectVerifyTimeoutMs(std::size_t index) const; - bool hasFreshNoEffectDiagnostics(std::size_t index) const; - void requestMotorDiagnostics(std::size_t index); - std::string classifyNoEffectSuspect(std::size_t index) const; - std::string buildNoEffectSummary(std::size_t index) const; - void updateMotorCommandTracking(std::size_t index, float sim_command, const std::string & target_source); - void updateNoEffectDetection(std::size_t index); - bool startMotorRecoverySequence( - std::size_t index, - const std::string & trigger, - RecoveryKind kind, - std::uint32_t attempt_number); - void processMotorRecoverySequence(std::size_t index); - void clearMotorRecoverySequence(std::size_t index); - bool shouldAttemptMotorRecovery(std::size_t index) const; - bool attemptMotorRecovery(std::size_t index, const std::string & trigger); - bool shouldAttemptNoEffectRecovery(std::size_t index) const; - bool attemptNoEffectRecovery(std::size_t index, const std::string & trigger); - const char * jointName(std::size_t index) const; - std::string motorTag(std::size_t index) const; - float estimateCurrentArms(float torque_nm) const; - std::string decodeFaultCode(std::uint16_t fault_code) const; - std::string decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const; - std::string buildMotorFaultSummary(std::size_t index) const; - std::string formatProtectionReason(const std::string & trigger, const std::string & reason) const; - void logProtectionEvent(const std::string & trigger, const std::string & reason, const std::string & action); - void logMotorPowerEvent(std::size_t index, const std::string & state, const std::string & reason); - void logMotorDiagnosticEvent(std::size_t index, const std::string & event, const std::string & reason, const char * level = "WARN"); - void updateMotorTelemetry(std::size_t index, float pos_sim, float vel_sim, float torque_sim, float temperature_c); - void handleParameterResponse(const struct can_frame & frame, int bus_id); - void updateMotorDiagnostics(std::size_t index); - void pollMotorDiagnostics(); - void finalizeRunSummary(); rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Subscription::SharedPtr target_sub_; - rclcpp::Subscription::SharedPtr model_status_sub_; rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr estop_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr read_timer_; rclcpp::TimerBase::SharedPtr write_timer_; - rclcpp::CallbackGroup::SharedPtr motor_callback_group_; - rclcpp::CallbackGroup::SharedPtr sensor_callback_group_; - rclcpp::CallbackGroup::SharedPtr control_callback_group_; std::mutex target_mutex_; std::array latest_target_{}; std::array latest_raw_action_{}; std::string latest_target_source_{"boot_hold"}; rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME}; - std::array rough_default_dof_pos_{}; - std::array crawl_default_dof_pos_{}; - std::array wall_default_dof_pos_{}; - std::array active_default_dof_pos_{}; - ActiveModelMode active_model_mode_{ActiveModelMode::Rough}; - bool model_switch_active_{false}; std::uint32_t target_sequence_{0}; std::uint32_t state_sequence_{0}; double target_timeout_ms_{150.0}; @@ -220,7 +101,6 @@ private: // Hold-over constants static constexpr std::uint32_t kHoldoverThreshold = 2; - static constexpr std::uint32_t kMotorDropReportThreshold = 40; // Motor configurations and states std::array motors_; @@ -253,71 +133,24 @@ private: std::unique_ptr lpf_wheels_; std::unique_ptr mahony_filter_; std::unique_ptr safety_monitor_; - std::unique_ptr model_switch_safety_monitor_; std::unique_ptr runtime_guard_; - std::atomic mahony_initialized_{false}; + bool mahony_initialized_{false}; rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME}; rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME}; - rclcpp::Time last_diag_poll_time_{0, 0, RCL_ROS_TIME}; - std::size_t diag_poll_motor_index_{0}; // Telemetry std::uint32_t fresh_count_{0}; std::uint32_t holdover_count_{0}; std::uint32_t stale_max_{0}; std::uint32_t holdover_events_total_{0}; - std::uint32_t protection_trigger_count_{0}; - std::uint32_t motor_drop_event_count_{0}; - std::uint32_t motor_recover_event_count_{0}; - std::uint32_t motor_fault_event_count_{0}; - bool timeout_hold_logged_{false}; - bool clip_active_logged_{false}; bool dry_run_{false}; std::atomic estop_triggered_{false}; std::atomic safety_enabled_{true}; std::atomic safety_triggered_{false}; std::string safety_reason_{""}; - sim2real_common::EventLogger event_logger_; - std::string run_log_dir_; - float motor_temp_warn_c_{100.0f}; - float motor_temp_fault_c_{135.0f}; - float motor_bus_overvoltage_v_{60.0f}; - float motor_bus_undervoltage_v_{12.0f}; - float motor_current_warn_arms_{10.5f}; - float motor_current_peak_arms_{14.0f}; - float motor_torque_warn_nm_{13.0f}; - float motor_torque_peak_nm_{17.0f}; - double diag_poll_period_s_{0.10}; - float wheel_no_effect_command_threshold_{1.0f}; - float wheel_no_effect_min_response_ratio_{0.20f}; - float wheel_no_effect_velocity_epsilon_{0.25f}; - float wheel_no_effect_max_temperature_c_{90.0f}; - float wheel_no_effect_min_bus_voltage_v_{18.0f}; - std::uint32_t wheel_no_effect_command_warmup_cycles_{12}; - std::uint32_t wheel_no_effect_trigger_cycles_{30}; - std::uint32_t wheel_no_effect_attempt_limit_{2}; - std::uint32_t wheel_no_effect_cooldown_ms_{1200}; - std::uint32_t wheel_recovery_verify_timeout_ms_{180}; - std::uint32_t wheel_no_effect_diag_freshness_ms_{350}; - std::uint32_t wheel_no_effect_diag_request_period_ms_{80}; - float leg_no_effect_position_error_threshold_{0.18f}; - float leg_no_effect_velocity_epsilon_{0.12f}; - float leg_no_effect_max_estimated_current_arms_{4.0f}; - float leg_no_effect_max_abs_torque_nm_{5.0f}; - float leg_no_effect_max_temperature_c_{100.0f}; - float leg_no_effect_min_bus_voltage_v_{18.0f}; - std::uint32_t leg_no_effect_command_warmup_cycles_{40}; - std::uint32_t leg_no_effect_trigger_cycles_{25}; - std::uint32_t leg_no_effect_attempt_limit_{2}; - std::uint32_t leg_no_effect_cooldown_ms_{1200}; - std::uint32_t leg_recovery_verify_timeout_ms_{220}; void onEstop(const std_msgs::msg::Bool::SharedPtr msg); - void logEvent( - const std::string & level, - const std::string & event, - const std::string & message); }; } // namespace sim2real_hw diff --git a/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp b/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp index 0331bf2..09131aa 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp +++ b/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp @@ -3,11 +3,8 @@ #include #include #include -#include #include -#include #include -#include #include #include @@ -18,7 +15,6 @@ #include #include -#include "rclcpp/executors/multi_threaded_executor.hpp" #include "sim2real_common/deployment_contract.hpp" using namespace std::chrono_literals; @@ -30,23 +26,13 @@ namespace sim2real_hw const std::uint32_t COMM_ENABLE = 3; const std::uint32_t COMM_DISABLE = 4; const std::uint32_t COMM_WRITE_PARAMETER = 18; -const std::uint32_t COMM_READ_PARAMETER = 17; const std::uint32_t COMM_OPERATION_CONTROL = 1; const std::uint32_t COMM_SET_ZERO_POSITION = 6; const std::uint16_t PARAM_MODE = 0x7005; const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017; const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B; -const std::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; -const std::uint16_t PARAM_DRV_FAULT_DETAIL_2 = 0x3025; const std::uint8_t HOST_ID = 0xFD; -constexpr int kMotorInitRetrySleepMs = 15; -constexpr int kMotorInitConfirmTimeoutMs = 120; -constexpr int kMotorRecoveryCooldownMs = 500; -constexpr std::uint32_t kMotorRecoveryTriggerStaleCount = 10; -constexpr std::uint32_t kMotorRecoveryAttemptLimit = 3; + inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val) { buf[0] = (val >> 8) & 0xFF; @@ -60,173 +46,26 @@ inline float nearest_periodic(float val, float ref) return ref + wrapped; } -std::string extractJsonStringField(const std::string & payload, const std::string & key) -{ - const std::string needle = "\"" + key + "\":\""; - const std::size_t start = payload.find(needle); - if (start == std::string::npos) { - return {}; - } - - const std::size_t value_start = start + needle.size(); - const std::size_t value_end = payload.find('"', value_start); - if (value_end == std::string::npos) { - return {}; - } - - return payload.substr(value_start, value_end - value_start); -} - -std::string toLowerCopy(std::string value) -{ - std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { - return static_cast(std::tolower(c)); - }); - return value; -} - HardwareBridgeNode::HardwareBridgeNode() : Node("sim2real_hw_node") { // 1. Declare and get parameters - const std::string event_log_dir = declare_parameter( - "event_log_dir", "logs_v2_web"); - run_log_dir_ = event_log_dir; - event_logger_.configure(event_log_dir, "sim2real_hw_events"); target_timeout_ms_ = declare_parameter("target_timeout_ms", 150.0); can0_name_ = declare_parameter("can0_name", "can0"); can1_name_ = declare_parameter("can1_name", "can1"); dry_run_ = declare_parameter("dry_run", true); // Default to dry-run for safety - const std::vector default_rough_pose( - sim2real_common::DeploymentContract::kDefaultDofPos.begin(), - sim2real_common::DeploymentContract::kDefaultDofPos.end()); - const std::vector configured_rough_pose = declare_parameter>( - "rough_default_dof_pos", default_rough_pose); - if (configured_rough_pose.size() == rough_default_dof_pos_.size()) { - for (std::size_t i = 0; i < rough_default_dof_pos_.size(); ++i) { - rough_default_dof_pos_[i] = static_cast(configured_rough_pose[i]); - } - } else { - rough_default_dof_pos_ = sim2real_common::DeploymentContract::kDefaultDofPos; - RCLCPP_WARN( - get_logger(), - "Parameter rough_default_dof_pos has %zu entries, expected 16. Falling back to deployment default pose.", - configured_rough_pose.size()); - } - active_default_dof_pos_ = rough_default_dof_pos_; - const std::vector default_crawl_pose = declare_parameter>( - "crawl_default_dof_pos", - std::vector{ - 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 - }); - if (default_crawl_pose.size() == crawl_default_dof_pos_.size()) { - for (std::size_t i = 0; i < crawl_default_dof_pos_.size(); ++i) { - crawl_default_dof_pos_[i] = static_cast(default_crawl_pose[i]); - } - } else { - crawl_default_dof_pos_ = { - 0.2f, 1.697f, -2.650f, - -0.2f, 1.697f, -2.650f, - 0.2f, 1.697f, -2.650f, - -0.2f, 1.697f, -2.650f, - 0.0f, 0.0f, 0.0f, 0.0f - }; - RCLCPP_WARN( - get_logger(), - "Parameter crawl_default_dof_pos has %zu entries, expected 16. Falling back to configured crawl pose.", - default_crawl_pose.size()); - } - const std::vector default_wall_pose_param( - rough_default_dof_pos_.begin(), rough_default_dof_pos_.end()); - const std::vector default_wall_pose = declare_parameter>( - "wall_default_dof_pos", default_wall_pose_param); - if (default_wall_pose.size() == wall_default_dof_pos_.size()) { - for (std::size_t i = 0; i < wall_default_dof_pos_.size(); ++i) { - wall_default_dof_pos_[i] = static_cast(default_wall_pose[i]); - } - } else { - wall_default_dof_pos_ = rough_default_dof_pos_; - RCLCPP_WARN( - get_logger(), - "Parameter wall_default_dof_pos has %zu entries, expected 16. Falling back to rough default pose.", - default_wall_pose.size()); - } // Safety parameters safety_enabled_ = declare_parameter("safety_enabled", true); double max_target_offset = declare_parameter("max_target_offset", 0.6); - double model_switch_max_target_offset = declare_parameter( - "model_switch_max_target_offset", std::max(max_target_offset, 1.8)); double hard_target_offset = declare_parameter("hard_target_offset", 1.2); double max_ang_vel = declare_parameter("max_ang_vel", 10.0); double max_tilt_z = declare_parameter("max_tilt_z", -0.3); int clip_to_brake = declare_parameter("clip_to_brake", 0); double imu_age_warn_ms = declare_parameter("imu_age_warn_ms", 60.0); double imu_age_stop_ms = declare_parameter("imu_age_stop_ms", 200.0); - motor_temp_warn_c_ = static_cast(declare_parameter("motor_temp_warn_c", 100.0)); - motor_temp_fault_c_ = static_cast(declare_parameter("motor_temp_fault_c", 135.0)); - motor_bus_overvoltage_v_ = static_cast(declare_parameter("motor_bus_overvoltage_v", 60.0)); - motor_bus_undervoltage_v_ = static_cast(declare_parameter("motor_bus_undervoltage_v", 12.0)); - motor_current_warn_arms_ = static_cast(declare_parameter("motor_current_warn_arms", 10.5)); - motor_current_peak_arms_ = static_cast(declare_parameter("motor_current_peak_arms", 14.0)); - motor_torque_warn_nm_ = static_cast(declare_parameter("motor_torque_warn_nm", 13.0)); - motor_torque_peak_nm_ = static_cast(declare_parameter("motor_torque_peak_nm", 17.0)); - diag_poll_period_s_ = declare_parameter("motor_diag_poll_period_s", 0.10); - wheel_no_effect_command_threshold_ = static_cast( - declare_parameter("wheel_no_effect_command_threshold", 1.0)); - wheel_no_effect_min_response_ratio_ = static_cast( - declare_parameter("wheel_no_effect_min_response_ratio", 0.20)); - wheel_no_effect_velocity_epsilon_ = static_cast( - declare_parameter("wheel_no_effect_velocity_epsilon", 0.25)); - wheel_no_effect_max_temperature_c_ = static_cast( - declare_parameter("wheel_no_effect_max_temperature_c", 90.0)); - wheel_no_effect_min_bus_voltage_v_ = static_cast( - declare_parameter("wheel_no_effect_min_bus_voltage_v", 18.0)); - wheel_no_effect_command_warmup_cycles_ = static_cast( - declare_parameter("wheel_no_effect_command_warmup_cycles", 12)); - wheel_no_effect_trigger_cycles_ = static_cast( - declare_parameter("wheel_no_effect_trigger_cycles", 30)); - wheel_no_effect_attempt_limit_ = static_cast( - declare_parameter("wheel_no_effect_attempt_limit", 2)); - wheel_no_effect_cooldown_ms_ = static_cast( - declare_parameter("wheel_no_effect_cooldown_ms", 1200)); - wheel_recovery_verify_timeout_ms_ = static_cast( - declare_parameter("wheel_recovery_verify_timeout_ms", 180)); - wheel_no_effect_diag_freshness_ms_ = static_cast( - declare_parameter("wheel_no_effect_diag_freshness_ms", 350)); - wheel_no_effect_diag_request_period_ms_ = static_cast( - declare_parameter("wheel_no_effect_diag_request_period_ms", 80)); - leg_no_effect_position_error_threshold_ = static_cast( - declare_parameter("leg_no_effect_position_error_threshold", 0.18)); - leg_no_effect_velocity_epsilon_ = static_cast( - declare_parameter("leg_no_effect_velocity_epsilon", 0.12)); - leg_no_effect_max_estimated_current_arms_ = static_cast( - declare_parameter("leg_no_effect_max_estimated_current_arms", 4.0)); - leg_no_effect_max_abs_torque_nm_ = static_cast( - declare_parameter("leg_no_effect_max_abs_torque_nm", 5.0)); - leg_no_effect_max_temperature_c_ = static_cast( - declare_parameter("leg_no_effect_max_temperature_c", 100.0)); - leg_no_effect_min_bus_voltage_v_ = static_cast( - declare_parameter("leg_no_effect_min_bus_voltage_v", 18.0)); - leg_no_effect_command_warmup_cycles_ = static_cast( - declare_parameter("leg_no_effect_command_warmup_cycles", 40)); - leg_no_effect_trigger_cycles_ = static_cast( - declare_parameter("leg_no_effect_trigger_cycles", 25)); - leg_no_effect_attempt_limit_ = static_cast( - declare_parameter("leg_no_effect_attempt_limit", 2)); - leg_no_effect_cooldown_ms_ = static_cast( - declare_parameter("leg_no_effect_cooldown_ms", 1200)); - leg_recovery_verify_timeout_ms_ = static_cast( - declare_parameter("leg_recovery_verify_timeout_ms", 220)); RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false"); - RCLCPP_INFO(get_logger(), "Event log file: %s", event_logger_.componentLogPath().c_str()); - RCLCPP_INFO(get_logger(), "Run log directory: %s", run_log_dir_.c_str()); - logEvent("INFO", "node_start", dry_run_ ? "Hardware bridge node started in dry-run mode." : "Hardware bridge node started."); if (safety_enabled_) { RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel); } else { @@ -254,13 +93,6 @@ HardwareBridgeNode::HardwareBridgeNode() clip_to_brake, static_cast(hard_target_offset) ); - model_switch_safety_monitor_ = std::make_unique( - static_cast(model_switch_max_target_offset), - static_cast(max_ang_vel), - static_cast(max_tilt_z), - clip_to_brake, - static_cast(hard_target_offset) - ); runtime_guard_ = std::make_unique( static_cast(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop @@ -278,58 +110,43 @@ HardwareBridgeNode::HardwareBridgeNode() } // 5. Initialize motor target states - latest_target_ = rough_default_dof_pos_; + latest_target_ = sim2real_common::DeploymentContract::kDefaultDofPos; latest_raw_action_.fill(0.0f); // 6. Set up ROS publishers & subscriptions - motor_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - sensor_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - control_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::SubscriptionOptions sensor_sub_options; - sensor_sub_options.callback_group = sensor_callback_group_; - rclcpp::SubscriptionOptions control_sub_options; - control_sub_options.callback_group = control_callback_group_; - state_pub_ = create_publisher("runtime/state", 10); target_sub_ = create_subscription( "runtime/target", 10, - std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1), - control_sub_options); - model_status_sub_ = create_subscription( - "runtime/model_status", 10, - std::bind(&HardwareBridgeNode::onModelStatus, this, std::placeholders::_1), - control_sub_options); + std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1)); std::string imu_topic = declare_parameter("imu_topic", "/odin1/imu"); imu_sub_ = create_subscription( imu_topic, 10, - std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1), - sensor_sub_options); + std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1)); estop_sub_ = create_subscription( "/safety/estop", 10, - std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1), - control_sub_options); + std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1)); // Odom subscription std::string odom_topic = declare_parameter("odom_topic", "/odom"); odom_sub_ = create_subscription( odom_topic, 10, - std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1), - sensor_sub_options); + std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1)); // 7. Enable motors on total startup if (!dry_run_) { RCLCPP_INFO(get_logger(), "Enabling RobStride motors..."); for (std::size_t i = 0; i < 16; ++i) { - initializeMotor(i, "startup_init"); + int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; + enableMotor(fd, motors_[i].id); + setModeRaw(fd, motors_[i].id, 0); // MIT Mode + writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f); + writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f); } } // 8. Timers at 200Hz (5ms) - read_timer_ = create_wall_timer( - 5ms, std::bind(&HardwareBridgeNode::onReadLoop, this), motor_callback_group_); - write_timer_ = create_wall_timer( - 5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this), motor_callback_group_); + read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this)); + write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this)); } HardwareBridgeNode::~HardwareBridgeNode() @@ -338,7 +155,6 @@ HardwareBridgeNode::~HardwareBridgeNode() RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown..."); for (std::size_t i = 0; i < 16; ++i) { int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; - logMotorPowerEvent(i, "disabled", "node shutdown requested motor disable"); disableMotor(fd, motors_[i].id); } if (can0_fd_ >= 0) { @@ -352,989 +168,6 @@ HardwareBridgeNode::~HardwareBridgeNode() } } } - logEvent("INFO", "node_stop", "Hardware bridge node stopped."); - finalizeRunSummary(); -} - -void HardwareBridgeNode::logEvent( - const std::string & level, - const std::string & event, - const std::string & message) -{ - event_logger_.log(level, "sim2real_hw_node", event, message); -} - -const char * HardwareBridgeNode::jointName(std::size_t index) const -{ - return sim2real_common::kJointLabels[index]; -} - -std::string HardwareBridgeNode::motorTag(std::size_t index) const -{ - std::ostringstream oss; - oss << jointName(index) - << "(index=" << index - << ",bus=" << motors_[index].bus - << ",id=" << motors_[index].id - << ")"; - return oss.str(); -} - -float HardwareBridgeNode::estimateCurrentArms(float torque_nm) const -{ - constexpr float kTorqueConstantNmPerArms = 1.22f; - return std::abs(torque_nm) / kTorqueConstantNmPerArms; -} - -std::string HardwareBridgeNode::decodeFaultCode(std::uint16_t fault_code) const -{ - if (fault_code == 0) { - return "none"; - } - - std::vector reasons; - if (fault_code & (1u << 14)) reasons.emplace_back("stall_or_overload"); - if (fault_code & (1u << 7)) reasons.emplace_back("encoder_not_calibrated"); - if (fault_code & (1u << 3)) reasons.emplace_back("bus_overvoltage"); - if (fault_code & (1u << 2)) reasons.emplace_back("bus_undervoltage"); - if (fault_code & (1u << 1)) reasons.emplace_back("driver_chip_fault"); - if (fault_code & (1u << 0)) reasons.emplace_back("overtemperature"); - if (reasons.empty()) reasons.emplace_back("unknown_fault_bits"); - - std::ostringstream oss; - for (std::size_t i = 0; i < reasons.size(); ++i) { - if (i > 0) { - oss << '|'; - } - oss << reasons[i]; - } - return oss.str(); -} - -std::string HardwareBridgeNode::decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const -{ - if (register_value == 0) { - return "none"; - } - - std::ostringstream oss; - if (register_index == 1) { - oss << "driver_fault_reg1=0x" << std::hex << register_value - << " (possible: mos_overcurrent_or_uvlo)"; - } else { - oss << "driver_fault_reg2=0x" << std::hex << register_value - << " (possible: gate_driver_short_or_half_bridge_damage)"; - } - return oss.str(); -} - -std::string HardwareBridgeNode::buildMotorFaultSummary(std::size_t index) const -{ - const auto & state = motor_states_[index]; - std::ostringstream oss; - oss << "motor=" << motorTag(index) - << ", fault_code=0x" << std::hex << state.fault_code << std::dec - << ", decoded_fault=" << decodeFaultCode(state.fault_code) - << ", temperature_c=" << state.temperature - << ", estimated_current_arms=" << state.estimated_current_arms; - if (state.has_bus_voltage) { - oss << ", bus_voltage_v=" << state.bus_voltage; - } - if (state.fault_detail_1 != 0) { - oss << ", " << decodeFaultDetailRegister(state.fault_detail_1, 1); - } - if (state.fault_detail_2 != 0) { - oss << ", " << decodeFaultDetailRegister(state.fault_detail_2, 2); - } - return oss.str(); -} - -std::string HardwareBridgeNode::formatProtectionReason( - const std::string & trigger, - const std::string & reason) const -{ - std::ostringstream oss; - oss << "trigger=" << trigger - << ", protection_action=safety_brake, reason=" << reason; - return oss.str(); -} - -void HardwareBridgeNode::logProtectionEvent( - const std::string & trigger, - const std::string & reason, - const std::string & action) -{ - protection_trigger_count_++; - std::ostringstream oss; - oss << "trigger=" << trigger - << ", protection_action=" << action - << ", reason=" << reason; - std::size_t suspect_index = motors_.size(); - float suspect_score = -1.0f; - for (std::size_t i = 0; i < motors_.size(); ++i) { - float score = 0.0f; - if (motor_states_[i].fault_code != 0 || motor_states_[i].fault_detail_1 != 0 || motor_states_[i].fault_detail_2 != 0) { - score += 100.0f; - } - score += motor_states_[i].temperature; - score += 2.0f * motor_states_[i].estimated_current_arms; - score += std::min(50.0f, static_cast(motor_states_[i].stale_count)); - if (score > suspect_score) { - suspect_score = score; - suspect_index = i; - } - } - if (suspect_index < motors_.size()) { - oss << ", suspect_motor={" << buildMotorFaultSummary(suspect_index) << "}"; - } - logEvent("ERROR", "protection_triggered", oss.str()); -} - -void HardwareBridgeNode::logMotorPowerEvent( - std::size_t index, - const std::string & state, - const std::string & reason) -{ - std::ostringstream oss; - oss << "motor=" << motorTag(index) - << ", state=" << state - << ", reason=" << reason; - logEvent(state == "recovered" ? "INFO" : "WARN", "motor_power_state", oss.str()); -} - -void HardwareBridgeNode::logMotorDiagnosticEvent( - std::size_t index, - const std::string & event, - const std::string & reason, - const char * level) -{ - std::ostringstream oss; - oss << "motor=" << motorTag(index) << ", " << reason; - logEvent(level, event, oss.str()); -} - -void HardwareBridgeNode::finalizeRunSummary() -{ - std::ostringstream oss; - oss << "run_dir=" << run_log_dir_ - << ", protection_trigger_count=" << protection_trigger_count_ - << ", motor_drop_event_count=" << motor_drop_event_count_ - << ", motor_recover_event_count=" << motor_recover_event_count_ - << ", motor_fault_event_count=" << motor_fault_event_count_ - << ", holdover_events_total=" << holdover_events_total_ - << ", final_safety_triggered=" << (safety_triggered_ ? "true" : "false"); - if (!safety_reason_.empty()) { - oss << ", final_safety_reason=" << safety_reason_; - } - event_logger_.logSummary("sim2real_hw_node", oss.str()); -} - -void HardwareBridgeNode::updateMotorTelemetry( - std::size_t index, - float pos_sim, - float vel_sim, - float torque_sim, - float temperature_c) -{ - auto & state = motor_states_[index]; - state.position = pos_sim; - state.velocity = vel_sim; - state.torque = torque_sim; - state.temperature = temperature_c; - state.estimated_current_arms = estimateCurrentArms(torque_sim); - state.update_count++; - state.stale_count = 0; - state.last_valid_pos = pos_sim; - state.last_valid_vel = vel_sim; - state.last_valid_torque = torque_sim; - state.has_valid_data = true; - state.init_confirmed = true; - state.recovery_attempt_count = 0; -} - -bool HardwareBridgeNode::isWheelMotor(std::size_t index) const -{ - return index >= sim2real_common::DeploymentContract::kLegJointCount; -} - -bool HardwareBridgeNode::isLegMotor(std::size_t index) const -{ - return index < sim2real_common::DeploymentContract::kLegJointCount; -} - -bool HardwareBridgeNode::motorHasBlockingFault(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { - return true; - } - const float max_temperature = isWheelMotor(index) - ? wheel_no_effect_max_temperature_c_ - : leg_no_effect_max_temperature_c_; - if (state.temperature >= max_temperature) { - return true; - } - const float min_bus_voltage = isWheelMotor(index) - ? wheel_no_effect_min_bus_voltage_v_ - : leg_no_effect_min_bus_voltage_v_; - if (state.has_bus_voltage && state.bus_voltage <= min_bus_voltage) { - return true; - } - return false; -} - -std::uint32_t HardwareBridgeNode::noEffectCommandWarmupCycles(std::size_t index) const -{ - return isWheelMotor(index) ? wheel_no_effect_command_warmup_cycles_ : leg_no_effect_command_warmup_cycles_; -} - -std::uint32_t HardwareBridgeNode::noEffectTriggerCycles(std::size_t index) const -{ - return isWheelMotor(index) ? wheel_no_effect_trigger_cycles_ : leg_no_effect_trigger_cycles_; -} - -std::uint32_t HardwareBridgeNode::noEffectAttemptLimit(std::size_t index) const -{ - return isWheelMotor(index) ? wheel_no_effect_attempt_limit_ : leg_no_effect_attempt_limit_; -} - -std::uint32_t HardwareBridgeNode::noEffectCooldownMs(std::size_t index) const -{ - return isWheelMotor(index) ? wheel_no_effect_cooldown_ms_ : leg_no_effect_cooldown_ms_; -} - -std::uint32_t HardwareBridgeNode::noEffectVerifyTimeoutMs(std::size_t index) const -{ - return isWheelMotor(index) ? wheel_recovery_verify_timeout_ms_ : leg_recovery_verify_timeout_ms_; -} - -bool HardwareBridgeNode::hasFreshNoEffectDiagnostics(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (!state.has_bus_voltage || !state.has_fault_snapshot) { - return false; - } - if (state.last_diag_snapshot_time_.time_since_epoch().count() == 0) { - return false; - } - const auto age = std::chrono::steady_clock::now() - state.last_diag_snapshot_time_; - return age <= std::chrono::milliseconds(wheel_no_effect_diag_freshness_ms_); -} - -void HardwareBridgeNode::requestMotorDiagnostics(std::size_t index) -{ - if (dry_run_ || index >= motors_.size()) { - return; - } - - auto & state = motor_states_[index]; - const auto now_tp = std::chrono::steady_clock::now(); - if (state.last_diag_request_time_.time_since_epoch().count() != 0) { - const auto since_last = now_tp - state.last_diag_request_time_; - if (since_last < std::chrono::milliseconds(wheel_no_effect_diag_request_period_ms_)) { - return; - } - } - - const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; - readParameter(fd, motors_[index].id, PARAM_VBUS); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_1); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_2); - state.last_diag_request_time_ = now_tp; -} - -std::string HardwareBridgeNode::classifyNoEffectSuspect(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { - return "suspect_driver_fault_or_protection"; - } - const float min_bus_voltage = isWheelMotor(index) - ? wheel_no_effect_min_bus_voltage_v_ - : leg_no_effect_min_bus_voltage_v_; - if (state.has_bus_voltage && state.bus_voltage <= min_bus_voltage) { - return "suspect_low_voltage"; - } - const float max_temperature = isWheelMotor(index) - ? wheel_no_effect_max_temperature_c_ - : leg_no_effect_max_temperature_c_; - if (state.temperature >= max_temperature) { - return "suspect_overtemp"; - } - if (state.estimated_current_arms >= motor_current_warn_arms_ || - std::abs(state.torque) >= motor_torque_warn_nm_) { - return "suspect_mechanical_stall_or_overload"; - } - if (isLegMotor(index)) { - return "suspect_enable_or_position_loop_drop"; - } - return "suspect_enable_or_mode_drop"; -} - -std::string HardwareBridgeNode::buildNoEffectSummary(std::size_t index) const -{ - const auto & state = motor_states_[index]; - const float commanded = std::abs(state.last_command_sim); - const float actual = std::abs(state.velocity); - const float response_ratio = isWheelMotor(index) && commanded > 1.0e-4f ? actual / commanded : 1.0f; - const float position_error = std::abs(state.last_command_sim - state.position); - - std::ostringstream oss; - oss << "suspect=" << classifyNoEffectSuspect(index) - << ", command=" << state.last_command_sim - << ", position=" << state.position - << ", position_error=" << position_error - << ", feedback_vel=" << state.velocity - << ", response_ratio=" << response_ratio - << ", torque_nm=" << state.torque - << ", estimated_current_arms=" << state.estimated_current_arms - << ", temperature_c=" << state.temperature; - if (state.has_bus_voltage) { - oss << ", bus_voltage_v=" << state.bus_voltage; - } - if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { - oss << ", fault_summary={" << buildMotorFaultSummary(index) << "}"; - } - return oss.str(); -} - -void HardwareBridgeNode::updateMotorCommandTracking( - std::size_t index, - float sim_command, - const std::string & target_source) -{ - auto & state = motor_states_[index]; - state.last_command_sim = sim_command; - - if (target_source == "safety_brake" || target_source == "safety_estop") { - state.command_active_count = 0; - state.no_effect_count = 0; - state.no_effect_recovery_attempt_count = 0; - state.command_effect_monitoring_active = false; - state.no_effect_reported = false; - return; - } - - bool command_active = false; - if (isWheelMotor(index)) { - command_active = std::abs(sim_command) >= wheel_no_effect_command_threshold_; - } else { - command_active = std::abs(sim_command - state.position) >= leg_no_effect_position_error_threshold_; - } - - if (command_active) { - state.command_active_count++; - state.command_effect_monitoring_active = true; - } else { - state.command_active_count = 0; - state.no_effect_count = 0; - state.no_effect_recovery_attempt_count = 0; - state.command_effect_monitoring_active = false; - state.no_effect_reported = false; - } -} - -void HardwareBridgeNode::updateNoEffectDetection(std::size_t index) -{ - auto & state = motor_states_[index]; - if (!state.has_valid_data || state.stale_count > 0) { - state.no_effect_count = 0; - state.command_effect_monitoring_active = false; - state.no_effect_reported = false; - return; - } - - if (!state.command_effect_monitoring_active || - state.command_active_count < noEffectCommandWarmupCycles(index)) { - state.no_effect_count = 0; - state.no_effect_reported = false; - return; - } - - if (!hasFreshNoEffectDiagnostics(index)) { - requestMotorDiagnostics(index); - state.no_effect_count = 0; - return; - } - - if (motorHasBlockingFault(index)) { - state.no_effect_count = 0; - return; - } - - if (isNoEffectConditionPresent(index)) { - state.no_effect_count++; - if (!state.no_effect_reported && - state.no_effect_count >= noEffectTriggerCycles(index)) { - state.no_effect_reported = true; - std::ostringstream oss; - oss << "trigger=" << (isWheelMotor(index) ? "wheel_no_effect" : "leg_no_effect") - << ", no_effect_count=" << state.no_effect_count - << ", " << buildNoEffectSummary(index); - logMotorDiagnosticEvent(index, "motor_no_effect_detected", oss.str(), "WARN"); - } - } else { - state.no_effect_count = 0; - state.no_effect_recovery_attempt_count = 0; - state.no_effect_reported = false; - } -} - -bool HardwareBridgeNode::isNoEffectConditionPresent(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (isWheelMotor(index)) { - const float commanded = std::abs(state.last_command_sim); - const float actual = std::abs(state.velocity); - const float response_ratio = commanded > 1.0e-4f ? actual / commanded : 1.0f; - return actual <= wheel_no_effect_velocity_epsilon_ || - response_ratio < wheel_no_effect_min_response_ratio_; - } - - const float position_error = std::abs(state.last_command_sim - state.position); - const float actual_velocity = std::abs(state.velocity); - const float estimated_current = std::abs(state.estimated_current_arms); - const float measured_torque = std::abs(state.torque); - return position_error >= leg_no_effect_position_error_threshold_ && - actual_velocity <= leg_no_effect_velocity_epsilon_ && - estimated_current <= leg_no_effect_max_estimated_current_arms_ && - measured_torque <= leg_no_effect_max_abs_torque_nm_; -} - -void HardwareBridgeNode::handleParameterResponse(const struct can_frame & frame, int bus_id) -{ - const std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; - const int motor_id = extra_data & 0xFF; - const std::uint16_t param_id = static_cast((frame.data[1] << 8) | frame.data[0]); - - for (std::size_t i = 0; i < motors_.size(); ++i) { - if (motors_[i].bus != bus_id || motors_[i].id != motor_id) { - continue; - } - - auto & state = motor_states_[i]; - if (param_id == PARAM_VBUS) { - float vbus = 0.0f; - std::memcpy(&vbus, &frame.data[4], sizeof(float)); - state.bus_voltage = vbus; - state.has_bus_voltage = true; - state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); - } else if (param_id == PARAM_DRV_FAULT) { - const std::uint16_t fault = static_cast((frame.data[5] << 8) | frame.data[4]); - state.fault_code = fault; - state.has_fault_snapshot = true; - state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); - } else if (param_id == PARAM_DRV_FAULT_DETAIL_1) { - state.fault_detail_1 = static_cast((frame.data[5] << 8) | frame.data[4]); - state.has_fault_snapshot = true; - state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); - } else if (param_id == PARAM_DRV_FAULT_DETAIL_2) { - state.fault_detail_2 = static_cast((frame.data[5] << 8) | frame.data[4]); - state.has_fault_snapshot = true; - state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); - } - break; - } -} - -bool HardwareBridgeNode::waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - const std::uint32_t prev_update_count = motor_states_[index].update_count; - - while (std::chrono::steady_clock::now() < deadline) { - if (motor_states_[index].update_count > prev_update_count) { - motor_states_[index].init_confirmed = true; - return true; - } - - if (can0_fd_ >= 0) { - drainCanFrames(can0_fd_, 1, 1000); - } - if (can1_fd_ >= 0) { - drainCanFrames(can1_fd_, 2, 1000); - } - std::this_thread::sleep_for(2ms); - } - - return motor_states_[index].update_count > prev_update_count; -} - -bool HardwareBridgeNode::startMotorRecoverySequence( - std::size_t index, - const std::string & trigger, - RecoveryKind kind, - std::uint32_t attempt_number) -{ - if (dry_run_ || index >= motors_.size()) { - return false; - } - - auto & state = motor_states_[index]; - if (state.recovery_stage != RecoveryStage::Idle) { - return false; - } - - const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; - if (fd < 0) { - logMotorDiagnosticEvent(index, "motor_init_failed", - "reason=" + trigger + ", detail=invalid_can_fd", "ERROR"); - return false; - } - - bool step_ok = true; - 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", - "reason=" + trigger + ", attempt=1, sent_ok=false, feedback_confirmed=false", "ERROR"); - return false; - } - - state.recovery_kind = kind; - state.recovery_stage = RecoveryStage::AwaitInitFeedback; - state.recovery_start_update_count = state.update_count; - state.recovery_active_attempt_number = attempt_number; - state.recovery_trigger = trigger; - state.recovery_stage_deadline_ = - std::chrono::steady_clock::now() + std::chrono::milliseconds(kMotorInitConfirmTimeoutMs); - state.init_confirmed = false; - return true; -} - -void HardwareBridgeNode::clearMotorRecoverySequence(std::size_t index) -{ - auto & state = motor_states_[index]; - state.recovery_kind = RecoveryKind::None; - state.recovery_stage = RecoveryStage::Idle; - state.recovery_start_update_count = 0; - state.recovery_active_attempt_number = 0; - state.recovery_stage_deadline_ = std::chrono::steady_clock::time_point{}; - state.recovery_trigger.clear(); -} - -void HardwareBridgeNode::processMotorRecoverySequence(std::size_t index) -{ - auto & state = motor_states_[index]; - if (state.recovery_stage == RecoveryStage::Idle) { - return; - } - - const auto now_tp = std::chrono::steady_clock::now(); - if (state.recovery_stage == RecoveryStage::AwaitInitFeedback) { - if (state.update_count > state.recovery_start_update_count) { - state.init_confirmed = true; - if (state.recovery_kind == RecoveryKind::NoEffect) { - state.recovery_stage = RecoveryStage::AwaitEffectVerification; - state.recovery_stage_deadline_ = - now_tp + std::chrono::milliseconds(noEffectVerifyTimeoutMs(index)); - } else { - state.recovered_reported = true; - state.stale_reported = false; - logMotorPowerEvent(index, "recovered_after_reinit", - "trigger=" + state.recovery_trigger + ", recovery_attempt=" + - std::to_string(state.recovery_active_attempt_number)); - clearMotorRecoverySequence(index); - } - return; - } - - if (now_tp >= state.recovery_stage_deadline_) { - logMotorDiagnosticEvent(index, "motor_init_retry", - "reason=" + state.recovery_trigger + - ", attempt=" + std::to_string(state.recovery_active_attempt_number) + - ", sent_ok=true, feedback_confirmed=false", "ERROR"); - clearMotorRecoverySequence(index); - } - return; - } - - if (state.recovery_stage == RecoveryStage::AwaitEffectVerification) { - if (state.stale_count == 0 && !motorHasBlockingFault(index)) { - bool recovered = false; - if (isWheelMotor(index)) { - const float commanded = std::abs(state.last_command_sim); - const float actual = std::abs(state.velocity); - const float response_ratio = commanded > 1.0e-4f ? actual / commanded : 1.0f; - recovered = - commanded >= wheel_no_effect_command_threshold_ && - actual > wheel_no_effect_velocity_epsilon_ && - response_ratio >= wheel_no_effect_min_response_ratio_; - } else { - const float position_error = std::abs(state.last_command_sim - state.position); - recovered = position_error < (leg_no_effect_position_error_threshold_ * 0.5f); - } - if (recovered) { - state.no_effect_count = 0; - state.no_effect_reported = false; - state.command_active_count = 0; - state.no_effect_recovery_attempt_count = 0; - logMotorPowerEvent(index, "recovered_after_no_effect_reinit", - "trigger=" + state.recovery_trigger + ", recovery_attempt=" + - std::to_string(state.recovery_active_attempt_number)); - clearMotorRecoverySequence(index); - return; - } - } - - if (now_tp >= state.recovery_stage_deadline_) { - logMotorDiagnosticEvent(index, "motor_no_effect_recovery_failed", - "trigger=" + state.recovery_trigger + - ", no_effect_count=" + std::to_string(state.no_effect_count) + - ", recovery_attempt=" + std::to_string(state.recovery_active_attempt_number) + - ", " + buildNoEffectSummary(index), "ERROR"); - clearMotorRecoverySequence(index); - } - } -} - -bool HardwareBridgeNode::initializeMotor(std::size_t index, const std::string & reason, int max_attempts) -{ - if (dry_run_ || index >= motors_.size()) { - return true; - } - - auto & state = motor_states_[index]; - const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; - if (fd < 0) { - logMotorDiagnosticEvent(index, "motor_init_failed", - "reason=" + reason + ", detail=invalid_can_fd", "ERROR"); - return false; - } - - state.init_confirmed = false; - bool success = false; - for (int attempt = 1; attempt <= max_attempts; ++attempt) { - state.init_attempt_count++; - bool step_ok = true; - 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)); - } - - if (success) { - std::ostringstream oss; - oss << "reason=" << reason - << ", attempt=" << attempt - << ", init_attempt_count=" << state.init_attempt_count; - logMotorDiagnosticEvent(index, "motor_init_confirmed", oss.str(), "INFO"); - return true; - } - - std::ostringstream oss; - oss << "reason=" << reason - << ", attempt=" << attempt - << ", sent_ok=" << (step_ok ? "true" : "false") - << ", feedback_confirmed=" << (state.init_confirmed ? "true" : "false"); - logMotorDiagnosticEvent(index, "motor_init_retry", oss.str(), attempt == max_attempts ? "ERROR" : "WARN"); - std::this_thread::sleep_for(std::chrono::milliseconds(kMotorInitRetrySleepMs)); - } - - return false; -} - -bool HardwareBridgeNode::initializeMotorsOnBus(int bus_id, const std::string & reason) -{ - bool all_ok = true; - for (std::size_t i = 0; i < motors_.size(); ++i) { - if (motors_[i].bus != bus_id) { - continue; - } - const bool ok = initializeMotor(i, reason, 3); - all_ok = ok && all_ok; - } - return all_ok; -} - -void HardwareBridgeNode::processCanFrame(const struct can_frame & frame, int bus_id) -{ - if (!(frame.can_id & CAN_EFF_FLAG)) { - return; - } - - const std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F; - if (comm_type == 2) { - const std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; - const int motor_id = extra_data & 0xFF; - - for (std::size_t i = 0; i < motors_.size(); ++i) { - if (motors_[i].bus != bus_id || motors_[i].id != motor_id) { - continue; - } - - std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1]; - std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3]; - std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5]; - std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7]; - - double pos_raw = (static_cast(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI); - double vel_raw = (static_cast(v_u16) / 32767.0 - 1.0) * 44.0; - double torque_raw = (static_cast(t_u16) / 32767.0 - 1.0) * 17.0; - - float pos_sim = (static_cast(pos_raw) - motors_[i].offset) / motors_[i].direction; - float vel_sim = static_cast(vel_raw) / motors_[i].direction; - float torque_sim = static_cast(torque_raw) / motors_[i].direction; - - if (i < 12) { - pos_sim = nearest_periodic(pos_sim, rough_default_dof_pos_[i]); - } - - updateMotorTelemetry(i, pos_sim, vel_sim, torque_sim, static_cast(temp_u16) * 0.1f); - updateMotorDiagnostics(i); - return; - } - } else if (comm_type == COMM_READ_PARAMETER) { - handleParameterResponse(frame, bus_id); - } -} - -void HardwareBridgeNode::drainCanFrames(int fd, int bus_id, int timeout_us) -{ - if (fd < 0) { - return; - } - - struct can_frame frame; - while (readCanFrame(fd, &frame, timeout_us)) { - processCanFrame(frame, bus_id); - timeout_us = 0; - } -} - -bool HardwareBridgeNode::shouldAttemptMotorRecovery(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (state.recovery_stage != RecoveryStage::Idle) { - return false; - } - if (!state.has_valid_data) { - return false; - } - if (state.stale_count < kMotorRecoveryTriggerStaleCount) { - return false; - } - if (state.recovery_attempt_count >= kMotorRecoveryAttemptLimit) { - return false; - } - if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { - return false; - } - - const auto now_tp = std::chrono::steady_clock::now(); - if (state.last_recovery_attempt_time_.time_since_epoch().count() != 0) { - const auto since_last = now_tp - state.last_recovery_attempt_time_; - if (since_last < std::chrono::milliseconds(kMotorRecoveryCooldownMs)) { - return false; - } - } - return true; -} - -bool HardwareBridgeNode::attemptMotorRecovery(std::size_t index, const std::string & trigger) -{ - if (!shouldAttemptMotorRecovery(index)) { - return false; - } - - auto & state = motor_states_[index]; - const std::uint32_t attempt_number = state.recovery_attempt_count + 1; - - std::ostringstream start_oss; - start_oss << "trigger=" << trigger - << ", stale_count=" << state.stale_count - << ", recovery_attempt=" << attempt_number; - logMotorDiagnosticEvent(index, "motor_recovery_attempt", start_oss.str(), - attempt_number >= kMotorRecoveryAttemptLimit ? "ERROR" : "WARN"); - - if (!startMotorRecoverySequence(index, trigger, RecoveryKind::Stale, attempt_number)) { - return false; - } - state.recovery_attempt_count = attempt_number; - state.last_recovery_attempt_time_ = std::chrono::steady_clock::now(); - return true; -} - -bool HardwareBridgeNode::shouldAttemptNoEffectRecovery(std::size_t index) const -{ - const auto & state = motor_states_[index]; - if (state.recovery_stage != RecoveryStage::Idle) { - return false; - } - if (!state.has_valid_data || state.stale_count > 0) { - return false; - } - if (!hasFreshNoEffectDiagnostics(index)) { - return false; - } - if (state.no_effect_count < noEffectTriggerCycles(index)) { - return false; - } - if (motorHasBlockingFault(index)) { - return false; - } - if (state.no_effect_recovery_attempt_count >= noEffectAttemptLimit(index)) { - return false; - } - - const auto now_tp = std::chrono::steady_clock::now(); - if (state.last_no_effect_recovery_attempt_time_.time_since_epoch().count() != 0) { - const auto since_last = now_tp - state.last_no_effect_recovery_attempt_time_; - if (since_last < std::chrono::milliseconds(noEffectCooldownMs(index))) { - return false; - } - } - return true; -} - -bool HardwareBridgeNode::attemptNoEffectRecovery(std::size_t index, const std::string & trigger) -{ - if (!shouldAttemptNoEffectRecovery(index)) { - return false; - } - - auto & state = motor_states_[index]; - const std::uint32_t attempt_number = state.no_effect_recovery_attempt_count + 1; - - std::ostringstream start_oss; - start_oss << "trigger=" << trigger - << ", no_effect_count=" << state.no_effect_count - << ", recovery_attempt=" << attempt_number - << ", " << buildNoEffectSummary(index); - logMotorDiagnosticEvent(index, "motor_no_effect_recovery_attempt", start_oss.str(), - attempt_number >= noEffectAttemptLimit(index) ? "ERROR" : "WARN"); - - if (!startMotorRecoverySequence(index, trigger, RecoveryKind::NoEffect, attempt_number)) { - return false; - } - state.no_effect_recovery_attempt_count = attempt_number; - state.last_no_effect_recovery_attempt_time_ = std::chrono::steady_clock::now(); - return true; -} - -void HardwareBridgeNode::updateMotorDiagnostics(std::size_t index) -{ - auto & state = motor_states_[index]; - - if (state.temperature >= motor_temp_warn_c_) { - if (!state.high_temp_reported) { - state.high_temp_reported = true; - motor_fault_event_count_++; - std::ostringstream oss; - oss << "temperature_c=" << state.temperature - << ", trigger=temperature_high" - << ", protection_hint=" << (state.temperature >= motor_temp_fault_c_ ? "motor_overtemperature_fault" : "thermal_warning") - << ", estimated_current_arms=" << state.estimated_current_arms; - logMotorDiagnosticEvent(index, "motor_temperature_alert", oss.str(), - state.temperature >= motor_temp_fault_c_ ? "ERROR" : "WARN"); - } - } else { - state.high_temp_reported = false; - } - - if (state.estimated_current_arms >= motor_current_warn_arms_ || std::abs(state.torque) >= motor_torque_warn_nm_) { - if (!state.high_current_reported) { - state.high_current_reported = true; - motor_fault_event_count_++; - std::ostringstream oss; - oss << "estimated_current_arms=" << state.estimated_current_arms - << ", torque_nm=" << state.torque - << ", trigger=" << (std::abs(state.torque) >= motor_torque_peak_nm_ || state.estimated_current_arms >= motor_current_peak_arms_ - ? "overload_peak" : "overload_warning") - << ", protection_hint=stall_or_overload_protection"; - logMotorDiagnosticEvent(index, "motor_overload_alert", oss.str(), - (std::abs(state.torque) >= motor_torque_peak_nm_ || state.estimated_current_arms >= motor_current_peak_arms_) ? "ERROR" : "WARN"); - } - } else { - state.high_current_reported = false; - } - - if (state.has_bus_voltage) { - if (state.bus_voltage >= motor_bus_overvoltage_v_) { - if (!state.high_voltage_reported) { - state.high_voltage_reported = true; - motor_fault_event_count_++; - logMotorDiagnosticEvent(index, "motor_bus_voltage_alert", - "bus_voltage_v=" + std::to_string(state.bus_voltage) + - ", trigger=bus_overvoltage, protection_hint=overvoltage_fault", "ERROR"); - } - } else { - state.high_voltage_reported = false; - } - - if (state.bus_voltage <= motor_bus_undervoltage_v_) { - if (!state.low_voltage_reported) { - state.low_voltage_reported = true; - motor_fault_event_count_++; - logMotorDiagnosticEvent(index, "motor_bus_voltage_alert", - "bus_voltage_v=" + std::to_string(state.bus_voltage) + - ", trigger=bus_undervoltage, protection_hint=undervoltage_fault", "ERROR"); - } - } else { - state.low_voltage_reported = false; - } - } - - if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { - if (!state.fault_code_reported) { - state.fault_code_reported = true; - motor_fault_event_count_++; - logMotorDiagnosticEvent(index, "motor_fault_code", buildMotorFaultSummary(index), "ERROR"); - } - } else { - state.fault_code_reported = false; - } -} - -void HardwareBridgeNode::pollMotorDiagnostics() -{ - if (dry_run_ || diag_poll_period_s_ <= 0.0) { - return; - } - - const auto now_time = now(); - if (last_diag_poll_time_.nanoseconds() > 0 && - (now_time - last_diag_poll_time_).seconds() < diag_poll_period_s_) { - return; - } - last_diag_poll_time_ = now_time; - - const std::size_t index = diag_poll_motor_index_ % motors_.size(); - const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; - readParameter(fd, motors_[index].id, PARAM_VBUS); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_1); - readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_2); - diag_poll_motor_index_ = (diag_poll_motor_index_ + 1) % motors_.size(); } void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg) @@ -1347,43 +180,6 @@ void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget: target_sequence_ = msg->sequence; } -void HardwareBridgeNode::onModelStatus(const std_msgs::msg::String::SharedPtr msg) -{ - const std::string current_model = toLowerCopy(extractJsonStringField(msg->data, "current_model")); - const std::string switch_state = toLowerCopy(extractJsonStringField(msg->data, "switch_state")); - - std::scoped_lock lock(target_mutex_); - model_switch_active_ = !switch_state.empty() && switch_state != "idle"; - if (current_model.empty()) { - return; - } - ActiveModelMode next_mode = ActiveModelMode::Rough; - if (current_model == "crawl" || current_model == "ik") { - next_mode = ActiveModelMode::Crawl; - } else if (current_model == "wall") { - next_mode = ActiveModelMode::Wall; - } - if (next_mode == active_model_mode_) { - return; - } - - active_model_mode_ = next_mode; - const char * mode_name = "rough"; - if (active_model_mode_ == ActiveModelMode::Crawl) { - active_default_dof_pos_ = crawl_default_dof_pos_; - mode_name = "ik/crawl"; - } else if (active_model_mode_ == ActiveModelMode::Wall) { - active_default_dof_pos_ = wall_default_dof_pos_; - mode_name = "wall"; - } else { - active_default_dof_pos_ = rough_default_dof_pos_; - } - RCLCPP_INFO( - get_logger(), - "Hardware active model reference updated to %s pose.", - mode_name); -} - void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg) { std::scoped_lock lock(imu_mutex_); @@ -1398,8 +194,7 @@ void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg) imu_gyro_ = {gyro_x, gyro_y, gyro_z}; imu_accel_ = {accel_x, accel_y, accel_z}; - if (!mahony_initialized_.load(std::memory_order_relaxed) && - imu_gravity_sample_count_ < kImuGravityAlignSamples) { + if (!mahony_initialized_ && imu_gravity_sample_count_ < kImuGravityAlignSamples) { imu_gravity_sum_[0] += accel_x; imu_gravity_sum_[1] += accel_y; imu_gravity_sum_[2] += accel_z; @@ -1417,24 +212,11 @@ void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg) void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg) { std::scoped_lock lock(target_mutex_); - const bool was_estop = estop_triggered_; estop_triggered_ = msg->data; if (estop_triggered_) { - RCLCPP_WARN(get_logger(), "!!! E-stop received over /safety/estop !!!"); - logEvent("WARN", "estop_triggered", "E-stop received over /safety/estop."); + RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!"); } else { - RCLCPP_INFO(get_logger(), "E-stop reset."); - logEvent("INFO", "estop_reset", "E-stop reset via /safety/estop."); - const bool user_estop_latch = - safety_reason_.find("user E-stop") != std::string::npos || - (was_estop && safety_reason_.empty()); - if (safety_triggered_ && user_estop_latch) { - safety_triggered_ = false; - safety_reason_.clear(); - clip_active_logged_ = false; - RCLCPP_INFO(get_logger(), "Cleared user E-stop safety latch."); - logEvent("INFO", "safety_latch_reset", "Cleared user E-stop safety latch."); - } + RCLCPP_INFO(get_logger(), "Physical E-stop reset."); } } @@ -1471,8 +253,96 @@ void HardwareBridgeNode::onReadLoop() motor_states_[i].stale_count++; } - drainCanFrames(can0_fd_, 1, 50); - drainCanFrames(can1_fd_, 2, 50); + struct can_frame frame; + // Process can0 (bus 1) + while (readCanFrame(can0_fd_, &frame, 50)) { + if (!(frame.can_id & CAN_EFF_FLAG)) continue; + std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F; + if (comm_type == 2) { // Status Frame + std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; + int motor_id = extra_data & 0xFF; + + for (std::size_t i = 0; i < 16; ++i) { + if (motors_[i].bus == 1 && motors_[i].id == motor_id) { + std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1]; + std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3]; + std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5]; + std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7]; + + double pos_raw = (static_cast(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI); + double vel_raw = (static_cast(v_u16) / 32767.0 - 1.0) * 44.0; + double torque_raw = (static_cast(t_u16) / 32767.0 - 1.0) * 17.0; + + // Apply motor mapping: real_to_sim + // real = sign * sim + offset -> sim = (real - offset) / sign + float pos_sim = (static_cast(pos_raw) - motors_[i].offset) / motors_[i].direction; + float vel_sim = static_cast(vel_raw) / motors_[i].direction; + float torque_sim = static_cast(torque_raw) / motors_[i].direction; + + if (i < 12) { + pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]); + } + + motor_states_[i].position = pos_sim; + motor_states_[i].velocity = vel_sim; + motor_states_[i].torque = torque_sim; + motor_states_[i].temperature = static_cast(temp_u16) * 0.1f; + motor_states_[i].update_count++; + motor_states_[i].stale_count = 0; + // Update hold-over valid data + motor_states_[i].last_valid_pos = pos_sim; + motor_states_[i].last_valid_vel = vel_sim; + motor_states_[i].last_valid_torque = torque_sim; + motor_states_[i].has_valid_data = true; + break; + } + } + } + } + + // Process can1 (bus 2) + while (readCanFrame(can1_fd_, &frame, 50)) { + if (!(frame.can_id & CAN_EFF_FLAG)) continue; + std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F; + if (comm_type == 2) { + std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; + int motor_id = extra_data & 0xFF; + + for (std::size_t i = 0; i < 16; ++i) { + if (motors_[i].bus == 2 && motors_[i].id == motor_id) { + std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1]; + std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3]; + std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5]; + std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7]; + + double pos_raw = (static_cast(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI); + double vel_raw = (static_cast(v_u16) / 32767.0 - 1.0) * 44.0; + double torque_raw = (static_cast(t_u16) / 32767.0 - 1.0) * 17.0; + + float pos_sim = (static_cast(pos_raw) - motors_[i].offset) / motors_[i].direction; + float vel_sim = static_cast(vel_raw) / motors_[i].direction; + float torque_sim = static_cast(torque_raw) / motors_[i].direction; + + if (i < 12) { + pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]); + } + + motor_states_[i].position = pos_sim; + motor_states_[i].velocity = vel_sim; + motor_states_[i].torque = torque_sim; + motor_states_[i].temperature = static_cast(temp_u16) * 0.1f; + motor_states_[i].update_count++; + motor_states_[i].stale_count = 0; + // Update hold-over valid data + motor_states_[i].last_valid_pos = pos_sim; + motor_states_[i].last_valid_vel = vel_sim; + motor_states_[i].last_valid_torque = torque_sim; + motor_states_[i].has_valid_data = true; + break; + } + } + } + } // Hold-over: apply last valid data for stale motors for (std::size_t i = 0; i < 16; ++i) { @@ -1481,41 +351,10 @@ void HardwareBridgeNode::onReadLoop() motor_states_[i].velocity = motor_states_[i].last_valid_vel; motor_states_[i].torque = motor_states_[i].last_valid_torque; holdover_events_total_++; - if (!motor_states_[i].stale_reported && - motor_states_[i].stale_count >= kMotorDropReportThreshold) { - motor_states_[i].stale_reported = true; - motor_states_[i].recovered_reported = false; - motor_states_[i].disable_reported = true; - motor_states_[i].last_power_event_reason = - "no status frame received, stale_count=" + std::to_string(motor_states_[i].stale_count); - motor_drop_event_count_++; - logMotorPowerEvent(i, "dropped_or_unresponsive", motor_states_[i].last_power_event_reason); - } - if (shouldAttemptMotorRecovery(i)) { - attemptMotorRecovery(i, "stale_feedback"); - } - } else if (motor_states_[i].stale_count == 0 && motor_states_[i].stale_reported) { - motor_states_[i].stale_reported = false; - if (!motor_states_[i].recovered_reported) { - motor_states_[i].recovered_reported = true; - motor_recover_event_count_++; - logMotorPowerEvent(i, "recovered", "status frame reception resumed"); - } - } - - processMotorRecoverySequence(i); - - if (motor_states_[i].stale_count == 0) { - updateNoEffectDetection(i); - if (shouldAttemptNoEffectRecovery(i)) { - attemptNoEffectRecovery(i, "wheel_no_effect"); - } } } } - pollMotorDiagnostics(); - // 2. Fetch IMU data & update MahonyFilter auto now_time = now(); double dt = 0.005; @@ -1548,9 +387,8 @@ void HardwareBridgeNode::onReadLoop() } std::array quat{1.0f, 0.0f, 0.0f, 0.0f}; - const bool mahony_initialized = mahony_initialized_.load(std::memory_order_relaxed); - if (imu_fresh || mahony_initialized) { - if (!mahony_initialized) { + if (imu_fresh || mahony_initialized_) { + if (!mahony_initialized_) { std::array gravity_init = accel; { std::scoped_lock lock(imu_mutex_); @@ -1563,7 +401,7 @@ void HardwareBridgeNode::onReadLoop() } } mahony_filter_->reset_with_accel(gravity_init); - mahony_initialized_.store(true, std::memory_order_relaxed); + mahony_initialized_ = true; } quat = mahony_filter_->update(accel, gyro, static_cast(dt)); } @@ -1592,8 +430,6 @@ void HardwareBridgeNode::onReadLoop() safety_triggered_ = true; safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason; RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str()); - logEvent("ERROR", "safety_triggered", safety_reason_); - logProtectionEvent("runtime_guard_stop", guard_decision.reason, "safety_brake"); } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str()); } @@ -1608,13 +444,6 @@ void HardwareBridgeNode::onReadLoop() fresh_count_ = 0; holdover_count_ = 0; stale_max_ = 0; - std::array dry_run_target_snapshot{}; - std::uint32_t dry_run_target_sequence = 0; - if (dry_run_) { - std::scoped_lock lock(target_mutex_); - dry_run_target_snapshot = latest_target_; - dry_run_target_sequence = target_sequence_; - } for (std::size_t i = 0; i < 16; ++i) { // Stale frames detection & holdover count @@ -1629,10 +458,10 @@ void HardwareBridgeNode::onReadLoop() if (dry_run_) { // Mock motor positions tracking target - msg.joint_pos[i] = dry_run_target_snapshot[i]; + msg.joint_pos[i] = latest_target_[i]; msg.joint_vel[i] = 0.0f; msg.joint_torque[i] = 0.0f; - msg.update_counts[i] = dry_run_target_sequence; + msg.update_counts[i] = target_sequence_; } else { msg.joint_pos[i] = motor_states_[i].position; msg.joint_vel[i] = motor_states_[i].velocity; @@ -1689,43 +518,24 @@ void HardwareBridgeNode::onWriteLoop() { const auto now_time = now(); std::array target{}; - std::array active_default_pose = rough_default_dof_pos_; std::string target_source; - bool model_switch_active = false; - rclcpp::Time latest_target_stamp{0, 0, RCL_ROS_TIME}; double age_ms = 0.0; { std::scoped_lock lock(target_mutex_); target = latest_target_; target_source = latest_target_source_; - active_default_pose = active_default_dof_pos_; - model_switch_active = model_switch_active_; - latest_target_stamp = latest_target_stamp_; - if (latest_target_stamp.nanoseconds() > 0) { - const auto age_ns = (now_time - latest_target_stamp).nanoseconds(); + if (latest_target_stamp_.nanoseconds() > 0) { + const auto age_ns = (now_time - latest_target_stamp_).nanoseconds(); age_ms = age_ns > 0 ? static_cast(age_ns) / 1.0e6 : 0.0; } } - if (target_source == "model_switch_to_model_pose") { - active_default_pose = rough_default_dof_pos_; - } - const bool target_is_model_switch = - target_source.rfind("model_switch_", 0) == 0; - - // Timeout guard: fall back to the active model's reference pose if target is stale - if (latest_target_stamp.nanoseconds() == 0 || age_ms > target_timeout_ms_) { - target = active_default_pose; + // Timeout guard: default stand pose if target is stale + if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) { + target = sim2real_common::DeploymentContract::kDefaultDofPos; target_source = "timeout_hold"; - if (!timeout_hold_logged_) { - timeout_hold_logged_ = true; - logEvent("WARN", "target_timeout_hold", - "runtime target stale, switched to timeout_hold, age_ms=" + std::to_string(age_ms)); - } - } else { - timeout_hold_logged_ = false; } // Run SafetyMonitor check on incoming target commands @@ -1743,28 +553,15 @@ void HardwareBridgeNode::onWriteLoop() } if (safety_enabled_ && !safety_triggered_) { - auto * active_safety_monitor = - (model_switch_active || target_is_model_switch) && model_switch_safety_monitor_ ? - model_switch_safety_monitor_.get() : - safety_monitor_.get(); - auto safety_decision = active_safety_monitor->check(target, active_default_pose, gyro, proj_grav, estop_active); + auto safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active); if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { safety_triggered_ = true; safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str()); - logEvent("ERROR", "safety_triggered", safety_reason_); - logProtectionEvent("safety_monitor_stop", safety_decision.message, "safety_brake"); } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { target = safety_decision.clipped_target; target_source = "safety_clip"; RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped."); - if (!clip_active_logged_) { - clip_active_logged_ = true; - logEvent("WARN", "target_clipped", - "protection=safety_clip, reason=" + safety_decision.message + ", source=" + target_source); - } - } else { - clip_active_logged_ = false; } } @@ -1815,26 +612,15 @@ void HardwareBridgeNode::onWriteLoop() } double elapsed = (now_time - startup_soft_hold_start_time_).seconds(); double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp - kp_val = sim2real_common::DeploymentContract::kLegKp * kp_scale; - kd_val = sim2real_common::DeploymentContract::kLegKd; + kp_val = sim2real_common::DeploymentContract::kLegHoldKp * kp_scale; + kd_val = sim2real_common::DeploymentContract::kLegHoldKd; } else { startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - if ( - target_source == "timeout_hold" || - target_source == "boot_hold" || - target_source == "runtime_zero_hold" || - target_source == "runtime_keep_pose" || - target_source == "runtime_keep_return_default" || - target_source == "startup_hold" || - target_source == "model_switch_to_stand" || - target_source == "model_switch_stand_hold" || - target_source == "model_switch_to_model_pose") - { - kp_val = sim2real_common::DeploymentContract::kLegKp; - kd_val = sim2real_common::DeploymentContract::kLegKd; + if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") { + kp_val = sim2real_common::DeploymentContract::kLegHoldKp; + kd_val = sim2real_common::DeploymentContract::kLegHoldKd; } } - updateMotorCommandTracking(i, sim_val, target_source); writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0); } else { // Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0) @@ -1844,7 +630,6 @@ void HardwareBridgeNode::onWriteLoop() vel_real = 0.0; kd_val = 2.0; // Wheel damping Kd } - updateMotorCommandTracking(i, sim_val, target_source); writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0); } } @@ -1854,30 +639,24 @@ void HardwareBridgeNode::onWriteLoop() bool HardwareBridgeNode::enableMotor(int fd, int motor_id) { std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id; - std::uint8_t data[8] = {0}; - return sendCanFrame(fd, ext_id, data, 8); + return sendCanFrame(fd, ext_id, nullptr, 0); } -bool HardwareBridgeNode::disableMotor(int fd, int motor_id, bool clear_fault) +bool HardwareBridgeNode::disableMotor(int fd, int motor_id) { std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id; std::uint8_t data[8] = {0}; - data[0] = clear_fault ? 1 : 0; - return sendCanFrame(fd, ext_id, data, 8); -} - -bool HardwareBridgeNode::readParameter(int fd, int motor_id, std::uint16_t param_id) -{ - std::uint32_t ext_id = (COMM_READ_PARAMETER << 24) | (HOST_ID << 8) | motor_id; - std::uint8_t data[8] = {0}; - data[0] = param_id & 0xFF; - data[1] = (param_id >> 8) & 0xFF; return sendCanFrame(fd, ext_id, data, 8); } bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode) { - return writeParameterInt(fd, motor_id, PARAM_MODE, static_cast(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(mode); + return sendCanFrame(fd, ext_id, data, 8); } bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit) @@ -1890,16 +669,6 @@ 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; @@ -1996,7 +765,6 @@ bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::u RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake."); safety_triggered_ = true; safety_reason_ = "CAN0 bus failure - reinit failed"; - logEvent("FATAL", "safety_triggered", safety_reason_); } } } else if (fd == can1_fd_) { @@ -2008,7 +776,6 @@ bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::u RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake."); safety_triggered_ = true; safety_reason_ = "CAN1 bus failure - reinit failed"; - logEvent("FATAL", "safety_triggered", safety_reason_); } } } @@ -2062,7 +829,6 @@ bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us) bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count) { RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str()); - logEvent("WARN", "can_reinit_attempt", "interface=" + ifname + ", consecutive_errors=" + std::to_string(error_count)); if (fd >= 0) { ::close(fd); fd = -1; @@ -2070,15 +836,7 @@ bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& erro bool success = initCan(ifname, fd); if (success) { error_count = 0; - const int recovered_bus = (&fd == &can0_fd_) ? 1 : 2; - if (!initializeMotorsOnBus(recovered_bus, "can_reinit")) { - logEvent("ERROR", "motor_reinit_after_can_reinit_failed", - "interface=" + ifname + ", bus=" + std::to_string(recovered_bus)); - } RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str()); - logEvent("INFO", "can_reinit_success", "interface=" + ifname); - } else { - logEvent("ERROR", "can_reinit_failed", "interface=" + ifname); } return success; } @@ -2088,10 +846,7 @@ bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& erro int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 6); - executor.add_node(node); - executor.spin(); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/05_software/real/sim2real_ros2/src/sim2real_nav2/config/nav2_params.yaml b/05_software/real/sim2real_ros2/src/sim2real_nav2/config/nav2_params.yaml index 52c755b..f847f1e 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_nav2/config/nav2_params.yaml +++ b/05_software/real/sim2real_ros2/src/sim2real_nav2/config/nav2_params.yaml @@ -133,7 +133,7 @@ planner_server: GridTransition: plugin: "nav2_navfn_planner/NavfnPlanner" tolerance: 0.5 - use_astar: true + use_astar: false allow_unknown: true behavior_server: diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt index 152afd2..58decba 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt @@ -2,7 +2,6 @@ cmake_minimum_required(VERSION 3.8) project(sim2real_runtime) find_package(ament_cmake REQUIRED) -find_package(CUDAToolkit QUIET) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) @@ -32,33 +31,10 @@ find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY) -find_path(TENSORRT_INCLUDE_DIR NvInfer.h - PATHS - /usr/include - /usr/include/aarch64-linux-gnu - /usr/local/include -) - -find_library(TENSORRT_LIBRARY NAMES nvinfer - PATHS - /usr/lib - /usr/lib/aarch64-linux-gnu - /usr/lib/x86_64-linux-gnu - /usr/local/lib -) - if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY) message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.") endif() -set(SIM2REAL_RUNTIME_HAS_TENSORRT FALSE) -if(TENSORRT_INCLUDE_DIR AND TENSORRT_LIBRARY AND CUDAToolkit_FOUND) - set(SIM2REAL_RUNTIME_HAS_TENSORRT TRUE) - message(STATUS "TensorRT support enabled for sim2real_runtime") -else() - message(STATUS "TensorRT support disabled for sim2real_runtime (missing TensorRT or CUDA toolkit)") -endif() - add_executable(sim2real_runtime_node src/policy_runtime_node.cpp ) @@ -67,60 +43,23 @@ add_executable(odom_relay_node src/odom_relay_node.cpp ) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - add_executable(compare_onnx_trt - src/compare_onnx_trt.cpp - ) -endif() - target_include_directories(sim2real_runtime_node PRIVATE include ${ONNXRUNTIME_INCLUDE_DIR} ) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - target_include_directories(sim2real_runtime_node PRIVATE - ${TENSORRT_INCLUDE_DIR} - ) - target_include_directories(compare_onnx_trt PRIVATE - ${TENSORRT_INCLUDE_DIR} - ${ONNXRUNTIME_INCLUDE_DIR} - ) -endif() - target_include_directories(odom_relay_node PRIVATE include) target_link_libraries(sim2real_runtime_node ${ONNXRUNTIME_LIBRARY} ) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - target_link_libraries(sim2real_runtime_node - ${TENSORRT_LIBRARY} - CUDA::cudart - ) - target_link_libraries(compare_onnx_trt - ${ONNXRUNTIME_LIBRARY} - ${TENSORRT_LIBRARY} - CUDA::cudart - ) - target_compile_definitions(sim2real_runtime_node PRIVATE - SIM2REAL_RUNTIME_HAS_TENSORRT=1 - ) - target_compile_definitions(compare_onnx_trt PRIVATE - SIM2REAL_RUNTIME_HAS_TENSORRT=1 - ) -endif() - set_target_properties(sim2real_runtime_node PROPERTIES BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" ) target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - target_compile_features(compare_onnx_trt PRIVATE cxx_std_17) -endif() ament_target_dependencies(sim2real_runtime_node geometry_msgs @@ -132,12 +71,6 @@ ament_target_dependencies(sim2real_runtime_node sim2real_interfaces ) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - ament_target_dependencies(compare_onnx_trt - sim2real_common - ) -endif() - ament_target_dependencies(odom_relay_node geometry_msgs nav_msgs @@ -155,26 +88,11 @@ install( DESTINATION lib/${PROJECT_NAME} ) -if(SIM2REAL_RUNTIME_HAS_TENSORRT) - install( - TARGETS compare_onnx_trt - DESTINATION lib/${PROJECT_NAME} - ) -endif() - install( PROGRAMS src/remote_uart_node.py src/cmd_mux_node.py src/web_udp_bridge_node.py - src/simple_nav_node.py - src/pcd_nav_click_tool.py - DESTINATION lib/${PROJECT_NAME} -) - -install( - FILES - src/deadzone_velocity_limiter.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp b/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp index 4c689cb..ee9496b 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp @@ -5,18 +5,15 @@ #include #include #include -#include #include #include #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/string.hpp" #include "rclcpp/rclcpp.hpp" #include "sim2real_interfaces/msg/runtime_state.hpp" #include "sim2real_interfaces/msg/runtime_target.hpp" -#include "sim2real_common/event_logger.hpp" #include "sim2real_common/stand_balance_controller.hpp" #include "sim2real_common/safety_monitor.hpp" #include "sim2real_common/runtime_guard.hpp" @@ -24,11 +21,6 @@ // ONNXRuntime C++ API #include -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT -#include -#include -#endif - namespace sim2real_runtime { @@ -36,86 +28,13 @@ class PolicyRuntimeNode : public rclcpp::Node { public: PolicyRuntimeNode(); - ~PolicyRuntimeNode() override; private: - enum class InferenceBackend { - None, - TensorRT, - OnnxRuntime, - }; - - enum class ModelMode { - Rough, - Crawl, - Wall, - }; - - enum class CrawlBackend { - Ik, - Rl, - }; - - enum class ModelSwitchState { - Idle, - ToStand, - StandHold, - ToModelPose, - }; - - enum class StartupState { - BOOT_HOLD, - STARTUP_SOFT_HOLD, - STARTUP_TRANSITION, - STARTUP_HOLD_AFTER, - RUNTIME - }; - - enum class PostureHoldMode { - None, - Keep, - ReturnDefault, - }; - void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg); void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg); void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg); - void onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg); - void onPostureCmd(const std_msgs::msg::String::SharedPtr msg); void applyCmdVel(float vx, float vy, float vyaw); void onPolicyLoop(); - bool initInferenceBackend(); - bool initTensorRt(); - bool initOnnxRuntime(); - void shutdownOnnxRuntime(); - void shutdownTensorRt(); - std::string deriveTensorRtEnginePath(const std::string & onnx_model_path) const; - const std::array & defaultPoseForMode(ModelMode mode) const; - const std::string & modelPathForMode(ModelMode mode) const; - const std::string & modelEnginePathForMode(ModelMode mode) const; - bool switchInferenceModel(ModelMode target_mode); - void publishModelStatus(); - bool modeUsesInference(ModelMode mode) const; - std::array computeHoldTarget( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & cmd); - std::array computeIkCrawlTarget( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & cmd); - float computeCrawlIkCommandScale( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & leg_target) const; - float projectedGravityTiltRad(const std::array & projected_gravity) const; - const char * startupStateName(StartupState state) const; - const char * modelModeName(ModelMode mode) const; - const char * modelSwitchStateName(ModelSwitchState state) const; - const char * postureHoldModeName(PostureHoldMode mode) const; - const char * inferenceBackendName() const; - const char * crawlBackendName() const; - void initializeDebugTrace(); - void appendDebugTrace( - const sim2real_interfaces::msg::RuntimeState & state, - const sim2real_interfaces::msg::RuntimeTarget & target); std::array buildObservation( const sim2real_interfaces::msg::RuntimeState & state, @@ -125,19 +44,11 @@ private: std::array runPolicy(const std::array & obs); bool isZeroCommand(const std::array & cmd, const std::array & imu_gyro) const; bool isCommandActive(const std::array & cmd) const; - void startPostureTransition( - PostureHoldMode mode, - const std::array & start_pose, - const std::array & target_pose, - const rclcpp::Time & now_time); rclcpp::Publisher::SharedPtr target_pub_; - rclcpp::Publisher::SharedPtr model_status_pub_; rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr cmd_sub_; rclcpp::Subscription::SharedPtr cmd_stamped_sub_; - rclcpp::Subscription::SharedPtr model_switch_sub_; - rclcpp::Subscription::SharedPtr posture_cmd_sub_; rclcpp::TimerBase::SharedPtr policy_timer_; std::mutex mutex_; @@ -150,6 +61,13 @@ private: std::uint32_t sequence_{0}; // Startup State Machine + enum class StartupState { + BOOT_HOLD, + STARTUP_SOFT_HOLD, + STARTUP_TRANSITION, + STARTUP_HOLD_AFTER, + RUNTIME + }; StartupState startup_state_{StartupState::BOOT_HOLD}; std::array start_pose_{}; std::array startup_delta_{}; @@ -158,65 +76,9 @@ private: double hold_time_{1.0}; std::unique_ptr stand_balance_; - ModelMode current_model_mode_{ModelMode::Rough}; - ModelMode requested_model_mode_{ModelMode::Rough}; - ModelMode loaded_model_mode_{ModelMode::Rough}; - CrawlBackend crawl_backend_{CrawlBackend::Ik}; - ModelSwitchState model_switch_state_{ModelSwitchState::Idle}; - bool model_switch_requested_{false}; - bool hold_active_model_pose_when_unreleased_{false}; - std::array rough_default_dof_pos_{}; - std::array crawl_default_dof_pos_{}; - std::array wall_default_dof_pos_{}; - std::array active_default_dof_pos_{}; - std::array safety_reference_dof_pos_{}; - std::array keep_pose_dof_pos_{}; - std::array posture_start_pose_{}; - std::array posture_target_pose_{}; - std::array posture_delta_{}; - PostureHoldMode posture_hold_mode_{PostureHoldMode::None}; - bool posture_transition_active_{false}; - rclcpp::Time posture_transition_start_time_{0, 0, RCL_ROS_TIME}; - double posture_transition_s_{0.8}; - std::array switch_start_pose_{}; - std::array switch_delta_{}; - rclcpp::Time model_switch_state_start_time_{0, 0, RCL_ROS_TIME}; - double model_switch_transition_s_{1.2}; - double model_switch_to_stand_transition_scale_{1.35}; - double model_switch_to_model_transition_scale_{1.55}; - double model_switch_min_transition_s_{0.35}; - double model_switch_stand_hold_s_{0.45}; - double model_switch_stand_max_err_{0.18}; - double model_switch_stand_max_vel_{0.8}; - double active_switch_transition_s_{1.2}; - - std::string rough_model_path_{"policies/model_rough.onnx"}; - std::string rough_model_engine_path_{""}; - std::string crawl_model_path_{"policies/model_crawl.onnx"}; - std::string crawl_model_engine_path_{""}; - std::string wall_model_path_{"policies/model_wall.onnx"}; - std::string wall_model_engine_path_{""}; - float crawl_ik_wheel_linear_gain_{6.25f}; - float crawl_ik_wheel_yaw_gain_{4.0f}; - float crawl_ik_max_wheel_speed_{6.0f}; - float crawl_ik_abduction_clip_{0.45f}; - float crawl_ik_yaw_rate_kp_{0.0f}; - bool crawl_ik_imu_posture_{false}; - float crawl_ik_encoder_posture_kp_{0.0f}; - float crawl_ik_encoder_posture_max_{0.03f}; - bool crawl_ik_encoder_guard_{true}; - float crawl_ik_encoder_guard_start_{0.28f}; - float crawl_ik_encoder_guard_stop_{0.65f}; - bool crawl_ik_imu_guard_{true}; - float crawl_ik_imu_guard_start_rad_{0.20943952f}; - float crawl_ik_imu_guard_stop_rad_{0.48869219f}; - // ONNX Runtime members std::string model_path_{"policies/model_rough.onnx"}; - std::string model_engine_path_{""}; - bool prefer_tensorrt_{true}; bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano - InferenceBackend inference_backend_{InferenceBackend::None}; std::unique_ptr env_; std::unique_ptr session_; std::unique_ptr memory_info_; @@ -229,35 +91,19 @@ private: std::vector input_shape_; std::vector output_shape_; -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT - nvinfer1::IRuntime * trt_runtime_{nullptr}; - nvinfer1::ICudaEngine * trt_engine_{nullptr}; - nvinfer1::IExecutionContext * trt_context_{nullptr}; - cudaStream_t trt_stream_{nullptr}; - void * trt_input_buffer_{nullptr}; - void * trt_output_buffer_{nullptr}; - std::string trt_input_name_; - std::string trt_output_name_; -#endif - // Command filter and release states std::array filtered_cmd_{{0.0f, 0.0f, 0.0f}}; - float runtime_max_vx_{1.0f}; - float runtime_max_vy_{0.3f}; - float runtime_max_yaw_rate_{1.0f}; float release_alpha_{0.0f}; float command_release_s_{0.35f}; float release_command_hold_s_{0.12f}; float release_posture_max_err_{0.35f}; float release_target_blend_s_{0.30f}; - float model_switch_release_scale_{1.3f}; float clip_obs_{100.0f}; bool hold_zero_command_pose_{true}; bool enable_zero_cmd_suppression_{true}; bool require_active_command_to_release_{true}; bool zero_cmd_use_yaw_rate_{false}; bool runtime_released_{false}; - bool slow_release_after_model_switch_{false}; float release_active_time_{0.0f}; float zero_cmd_lin_thresh_{0.05f}; float zero_cmd_yaw_thresh_{0.05f}; @@ -269,29 +115,10 @@ private: std::atomic safety_enabled_{true}; std::atomic safety_triggered_{false}; std::string safety_reason_{""}; - sim2real_common::EventLogger event_logger_; - std::string run_log_dir_; - std::string debug_trace_path_; - bool debug_trace_enabled_{true}; - std::uint32_t debug_trace_decimation_{1}; - std::uint32_t debug_trace_counter_{0}; - std::uint32_t protection_trigger_count_{0}; - std::uint32_t target_clip_count_{0}; - bool clip_active_logged_{false}; std::unique_ptr safety_monitor_; - std::unique_ptr model_switch_safety_monitor_; std::unique_ptr runtime_guard_; void onEstop(const std_msgs::msg::Bool::SharedPtr msg); - void logEvent( - const std::string & level, - const std::string & event, - const std::string & message); - void logProtectionEvent( - const std::string & trigger, - const std::string & reason, - const std::string & action); - void finalizeRunSummary(); }; } // namespace sim2real_runtime diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/package.xml b/05_software/real/sim2real_ros2/src/sim2real_runtime/package.xml index 00ea0e8..a9aefb2 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/package.xml +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/package.xml @@ -12,13 +12,10 @@ nav_msgs rclcpp std_msgs - tf2_msgs tf2_ros sim2real_common sim2real_interfaces - python3-matplotlib python3-serial - python3-yaml rclpy diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/cmd_mux_node.py b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/cmd_mux_node.py index 2a4fd9c..cccf5fe 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/cmd_mux_node.py +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/cmd_mux_node.py @@ -10,15 +10,12 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node from std_msgs.msg import Bool, String -from deadzone_velocity_limiter import limit_deadzone_axis - class ControlMode(str, Enum): DISABLED = "DISABLED" REMOTE = "REMOTE" WEB = "WEB" NAV = "NAV" - KEEP = "KEEP" class CmdMuxNode(Node): @@ -36,37 +33,6 @@ class CmdMuxNode(Node): self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value) self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value) self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value) - self.max_vx_decel = float( - self.declare_parameter("cmd_mux_max_vx_decel", self.max_vx_acc).value - ) - self.max_vy_decel = float( - self.declare_parameter("cmd_mux_max_vy_decel", self.max_vy_acc).value - ) - self.max_yaw_decel = float( - self.declare_parameter("cmd_mux_max_yaw_decel", self.max_yaw_acc).value - ) - self.linear_deadzone_epsilon = float( - self.declare_parameter("cmd_mux_linear_deadzone_epsilon", 0.0).value - ) - self.yaw_deadzone_epsilon = float( - self.declare_parameter("cmd_mux_yaw_deadzone_epsilon", 0.0).value - ) - self.min_effective_vx = float( - self.declare_parameter("cmd_mux_min_effective_vx", 0.0).value - ) - self.min_effective_vy = float( - self.declare_parameter("cmd_mux_min_effective_vy", 0.0).value - ) - self.min_effective_yaw = float( - self.declare_parameter("cmd_mux_min_effective_yaw_rate", 0.0).value - ) - self.deadzone_sources = { - item.strip().lower() - for item in str( - self.declare_parameter("cmd_mux_deadzone_sources", "nav").value - ).split(",") - if item.strip() - } self.mode = self.parse_mode(self.default_mode) self.estop = False @@ -163,16 +129,9 @@ class CmdMuxNode(Node): elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now): target = self.latest_nav source = "nav" - elif self.mode == ControlMode.KEEP: - source = "keep" target = self.limit_twist(target) - if self.estop: - target = Twist() - self.last_output = Twist() - self.last_pub_time = now - else: - target = self.accel_limit(target, now, source in self.deadzone_sources) + target = self.accel_limit(target, now) self.cmd_pub.publish(target) self.mode_pub.publish(String(data=self.mode.value)) self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}")) @@ -190,46 +149,12 @@ class CmdMuxNode(Node): out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw) return out - def accel_limit( - self, - target: Twist, - now: rclpy.time.Time, - apply_deadzone: bool, - ) -> Twist: + def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist: dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3) - min_effective_vx = self.min_effective_vx if apply_deadzone else 0.0 - min_effective_vy = self.min_effective_vy if apply_deadzone else 0.0 - min_effective_yaw = self.min_effective_yaw if apply_deadzone else 0.0 - linear_deadzone_epsilon = self.linear_deadzone_epsilon if apply_deadzone else 0.0 - yaw_deadzone_epsilon = self.yaw_deadzone_epsilon if apply_deadzone else 0.0 out = Twist() - out.linear.x = limit_deadzone_axis( - self.last_output.linear.x, - target.linear.x, - dt, - self.max_vx_acc, - self.max_vx_decel, - min_effective_vx, - linear_deadzone_epsilon, - ) - out.linear.y = limit_deadzone_axis( - self.last_output.linear.y, - target.linear.y, - dt, - self.max_vy_acc, - self.max_vy_decel, - min_effective_vy, - linear_deadzone_epsilon, - ) - out.angular.z = limit_deadzone_axis( - self.last_output.angular.z, - target.angular.z, - dt, - self.max_yaw_acc, - self.max_yaw_decel, - min_effective_yaw, - yaw_deadzone_epsilon, - ) + out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt) + out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt) + out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt) self.last_output = out self.last_pub_time = now return out @@ -238,6 +163,16 @@ class CmdMuxNode(Node): def clamp(value: float, low: float, high: float) -> float: return max(low, min(high, float(value))) + @staticmethod + def step(current: float, target: float, max_delta: float) -> float: + delta = target - current + if delta > max_delta: + return current + max_delta + if delta < -max_delta: + return current - max_delta + return target + + def main(args: Optional[list[str]] = None) -> None: rclpy.init(args=args) node = CmdMuxNode() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp index 97a6d6c..8fa328d 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp @@ -3,13 +3,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include "sim2real_common/deployment_contract.hpp" @@ -18,236 +11,21 @@ using namespace std::chrono_literals; namespace sim2real_runtime { -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT -namespace -{ - -class TensorRtLogger final : public nvinfer1::ILogger -{ -public: - explicit TensorRtLogger(rclcpp::Logger logger) - : logger_(std::move(logger)) - { - } - - void log(Severity severity, const char * msg) noexcept override - { - if (msg == nullptr) { - return; - } - - switch (severity) { - case Severity::kINTERNAL_ERROR: - case Severity::kERROR: - RCLCPP_ERROR(logger_, "[TensorRT] %s", msg); - break; - case Severity::kWARNING: - RCLCPP_WARN(logger_, "[TensorRT] %s", msg); - break; - case Severity::kINFO: - RCLCPP_INFO(logger_, "[TensorRT] %s", msg); - break; - default: - RCLCPP_DEBUG(logger_, "[TensorRT] %s", msg); - break; - } - } - -private: - rclcpp::Logger logger_; -}; - -TensorRtLogger & getTensorRtLogger(rclcpp::Logger logger) -{ - static TensorRtLogger trt_logger(logger); - return trt_logger; -} - -template -void destroyTensorRtObject(T *& object) -{ - if (object == nullptr) { - return; - } -#if NV_TENSORRT_MAJOR >= 10 - delete object; -#else - object->destroy(); -#endif - object = nullptr; -} - -} // namespace -#endif - // Named constants for timing and command filtering constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz) constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz) constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz) -std::string toLowerCopy(std::string value) -{ - std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { - return static_cast(std::tolower(c)); - }); - return value; -} - -double rosTimeToSeconds(const builtin_interfaces::msg::Time & stamp) -{ - return static_cast(stamp.sec) + static_cast(stamp.nanosec) * 1.0e-9; -} - PolicyRuntimeNode::PolicyRuntimeNode() : Node("sim2real_runtime_node") { // 1. Declare and get parameters - const std::string event_log_dir = declare_parameter( - "event_log_dir", "logs_v2_web"); - run_log_dir_ = event_log_dir; - event_logger_.configure(event_log_dir, "sim2real_runtime_events"); model_path_ = declare_parameter("model_path", "policies/model_rough.onnx"); - model_engine_path_ = declare_parameter("model_engine_path", ""); - prefer_tensorrt_ = declare_parameter("prefer_tensorrt", true); use_cuda_ = declare_parameter("use_cuda", false); // enable CUDA EP on Orin Nano - const std::vector default_rough_pose( - sim2real_common::DeploymentContract::kDefaultDofPos.begin(), - sim2real_common::DeploymentContract::kDefaultDofPos.end()); - const std::vector configured_rough_pose = declare_parameter>( - "rough_default_dof_pos", default_rough_pose); - if (configured_rough_pose.size() == rough_default_dof_pos_.size()) { - for (std::size_t i = 0; i < rough_default_dof_pos_.size(); ++i) { - rough_default_dof_pos_[i] = static_cast(configured_rough_pose[i]); - } - } else { - rough_default_dof_pos_ = sim2real_common::DeploymentContract::kDefaultDofPos; - RCLCPP_WARN( - get_logger(), - "Parameter rough_default_dof_pos has %zu entries, expected 16. Falling back to deployment default pose.", - configured_rough_pose.size()); - } - active_default_dof_pos_ = rough_default_dof_pos_; - safety_reference_dof_pos_ = rough_default_dof_pos_; - const std::vector default_crawl_pose = declare_parameter>( - "crawl_default_dof_pos", - std::vector{ - 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 - }); - if (default_crawl_pose.size() == crawl_default_dof_pos_.size()) { - for (std::size_t i = 0; i < crawl_default_dof_pos_.size(); ++i) { - crawl_default_dof_pos_[i] = static_cast(default_crawl_pose[i]); - } - } else { - crawl_default_dof_pos_ = { - 0.2f, 1.697f, -2.650f, - -0.2f, 1.697f, -2.650f, - 0.2f, 1.697f, -2.650f, - -0.2f, 1.697f, -2.650f, - 0.0f, 0.0f, 0.0f, 0.0f - }; - RCLCPP_WARN( - get_logger(), - "Parameter crawl_default_dof_pos has %zu entries, expected 16. Falling back to training default pose.", - default_crawl_pose.size()); - } - const std::vector default_wall_pose_param( - rough_default_dof_pos_.begin(), rough_default_dof_pos_.end()); - const std::vector default_wall_pose = declare_parameter>( - "wall_default_dof_pos", default_wall_pose_param); - if (default_wall_pose.size() == wall_default_dof_pos_.size()) { - for (std::size_t i = 0; i < wall_default_dof_pos_.size(); ++i) { - wall_default_dof_pos_[i] = static_cast(default_wall_pose[i]); - } - } else { - wall_default_dof_pos_ = rough_default_dof_pos_; - RCLCPP_WARN( - get_logger(), - "Parameter wall_default_dof_pos has %zu entries, expected 16. Falling back to rough default pose.", - default_wall_pose.size()); - } - const double keep_pose_hip_pitch = declare_parameter("keep_pose_hip_pitch", 0.610); - const double keep_pose_knee = declare_parameter("keep_pose_knee", -1.250); - const std::vector default_keep_pose = { - 0.0, keep_pose_hip_pitch, keep_pose_knee, - 0.0, keep_pose_hip_pitch, keep_pose_knee, - 0.0, keep_pose_hip_pitch, keep_pose_knee, - 0.0, keep_pose_hip_pitch, keep_pose_knee, - 0.0, 0.0, 0.0, 0.0 - }; - const std::vector configured_keep_pose = declare_parameter>( - "keep_pose_dof_pos", default_keep_pose); - if (configured_keep_pose.size() == keep_pose_dof_pos_.size()) { - for (std::size_t i = 0; i < keep_pose_dof_pos_.size(); ++i) { - keep_pose_dof_pos_[i] = static_cast(configured_keep_pose[i]); - } - } else { - for (std::size_t i = 0; i < keep_pose_dof_pos_.size(); ++i) { - keep_pose_dof_pos_[i] = static_cast(default_keep_pose[i]); - } - RCLCPP_WARN( - get_logger(), - "Parameter keep_pose_dof_pos has %zu entries, expected 16. Falling back to keep_pose_hip_pitch/keep_pose_knee.", - configured_keep_pose.size()); - } - posture_transition_s_ = declare_parameter("keep_pose_transition_s", 0.8); - rough_model_path_ = model_path_; - rough_model_engine_path_ = declare_parameter("rough_model_engine_path", model_engine_path_); - crawl_model_path_ = declare_parameter("crawl_model_path", "policies/model_crawl.onnx"); - crawl_model_engine_path_ = declare_parameter("crawl_model_engine_path", ""); - wall_model_path_ = declare_parameter("wall_model_path", "policies/model_wall.onnx"); - wall_model_engine_path_ = declare_parameter("wall_model_engine_path", ""); - const std::string crawl_backend = toLowerCopy( - declare_parameter("crawl_backend", "ik")); - crawl_backend_ = crawl_backend == "rl" ? CrawlBackend::Rl : CrawlBackend::Ik; - crawl_ik_wheel_linear_gain_ = static_cast( - declare_parameter("crawl_ik_wheel_linear_gain", 6.25)); - crawl_ik_wheel_yaw_gain_ = static_cast( - declare_parameter("crawl_ik_wheel_yaw_gain", 4.0)); - crawl_ik_max_wheel_speed_ = static_cast( - declare_parameter("crawl_ik_max_wheel_speed", 6.0)); - crawl_ik_abduction_clip_ = static_cast( - declare_parameter("crawl_ik_abduction_clip", 0.45)); - crawl_ik_yaw_rate_kp_ = static_cast( - declare_parameter("crawl_ik_yaw_rate_kp", 1.6)); - crawl_ik_imu_posture_ = declare_parameter("crawl_ik_imu_posture", false); - crawl_ik_encoder_posture_kp_ = static_cast( - declare_parameter("crawl_ik_encoder_posture_kp", 0.0)); - crawl_ik_encoder_posture_max_ = static_cast( - declare_parameter("crawl_ik_encoder_posture_max", 0.03)); - crawl_ik_encoder_guard_ = declare_parameter("crawl_ik_encoder_guard", true); - crawl_ik_encoder_guard_start_ = static_cast( - declare_parameter("crawl_ik_encoder_guard_start", 0.28)); - crawl_ik_encoder_guard_stop_ = static_cast( - declare_parameter("crawl_ik_encoder_guard_stop", 0.65)); - crawl_ik_imu_guard_ = declare_parameter("crawl_ik_imu_guard", true); - const double crawl_ik_imu_guard_start_deg = declare_parameter( - "crawl_ik_imu_guard_start_deg", 12.0); - const double crawl_ik_imu_guard_stop_deg = declare_parameter( - "crawl_ik_imu_guard_stop_deg", 28.0); - constexpr double kPi = 3.14159265358979323846; - crawl_ik_imu_guard_start_rad_ = static_cast( - crawl_ik_imu_guard_start_deg * kPi / 180.0); - crawl_ik_imu_guard_stop_rad_ = static_cast( - crawl_ik_imu_guard_stop_deg * kPi / 180.0); - model_switch_transition_s_ = declare_parameter("model_switch_transition_s", 1.2); - model_switch_to_stand_transition_scale_ = declare_parameter( - "model_switch_to_stand_transition_scale", 1.35); - model_switch_to_model_transition_scale_ = declare_parameter( - "model_switch_to_model_transition_scale", 1.55); - model_switch_min_transition_s_ = declare_parameter("model_switch_min_transition_s", 0.35); - model_switch_stand_hold_s_ = declare_parameter("model_switch_stand_hold_s", 0.45); - model_switch_stand_max_err_ = declare_parameter("model_switch_stand_max_err", 0.18); - model_switch_stand_max_vel_ = declare_parameter("model_switch_stand_max_vel", 0.8); // Safety parameters safety_enabled_ = declare_parameter("safety_enabled", true); double max_target_offset = declare_parameter("max_target_offset", 0.6); - double model_switch_max_target_offset = declare_parameter( - "model_switch_max_target_offset", std::max(max_target_offset, 1.8)); double hard_target_offset = declare_parameter("hard_target_offset", 1.2); double max_ang_vel = declare_parameter("max_ang_vel", 10.0); double max_tilt_z = declare_parameter("max_tilt_z", -0.3); @@ -259,15 +37,6 @@ PolicyRuntimeNode::PolicyRuntimeNode() release_command_hold_s_ = static_cast(declare_parameter("release_command_hold_s", 0.12)); release_posture_max_err_ = static_cast(declare_parameter("release_posture_max_err", 0.35)); release_target_blend_s_ = static_cast(declare_parameter("release_target_blend_s", 0.30)); - model_switch_release_scale_ = static_cast( - declare_parameter("model_switch_release_scale", 1.3)); - runtime_max_vx_ = static_cast(declare_parameter("runtime_max_vx", 1.0)); - runtime_max_vy_ = static_cast(declare_parameter("runtime_max_vy", 0.3)); - runtime_max_yaw_rate_ = static_cast( - declare_parameter("runtime_max_yaw_rate", 1.0)); - debug_trace_enabled_ = declare_parameter("debug_trace_enabled", true); - const int debug_trace_decimation = declare_parameter("debug_trace_decimation", 1); - debug_trace_decimation_ = static_cast(std::max(debug_trace_decimation, 1)); clip_obs_ = static_cast(declare_parameter("clip_obs", 100.0)); hold_zero_command_pose_ = declare_parameter("hold_zero_command_pose", true); enable_zero_cmd_suppression_ = declare_parameter("enable_zero_cmd_suppression", true); @@ -275,50 +44,10 @@ PolicyRuntimeNode::PolicyRuntimeNode() zero_cmd_use_yaw_rate_ = declare_parameter("zero_cmd_use_yaw_rate", true); runtime_released_ = !require_active_command_to_release_; - if (rough_model_engine_path_.empty()) { - rough_model_engine_path_ = deriveTensorRtEnginePath(rough_model_path_); - } - if (model_engine_path_.empty()) { - model_engine_path_ = rough_model_engine_path_; - } - if (crawl_model_engine_path_.empty()) { - crawl_model_engine_path_ = deriveTensorRtEnginePath(crawl_model_path_); - } - if (wall_model_engine_path_.empty()) { - wall_model_engine_path_ = deriveTensorRtEnginePath(wall_model_path_); - } - - RCLCPP_INFO( - get_logger(), - "Policy inference setup: rough_model=%s, rough_engine=%s, crawl_model=%s, crawl_engine=%s, wall_model=%s, wall_engine=%s, crawl_backend=%s, prefer_tensorrt=%s", - rough_model_path_.c_str(), - rough_model_engine_path_.c_str(), - crawl_model_path_.c_str(), - crawl_model_engine_path_.c_str(), - wall_model_path_.c_str(), - wall_model_engine_path_.c_str(), - crawlBackendName(), - prefer_tensorrt_ ? "true" : "false"); - RCLCPP_INFO( - get_logger(), - "Event log file: %s", - event_logger_.componentLogPath().c_str()); - RCLCPP_INFO( - get_logger(), - "Run log directory: %s", - run_log_dir_.c_str()); - logEvent("INFO", "node_start", "Policy runtime node started."); - initializeDebugTrace(); + RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str()); // Initialize StandBalanceController stand_balance_ = std::make_unique(0.02); - const float rough_hip_mean = - (rough_default_dof_pos_[1] + rough_default_dof_pos_[4] + - rough_default_dof_pos_[7] + rough_default_dof_pos_[10]) * 0.25f; - const float rough_knee_mean = - (rough_default_dof_pos_[2] + rough_default_dof_pos_[5] + - rough_default_dof_pos_[8] + rough_default_dof_pos_[11]) * 0.25f; - stand_balance_->setNominalLegPose(rough_hip_mean, rough_knee_mean); // Initialize SafetyMonitor and RuntimeGuard safety_monitor_ = std::make_unique( @@ -328,13 +57,6 @@ PolicyRuntimeNode::PolicyRuntimeNode() clip_to_brake, static_cast(hard_target_offset) ); - model_switch_safety_monitor_ = std::make_unique( - static_cast(model_switch_max_target_offset), - static_cast(max_ang_vel), - static_cast(max_tilt_z), - clip_to_brake, - static_cast(hard_target_offset) - ); runtime_guard_ = std::make_unique( static_cast(max_ang_vel + 2.0), @@ -343,381 +65,26 @@ PolicyRuntimeNode::PolicyRuntimeNode() static_cast(imu_age_stop_ms) ); - // 2. Initialize inference backend - if (!initInferenceBackend()) { - RCLCPP_FATAL(get_logger(), "Failed to initialize any inference backend."); - throw std::runtime_error("failed to initialize inference backend"); - } - - // 3. Create publishers and subscriptions - target_pub_ = create_publisher("runtime/target", 10); - model_status_pub_ = create_publisher("runtime/model_status", 10); - state_sub_ = create_subscription( - "runtime/state", 10, - std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1)); - cmd_sub_ = create_subscription( - "cmd_vel", 10, - std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1)); - cmd_stamped_sub_ = create_subscription( - "cmd_vel_stamped", 10, - std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1)); - model_switch_sub_ = create_subscription( - "runtime/model_cmd", 10, - std::bind(&PolicyRuntimeNode::onModelSwitchCmd, this, std::placeholders::_1)); - posture_cmd_sub_ = create_subscription( - "runtime/posture_cmd", 10, - std::bind(&PolicyRuntimeNode::onPostureCmd, this, std::placeholders::_1)); - estop_sub_ = create_subscription( - "/safety/estop", 10, - std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1)); - - // 4. Timer at 50Hz (20ms) - policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this)); - - last_actions_.fill(0.0f); - publishModelStatus(); -} - -PolicyRuntimeNode::~PolicyRuntimeNode() -{ - logEvent("INFO", "node_stop", "Policy runtime node stopped."); - finalizeRunSummary(); - shutdownTensorRt(); - shutdownOnnxRuntime(); -} - -void PolicyRuntimeNode::logEvent( - const std::string & level, - const std::string & event, - const std::string & message) -{ - event_logger_.log(level, "sim2real_runtime_node", event, message); -} - -void PolicyRuntimeNode::initializeDebugTrace() -{ - if (!debug_trace_enabled_ || run_log_dir_.empty()) { - return; - } - - std::error_code ec; - std::filesystem::create_directories(run_log_dir_, ec); - debug_trace_path_ = (std::filesystem::path(run_log_dir_) / "runtime_debug_trace.csv").string(); - - std::ofstream stream(debug_trace_path_, std::ios::trunc); - if (!stream.is_open()) { - RCLCPP_WARN(get_logger(), "Failed to open runtime debug trace file: %s", debug_trace_path_.c_str()); - debug_trace_enabled_ = false; - return; - } - - stream - << "stamp_sec,target_seq,state_seq,state_source,startup_state,current_model,requested_model,loaded_model," - << "switch_state,target_source,runtime_released,release_alpha,zero_command,target_age_ms,imu_age_ms,odom_age_ms," - << "cmd_vx,cmd_vy,cmd_wz,raw_cmd_vx,raw_cmd_vy,raw_cmd_wz"; - for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { - stream << ",joint_pos_" << i; - } - for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { - stream << ",target_" << i; - } - stream << '\n'; -} - -void PolicyRuntimeNode::appendDebugTrace( - const sim2real_interfaces::msg::RuntimeState & state, - const sim2real_interfaces::msg::RuntimeTarget & target) -{ - if (!debug_trace_enabled_ || debug_trace_path_.empty()) { - return; - } - - if ((debug_trace_counter_++ % debug_trace_decimation_) != 0) { - return; - } - - std::ofstream stream(debug_trace_path_, std::ios::app); - if (!stream.is_open()) { - return; - } - - stream - << std::fixed << std::setprecision(6) - << rosTimeToSeconds(target.stamp) - << ',' << target.sequence - << ',' << state.sequence - << ',' << state.source - << ',' << startupStateName(startup_state_) - << ',' << modelModeName(current_model_mode_) - << ',' << modelModeName(requested_model_mode_) - << ',' << modelModeName(loaded_model_mode_) - << ',' << modelSwitchStateName(model_switch_state_) - << ',' << target.target_source - << ',' << (target.runtime_released ? 1 : 0) - << ',' << target.release_alpha - << ',' << (target.zero_command ? 1 : 0) - << ',' << target.target_age_ms - << ',' << state.imu_age_ms - << ',' << state.odom_age_ms - << ',' << target.command[0] - << ',' << target.command[1] - << ',' << target.command[2] - << ',' << target.raw_command[0] - << ',' << target.raw_command[1] - << ',' << target.raw_command[2]; - - for (float value : state.joint_pos) { - stream << ',' << value; - } - for (float value : target.target) { - stream << ',' << value; - } - stream << '\n'; -} - -void PolicyRuntimeNode::logProtectionEvent( - const std::string & trigger, - const std::string & reason, - const std::string & action) -{ - protection_trigger_count_++; - std::ostringstream oss; - oss << "trigger=" << trigger - << ", protection_action=" << action - << ", reason=" << reason; - logEvent("ERROR", "protection_triggered", oss.str()); -} - -void PolicyRuntimeNode::finalizeRunSummary() -{ - std::ostringstream oss; - oss << "run_dir=" << run_log_dir_ - << ", protection_trigger_count=" << protection_trigger_count_ - << ", target_clip_count=" << target_clip_count_ - << ", final_safety_triggered=" << (safety_triggered_ ? "true" : "false"); - if (!safety_reason_.empty()) { - oss << ", final_safety_reason=" << safety_reason_; - } - event_logger_.logSummary("sim2real_runtime_node", oss.str()); -} - -std::string PolicyRuntimeNode::deriveTensorRtEnginePath(const std::string & onnx_model_path) const -{ - constexpr const char * kSuffix = ".onnx"; - if (onnx_model_path.size() > std::strlen(kSuffix) && - onnx_model_path.compare(onnx_model_path.size() - std::strlen(kSuffix), std::strlen(kSuffix), kSuffix) == 0) { - return onnx_model_path.substr(0, onnx_model_path.size() - std::strlen(kSuffix)) + "_fp16.engine"; - } - return onnx_model_path + ".engine"; -} - -bool PolicyRuntimeNode::initInferenceBackend() -{ - if (prefer_tensorrt_) { - if (initTensorRt()) { - inference_backend_ = InferenceBackend::TensorRT; - return true; - } - RCLCPP_WARN(get_logger(), "TensorRT initialization failed. Falling back to ONNX Runtime."); - } - - if (initOnnxRuntime()) { - inference_backend_ = InferenceBackend::OnnxRuntime; - return true; - } - - inference_backend_ = InferenceBackend::None; - return false; -} - -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT -bool PolicyRuntimeNode::initTensorRt() -{ - shutdownTensorRt(); - inference_backend_ = InferenceBackend::None; - - if (model_engine_path_.empty()) { - RCLCPP_WARN(get_logger(), "TensorRT engine path is empty."); - return false; - } - - std::ifstream engine_file(model_engine_path_, std::ios::binary); - if (!engine_file) { - RCLCPP_WARN(get_logger(), "TensorRT engine file not found: %s", model_engine_path_.c_str()); - return false; - } - - engine_file.seekg(0, std::ios::end); - const std::streamsize engine_size = engine_file.tellg(); - if (engine_size <= 0) { - RCLCPP_WARN(get_logger(), "TensorRT engine file is empty: %s", model_engine_path_.c_str()); - return false; - } - engine_file.seekg(0, std::ios::beg); - - std::vector engine_data(static_cast(engine_size)); - if (!engine_file.read(engine_data.data(), engine_size)) { - RCLCPP_WARN(get_logger(), "Failed to read TensorRT engine file: %s", model_engine_path_.c_str()); - return false; - } - - auto & logger = getTensorRtLogger(get_logger()); - trt_runtime_ = nvinfer1::createInferRuntime(logger); - if (trt_runtime_ == nullptr) { - RCLCPP_WARN(get_logger(), "Failed to create TensorRT runtime."); - return false; - } - - trt_engine_ = trt_runtime_->deserializeCudaEngine(engine_data.data(), engine_data.size()); - if (trt_engine_ == nullptr) { - RCLCPP_WARN(get_logger(), "Failed to deserialize TensorRT engine: %s", model_engine_path_.c_str()); - shutdownTensorRt(); - return false; - } - - trt_context_ = trt_engine_->createExecutionContext(); - if (trt_context_ == nullptr) { - RCLCPP_WARN(get_logger(), "Failed to create TensorRT execution context."); - shutdownTensorRt(); - return false; - } - - if (trt_engine_->getNbIOTensors() != 2) { - RCLCPP_WARN( - get_logger(), - "Unexpected TensorRT IO tensor count: %d (expected 2).", - trt_engine_->getNbIOTensors()); - shutdownTensorRt(); - return false; - } - - for (int i = 0; i < trt_engine_->getNbIOTensors(); ++i) { - const char * tensor_name = trt_engine_->getIOTensorName(i); - if (trt_engine_->getTensorIOMode(tensor_name) == nvinfer1::TensorIOMode::kINPUT) { - trt_input_name_ = tensor_name; - } else { - trt_output_name_ = tensor_name; - } - } - - if (trt_input_name_.empty() || trt_output_name_.empty()) { - RCLCPP_WARN(get_logger(), "Failed to resolve TensorRT input/output tensor names."); - shutdownTensorRt(); - return false; - } - - const auto input_dims = trt_engine_->getTensorShape(trt_input_name_.c_str()); - const auto output_dims = trt_engine_->getTensorShape(trt_output_name_.c_str()); - if (input_dims.nbDims != 2 || output_dims.nbDims != 2) { - RCLCPP_WARN(get_logger(), "Unexpected TensorRT tensor ranks. input=%d output=%d", input_dims.nbDims, output_dims.nbDims); - shutdownTensorRt(); - return false; - } - - if (input_dims.d[1] != 53 || output_dims.d[1] != static_cast(sim2real_common::DeploymentContract::kActionDim)) { - RCLCPP_WARN( - get_logger(), - "TensorRT engine shape mismatch. input second dim=%ld output second dim=%ld", - static_cast(input_dims.d[1]), - static_cast(output_dims.d[1])); - shutdownTensorRt(); - return false; - } - - input_shape_ = {1, 53}; - output_shape_ = {1, static_cast(sim2real_common::DeploymentContract::kActionDim)}; - - if (cudaStreamCreate(&trt_stream_) != cudaSuccess) { - RCLCPP_WARN(get_logger(), "Failed to create CUDA stream for TensorRT."); - shutdownTensorRt(); - return false; - } - - const std::size_t input_bytes = sizeof(float) * 53; - const std::size_t output_bytes = sizeof(float) * sim2real_common::DeploymentContract::kActionDim; - if (cudaMalloc(&trt_input_buffer_, input_bytes) != cudaSuccess || - cudaMalloc(&trt_output_buffer_, output_bytes) != cudaSuccess) { - RCLCPP_WARN(get_logger(), "Failed to allocate TensorRT CUDA buffers."); - shutdownTensorRt(); - return false; - } - - if (!trt_context_->setInputShape(trt_input_name_.c_str(), nvinfer1::Dims2{1, 53})) { - RCLCPP_WARN(get_logger(), "Failed to set TensorRT input shape."); - shutdownTensorRt(); - return false; - } - - if (!trt_context_->setTensorAddress(trt_input_name_.c_str(), trt_input_buffer_) || - !trt_context_->setTensorAddress(trt_output_name_.c_str(), trt_output_buffer_)) { - RCLCPP_WARN(get_logger(), "Failed to bind TensorRT IO buffers."); - shutdownTensorRt(); - return false; - } - - RCLCPP_INFO( - get_logger(), - "TensorRT engine loaded successfully from %s. Input=%s[1x53], Output=%s[1x%ld]", - model_engine_path_.c_str(), - trt_input_name_.c_str(), - trt_output_name_.c_str(), - static_cast(sim2real_common::DeploymentContract::kActionDim)); - return true; -} -#else -bool PolicyRuntimeNode::initTensorRt() -{ - RCLCPP_INFO(get_logger(), "TensorRT support not compiled in; skipping TensorRT initialization."); - return false; -} -#endif - -bool PolicyRuntimeNode::initOnnxRuntime() -{ - shutdownOnnxRuntime(); - + // 2. Initialize Ort C++ environment try { env_ = std::make_unique(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env"); - - const auto available_providers = Ort::GetAvailableProviders(); - std::ostringstream provider_stream; - for (std::size_t i = 0; i < available_providers.size(); ++i) { - if (i > 0) { - provider_stream << ", "; - } - provider_stream << available_providers[i]; - } - RCLCPP_INFO( - get_logger(), - "ONNX Runtime available providers: [%s]", - provider_stream.str().c_str()); - - if (use_cuda_) { - const bool has_cuda_provider = std::find( - available_providers.begin(), - available_providers.end(), - "CUDAExecutionProvider") != available_providers.end(); - if (!has_cuda_provider) { - RCLCPP_WARN( - get_logger(), - "Parameter use_cuda=true, but CUDAExecutionProvider is not available in the current ONNX Runtime build."); - } - } - + Ort::SessionOptions session_options; + // single-thread ORIN optimization to prevent thread scheduling jitter session_options.SetIntraOpNumThreads(1); session_options.SetInterOpNumThreads(1); session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); + // CUDA Execution Provider (Orin Nano GPU acceleration) if (use_cuda_) { try { OrtCUDAProviderOptions cuda_opts{}; cuda_opts.device_id = 0; + // enable_cuda_graph: false for single-inference RL policy (avoids overhead) session_options.AppendExecutionProvider_CUDA(cuda_opts); RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)"); } catch (const std::exception& e) { - RCLCPP_WARN( - get_logger(), + RCLCPP_WARN(get_logger(), "CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.", e.what()); use_cuda_ = false; @@ -727,27 +94,24 @@ bool PolicyRuntimeNode::initOnnxRuntime() session_ = std::make_unique(*env_, model_path_.c_str(), session_options); memory_info_ = std::make_unique(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU)); + // Get input/output nodes names and shapes Ort::AllocatorWithDefaultOptions allocator; - input_names_str_.clear(); - output_names_str_.clear(); - input_names_char_.clear(); - output_names_char_.clear(); - - const std::size_t num_inputs = session_->GetInputCount(); + + std::size_t num_inputs = session_->GetInputCount(); for (std::size_t i = 0; i < num_inputs; ++i) { auto name = session_->GetInputNameAllocated(i, allocator); input_names_str_.push_back(std::string(name.get())); } - for (const auto & name : input_names_str_) { + for (const auto& name : input_names_str_) { input_names_char_.push_back(name.c_str()); } - - const std::size_t num_outputs = session_->GetOutputCount(); + + std::size_t num_outputs = session_->GetOutputCount(); for (std::size_t i = 0; i < num_outputs; ++i) { auto name = session_->GetOutputNameAllocated(i, allocator); output_names_str_.push_back(std::string(name.get())); } - for (const auto & name : output_names_str_) { + for (const auto& name : output_names_str_) { output_names_char_.push_back(name.c_str()); } @@ -757,7 +121,7 @@ bool PolicyRuntimeNode::initOnnxRuntime() if (input_shape_[0] < 0) { input_shape_[0] = 1; } - + auto output_type_info = session_->GetOutputTypeInfo(0); auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo(); output_shape_ = output_tensor_info.GetShape(); @@ -765,64 +129,41 @@ bool PolicyRuntimeNode::initOnnxRuntime() output_shape_[0] = 1; } + // Validate output shape matches expected action dimension if (output_shape_.size() < 2 || output_shape_[1] != static_cast(sim2real_common::DeploymentContract::kActionDim)) { + RCLCPP_FATAL(get_logger(), + "ONNX model output dimension mismatch! Expected %ld, got %ld. Wrong model?", + static_cast(sim2real_common::DeploymentContract::kActionDim), + output_shape_.size() >= 2 ? output_shape_[1] : -1); throw std::runtime_error("ONNX model output shape mismatch"); } - RCLCPP_INFO( - get_logger(), - "ONNX Runtime model loaded successfully from %s. Input shape: [%ld, %ld], Output shape: [%ld, %ld]", - model_path_.c_str(), - input_shape_[0], - input_shape_[1], - output_shape_[0], - output_shape_[1]); - return true; + RCLCPP_INFO(get_logger(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]", + input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]); } catch (const std::exception& e) { - RCLCPP_ERROR(get_logger(), "Failed to load ONNX Runtime model: %s", e.what()); - shutdownOnnxRuntime(); - return false; + RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what()); + throw; } -} -void PolicyRuntimeNode::shutdownOnnxRuntime() -{ - memory_info_.reset(); - session_.reset(); - env_.reset(); - input_names_char_.clear(); - output_names_char_.clear(); - input_names_str_.clear(); - output_names_str_.clear(); -} + // 3. Create publishers and subscriptions + target_pub_ = create_publisher("runtime/target", 10); + state_sub_ = create_subscription( + "runtime/state", 10, + std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1)); + cmd_sub_ = create_subscription( + "cmd_vel", 10, + std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1)); + cmd_stamped_sub_ = create_subscription( + "cmd_vel_stamped", 10, + std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1)); + estop_sub_ = create_subscription( + "/safety/estop", 10, + std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1)); -void PolicyRuntimeNode::shutdownTensorRt() -{ -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT - if (trt_input_buffer_ != nullptr) { - cudaFree(trt_input_buffer_); - trt_input_buffer_ = nullptr; - } - if (trt_output_buffer_ != nullptr) { - cudaFree(trt_output_buffer_); - trt_output_buffer_ = nullptr; - } - if (trt_stream_ != nullptr) { - cudaStreamDestroy(trt_stream_); - trt_stream_ = nullptr; - } - if (trt_context_ != nullptr) { - destroyTensorRtObject(trt_context_); - } - if (trt_engine_ != nullptr) { - destroyTensorRtObject(trt_engine_); - } - if (trt_runtime_ != nullptr) { - destroyTensorRtObject(trt_runtime_); - } - trt_input_name_.clear(); - trt_output_name_.clear(); -#endif + // 4. Timer at 50Hz (20ms) + policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this)); + + last_actions_.fill(0.0f); } void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg) @@ -835,10 +176,15 @@ void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::Sh void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw) { + // Velocity saturation limits (consistent with training domain) + constexpr float kMaxLinVelX = 0.8f; // m/s + constexpr float kMaxLinVelY = 0.3f; // m/s + constexpr float kMaxAngVelZ = 0.5f; // rad/s + std::scoped_lock lock(mutex_); - raw_cmd_[0] = std::clamp(vx, -runtime_max_vx_, runtime_max_vx_); - raw_cmd_[1] = std::clamp(vy, -runtime_max_vy_, runtime_max_vy_); - raw_cmd_[2] = std::clamp(vyaw, -runtime_max_yaw_rate_, runtime_max_yaw_rate_); + raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX); + raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY); + raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ); cmd_ = raw_cmd_; } @@ -858,167 +204,14 @@ void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped:: static_cast(msg->twist.angular.z)); } -void PolicyRuntimeNode::onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg) -{ - const std::string command = msg ? msg->data : ""; - const auto to_lower = [](std::string value) { - std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { - return static_cast(std::tolower(c)); - }); - return value; - }; - - const std::string normalized = to_lower(command); - ModelMode target_mode = current_model_mode_; - bool recognized = true; - if (normalized == "toggle" || normalized == "switch") { - target_mode = current_model_mode_ == ModelMode::Rough ? ModelMode::Crawl : ModelMode::Rough; - } else if (normalized == "rough") { - target_mode = ModelMode::Rough; - } else if (normalized == "crawl" || normalized == "ik") { - target_mode = ModelMode::Crawl; - } else if (normalized == "wall") { - target_mode = ModelMode::Wall; - } else { - recognized = false; - } - - if (!recognized) { - RCLCPP_WARN(get_logger(), "Ignoring unknown model switch command: %s", command.c_str()); - return; - } - - if (startup_state_ != StartupState::RUNTIME) { - RCLCPP_WARN(get_logger(), "Ignoring model switch command before runtime release: %s", command.c_str()); - return; - } - - if (model_switch_state_ != ModelSwitchState::Idle) { - RCLCPP_WARN( - get_logger(), - "Ignoring model switch command while another switch is active. current_state=%s", - modelSwitchStateName(model_switch_state_)); - return; - } - - if (posture_hold_mode_ != PostureHoldMode::None) { - RCLCPP_WARN( - get_logger(), - "Ignoring model switch command while posture hold is active. posture=%s", - postureHoldModeName(posture_hold_mode_)); - return; - } - if (target_mode == current_model_mode_) { - RCLCPP_INFO(get_logger(), "Model switch requested to current model %s; ignoring.", modelModeName(target_mode)); - return; - } - - { - std::scoped_lock lock(mutex_); - requested_model_mode_ = target_mode; - model_switch_requested_ = true; - } - RCLCPP_INFO( - get_logger(), - "Queued model switch from %s to %s", - modelModeName(current_model_mode_), - modelModeName(target_mode)); - publishModelStatus(); -} - -void PolicyRuntimeNode::startPostureTransition( - PostureHoldMode mode, - const std::array & start_pose, - const std::array & target_pose, - const rclcpp::Time & now_time) -{ - posture_hold_mode_ = mode; - posture_transition_active_ = true; - posture_transition_start_time_ = now_time; - posture_start_pose_ = start_pose; - posture_target_pose_ = target_pose; - posture_start_pose_[12] = posture_start_pose_[13] = posture_start_pose_[14] = posture_start_pose_[15] = 0.0f; - posture_target_pose_[12] = posture_target_pose_[13] = posture_target_pose_[14] = posture_target_pose_[15] = 0.0f; - - for (std::size_t i = 0; i < posture_delta_.size(); ++i) { - float delta = posture_target_pose_[i] - posture_start_pose_[i]; - if (i < sim2real_common::DeploymentContract::kLegJointCount) { - delta = delta - 2.0f * static_cast(M_PI) * std::floor( - (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); - } - posture_delta_[i] = delta; - } - - cmd_.fill(0.0f); - raw_cmd_.fill(0.0f); - filtered_cmd_.fill(0.0f); - last_actions_.fill(0.0f); - runtime_released_ = false; - release_alpha_ = 0.0f; - release_active_time_ = 0.0f; - hold_active_model_pose_when_unreleased_ = true; - slow_release_after_model_switch_ = true; - safety_reference_dof_pos_ = mode == PostureHoldMode::Keep ? keep_pose_dof_pos_ : active_default_dof_pos_; -} - -void PolicyRuntimeNode::onPostureCmd(const std_msgs::msg::String::SharedPtr msg) -{ - const std::string command = msg ? msg->data : ""; - const std::string normalized = toLowerCopy(command); - const rclcpp::Time now_time = now(); - - std::scoped_lock lock(mutex_); - if (startup_state_ != StartupState::RUNTIME) { - RCLCPP_WARN(get_logger(), "Ignoring posture command before runtime release: %s", command.c_str()); - return; - } - if (model_switch_state_ != ModelSwitchState::Idle) { - RCLCPP_WARN( - get_logger(), - "Ignoring posture command while model switch is active. current_state=%s", - modelSwitchStateName(model_switch_state_)); - return; - } - std::array start_pose = has_state_ ? latest_state_.joint_pos : active_default_dof_pos_; - start_pose[12] = start_pose[13] = start_pose[14] = start_pose[15] = 0.0f; - - if (normalized == "keep") { - if (posture_hold_mode_ == PostureHoldMode::Keep && !posture_transition_active_) { - return; - } - startPostureTransition(PostureHoldMode::Keep, start_pose, keep_pose_dof_pos_, now_time); - RCLCPP_INFO(get_logger(), "Posture hold requested: keep pose."); - } else if (normalized == "default" || normalized == "release" || normalized == "off") { - if (posture_hold_mode_ == PostureHoldMode::None && !posture_transition_active_) { - return; - } - startPostureTransition(PostureHoldMode::ReturnDefault, start_pose, active_default_dof_pos_, now_time); - RCLCPP_INFO(get_logger(), "Posture hold requested: return to default pose."); - } else if (!normalized.empty()) { - RCLCPP_WARN(get_logger(), "Ignoring unknown posture command: %s", command.c_str()); - } -} void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg) { std::scoped_lock lock(mutex_); - const bool was_estop = estop_triggered_; estop_triggered_ = msg->data; if (estop_triggered_) { RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!"); - logEvent("WARN", "estop_triggered", "E-stop triggered via /safety/estop."); } else { RCLCPP_INFO(get_logger(), "E-stop reset."); - logEvent("INFO", "estop_reset", "E-stop reset via /safety/estop."); - const bool user_estop_latch = - safety_reason_.find("user E-stop") != std::string::npos || - (was_estop && safety_reason_.empty()); - if (safety_triggered_ && user_estop_latch) { - safety_triggered_ = false; - safety_reason_.clear(); - clip_active_logged_ = false; - RCLCPP_INFO(get_logger(), "Cleared user E-stop safety latch."); - logEvent("INFO", "safety_latch_reset", "Cleared user E-stop safety latch."); - } } } @@ -1040,7 +233,7 @@ std::array PolicyRuntimeNode::buildObservation( obs[cursor++] = v; } for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { - obs[cursor++] = state.joint_pos[i] - active_default_dof_pos_[i]; + obs[cursor++] = state.joint_pos[i] - sim2real_common::DeploymentContract::kDefaultDofPos[i]; } for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { obs[cursor++] = state.joint_vel[i] * 0.05f; @@ -1065,79 +258,35 @@ std::array PolicyRuntimeNode::buildObservation( std::array PolicyRuntimeNode::runPolicy(const std::array & obs) { std::array action{}; + try { + auto input_tensor = Ort::Value::CreateTensor( + *memory_info_, + const_cast(obs.data()), + obs.size(), + input_shape_.data(), + input_shape_.size() + ); - switch (inference_backend_) { - case InferenceBackend::TensorRT: -#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT - if (cudaMemcpyAsync( - trt_input_buffer_, - obs.data(), - sizeof(float) * obs.size(), - cudaMemcpyHostToDevice, - trt_stream_) != cudaSuccess) { - RCLCPP_ERROR(get_logger(), "TensorRT H2D copy failed."); - action.fill(0.0f); - break; - } - if (!trt_context_->enqueueV3(trt_stream_)) { - RCLCPP_ERROR(get_logger(), "TensorRT enqueue failed."); - action.fill(0.0f); - break; - } - if (cudaMemcpyAsync( - action.data(), - trt_output_buffer_, - sizeof(float) * action.size(), - cudaMemcpyDeviceToHost, - trt_stream_) != cudaSuccess) { - RCLCPP_ERROR(get_logger(), "TensorRT D2H copy failed."); - action.fill(0.0f); - break; - } - if (cudaStreamSynchronize(trt_stream_) != cudaSuccess) { - RCLCPP_ERROR(get_logger(), "TensorRT stream synchronization failed."); - action.fill(0.0f); - } -#else - action.fill(0.0f); -#endif - break; - case InferenceBackend::OnnxRuntime: - try { - auto input_tensor = Ort::Value::CreateTensor( - *memory_info_, - const_cast(obs.data()), - obs.size(), - input_shape_.data(), - input_shape_.size() - ); + auto output_tensor = Ort::Value::CreateTensor( + *memory_info_, + action.data(), + action.size(), + output_shape_.data(), + output_shape_.size() + ); - auto output_tensor = Ort::Value::CreateTensor( - *memory_info_, - action.data(), - action.size(), - output_shape_.data(), - output_shape_.size() - ); - - session_->Run( - Ort::RunOptions{nullptr}, - input_names_char_.data(), - &input_tensor, - 1, - output_names_char_.data(), - &output_tensor, - 1 - ); - } catch (const std::exception& e) { - RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what()); - action.fill(0.0f); - } - break; - default: - RCLCPP_ERROR(get_logger(), "No inference backend available."); - action.fill(0.0f); - break; + session_->Run( + Ort::RunOptions{nullptr}, + input_names_char_.data(), + &input_tensor, + 1, + output_names_char_.data(), + &output_tensor, + 1 + ); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what()); + action.fill(0.0f); } for (float& v : action) { @@ -1163,350 +312,6 @@ bool PolicyRuntimeNode::isCommandActive(const std::array & cmd) const return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_; } -bool PolicyRuntimeNode::modeUsesInference(ModelMode mode) const -{ - return mode == ModelMode::Rough || - mode == ModelMode::Wall || - (mode == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Rl); -} - -const std::array & PolicyRuntimeNode::defaultPoseForMode(ModelMode mode) const -{ - switch (mode) { - case ModelMode::Crawl: - return crawl_default_dof_pos_; - case ModelMode::Wall: - return wall_default_dof_pos_; - case ModelMode::Rough: - default: - return rough_default_dof_pos_; - } -} - -const std::string & PolicyRuntimeNode::modelPathForMode(ModelMode mode) const -{ - switch (mode) { - case ModelMode::Crawl: - return crawl_model_path_; - case ModelMode::Wall: - return wall_model_path_; - case ModelMode::Rough: - default: - return rough_model_path_; - } -} - -const std::string & PolicyRuntimeNode::modelEnginePathForMode(ModelMode mode) const -{ - switch (mode) { - case ModelMode::Crawl: - return crawl_model_engine_path_; - case ModelMode::Wall: - return wall_model_engine_path_; - case ModelMode::Rough: - default: - return rough_model_engine_path_; - } -} - -std::array PolicyRuntimeNode::computeHoldTarget( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & cmd) -{ - if (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) { - auto target = crawl_default_dof_pos_; - if (crawl_ik_imu_posture_) { - const auto balance_target = stand_balance_->computeTarget( - state.projected_gravity, state.imu_gyro, cmd); - for (std::size_t leg_idx = 0; leg_idx < 4; ++leg_idx) { - const std::size_t abduction_index = leg_idx * 3; - target[abduction_index] = std::clamp( - balance_target[abduction_index], -crawl_ik_abduction_clip_, crawl_ik_abduction_clip_); - } - } else { - stand_balance_->computeTarget( - state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); - } - if (crawl_ik_encoder_posture_kp_ > 0.0f) { - for (std::size_t i = 0; i < 12; ++i) { - const float encoder_err = target[i] - state.joint_pos[i]; - const float correction = std::clamp( - crawl_ik_encoder_posture_kp_ * encoder_err, - -crawl_ik_encoder_posture_max_, - crawl_ik_encoder_posture_max_); - target[i] += correction; - } - } - target[12] = 0.0f; - target[13] = 0.0f; - target[14] = 0.0f; - target[15] = 0.0f; - return target; - } - - // Keep the balance controller updated for stability monitoring, while holding - // the active model's default pose for rough/wall runtime zero-command holds. - stand_balance_->computeTarget( - state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); - return active_default_dof_pos_; -} - -std::array PolicyRuntimeNode::computeIkCrawlTarget( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & cmd) -{ - auto target = computeHoldTarget(state, cmd); - const float yaw_rate_cmd = - cmd[2] + crawl_ik_yaw_rate_kp_ * (cmd[2] - state.imu_gyro[2]); - const float command_scale = computeCrawlIkCommandScale(state, target); - - const float left_wheel = std::clamp( - cmd[0] * crawl_ik_wheel_linear_gain_ - yaw_rate_cmd * crawl_ik_wheel_yaw_gain_, - -crawl_ik_max_wheel_speed_, crawl_ik_max_wheel_speed_) * command_scale; - const float right_wheel = std::clamp( - cmd[0] * crawl_ik_wheel_linear_gain_ + yaw_rate_cmd * crawl_ik_wheel_yaw_gain_, - -crawl_ik_max_wheel_speed_, crawl_ik_max_wheel_speed_) * command_scale; - - target[12] = left_wheel; - target[13] = right_wheel; - target[14] = left_wheel; - target[15] = right_wheel; - return target; -} - -float PolicyRuntimeNode::projectedGravityTiltRad( - const std::array & projected_gravity) const -{ - const float lateral = std::hypot(projected_gravity[0], projected_gravity[1]); - const float vertical = std::max(1.0e-6f, std::abs(projected_gravity[2])); - return std::atan2(lateral, vertical); -} - -float PolicyRuntimeNode::computeCrawlIkCommandScale( - const sim2real_interfaces::msg::RuntimeState & state, - const std::array & leg_target) const -{ - float scale = 1.0f; - if (crawl_ik_encoder_guard_) { - float max_leg_err = 0.0f; - for (std::size_t i = 0; i < 12; ++i) { - max_leg_err = std::max(max_leg_err, std::abs(state.joint_pos[i] - leg_target[i])); - } - if (max_leg_err >= crawl_ik_encoder_guard_stop_) { - scale = 0.0f; - } else if (max_leg_err > crawl_ik_encoder_guard_start_) { - const float span = std::max( - 1.0e-6f, crawl_ik_encoder_guard_stop_ - crawl_ik_encoder_guard_start_); - scale *= 1.0f - (max_leg_err - crawl_ik_encoder_guard_start_) / span; - } - } - - if (crawl_ik_imu_guard_) { - const float tilt = projectedGravityTiltRad(state.projected_gravity); - if (tilt >= crawl_ik_imu_guard_stop_rad_) { - scale = 0.0f; - } else if (tilt > crawl_ik_imu_guard_start_rad_) { - const float span = std::max( - 1.0e-6f, crawl_ik_imu_guard_stop_rad_ - crawl_ik_imu_guard_start_rad_); - scale *= 1.0f - (tilt - crawl_ik_imu_guard_start_rad_) / span; - } - } - - return std::clamp(scale, 0.0f, 1.0f); -} - -bool PolicyRuntimeNode::switchInferenceModel(ModelMode target_mode) -{ - const ModelMode previous_mode = current_model_mode_; - const ModelMode previous_loaded_mode = loaded_model_mode_; - const std::array previous_default_pose = active_default_dof_pos_; - const std::string previous_model_path = model_path_; - const std::string previous_model_engine_path = model_engine_path_; - const bool target_uses_inference = modeUsesInference(target_mode); - - if (!target_uses_inference) { - current_model_mode_ = target_mode; - active_default_dof_pos_ = defaultPoseForMode(target_mode); - RCLCPP_INFO( - get_logger(), - "Switched active model to %s using backend %s", - modelModeName(current_model_mode_), - crawlBackendName()); - publishModelStatus(); - return true; - } - - if (loaded_model_mode_ == target_mode) { - current_model_mode_ = target_mode; - active_default_dof_pos_ = defaultPoseForMode(target_mode); - RCLCPP_INFO( - get_logger(), - "Switched active model to %s using already loaded backend %s", - modelModeName(current_model_mode_), - inferenceBackendName()); - publishModelStatus(); - return true; - } - - const std::string next_model_path = modelPathForMode(target_mode); - const std::string next_model_engine_path = modelEnginePathForMode(target_mode); - - shutdownTensorRt(); - shutdownOnnxRuntime(); - - model_path_ = next_model_path; - model_engine_path_ = next_model_engine_path; - - if (!initInferenceBackend()) { - RCLCPP_ERROR( - get_logger(), - "Failed to switch inference model to %s (model=%s engine=%s)", - modelModeName(target_mode), - model_path_.c_str(), - model_engine_path_.c_str()); - shutdownTensorRt(); - shutdownOnnxRuntime(); - model_path_ = previous_model_path; - model_engine_path_ = previous_model_engine_path; - current_model_mode_ = previous_mode; - loaded_model_mode_ = previous_loaded_mode; - active_default_dof_pos_ = previous_default_pose; - if (!initInferenceBackend()) { - RCLCPP_FATAL( - get_logger(), - "Failed to restore previous inference model %s after switch failure.", - modelModeName(previous_loaded_mode)); - } else { - RCLCPP_WARN( - get_logger(), - "Restored previous inference model %s after switch failure.", - modelModeName(previous_loaded_mode)); - } - publishModelStatus(); - return false; - } - - loaded_model_mode_ = target_mode; - current_model_mode_ = target_mode; - active_default_dof_pos_ = defaultPoseForMode(target_mode); - RCLCPP_INFO( - get_logger(), - "Switched active model to %s using backend %s", - modelModeName(current_model_mode_), - inferenceBackendName()); - publishModelStatus(); - return true; -} - -const char * PolicyRuntimeNode::modelModeName(ModelMode mode) const -{ - switch (mode) { - case ModelMode::Rough: - return "rough"; - case ModelMode::Crawl: - return crawl_backend_ == CrawlBackend::Ik ? "ik" : "crawl"; - case ModelMode::Wall: - return "wall"; - default: - return "unknown"; - } -} - -const char * PolicyRuntimeNode::startupStateName(StartupState state) const -{ - switch (state) { - case StartupState::BOOT_HOLD: - return "boot_hold"; - case StartupState::STARTUP_SOFT_HOLD: - return "startup_soft_hold"; - case StartupState::STARTUP_TRANSITION: - return "startup_transition"; - case StartupState::STARTUP_HOLD_AFTER: - return "startup_hold_after"; - case StartupState::RUNTIME: - return "runtime"; - default: - return "unknown"; - } -} - -const char * PolicyRuntimeNode::modelSwitchStateName(ModelSwitchState state) const -{ - switch (state) { - case ModelSwitchState::Idle: - return "idle"; - case ModelSwitchState::ToStand: - return "to_stand"; - case ModelSwitchState::StandHold: - return "stand_hold"; - case ModelSwitchState::ToModelPose: - return "to_model_pose"; - default: - return "unknown"; - } -} - -const char * PolicyRuntimeNode::postureHoldModeName(PostureHoldMode mode) const -{ - switch (mode) { - case PostureHoldMode::Keep: - return "keep"; - case PostureHoldMode::ReturnDefault: - return "return_default"; - case PostureHoldMode::None: - default: - return "none"; - } -} -const char * PolicyRuntimeNode::inferenceBackendName() const -{ - switch (inference_backend_) { - case InferenceBackend::TensorRT: - return "tensorrt"; - case InferenceBackend::OnnxRuntime: - return "onnxruntime"; - default: - return "none"; - } -} - -const char * PolicyRuntimeNode::crawlBackendName() const -{ - switch (crawl_backend_) { - case CrawlBackend::Ik: - return "ik"; - case CrawlBackend::Rl: - return "rl"; - default: - return "unknown"; - } -} - -void PolicyRuntimeNode::publishModelStatus() -{ - if (!model_status_pub_) { - return; - } - - const char * active_backend = - (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) ? - crawlBackendName() : inferenceBackendName(); - - std::ostringstream stream; - stream << "{" - << "\"current_model\":\"" << modelModeName(current_model_mode_) << "\"," - << "\"requested_model\":\"" << modelModeName(requested_model_mode_) << "\"," - << "\"switch_state\":\"" << modelSwitchStateName(model_switch_state_) << "\"," - << "\"backend\":\"" << active_backend << "\"," - << "\"crawl_backend\":\"" << crawlBackendName() << "\"," - << "\"inference_backend\":\"" << inferenceBackendName() << "\"," - << "\"switching\":" << (model_switch_state_ != ModelSwitchState::Idle ? "true" : "false") - << "}"; - std_msgs::msg::String msg; - msg.data = stream.str(); - model_status_pub_->publish(msg); -} - void PolicyRuntimeNode::onPolicyLoop() { sim2real_interfaces::msg::RuntimeState state; @@ -1516,15 +321,6 @@ void PolicyRuntimeNode::onPolicyLoop() bool estop_active = false; bool safety_active = false; double state_age_ms = 0.0; - bool model_switch_requested = false; - ModelMode requested_model_mode = current_model_mode_; - PostureHoldMode posture_hold_mode = PostureHoldMode::None; - bool posture_transition_active = false; - std::array posture_start_pose{}; - std::array posture_target_pose{}; - std::array posture_delta{}; - rclcpp::Time posture_transition_start_time{0, 0, RCL_ROS_TIME}; - double posture_transition_s = posture_transition_s_; { std::scoped_lock lock(mutex_); if (!has_state_) { @@ -1536,15 +332,6 @@ void PolicyRuntimeNode::onPolicyLoop() last_actions = last_actions_; estop_active = estop_triggered_; safety_active = safety_triggered_; - model_switch_requested = model_switch_requested_; - requested_model_mode = requested_model_mode_; - posture_hold_mode = posture_hold_mode_; - posture_transition_active = posture_transition_active_; - posture_start_pose = posture_start_pose_; - posture_target_pose = posture_target_pose_; - posture_delta = posture_delta_; - posture_transition_start_time = posture_transition_start_time_; - posture_transition_s = posture_transition_s_; if (last_state_recv_time_.time_since_epoch().count() != 0) { state_age_ms = std::chrono::duration( std::chrono::steady_clock::now() - last_state_recv_time_).count(); @@ -1572,8 +359,6 @@ void PolicyRuntimeNode::onPolicyLoop() safety_active = true; safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason; RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str()); - logEvent("ERROR", "safety_triggered", safety_reason_); - logProtectionEvent("runtime_guard_stop", guard_decision.reason, "safety_brake"); } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str()); } @@ -1587,11 +372,10 @@ void PolicyRuntimeNode::onPolicyLoop() target.command = cmd; target.raw_action.fill(0.0f); target.scaled_action.fill(0.0f); - target.target = safety_reference_dof_pos_; + target.target = sim2real_common::DeploymentContract::kDefaultDofPos; target.target_source = "safety_brake"; target.target_age_ms = 0.0f; target_pub_->publish(target); - appendDebugTrace(state, target); return; } @@ -1613,7 +397,7 @@ void PolicyRuntimeNode::onPolicyLoop() // 2. Shortest periodic delta to stand pose float max_dev = 0.0f; for (std::size_t i = 0; i < 12; ++i) { - float delta = active_default_dof_pos_[i] - start_pose_[i]; + float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i]; delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); startup_delta_[i] = delta; max_dev = std::max(max_dev, std::abs(delta)); @@ -1625,7 +409,6 @@ void PolicyRuntimeNode::onPolicyLoop() target.target = start_pose_; target.target_source = "boot_hold"; target_pub_->publish(target); - appendDebugTrace(state, target); return; } @@ -1662,7 +445,7 @@ void PolicyRuntimeNode::onPolicyLoop() // Settle check float max_pos_err = 0.0f; for (std::size_t i = 0; i < 12; ++i) { - float delta = active_default_dof_pos_[i] - state.joint_pos[i]; + float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i]; delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); max_pos_err = std::max(max_pos_err, std::abs(delta)); } @@ -1681,10 +464,8 @@ void PolicyRuntimeNode::onPolicyLoop() else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) { double elapsed = (now_time - state_start_time_).seconds(); - // Keep balance stability tracking running, while holding the exact default - // rough pose so startup hold matches the runtime stand posture. - stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd); - target.target = rough_default_dof_pos_; + // Run stand balance controller during holding phase + target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd); target.target_source = "startup_hold"; if (elapsed >= 1.0 && stand_balance_->isStable()) { @@ -1693,185 +474,11 @@ void PolicyRuntimeNode::onPolicyLoop() } } else if (startup_state_ == StartupState::RUNTIME) { - if (model_switch_requested && posture_hold_mode == PostureHoldMode::None && - model_switch_state_ == ModelSwitchState::Idle && requested_model_mode != current_model_mode_) { - switch_start_pose_ = state.joint_pos; - switch_start_pose_[12] = switch_start_pose_[13] = switch_start_pose_[14] = switch_start_pose_[15] = 0.0f; - float max_dev = 0.0f; - for (std::size_t i = 0; i < 12; ++i) { - float delta = rough_default_dof_pos_[i] - switch_start_pose_[i]; - delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); - switch_delta_[i] = delta; - max_dev = std::max(max_dev, std::abs(delta)); - } - switch_delta_[12] = switch_delta_[13] = switch_delta_[14] = switch_delta_[15] = 0.0f; - const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); - active_switch_transition_s_ = std::clamp( - max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * - std::max(0.1, model_switch_to_stand_transition_scale_); - model_switch_state_ = ModelSwitchState::ToStand; - model_switch_state_start_time_ = now_time; - safety_reference_dof_pos_ = rough_default_dof_pos_; - runtime_released_ = false; - release_active_time_ = 0.0f; - release_alpha_ = 0.0f; - last_actions.fill(0.0f); - hold_active_model_pose_when_unreleased_ = true; - { - std::scoped_lock lock(mutex_); - model_switch_requested_ = false; - } - RCLCPP_INFO( - get_logger(), - "Starting model switch transition: %s -> stand -> %s", - modelModeName(current_model_mode_), - modelModeName(requested_model_mode)); - publishModelStatus(); - } - - if (model_switch_state_ == ModelSwitchState::ToStand) { - const double elapsed = (now_time - model_switch_state_start_time_).seconds(); - const double phase = std::min(1.0, elapsed / std::max(1.0e-3, active_switch_transition_s_)); - const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); - for (std::size_t i = 0; i < 16; ++i) { - target.target[i] = switch_start_pose_[i] + static_cast(blend) * switch_delta_[i]; - } - target.target_source = "model_switch_to_stand"; - if (phase >= 1.0) { - model_switch_state_ = ModelSwitchState::StandHold; - model_switch_state_start_time_ = now_time; - publishModelStatus(); - } - } else if (model_switch_state_ == ModelSwitchState::StandHold) { - target.target = rough_default_dof_pos_; - target.target_source = "model_switch_stand_hold"; - const double elapsed = (now_time - model_switch_state_start_time_).seconds(); - float max_pos_err = 0.0f; - for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { - float delta = rough_default_dof_pos_[i] - state.joint_pos[i]; - delta = delta - 2.0f * static_cast(M_PI) * std::floor( - (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); - max_pos_err = std::max(max_pos_err, std::abs(delta)); - } - float max_vel_err = 0.0f; - for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { - max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i])); - } - const bool hold_elapsed = elapsed >= model_switch_stand_hold_s_; - const bool stand_ready = - max_pos_err <= static_cast(model_switch_stand_max_err_) && - max_vel_err <= static_cast(model_switch_stand_max_vel_); - if (hold_elapsed && stand_ready) { - switch_start_pose_ = state.joint_pos; - switch_start_pose_[12] = switch_start_pose_[13] = switch_start_pose_[14] = switch_start_pose_[15] = 0.0f; - if (switchInferenceModel(requested_model_mode)) { - float max_dev = 0.0f; - for (std::size_t i = 0; i < 16; ++i) { - float delta = active_default_dof_pos_[i] - switch_start_pose_[i]; - if (i < sim2real_common::DeploymentContract::kLegJointCount) { - delta = delta - 2.0f * static_cast(M_PI) * std::floor( - (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); - max_dev = std::max(max_dev, std::abs(delta)); - } - switch_delta_[i] = delta; - } - const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); - active_switch_transition_s_ = std::clamp( - max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * - std::max(0.1, model_switch_to_model_transition_scale_); - const bool has_pose_delta = std::any_of( - switch_delta_.begin(), switch_delta_.end(), [](float value) { return std::abs(value) > 1.0e-4f; }); - model_switch_state_ = has_pose_delta ? ModelSwitchState::ToModelPose : ModelSwitchState::Idle; - model_switch_state_start_time_ = now_time; - if (!has_pose_delta) { - safety_reference_dof_pos_ = active_default_dof_pos_; - slow_release_after_model_switch_ = true; - } - hold_active_model_pose_when_unreleased_ = true; - publishModelStatus(); - } else { - float max_dev = 0.0f; - for (std::size_t i = 0; i < 16; ++i) { - float delta = active_default_dof_pos_[i] - switch_start_pose_[i]; - if (i < sim2real_common::DeploymentContract::kLegJointCount) { - delta = delta - 2.0f * static_cast(M_PI) * std::floor( - (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); - max_dev = std::max(max_dev, std::abs(delta)); - } - switch_delta_[i] = delta; - } - const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); - active_switch_transition_s_ = std::clamp( - max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * - std::max(0.1, model_switch_to_model_transition_scale_); - model_switch_state_ = ModelSwitchState::ToModelPose; - model_switch_state_start_time_ = now_time; - hold_active_model_pose_when_unreleased_ = true; - publishModelStatus(); - } - } else if (hold_elapsed) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 1000, - "Model switch stand hold waiting for settle: max_pos_err=%.3f rad, max_vel_err=%.3f rad/s", - max_pos_err, max_vel_err); - } - } else if (model_switch_state_ == ModelSwitchState::ToModelPose) { - const double elapsed = (now_time - model_switch_state_start_time_).seconds(); - const double to_model_transition_s = std::max(1.0e-3, active_switch_transition_s_); - const double phase = std::min(1.0, elapsed / to_model_transition_s); - const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); - for (std::size_t i = 0; i < 16; ++i) { - target.target[i] = switch_start_pose_[i] + static_cast(blend) * switch_delta_[i]; - } - target.target_source = "model_switch_to_model_pose"; - if (phase >= 1.0) { - model_switch_state_ = ModelSwitchState::Idle; - safety_reference_dof_pos_ = active_default_dof_pos_; - hold_active_model_pose_when_unreleased_ = true; - slow_release_after_model_switch_ = true; - publishModelStatus(); - } - } else if (posture_hold_mode != PostureHoldMode::None) { - const double elapsed = posture_transition_active ? - (now_time - posture_transition_start_time).seconds() : posture_transition_s; - const double phase = posture_transition_active ? - std::min(1.0, elapsed / std::max(1.0e-3, posture_transition_s)) : 1.0; - const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); - for (std::size_t i = 0; i < target.target.size(); ++i) { - target.target[i] = posture_start_pose[i] + static_cast(blend) * posture_delta[i]; - } - target.target_source = posture_hold_mode == PostureHoldMode::Keep ? - "runtime_keep_pose" : "runtime_keep_return_default"; - target.runtime_released = false; - target.release_alpha = 0.0f; - target.zero_command = true; - target.command.fill(0.0f); - target.raw_command.fill(0.0f); - target.raw_action.fill(0.0f); - target.scaled_action.fill(0.0f); - last_actions.fill(0.0f); - release_alpha_ = 0.0f; - runtime_released_ = false; - release_active_time_ = 0.0f; - - if (phase >= 1.0) { - std::scoped_lock lock(mutex_); - if (posture_hold_mode_ == posture_hold_mode) { - posture_transition_active_ = false; - if (posture_hold_mode == PostureHoldMode::ReturnDefault) { - posture_hold_mode_ = PostureHoldMode::None; - safety_reference_dof_pos_ = active_default_dof_pos_; - } else { - safety_reference_dof_pos_ = keep_pose_dof_pos_; - } - } - } - } else { // Python template uses the command directly in policy obs/release logic. // Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here. filtered_cmd_ = cmd; - const auto target_hold = computeHoldTarget(state, cmd); + const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); const bool zero_command = isZeroCommand(cmd, state.imu_gyro); if (!runtime_released_) { @@ -1882,11 +489,9 @@ void PolicyRuntimeNode::onPolicyLoop() release_active_time_ = 0.0f; } - const auto & unreleased_hold_target = - hold_active_model_pose_when_unreleased_ ? active_default_dof_pos_ : target_hold; float max_hold_err = 0.0f; for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { - max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - unreleased_hold_target[i])); + max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i])); } const bool active_ready = release_active_time_ >= release_command_hold_s_; @@ -1908,61 +513,37 @@ void PolicyRuntimeNode::onPolicyLoop() target.scaled_action.fill(0.0f); last_actions.fill(0.0f); target.target_source = "runtime_zero_hold"; - target.target = hold_active_model_pose_when_unreleased_ ? active_default_dof_pos_ : target_hold; + target.target = target_hold; if (!runtime_released_) { target.target_source = "runtime_hold"; } } else { - hold_active_model_pose_when_unreleased_ = false; - const float effective_release_scale = slow_release_after_model_switch_ ? - std::max(1.0f, model_switch_release_scale_) : 1.0f; - const float effective_command_release_s = command_release_s_ * effective_release_scale; - const float effective_release_target_blend_s = release_target_blend_s_ * effective_release_scale; - release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(effective_command_release_s, 1.0e-3f)); + release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f)); target.runtime_released = (release_alpha_ >= 1.0f); target.release_alpha = release_alpha_; target.zero_command = false; target.command = cmd; - if (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) { - target.target = computeIkCrawlTarget(state, cmd); - target.target_source = "runtime_crawl_ik"; - target.raw_action.fill(0.0f); - target.scaled_action.fill(0.0f); - last_actions.fill(0.0f); - } else { - auto raw = runPolicy(buildObservation(state, cmd, last_actions)); - for (float & v : raw) { - v *= release_alpha_; - } - target.raw_action = raw; + auto raw = runPolicy(buildObservation(state, cmd, last_actions)); + for (float & v : raw) { + v *= release_alpha_; + } + target.raw_action = raw; - const float blend = std::min( - 1.0f, - release_alpha_ * (effective_command_release_s / std::max(effective_release_target_blend_s, kPolicyDt))); - for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { - target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i]; - const float policy_target = target.scaled_action[i] + active_default_dof_pos_[i]; - target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target; - last_actions[i] = raw[i]; - } - target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy"; + const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt))); + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i]; + const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i]; + target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target; + last_actions[i] = raw[i]; } - if (release_alpha_ >= 1.0f) { - slow_release_after_model_switch_ = false; - } - } + target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy"; } } // 2) Run SafetyMonitor check on computed target if (safety_enabled_) { - auto * active_safety_monitor = - (model_switch_state_ != ModelSwitchState::Idle || posture_hold_mode != PostureHoldMode::None) && model_switch_safety_monitor_ ? - model_switch_safety_monitor_.get() : - safety_monitor_.get(); - auto safety_decision = active_safety_monitor->check( - target.target, safety_reference_dof_pos_, state.imu_gyro, state.projected_gravity, estop_active); + auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active); if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { { std::scoped_lock lock(mutex_); @@ -1970,24 +551,14 @@ void PolicyRuntimeNode::onPolicyLoop() } safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str()); - logEvent("ERROR", "safety_triggered", safety_reason_); - logProtectionEvent("safety_monitor_stop", safety_decision.message, "safety_brake"); // Override target to safety_brake damping pose - target.target = safety_reference_dof_pos_; + target.target = sim2real_common::DeploymentContract::kDefaultDofPos; target.target_source = "safety_brake"; } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { target.target = safety_decision.clipped_target; target.target_source = "safety_clip"; RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime."); - target_clip_count_++; - if (!clip_active_logged_) { - clip_active_logged_ = true; - logEvent("WARN", "target_clipped", - "protection=safety_clip, reason=" + safety_decision.message + ", source=" + target.target_source); - } - } else { - clip_active_logged_ = false; } } @@ -1999,7 +570,6 @@ void PolicyRuntimeNode::onPolicyLoop() } target_pub_->publish(target); - appendDebugTrace(state, target); } } // namespace sim2real_runtime diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/remote_uart_node.py b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/remote_uart_node.py index 5e38c97..22f4c86 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/remote_uart_node.py +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/remote_uart_node.py @@ -10,7 +10,7 @@ import rclpy from geometry_msgs.msg import Twist from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from std_msgs.msg import Bool, String +from std_msgs.msg import Bool SBUS_FRAME_SIZE = 25 SBUS_RC_MID = 1024 @@ -23,22 +23,7 @@ SWITCH_HIGH = 1 @dataclass class RemoteSwitchState: - ch5: int = SWITCH_MID - ch6: int = SWITCH_MID ch7: int = SWITCH_MID - ch8: int = SWITCH_MID - ch9: int = SWITCH_MID - ch10: int = SWITCH_MID - - def get(self, channel: int) -> Optional[int]: - return { - 5: self.ch5, - 6: self.ch6, - 7: self.ch7, - 8: self.ch8, - 9: self.ch9, - 10: self.ch10, - }.get(int(channel)) @dataclass @@ -129,14 +114,7 @@ class SbusUartReceiver: ch2=self._normalize_axis(channels[1]), ch3=self._normalize_axis(channels[3]), ch4=self._normalize_axis(channels[2]), - switches=RemoteSwitchState( - ch5=self._decode_switch(channels[4]), - ch6=self._decode_switch(channels[5]), - ch7=self._decode_switch(channels[6]), - ch8=self._decode_switch(channels[7]), - ch9=self._decode_switch(channels[8]), - ch10=self._decode_switch(channels[9]), - ), + switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])), frame_ok=True, ) if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)): @@ -176,31 +154,12 @@ class RemoteUartNode(Node): self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value) self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value) self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value) - self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).strip().upper() - self.model_switch_enabled = bool(self.declare_parameter("remote_model_switch_enabled", True).value) - self.model_switch_channel = int(self.declare_parameter("remote_model_switch_channel", 10).value) - self.model_switch_debounce_frames = max(int(self.declare_parameter("remote_model_switch_debounce_frames", 3).value), 1) - self.model_switch_rough_level = self.parse_switch_level( - str(self.declare_parameter("remote_model_switch_rough_level", "low").value) - ) - legacy_ik_level = str(self.declare_parameter("remote_model_switch_crawl_level", "").value).strip() - ik_level_default = legacy_ik_level if legacy_ik_level else "high" - self.model_switch_ik_level = self.parse_switch_level( - str(self.declare_parameter("remote_model_switch_ik_level", ik_level_default).value) - ) self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10) self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) - self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10) self.receiver: Optional[SbusUartReceiver] = None self.estop_published = False self.open_error_logged = False - self.remote_mode_active = self.default_mode == "REMOTE" - self.model_switch_candidate: Optional[int] = None - self.model_switch_candidate_count = 0 - self.model_switch_stable: Optional[int] = None - - self.create_subscription(String, "control/mode_state", self.on_mode_state, 10) if self.enabled: self.receiver = SbusUartReceiver( @@ -254,8 +213,6 @@ class RemoteUartNode(Node): self.estop_pub.publish(Bool(data=False)) self.estop_published = False - self.handle_model_switch(state) - active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4)) if active or self.publish_inactive_zero: cmd = Twist() @@ -267,15 +224,6 @@ class RemoteUartNode(Node): def publish_zero_cmd(self) -> None: self.cmd_pub.publish(Twist()) - def on_mode_state(self, msg: String) -> None: - mode = str(msg.data).strip().upper() - remote_mode_active = mode == "REMOTE" - if remote_mode_active == self.remote_mode_active: - return - - self.remote_mode_active = remote_mode_active - self.reset_model_switch_tracking() - def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float: if abs(raw_value) <= self.active_threshold: return 0.0 @@ -284,53 +232,6 @@ class RemoteUartNode(Node): scaled = -scaled return float(scaled * limit) - @staticmethod - def parse_switch_level(value: str) -> int: - normalized = value.strip().lower() - if normalized == "low": - return SWITCH_LOW - if normalized == "high": - return SWITCH_HIGH - return SWITCH_MID - - def handle_model_switch(self, state: RemoteControlState) -> None: - if not self.model_switch_enabled or not self.remote_mode_active: - return - - switch_level = state.switches.get(self.model_switch_channel) - if switch_level is None: - return - - if switch_level == self.model_switch_candidate: - self.model_switch_candidate_count += 1 - else: - self.model_switch_candidate = switch_level - self.model_switch_candidate_count = 1 - - if self.model_switch_candidate_count < self.model_switch_debounce_frames: - return - - if switch_level == self.model_switch_stable: - return - - self.model_switch_stable = switch_level - - if switch_level == self.model_switch_rough_level: - self.model_cmd_pub.publish(String(data="rough")) - self.get_logger().info( - f"Remote model switch: CH{self.model_switch_channel} -> rough" - ) - elif switch_level == self.model_switch_ik_level: - self.model_cmd_pub.publish(String(data="ik")) - self.get_logger().info( - f"Remote model switch: CH{self.model_switch_channel} -> ik" - ) - - def reset_model_switch_tracking(self) -> None: - self.model_switch_candidate = None - self.model_switch_candidate_count = 0 - self.model_switch_stable = None - def main(args: Optional[list[str]] = None) -> None: rclpy.init(args=args) diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/web_udp_bridge_node.py b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/web_udp_bridge_node.py index f6ae53b..5ac698c 100644 --- a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/web_udp_bridge_node.py +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/web_udp_bridge_node.py @@ -2,25 +2,15 @@ from __future__ import annotations import json -import math import socket -import threading -from datetime import datetime -from http import HTTPStatus -from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer -from pathlib import Path from typing import Any, Optional -from urllib.parse import urlparse import rclpy -import yaml -from geometry_msgs.msg import TransformStamped, Twist +from geometry_msgs.msg import Twist from rclpy.executors import ExternalShutdownException from rclpy.node import Node from sim2real_interfaces.msg import RuntimeState, RuntimeTarget from std_msgs.msg import Bool, String -from tf2_msgs.msg import TFMessage -from tf2_ros import Buffer, TransformBroadcaster, TransformException, TransformListener class WebUdpBridgeNode(Node): @@ -40,98 +30,6 @@ class WebUdpBridgeNode(Node): self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value) self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value) - self.http_host = str(self.declare_parameter("web_http_host", "0.0.0.0").value) - self.http_port = int(self.declare_parameter("web_http_port", 18080).value) - self.web_static_dir = str(self.declare_parameter("web_static_dir", "").value) - raw_localization_mode = str(self.declare_parameter("localization_mode", "odom").value) - self.localization_mode = self.normalize_localization_mode(raw_localization_mode) - self.odom_fallback_allowed = self.localization_mode == "odom" - 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) - self.nav_route_file = str(self.declare_parameter("nav_route_file", "").value) - self.nav_route_task_file = str(self.declare_parameter("nav_route_task_file", "").value) - self.pcd_nav_file = str(self.declare_parameter("pcd_nav_file", "").value) - self.pcd_floor_z_min = float(self.declare_parameter("pcd_floor_z_min", -1.6).value) - self.pcd_floor_z_max = float(self.declare_parameter("pcd_floor_z_max", 0.4).value) - self.pcd_sample_step = max(1, int(self.declare_parameter("pcd_sample_step", 25).value)) - self.route_align_enabled = bool( - self.declare_parameter("nav_route_auto_align_enabled", True).value - ) - self.route_rotation_offset_deg = float( - self.declare_parameter("nav_route_rotation_offset_deg", 0.0).value - ) - self.route_align_max_angle_deg = abs( - float(self.declare_parameter("nav_route_align_max_angle_deg", 6.0).value) - ) - self.route_align_angle_step_deg = max( - 0.1, - float(self.declare_parameter("nav_route_align_angle_step_deg", 0.5).value), - ) - self.route_align_search_radius = max( - 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 - ) - default_odom_trace_dir = Path(__file__).resolve().parents[3] / "map" / "load" - self.odom_trace_export_dir = str( - self.declare_parameter("odom_trace_export_dir", str(default_odom_trace_dir)).value - ) - self.odom_trace_sample_hz = max( - 0.2, - float(self.declare_parameter("odom_trace_sample_hz", 5.0).value), - ) - self.odom_trace_min_distance = max( - 0.0, - float(self.declare_parameter("odom_trace_min_distance", 0.03).value), - ) - self.odom_trace_max_points = max( - 2, - int(self.declare_parameter("odom_trace_max_points", 20000).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.odom_trace_active = False - self.odom_trace_points: list[dict[str, Any]] = [] - self.odom_trace_started_at = 0.0 - self.odom_trace_mission_name = "" - self.odom_trace_anchor: dict[str, Any] = {} - self.odom_trace_last_pose: Optional[dict[str, Any]] = None - self.odom_trace_export_path = "" - 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) self.sock.bind((self.listen_host, self.listen_port)) @@ -142,831 +40,33 @@ class WebUdpBridgeNode(Node): self.latest_target: Optional[RuntimeTarget] = None self.latest_state: Optional[RuntimeState] = None - self.latest_model_status: dict[str, Any] = { - "current_model": "rough", - "requested_model": "rough", - "switch_state": "idle", - "backend": "unknown", - "switching": False, - } self.latest_cmd = Twist() self.latest_mode = "UNKNOWN" self.latest_mux_status = "" - self.latest_nav_status = "" - self.latest_nav_path: dict[str, Any] = { - "goal_name": "", - "stage": "idle", - "path_index": 0, - "points": [], - } self.estop = False self.web_enabled = False self.last_cmd_time = self.get_clock().now() self.timeout_estop_sent = False self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10) - self.nav_cmd_pub = self.create_publisher(String, "/simple_nav/cmd", 10) self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10) self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10) self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10) self.mode_pub = self.create_publisher(String, "control/mode", 10) - self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10) - self.posture_cmd_pub = self.create_publisher(String, "runtime/posture_cmd", 10) self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10) self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10) - self.create_subscription(String, "runtime/model_status", self.on_model_status, 10) self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10) self.create_subscription(Bool, "/safety/estop", self.on_estop, 10) self.create_subscription(String, "control/mode_state", self.on_mode_state, 10) self.create_subscription(String, "control/mux_status", self.on_mux_status, 10) - self.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]] = [] - self.mission_specs: list[dict[str, Any]] = [] - self.default_mission_name: Optional[str] = None - self.task_specs: list[dict[str, Any]] = [] - self.route_alignment_info: dict[str, Any] = {} - self.route_avoid_regions: list[dict[str, Any]] = [] - self.reload_nav_task_config() - - self.http_server: Optional[ThreadingHTTPServer] = None - self.http_thread: Optional[threading.Thread] = None - self.static_dir = ( - Path(self.web_static_dir) - if self.web_static_dir - else Path(__file__).resolve().parents[3] / "tools" / "win_web_debug" / "static" - ) - self.start_http_server() self.rx_timer = self.create_timer(0.01, self.on_rx_timer) - self.state_timer = self.create_timer( - 1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, - self.on_state_timer, - ) + self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer) self.guard_timer = self.create_timer(0.05, self.on_guard_timer) - self.odom_fallback_timer = self.create_timer(0.05, self.on_odom_fallback_timer) - self.odom_trace_timer = self.create_timer( - 1.0 / self.odom_trace_sample_hz, - self.on_odom_trace_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}") - self.get_logger().info( - f"Localization mode: {self.localization_mode} " - f"(odom_fallback_allowed={self.odom_fallback_allowed})" - ) - - def load_filtered_pcd(self, path: Path) -> list[list[float]]: - if not path.exists(): - self.get_logger().warn(f"PCD file not found: {path}") - return [] - - points: list[list[float]] = [] - data_started = False - with path.open("r", encoding="utf-8") as f: - for line in f: - stripped = line.strip() - if not stripped: - continue - if data_started: - parts = stripped.split() - if len(parts) < 3: - continue - try: - x = float(parts[0]) - y = float(parts[1]) - z = float(parts[2]) - except ValueError: - continue - if self.pcd_floor_z_min <= z <= self.pcd_floor_z_max: - points.append([round(x, 3), round(y, 3)]) - elif stripped.upper().startswith("DATA"): - data_started = True - return points[:: self.pcd_sample_step] - - def _load_yaml(self, path_value: str) -> dict[str, Any]: - if not path_value: - return {} - path = Path(path_value).expanduser() - if not path.exists(): - self.get_logger().warn(f"Navigation file not found: {path}") - return {} - try: - with path.open("r", encoding="utf-8") as f: - data = yaml.safe_load(f) or {} - return data if isinstance(data, dict) else {} - except Exception as exc: - self.get_logger().warn(f"Failed to load navigation file {path}: {exc}") - return {} - - @staticmethod - def _normalize_policy(value: Any) -> Optional[str]: - text = str(value).strip().lower() - if not text: - return None - if text == "ik": - return "crawl" - if text in {"rough", "crawl", "wall"}: - return text - return None - - @staticmethod - def _normalize_task(value: Any) -> Optional[str]: - text = str(value).strip().lower() - return text or None - - @staticmethod - def normalize_localization_mode(value: Any) -> str: - text = str(value or "").strip().lower() - if text in {"relocal", "reloc", "relocalization", "localization"}: - return "relocal" - return "odom" - - @staticmethod - def _get_value(data: dict[str, Any], *keys: str) -> Any: - for key in keys: - if key in data and data.get(key) is not None: - return data.get(key) - return None - - @classmethod - def _get_float(cls, data: dict[str, Any], *keys: str) -> Optional[float]: - value = cls._get_value(data, *keys) - if value is None: - return None - try: - return float(value) - except (TypeError, ValueError): - return None - - @classmethod - def _get_bool(cls, data: dict[str, Any], *keys: str) -> Optional[bool]: - value = cls._get_value(data, *keys) - if value is None: - return None - if isinstance(value, bool): - return value - text = str(value).strip().lower() - if text in {"1", "true", "yes", "on"}: - return True - if text in {"0", "false", "no", "off"}: - return False - return None - - @classmethod - def _get_int(cls, data: dict[str, Any], *keys: str) -> Optional[int]: - value = cls._get_value(data, *keys) - if value is None: - return None - try: - return int(value) - except (TypeError, ValueError): - return None - - def load_nav_task_config( - self, - ) -> tuple[ - list[dict[str, Any]], - list[dict[str, Any]], - Optional[str], - dict[str, Any], - list[dict[str, Any]], - list[dict[str, Any]], - ]: - goals_by_name = self._load_goals(self.nav_goals_file) - missions_by_name = self._load_missions(self.nav_missions_file) - route_goals, route_missions, route_default, route_alignment, route_regions = self._load_route( - self.get_route_source_file() - ) - goals_by_name.update(route_goals) - missions_by_name.update(route_missions) - goal_specs = list(goals_by_name.values()) - mission_specs = [ - {"name": mission_name, "goals": list(goal_names)} - for mission_name, goal_names in missions_by_name.items() - ] - default_mission_name = route_default or (mission_specs[0]["name"] if mission_specs else None) - task_specs = self.build_task_specs(goal_specs, mission_specs, default_mission_name) - return goal_specs, mission_specs, default_mission_name, route_alignment, route_regions, task_specs - - def reload_nav_task_config(self) -> None: - ( - self.goal_specs, - self.mission_specs, - self.default_mission_name, - self.route_alignment_info, - self.route_avoid_regions, - self.task_specs, - ) = self.load_nav_task_config() - - def build_task_specs( - self, - goal_specs: list[dict[str, Any]], - mission_specs: list[dict[str, Any]], - default_mission_name: Optional[str], - ) -> list[dict[str, Any]]: - if not default_mission_name: - return [] - goal_lookup = {str(goal.get("name", "")): goal for goal in goal_specs if goal.get("name")} - default_mission = next( - (mission for mission in mission_specs if mission.get("name") == default_mission_name), - None, - ) - if not default_mission: - return [] - - tasks: list[dict[str, Any]] = [] - task_by_name: dict[str, dict[str, Any]] = {} - for index, goal_name in enumerate(default_mission.get("goals", []), start=1): - goal = goal_lookup.get(str(goal_name)) - if not goal: - continue - task_name = self._normalize_task(goal.get("task")) - if not task_name: - continue - spec = task_by_name.get(task_name) - if spec is None: - spec = { - "name": task_name, - "start_index": index, - "end_index": index, - "count": 0, - "first_goal": str(goal_name), - "last_goal": str(goal_name), - "first_id": goal.get("id"), - "last_id": goal.get("id"), - "goals": [], - "policies": [], - } - task_by_name[task_name] = spec - tasks.append(spec) - spec["end_index"] = index - spec["count"] = int(spec["count"]) + 1 - spec["last_goal"] = str(goal_name) - spec["last_id"] = goal.get("id") - spec["goals"].append(str(goal_name)) - policy = goal.get("policy") - if policy and policy not in spec["policies"]: - spec["policies"].append(policy) - return tasks - - def get_route_source_file(self) -> str: - candidate = self.nav_route_task_file.strip() if self.nav_route_task_file else "" - if candidate: - return candidate - return self.nav_route_file - - def _load_goals(self, path_value: str) -> dict[str, dict[str, Any]]: - data = self._load_yaml(path_value) - raw_goals = data.get("goals", {}) - parsed: dict[str, dict[str, Any]] = {} - if not isinstance(raw_goals, dict): - return parsed - for name, spec in raw_goals.items(): - if not isinstance(spec, dict): - continue - pos = spec.get("position", [0.0, 0.0, 0.0]) - if not isinstance(pos, list) or len(pos) < 2: - continue - try: - yaw_tolerance_deg = self._get_float(spec, "yaw_tolerance_deg", "yawToleranceDeg") - yaw_deg = self._get_float(spec, "yaw_deg", "yawDeg") - require_yaw = self._get_bool(spec, "require_yaw", "requireYaw") - parsed[str(name)] = { - "name": str(name), - "id": self._get_int(spec, "id", "waypoint_id", "waypointId"), - "task": self._normalize_task(spec.get("task")), - "segment": self._normalize_task(spec.get("segment")), - "x": float(pos[0]), - "y": float(pos[1]), - "yaw_deg": yaw_deg, - "yaw_tolerance_deg": yaw_tolerance_deg, - "tolerance": float(spec.get("tolerance", 0.20)), - "policy": self._normalize_policy(spec.get("policy")), - "speed": float(spec["speed"]) if spec.get("speed") is not None else None, - "require_yaw": bool(yaw_deg is not None if require_yaw is None else require_yaw) and yaw_deg is not None, - "pre_dock_distance": self._get_float(spec, "pre_dock_distance", "preDockDistance"), - "pre_dock_tolerance": self._get_float(spec, "pre_dock_tolerance", "preDockTolerance"), - } - except (TypeError, ValueError): - continue - return parsed - - def _load_missions(self, path_value: str) -> dict[str, list[str]]: - data = self._load_yaml(path_value) - raw_missions = data.get("missions", {}) - parsed: dict[str, list[str]] = {} - if not isinstance(raw_missions, dict): - return parsed - for name, spec in raw_missions.items(): - if not isinstance(spec, dict): - continue - goals = spec.get("goals", []) - if isinstance(goals, list) and goals and all(isinstance(item, str) for item in goals): - parsed[str(name)] = [str(item) for item in goals] - return parsed - - def _load_route( - self, path_value: str - ) -> tuple[dict[str, dict[str, Any]], dict[str, list[str]], Optional[str], dict[str, Any], list[dict[str, Any]]]: - data = self._load_yaml(path_value) - if not data: - return {}, {}, None, {}, [] - - route_name = str(data.get("name", "")).strip() or Path(path_value).stem - route_default_yaw_tolerance_deg = self._get_float( - data, - "yaw_tolerance_deg_default", - "yawToleranceDegDefault", - ) - route_default_require_yaw = self._get_bool( - data, - "require_yaw_default", - "requireYawDefault", - ) - route_default_pre_dock_distance = self._get_float( - data, - "pre_dock_distance_default", - "preDockDistanceDefault", - ) - route_default_pre_dock_tolerance = self._get_float( - data, - "pre_dock_tolerance_default", - "preDockToleranceDefault", - ) - raw_segments = data.get("segments", []) - if not isinstance(raw_segments, list) or not raw_segments: - top_level_waypoints = data.get("waypoints", []) - if isinstance(top_level_waypoints, list) and top_level_waypoints: - raw_segments = [{ - "name": "segment_1", - "obstacle": str(data.get("obstacle", "flat") or "flat"), - "waypoints": top_level_waypoints, - }] - else: - return {}, {}, None, {}, [] - - raw_waypoints: list[dict[str, Any]] = [] - for segment_index, segment in enumerate(raw_segments, start=1): - if not isinstance(segment, dict): - continue - segment_name = str(segment.get("name", f"segment_{segment_index}")).strip() or f"segment_{segment_index}" - waypoints = segment.get("waypoints", []) - if not isinstance(waypoints, list): - continue - for waypoint_index, waypoint in enumerate(waypoints, start=1): - if not isinstance(waypoint, dict): - continue - try: - waypoint_yaw_deg = self._get_float(waypoint, "yaw_deg", "yawDeg") - waypoint_yaw_tolerance_deg = self._get_float( - waypoint, - "yaw_tolerance_deg", - "yawToleranceDeg", - ) - waypoint_require_yaw = self._get_bool( - waypoint, - "require_yaw", - "requireYaw", - ) - if waypoint_require_yaw is None: - waypoint_require_yaw = bool(route_default_require_yaw) if route_default_require_yaw is not None else False - if waypoint_yaw_tolerance_deg is None: - waypoint_yaw_tolerance_deg = route_default_yaw_tolerance_deg - waypoint_pre_dock_distance = self._get_float( - waypoint, - "pre_dock_distance", - "preDockDistance", - ) - if waypoint_pre_dock_distance is None: - waypoint_pre_dock_distance = route_default_pre_dock_distance - waypoint_pre_dock_tolerance = self._get_float( - waypoint, - "pre_dock_tolerance", - "preDockTolerance", - ) - if waypoint_pre_dock_tolerance is None: - waypoint_pre_dock_tolerance = route_default_pre_dock_tolerance - raw_waypoints.append( - { - "segment": segment_name, - "id": int(waypoint.get("id", waypoint_index)), - "task": self._normalize_task(waypoint.get("task", segment.get("obstacle", ""))), - "x": float(waypoint["x"]), - "y": float(waypoint["y"]), - "yaw_deg": waypoint_yaw_deg, - "yaw_tolerance_deg": waypoint_yaw_tolerance_deg, - "tolerance": float(waypoint.get("tolerance", 0.20)), - "policy": self._normalize_policy(waypoint.get("policy")), - "speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None, - "require_yaw": bool(waypoint_require_yaw) and waypoint_yaw_deg is not None, - "pre_dock_distance": waypoint_pre_dock_distance, - "pre_dock_tolerance": waypoint_pre_dock_tolerance, - } - ) - except (KeyError, TypeError, ValueError): - continue - - if not raw_waypoints: - return {}, {}, None, {}, [] - - aligned_waypoints, alignment_info = self._align_route_waypoints(raw_waypoints) - applied_angle_rad = math.radians(float(alignment_info.get("applied_deg", 0.0))) if alignment_info else 0.0 - aligned_regions = self._rotate_avoid_regions( - self._load_avoid_regions(data), - float(raw_waypoints[0]["x"]), - float(raw_waypoints[0]["y"]), - applied_angle_rad, - ) - goals: dict[str, dict[str, Any]] = {} - mission_goal_names: list[str] = [] - for index, waypoint in enumerate(aligned_waypoints, start=1): - goal_name = f"{route_name}_p{index:02d}" - goals[goal_name] = { - "name": goal_name, - "id": int(waypoint["id"]) if waypoint.get("id") is not None else None, - "task": self._normalize_task(waypoint.get("task")), - "segment": str(waypoint.get("segment", "") or "") or None, - "x": round(float(waypoint["x"]), 3), - "y": round(float(waypoint["y"]), 3), - "yaw_deg": round(float(waypoint["yaw_deg"]), 3) if waypoint.get("yaw_deg") is not None else None, - "yaw_tolerance_deg": round(float(waypoint["yaw_tolerance_deg"]), 3) - if waypoint.get("yaw_tolerance_deg") is not None else None, - "tolerance": round(float(waypoint.get("tolerance", 0.20)), 3), - "policy": self._normalize_policy(waypoint.get("policy")), - "speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None, - "require_yaw": bool(waypoint.get("require_yaw", False)), - "pre_dock_distance": round(float(waypoint["pre_dock_distance"]), 3) - if waypoint.get("pre_dock_distance") is not None else None, - "pre_dock_tolerance": round(float(waypoint["pre_dock_tolerance"]), 3) - if waypoint.get("pre_dock_tolerance") is not None else None, - } - mission_goal_names.append(goal_name) - - return goals, {route_name: mission_goal_names}, route_name, alignment_info, aligned_regions - - def _load_avoid_regions(self, data: dict[str, Any]) -> list[dict[str, Any]]: - rows = data.get("regions") - if not isinstance(rows, list) or not rows: - rows = data.get("avoid_regions", []) - if not isinstance(rows, list): - return [] - - regions: list[dict[str, Any]] = [] - for index, row in enumerate(rows, start=1): - if not isinstance(row, dict): - continue - kind = str(row.get("kind", "avoid")).strip().lower() - if kind not in {"avoid", "no_go", "no-go", "nogo", "forbidden", "blocked"}: - continue - polygon_rows = row.get("polygon", []) - if not isinstance(polygon_rows, list): - continue - polygon: list[dict[str, float]] = [] - for point in polygon_rows: - try: - if isinstance(point, dict): - polygon.append({"x": float(point["x"]), "y": float(point["y"])}) - elif isinstance(point, (list, tuple)) and len(point) >= 2: - polygon.append({"x": float(point[0]), "y": float(point[1])}) - except (KeyError, TypeError, ValueError): - continue - if len(polygon) >= 3: - regions.append( - { - "name": str(row.get("name", f"avoid_{index}")), - "kind": kind, - "polygon": polygon, - } - ) - return regions - - def _rotate_avoid_regions( - self, - regions: list[dict[str, Any]], - anchor_x: float, - anchor_y: float, - angle_rad: float, - ) -> list[dict[str, Any]]: - if not regions or abs(angle_rad) <= 1.0e-12: - return regions - rotated: list[dict[str, Any]] = [] - for region in regions: - polygon = [] - for point in region.get("polygon", []): - x, y = self._rotate_xy(float(point["x"]), float(point["y"]), anchor_x, anchor_y, angle_rad) - polygon.append({"x": x, "y": y}) - rotated.append({**region, "polygon": polygon}) - return rotated - - def _align_route_waypoints( - self, waypoints: list[dict[str, Any]] - ) -> tuple[list[dict[str, Any]], dict[str, Any]]: - if not waypoints: - return [], {} - - if not self.map_points: - info = { - "enabled": False, - "reason": "pcd unavailable", - "manual_offset_deg": round(self.route_rotation_offset_deg, 3), - "auto_offset_deg": 0.0, - "applied_deg": round(self.route_rotation_offset_deg, 3), - "hits": 0, - "total": len(waypoints), - } - return self._rotate_waypoints(waypoints, math.radians(self.route_rotation_offset_deg)), info - - manual_offset_deg = self.route_rotation_offset_deg - if not self.route_align_enabled or len(waypoints) < 2: - info = { - "enabled": False, - "reason": "auto align disabled", - "manual_offset_deg": round(manual_offset_deg, 3), - "auto_offset_deg": 0.0, - "applied_deg": round(manual_offset_deg, 3), - "hits": 0, - "total": len(waypoints), - } - return self._rotate_waypoints(waypoints, math.radians(manual_offset_deg)), info - - anchor_x = float(waypoints[0]["x"]) - anchor_y = float(waypoints[0]["y"]) - search_radius_sq = self.route_align_search_radius * self.route_align_search_radius - best_hits = -1 - best_score = float("inf") - best_angle_deg = manual_offset_deg - manual_hits = -1 - manual_score = float("inf") - steps = max( - 1, - int(round((self.route_align_max_angle_deg * 2.0) / self.route_align_angle_step_deg)), - ) - - for step_index in range(steps + 1): - auto_delta_deg = -self.route_align_max_angle_deg + step_index * self.route_align_angle_step_deg - angle_deg = manual_offset_deg + auto_delta_deg - angle_rad = math.radians(angle_deg) - hits = 0 - score = 0.0 - for waypoint in waypoints: - tx, ty = self._rotate_xy( - float(waypoint["x"]), - float(waypoint["y"]), - anchor_x, - anchor_y, - angle_rad, - ) - nearest_sq = search_radius_sq - for px, py in self.map_points: - dx = px - tx - dy = py - ty - dist_sq = dx * dx + dy * dy - if dist_sq < nearest_sq: - nearest_sq = dist_sq - if nearest_sq < search_radius_sq: - hits += 1 - score += nearest_sq - if abs(angle_deg - manual_offset_deg) <= 1.0e-9: - manual_hits = hits - manual_score = score - better_hits = hits > best_hits - better_score = hits == best_hits and ( - score < best_score - 1.0e-9 - or ( - abs(score - best_score) <= 1.0e-9 - and abs(auto_delta_deg) < abs(best_angle_deg - manual_offset_deg) - ) - ) - if better_hits or better_score: - best_hits = hits - best_score = score - best_angle_deg = angle_deg - - hits_improved = best_hits > manual_hits - score_improvement = ( - (manual_score - best_score) / max(manual_score, 1.0e-9) - if manual_score < float("inf") - else 0.0 - ) - if not hits_improved and score_improvement < 0.05: - applied_angle_deg = manual_offset_deg - rotated = self._rotate_waypoints(waypoints, math.radians(applied_angle_deg)) - info = { - "enabled": True, - "reason": "ambiguous-auto-align", - "manual_offset_deg": round(manual_offset_deg, 3), - "auto_offset_deg": 0.0, - "applied_deg": round(applied_angle_deg, 3), - "score": round(manual_score, 6) if manual_score < float("inf") else 0.0, - "hits": int(manual_hits if manual_hits >= 0 else 0), - "total": len(waypoints), - } - return rotated, info - - rotated = self._rotate_waypoints(waypoints, math.radians(best_angle_deg)) - info = { - "enabled": True, - "manual_offset_deg": round(manual_offset_deg, 3), - "auto_offset_deg": round(best_angle_deg - manual_offset_deg, 3), - "applied_deg": round(best_angle_deg, 3), - "score": round(best_score, 6), - "hits": int(best_hits), - "total": len(waypoints), - } - return rotated, info - - def _rotate_waypoints(self, waypoints: list[dict[str, Any]], angle_rad: float) -> list[dict[str, Any]]: - if not waypoints: - return [] - anchor_x = float(waypoints[0]["x"]) - anchor_y = float(waypoints[0]["y"]) - rotated: list[dict[str, Any]] = [] - for waypoint in waypoints: - x, y = self._rotate_xy( - float(waypoint["x"]), - float(waypoint["y"]), - anchor_x, - anchor_y, - angle_rad, - ) - updated = dict(waypoint) - updated["x"] = x - updated["y"] = y - if updated.get("yaw_deg") is not None: - updated["yaw_deg"] = round( - math.degrees( - self.normalize_angle(math.radians(float(updated["yaw_deg"])) + angle_rad) - ), - 3, - ) - rotated.append(updated) - return rotated - - @staticmethod - def _rotate_xy(x: float, y: float, anchor_x: float, anchor_y: float, angle_rad: float) -> tuple[float, float]: - dx = x - anchor_x - dy = y - anchor_y - cos_a = math.cos(angle_rad) - sin_a = math.sin(angle_rad) - return ( - anchor_x + dx * cos_a - dy * sin_a, - anchor_y + dx * sin_a + dy * cos_a, - ) - - def lookup_pose(self) -> Optional[dict[str, float]]: - try: - transform = self.tf_buffer.lookup_transform( - self.nav_map_frame, - self.nav_base_frame, - rclpy.time.Time(), - ) - except TransformException: - return None - t = transform.transform.translation - q = transform.transform.rotation - yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w) - return {"x": round(float(t.x), 3), "y": round(float(t.y), 3), "yaw": round(float(yaw), 6)} - - def start_http_server(self) -> None: - node = self - static_dir = self.static_dir - - class Handler(BaseHTTPRequestHandler): - def do_GET(self): - request_path = urlparse(self.path).path - if request_path in ["/", "/index.html"]: - self.serve_file(static_dir / "index.html", "text/html; charset=utf-8") - elif request_path == "/app.js": - self.serve_file(static_dir / "app.js", "application/javascript; charset=utf-8") - elif request_path == "/style.css": - self.serve_file(static_dir / "style.css", "text/css; charset=utf-8") - elif request_path == "/api/state": - self.send_json(node.build_state_packet()) - elif request_path == "/api/map": - self.send_json(node.build_map_packet()) - else: - self.send_error(HTTPStatus.NOT_FOUND) - - def do_POST(self): - if self.path != "/api/control": - self.send_error(HTTPStatus.NOT_FOUND) - return - try: - length = int(self.headers.get("Content-Length", "0")) - raw = self.rfile.read(length) - payload = json.loads(raw.decode("utf-8")) - node.handle_http_control(payload) - self.send_json({"ok": True}) - except Exception as exc: - self.send_json( - {"ok": False, "error": str(exc)}, - status=HTTPStatus.BAD_REQUEST, - ) - - def serve_file(self, path: Path, content_type: str): - if not path.exists(): - self.send_error(HTTPStatus.NOT_FOUND) - return - data = path.read_bytes() - self.send_response(HTTPStatus.OK) - self.send_header("Content-Type", content_type) - self.send_header("Content-Length", str(len(data))) - self.end_headers() - self.wfile.write(data) - - def send_json(self, payload: dict[str, Any], status: HTTPStatus = HTTPStatus.OK): - data = json.dumps(payload, separators=(",", ":")).encode("utf-8") - self.send_response(status) - self.send_header("Content-Type", "application/json") - self.send_header("Content-Length", str(len(data))) - self.end_headers() - self.wfile.write(data) - - def log_message(self, format: str, *args): - return - - self.http_server = ThreadingHTTPServer((self.http_host, self.http_port), Handler) - self.http_thread = threading.Thread(target=self.http_server.serve_forever, daemon=True) - self.http_thread.start() - - def build_odom_trace_packet(self) -> dict[str, Any]: - points = [[self._f(point["x"]), self._f(point["y"])] for point in self.odom_trace_points] - max_display_points = 240 - if len(points) > max_display_points: - stride = max(1, math.ceil(len(points) / max_display_points)) - points = points[::stride] - return { - "active": self.odom_trace_active, - "count": len(self.odom_trace_points), - "display_count": len(points), - "points": points, - "mission": self.odom_trace_mission_name, - "export_path": self.odom_trace_export_path, - "sample_hz": self.odom_trace_sample_hz, - "min_distance": self.odom_trace_min_distance, - } - def build_map_packet(self) -> dict[str, Any]: - 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, - "mission_specs": self.mission_specs, - "default_mission_name": self.default_mission_name, - "default_mission_goals": default_goals, - "task_specs": self.task_specs, - "route_alignment": self.route_alignment_info, - "avoid_regions": self.route_avoid_regions, - "localization_mode": self.localization_mode, - "odom_fallback_allowed": self.odom_fallback_allowed, - "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), - "odom_trace": self.build_odom_trace_packet(), - } - - def handle_http_control(self, payload: dict[str, Any]) -> None: - msg_type = str(payload.get("type", "")).lower() - if msg_type in { - "cmd_vel", - "zero", - "estop", - "mode", - "web_enable", - "remote_enable", - "nav_enable", - "ping", - "nav_cmd", - "nav_stop_keep", - "task_resume", - "task_only", - "task_skip", - "go_to", - "go_rel", - "odom_task", - "odom_stop", - "estop_reset", - "model_toggle", - "model_cmd", - }: - self.handle_packet(payload) - return - raise ValueError(f"unknown control type: {msg_type}") def on_target(self, msg: RuntimeTarget) -> None: self.latest_target = msg @@ -974,30 +74,11 @@ class WebUdpBridgeNode(Node): def on_state(self, msg: RuntimeState) -> None: self.latest_state = msg - def on_model_status(self, msg: String) -> None: - try: - data = json.loads(msg.data) if msg.data else {} - if isinstance(data, dict): - self.latest_model_status = { - "current_model": str(data.get("current_model", "rough")), - "requested_model": str( - data.get("requested_model", data.get("current_model", "rough")) - ), - "switch_state": str(data.get("switch_state", "idle")), - "backend": str(data.get("backend", "unknown")), - "switching": bool(data.get("switching", False)), - } - except Exception as exc: - self.get_logger().warn(f"Failed to parse model status: {exc}") - def on_cmd_vel(self, msg: Twist) -> None: self.latest_cmd = msg def on_estop(self, msg: Bool) -> None: - was_estop = self.estop self.estop = bool(msg.data) - if self.estop and not was_estop: - self.stop_navigation_for_keep("estop requested", enter_keep=False) def on_mode_state(self, msg: String) -> None: self.latest_mode = msg.data @@ -1005,95 +86,6 @@ class WebUdpBridgeNode(Node): def on_mux_status(self, msg: String) -> None: self.latest_mux_status = msg.data - def on_nav_status(self, msg: String) -> None: - self.latest_nav_status = msg.data - if msg.data.startswith("reloaded goals="): - self.reload_nav_task_config() - - 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), - "received_at": round(self.now_sec(), 3), - } - 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: - data = json.loads(msg.data) if msg.data else {} - except Exception: - return - if not isinstance(data, dict): - return - raw_points = data.get("points", []) - points: list[list[float]] = [] - if isinstance(raw_points, list): - for item in raw_points: - if ( - isinstance(item, (list, tuple)) - and len(item) >= 2 - ): - try: - points.append([round(float(item[0]), 3), round(float(item[1]), 3)]) - except (TypeError, ValueError): - continue - try: - path_index = max(0, int(data.get("path_index", 0) or 0)) - except (TypeError, ValueError): - path_index = 0 - self.latest_nav_path = { - "goal_name": str(data.get("goal_name", "")), - "stage": str(data.get("stage", "idle")), - "path_index": path_index, - "points": points, - } - def on_rx_timer(self) -> None: if not self.enabled: return @@ -1116,7 +108,6 @@ class WebUdpBridgeNode(Node): msg_type = str(payload.get("type", "")).lower() if msg_type == "cmd_vel": cmd = self.parse_twist(payload) - self.set_control_mode("WEB") self.cmd_pub.publish(cmd) self.last_cmd_time = self.get_clock().now() self.timeout_estop_sent = False @@ -1124,449 +115,34 @@ class WebUdpBridgeNode(Node): self.cmd_pub.publish(Twist()) self.last_cmd_time = self.get_clock().now() elif msg_type == "estop": - self.handle_estop_request(bool(payload.get("data", True))) + self.estop_pub.publish(Bool(data=bool(payload.get("data", True)))) elif msg_type == "mode": mode = str(payload.get("mode", "DISABLED")).upper() - self.set_control_mode(mode) + self.mode_pub.publish(String(data=mode)) + self.web_enabled = mode == "WEB" + self.web_enabled_pub.publish(Bool(data=mode == "WEB")) + self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE")) + self.nav_enabled_pub.publish(Bool(data=mode == "NAV")) elif msg_type == "web_enable": self.web_enabled = bool(payload.get("data", False)) self.web_enabled_pub.publish(Bool(data=self.web_enabled)) if self.web_enabled: - self.set_control_mode("WEB") + self.mode_pub.publish(String(data="WEB")) elif msg_type == "remote_enable": enabled = bool(payload.get("data", False)) self.remote_enabled_pub.publish(Bool(data=enabled)) if enabled: - self.set_control_mode("REMOTE") + self.mode_pub.publish(String(data="REMOTE")) elif msg_type == "nav_enable": enabled = bool(payload.get("data", False)) self.nav_enabled_pub.publish(Bool(data=enabled)) if enabled: - self.set_control_mode("NAV") - elif msg_type == "nav_cmd": - command = str(payload.get("command", "")).strip() - if command: - self.set_control_mode("NAV") - self.nav_cmd_pub.publish(String(data=command)) - elif msg_type == "nav_stop_keep": - self.stop_navigation_for_keep("manual stop") - elif msg_type == "task_resume": - task_name = str(payload.get("task", "")).strip() - if task_name: - self.ensure_navigation_pose(f"task resume {task_name}") - self.set_control_mode("NAV") - self.nav_cmd_pub.publish(String(data="reload")) - self.nav_cmd_pub.publish(String(data=f"run_from_task {task_name}")) - elif msg_type == "task_only": - task_name = str(payload.get("task", "")).strip() - if task_name: - self.ensure_navigation_pose(f"task only {task_name}") - self.set_control_mode("NAV") - self.nav_cmd_pub.publish(String(data="reload")) - self.nav_cmd_pub.publish(String(data=f"run_only_task {task_name}")) - elif msg_type == "task_skip": - task_name = str(payload.get("task", "")).strip() - self.ensure_navigation_pose(f"task skip {task_name or 'current'}") - self.set_control_mode("NAV") - command = f"skip_task {task_name}" if task_name else "skip_task" - self.nav_cmd_pub.publish(String(data="reload")) - self.nav_cmd_pub.publish(String(data=command)) - elif msg_type == "go_to": - x = float(payload.get("x", 0.0)) - y = float(payload.get("y", 0.0)) - self.set_control_mode("NAV") - self.nav_cmd_pub.publish(String(data=f"go {x:.3f} {y:.3f}")) - elif msg_type == "go_rel": - dx = float(payload.get("dx", 0.0)) - 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 == "estop_reset": - self.handle_estop_request(False) - elif msg_type == "model_toggle": - self.model_cmd_pub.publish(String(data="toggle")) - elif msg_type == "model_cmd": - command = str(payload.get("command", "")).strip() - if command: - self.model_cmd_pub.publish(String(data=command)) + self.mode_pub.publish(String(data="NAV")) elif msg_type == "ping": self.send_packet({"type": "pong", "stamp": self.now_sec()}) - elif msg_type == "map_request": - packet = self.build_map_packet() - packet["type"] = "map" - self.send_packet(packet) else: self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"}) - def set_control_mode(self, mode: str) -> None: - mode = str(mode).upper() - self.mode_pub.publish(String(data=mode)) - self.web_enabled = mode == "WEB" - self.web_enabled_pub.publish(Bool(data=mode == "WEB")) - self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE")) - self.nav_enabled_pub.publish(Bool(data=mode == "NAV")) - if mode == "KEEP": - self.posture_cmd_pub.publish(String(data="keep")) - elif mode in {"REMOTE", "NAV", "WEB"}: - self.posture_cmd_pub.publish(String(data="default")) - - def stop_navigation_for_keep(self, reason: str, enter_keep: bool = True) -> None: - self.nav_cmd_pub.publish(String(data="stop")) - if enter_keep: - self.set_control_mode("KEEP") - - def handle_estop_request(self, active: bool) -> None: - if active: - self.stop_navigation_for_keep("estop requested", enter_keep=False) - self.estop_pub.publish(Bool(data=True)) - else: - self.estop_pub.publish(Bool(data=False)) - self.stop_navigation_for_keep("estop reset") - - 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 get_current_task_name(self) -> str: - status = self.latest_nav_status or "" - marker = "task=" - marker_index = status.find(marker) - if marker_index >= 0: - tail = status[marker_index + len(marker):].strip() - task = tail.split()[0].strip() if tail else "" - if task: - return task - active_goal_name = str(self.latest_nav_path.get("goal_name", "") or "") - if active_goal_name: - for goal in self.goal_specs: - if goal.get("name") == active_goal_name: - return str(goal.get("task") or "") - return "" - - def ensure_navigation_pose(self, reason: str) -> None: - if self.lookup_pose() is not None: - return - if self.odom_fallback_active: - return - if not self.odom_fallback_allowed: - raise ValueError( - f"{reason} waiting for relocalization TF {self.nav_map_frame}->{self.nav_base_frame}; " - "move the robot until reloc shows on/tf ok" - ) - - self.reload_nav_task_config() - default_goals = self.get_default_mission_goals() - if not self.default_mission_name or not default_goals: - raise ValueError(f"{reason} failed: 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")), - ) - if not self.odom_trace_active: - self.start_odom_trace(self.default_mission_name or "odom_task", anchor_goal) - self.get_logger().info(f"{reason}: started odom fallback anchor before navigation command") - - def start_odom_fallback_task(self) -> None: - if not self.odom_fallback_allowed: - raise ValueError("odom task unavailable in relocalization mode; wait for reloc on/tf ok then use resume") - self.reload_nav_task_config() - self.ensure_navigation_pose("odom task") - if not self.default_mission_name: - raise ValueError("odom task unavailable: no default mission loaded") - 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 start_odom_trace(self, mission_name: str, anchor_goal: dict[str, Any]) -> None: - self.odom_trace_active = True - self.odom_trace_points = [] - self.odom_trace_started_at = self.now_sec() - self.odom_trace_mission_name = mission_name - self.odom_trace_last_pose = None - self.odom_trace_export_path = "" - self.odom_trace_anchor = { - "name": str(anchor_goal.get("name", "route_p01")), - "x": self._f(anchor_goal.get("x", 0.0)), - "y": self._f(anchor_goal.get("y", 0.0)), - "yaw_deg": self._f(anchor_goal.get("yaw_deg", anchor_goal.get("yawDeg", 0.0))), - } - self.record_odom_trace_sample(force=True) - self.get_logger().info( - f"Odom trace recording started: mission={self.odom_trace_mission_name}, " - f"export_dir={self.odom_trace_export_dir}" - ) - - def on_odom_trace_timer(self) -> None: - if self.odom_trace_active: - self.record_odom_trace_sample(force=False) - - def record_odom_trace_sample(self, force: bool = False) -> None: - if not self.odom_trace_active and not force: - return - if len(self.odom_trace_points) >= self.odom_trace_max_points: - return - pose = self.lookup_pose() - if pose is None: - return - now_sec = self.now_sec() - x = float(pose.get("x", 0.0)) - y = float(pose.get("y", 0.0)) - yaw = float(pose.get("yaw", 0.0)) - if self.odom_trace_last_pose is not None and not force: - dx = x - float(self.odom_trace_last_pose.get("x", 0.0)) - dy = y - float(self.odom_trace_last_pose.get("y", 0.0)) - if math.hypot(dx, dy) < self.odom_trace_min_distance: - return - sample = { - "t": round(now_sec - self.odom_trace_started_at, 3), - "x": round(x, 4), - "y": round(y, 4), - "yaw": round(yaw, 6), - "yawDeg": round(math.degrees(yaw), 3), - } - self.odom_trace_points.append(sample) - self.odom_trace_last_pose = sample - - def finish_odom_trace(self, reason: str) -> Optional[Path]: - if not self.odom_trace_active and not self.odom_trace_points: - return None - if self.odom_trace_active: - self.record_odom_trace_sample(force=True) - self.odom_trace_active = False - if not self.odom_trace_points: - return None - - try: - export_dir = Path(self.odom_trace_export_dir).expanduser() - export_dir.mkdir(parents=True, exist_ok=True) - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - safe_mission = "".join( - ch if ch.isalnum() or ch in {"_", "-"} else "_" - for ch in (self.odom_trace_mission_name or "odom") - ).strip("_") or "odom" - export_path = export_dir / f"odom_trace_{safe_mission}_{stamp}.json" - waypoints = [] - for index, point in enumerate(self.odom_trace_points, start=1): - waypoints.append({ - "id": index, - "x": point["x"], - "y": point["y"], - "world_x": point["x"], - "world_y": point["y"], - "yawDeg": point["yawDeg"], - "speed": 0.0, - "policy": "odom_trace", - "tolerance": 0.0, - }) - payload = { - "name": f"odom_trace_{safe_mission}_{stamp}", - "type": "odom_trace", - "created_at": datetime.now().isoformat(timespec="seconds"), - "frame": self.nav_map_frame, - "base_frame": self.nav_base_frame, - "odom_frame": self.nav_odom_frame, - "mission": self.odom_trace_mission_name, - "reason": reason, - "source_route_file": self.get_route_source_file(), - "anchor": dict(self.odom_trace_anchor), - "origin": {"mode": "world", "x": 0.0, "y": 0.0, "yaw_deg": 0.0}, - "sample_hz": self.odom_trace_sample_hz, - "min_distance": self.odom_trace_min_distance, - "samples": self.odom_trace_points, - "waypoints": waypoints, - "segments": [{"name": "odom_trace", "obstacle": "odom_trace", "waypoints": waypoints}], - } - export_path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8") - except Exception as exc: - self.get_logger().error(f"Odom trace export failed: {exc}") - self.odom_trace_export_path = "" - return None - self.odom_trace_export_path = str(export_path) - self.get_logger().info( - f"Odom trace exported: {export_path} points={len(self.odom_trace_points)} reason={reason}" - ) - return export_path - - def validate_odom_fallback_preconditions(self) -> None: - if not self.odom_fallback_allowed: - raise ValueError("odom fallback disabled in relocalization mode") - 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 - if was_active: - self.finish_odom_trace(reason) - 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 {} @@ -1597,34 +173,11 @@ class WebUdpBridgeNode(Node): "stamp": self.now_sec(), "mode": self.latest_mode, "mux_status": self.latest_mux_status, - "nav_status": self.latest_nav_status, "estop": self.estop, "web_enabled": self.web_enabled, - "connected": True, - "local_receive_time": self.now_sec(), "cmd_vel": self.twist_to_dict(self.latest_cmd), "runtime": {}, "robot": {}, - "nav": { - "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), - "current_task": self.get_current_task_name(), - "task_specs": self.task_specs, - "localization_mode": self.localization_mode, - "odom_fallback_allowed": self.odom_fallback_allowed, - "odom_trace": self.build_odom_trace_packet(), - "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, - }, - "relocalization": self.build_relocalization_packet(), - }, - "model": dict(self.latest_model_status), } if target is not None: packet["runtime"] = { @@ -1657,20 +210,6 @@ class WebUdpBridgeNode(Node): } return packet - def build_relocalization_packet(self) -> dict[str, Any]: - external_tf = dict(self.last_external_map_odom_tf or {}) - received_at = self._f(external_tf.get("received_at")) if external_tf else 0.0 - age_s = self.now_sec() - received_at if received_at > 0.0 else 0.0 - return { - "localization_mode": self.localization_mode, - "odom_fallback_allowed": self.odom_fallback_allowed, - "external_map_odom_tf": external_tf, - "external_tf_seen": bool(external_tf), - "external_tf_age_s": self._f(age_s), - "handoff_pending": self.odom_fallback_handoff_pending, - "handoff": dict(self.odom_fallback_handoff_info), - } - def send_packet(self, payload: dict[str, Any]) -> None: if self.client_addr is None: return @@ -1693,13 +232,6 @@ 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 { @@ -1707,16 +239,6 @@ class WebUdpBridgeNode(Node): "angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z}, } - @staticmethod - def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: - siny_cosp = 2.0 * (w * z + x * y) - cosy_cosp = 1.0 - 2.0 * (y * y + z * z) - return math.atan2(siny_cosp, cosy_cosp) - - @staticmethod - def normalize_angle(angle: float) -> float: - return (angle + math.pi) % (2.0 * math.pi) - math.pi - @staticmethod def clamp(value: float, low: float, high: float) -> float: return max(low, min(high, value)) @@ -1730,9 +252,6 @@ def main(args: Optional[list[str]] = None) -> None: except (KeyboardInterrupt, ExternalShutdownException): pass finally: - if node.http_server is not None: - node.http_server.shutdown() - node.http_server.server_close() node.destroy_node() if rclpy.ok(): rclpy.shutdown() diff --git a/05_software/real/sim2real_ros2/tools/win_web_debug/server.py b/05_software/real/sim2real_ros2/tools/win_web_debug/server.py new file mode 100644 index 0000000..d88503c --- /dev/null +++ b/05_software/real/sim2real_ros2/tools/win_web_debug/server.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import json +import socket +import threading +import time +from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from typing import Optional + +STATE_LOCK = threading.Lock() +LATEST_STATE: dict = {"type": "state", "connected": False} +NANO_ADDR: tuple[str, int] +UDP_SOCK: socket.socket + + +class Handler(SimpleHTTPRequestHandler): + def do_GET(self) -> None: + if self.path == "/api/state": + with STATE_LOCK: + data = json.dumps(LATEST_STATE).encode("utf-8") + self._json(200, data) + return + super().do_GET() + + def do_POST(self) -> None: + if self.path not in ("/api/control", "/api/state"): + self.send_error(404) + return + length = int(self.headers.get("Content-Length", "0")) + body = self.rfile.read(length) if length else b"{}" + try: + payload = json.loads(body.decode("utf-8")) + send_udp(payload) + self._json(200, b'{"ok":true}') + except Exception as exc: + self._json(400, json.dumps({"ok": False, "error": str(exc)}).encode()) + + def _json(self, code: int, data: bytes) -> None: + self.send_response(code) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(data))) + self.send_header("Access-Control-Allow-Origin", "*") + self.end_headers() + self.wfile.write(data) + + def log_message(self, format: str, *args: object) -> None: + return + + +def send_udp(payload: dict) -> None: + data = json.dumps(payload, separators=(",", ":")).encode("utf-8") + UDP_SOCK.sendto(data, NANO_ADDR) + + +def udp_rx_loop(sock: socket.socket) -> None: + global LATEST_STATE + while True: + try: + data, _ = sock.recvfrom(65535) + payload = json.loads(data.decode("utf-8")) + payload["connected"] = True + payload["local_receive_time"] = time.time() + with STATE_LOCK: + LATEST_STATE = payload + except Exception: + time.sleep(0.01) + + +def heartbeat_loop() -> None: + while True: + try: + send_udp({"type": "ping", "stamp": time.time()}) + except Exception: + pass + time.sleep(0.5) + + +def main() -> None: + global NANO_ADDR, UDP_SOCK + parser = argparse.ArgumentParser(description="Windows local web debug UI for sim2real_ros2") + parser.add_argument("--nano-host", required=True, help="Nano IP address") + parser.add_argument("--nano-port", type=int, default=15000, help="Nano UDP listen port") + parser.add_argument("--listen-host", default="0.0.0.0", help="Local HTTP host") + parser.add_argument("--http-port", type=int, default=8088, help="Local HTTP port") + parser.add_argument("--udp-port", type=int, default=15001, help="Local UDP receive port") + args = parser.parse_args() + + NANO_ADDR = (args.nano_host, args.nano_port) + UDP_SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + UDP_SOCK.bind(("0.0.0.0", args.udp_port)) + + threading.Thread(target=udp_rx_loop, args=(UDP_SOCK,), daemon=True).start() + threading.Thread(target=heartbeat_loop, daemon=True).start() + + static_dir = Path(__file__).resolve().parent / "static" + handler = lambda *a, **kw: Handler(*a, directory=str(static_dir), **kw) + httpd = ThreadingHTTPServer((args.listen_host, args.http_port), handler) + print(f"Open http://127.0.0.1:{args.http_port}") + print(f"UDP Nano={args.nano_host}:{args.nano_port} local={args.udp_port}") + httpd.serve_forever() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/tools/win_web_debug/static/app.js b/05_software/real/sim2real_ros2/tools/win_web_debug/static/app.js new file mode 100644 index 0000000..71e7d3c --- /dev/null +++ b/05_software/real/sim2real_ros2/tools/win_web_debug/static/app.js @@ -0,0 +1,259 @@ +'use strict'; + +const JOINT_NAMES = [ + 'FL_H_ABD','FL_H_PIT','FL_KNEE', + 'FR_H_ABD','FR_H_PIT','FR_KNEE', + 'RL_H_ABD','RL_H_PIT','RL_KNEE', + 'RR_H_ABD','RR_H_PIT','RR_KNEE', + 'FL_WHEEL','FR_WHEEL','RL_WHEEL','RR_WHEEL', +]; + +const $ = id => document.getElementById(id); +const cmd = { vx: 0, vy: 0, yaw: 0 }; +let cmdSendTimer = null; +let currentMode = 'UNKNOWN'; + +// ── API ────────────────────────────────────────────────────────────────────── +async function post(payload) { + try { + await fetch('/api/control', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify(payload), + }); + } catch (e) { + appendEvent('API_ERROR', e.message, 'bad'); + } +} + +function sendCmd() { + post({ + type: 'cmd_vel', + linear: { x: cmd.vx, y: cmd.vy, z: 0 }, + angular: { x: 0, y: 0, z: cmd.yaw }, + }); + $('cmd-display').textContent = + `vx=${cmd.vx.toFixed(2)} vy=${cmd.vy.toFixed(2)} yaw=${cmd.yaw.toFixed(2)}`; +} + +function zeroAll() { + cmd.vx = 0; cmd.vy = 0; cmd.yaw = 0; + $('cmd-vx').value = 0; + $('cmd-vy').value = 0; + $('cmd-yaw').value = 0; + $('cmd-vx-v').textContent = '0.00'; + $('cmd-vy-v').textContent = '0.00'; + $('cmd-yaw-v').textContent = '0.00'; + $('cmd-display').textContent = 'vx=0.00 vy=0.00 yaw=0.00'; + $('stick').style.transform = 'translate(-50%, -50%)'; + post({ type: 'zero' }); +} + +// ── Buttons ────────────────────────────────────────────────────────────────── +function setMode(mode) { + if (mode === 'WEB' && !confirm('确认切换到 WEB 控制？\n请确认机器人安全且速度为 0。')) return; + post({ type: 'mode', mode }); + appendEvent('MODE_SET', `→ ${mode}`, 'ok'); +} + +function highlightMode(mode) { + for (const m of ['DISABLED', 'REMOTE', 'WEB', 'NAV']) { + const btn = $('btn-' + m.toLowerCase()); + if (btn) btn.classList.toggle('active-mode', m === mode); + } + const el = $('stage'); + if (el) { + el.textContent = mode; + el.className = 'stage ' + mode; + } + currentMode = mode; +} + +$('btn-disabled').onclick = () => { zeroAll(); setMode('DISABLED'); }; +$('btn-remote').onclick = () => setMode('REMOTE'); +$('btn-web').onclick = () => setMode('WEB'); +$('btn-nav').onclick = () => setMode('NAV'); +$('btn-zero').onclick = zeroAll; +$('btn-estop').onclick = () => { + if (confirm('确认触发软急停？')) { + post({ type: 'estop', data: true }); + zeroAll(); + appendEvent('ESTOP', '软急停已触发', 'bad'); + } +}; + +// ── Sliders ────────────────────────────────────────────────────────────────── +for (const [id, key] of [['cmd-vx','vx'],['cmd-vy','vy'],['cmd-yaw','yaw']]) { + $(id).addEventListener('input', e => { + cmd[key] = parseFloat(e.target.value); + $(id + '-v').textContent = cmd[key].toFixed(2); + if (currentMode === 'WEB') sendCmd(); + }); +} + +// ── Joystick ───────────────────────────────────────────────────────────────── +const joystick = $('joystick'); +const stick = $('stick'); +let dragging = false; + +function updateJoystick(clientX, clientY) { + const rect = joystick.getBoundingClientRect(); + const cx = rect.left + rect.width / 2; + const cy = rect.top + rect.height / 2; + const maxR = rect.width * 0.42; + let dx = clientX - cx; + let dy = clientY - cy; + const dist = Math.hypot(dx, dy); + if (dist > maxR) { dx = dx / dist * maxR; dy = dy / dist * maxR; } + stick.style.transform = `translate(calc(-50% + ${dx}px), calc(-50% + ${dy}px))`; + cmd.vx = parseFloat((-(dy / maxR) * 0.8).toFixed(3)); + cmd.vy = parseFloat(( (dx / maxR) * 0.3).toFixed(3)); + $('cmd-vx').value = cmd.vx; + $('cmd-vy').value = cmd.vy; + $('cmd-vx-v').textContent = cmd.vx.toFixed(2); + $('cmd-vy-v').textContent = cmd.vy.toFixed(2); + if (currentMode === 'WEB') sendCmd(); +} + +joystick.addEventListener('pointerdown', e => { + dragging = true; + joystick.setPointerCapture(e.pointerId); + updateJoystick(e.clientX, e.clientY); +}); +joystick.addEventListener('pointermove', e => { if (dragging) updateJoystick(e.clientX, e.clientY); }); +joystick.addEventListener('pointerup', () => { dragging = false; zeroAll(); }); +joystick.addEventListener('pointercancel', () => { dragging = false; zeroAll(); }); + +// ── Joints grid init ───────────────────────────────────────────────────────── +function initJointsGrid() { + const grid = $('joints-grid'); + if (!grid) return; + grid.innerHTML = JOINT_NAMES.map((name, i) => ` +
+ ● + ${name} + 0.00 + 0.00 + 0.00 +
`).join(''); +} + +function updateJointsGrid(robot) { + if (!robot) return; + const pos = robot.joint_pos || []; + const vel = robot.joint_vel || []; + const tau = robot.joint_torque || []; + const upd = robot.update_counts || []; + for (let i = 0; i < 16; i++) { + const dot = $('ms-' + i); + const cnt = upd[i] ?? 0; + if (dot) dot.style.color = cnt > 0 ? '#30d158' : '#ef4444'; + const p = $('mp-' + i); if (p) p.textContent = (pos[i] || 0).toFixed(2); + const v = $('mv-' + i); if (v) v.textContent = (vel[i] || 0).toFixed(2); + const t = $('mt-' + i); + if (t) { + t.textContent = (tau[i] || 0).toFixed(2); + t.style.color = Math.abs(tau[i] || 0) > 16 ? '#ff453a' : '#ff9f0a'; + } + } +} + +// ── State polling ───────────────────────────────────────────────────────────── +function setText(id, text, cls) { + const el = $(id); + if (!el) return; + el.textContent = text; + if (cls !== undefined) el.className = 'diag-value ' + cls; +} + +function applyState(data) { + const connected = data.connected && + (!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5); + + const stage = $('stage'); + if (stage) { + if (!connected) { + stage.textContent = 'DISCONNECTED'; + stage.className = 'stage DISCONNECTED'; + return; + } + } + + const mode = data.mode || 'UNKNOWN'; + if (mode !== currentMode) highlightMode(mode); + + const rt = data.runtime || {}; + const src = rt.target_source || '--'; + const srcCls = src === 'safety_brake' ? 'bad' + : src === 'runtime_policy' ? 'ok' + : src === 'runtime_zero_hold' ? 'active' : ''; + setText('d-source', src, srcCls); + setText('d-released', String(rt.runtime_released ?? '--'), + rt.runtime_released ? 'ok' : ''); + setText('d-alpha', (rt.release_alpha ?? '--') !== '--' + ? Number(rt.release_alpha).toFixed(2) : '--'); + setText('d-zero', String(rt.zero_command ?? '--')); + setText('d-estop', String(data.estop ?? '--'), + data.estop ? 'bad' : 'ok'); + setText('d-mux', data.mux_status || '--'); + + const robot = data.robot || {}; + const imuAge = robot.imu_age_ms ?? null; + setText('d-imu-fresh', String(robot.imu_fresh ?? '--'), + robot.imu_fresh ? 'ok' : 'bad'); + setText('d-imu-age', imuAge !== null ? imuAge.toFixed(1) : '--', + imuAge !== null ? (imuAge > 200 ? 'bad' : imuAge > 60 ? 'warn' : 'ok') : ''); + + const grav = robot.projected_gravity; + setText('d-gravity', grav ? grav.map(v => Number(v).toFixed(2)).join(', ') : '--', + grav && grav[2] < -0.5 ? 'ok' : 'warn'); + + setText('d-holdover', String(robot.holdover_count ?? '--'), + (robot.holdover_count || 0) > 10 ? 'warn' : ''); + + const odomAge = robot.odom_age_ms ?? null; + setText('d-odom-age', odomAge !== null ? odomAge.toFixed(1) : '--', + odomAge !== null ? (odomAge > 500 ? 'bad' : odomAge > 200 ? 'warn' : 'ok') : ''); + + const lp = robot.odom_local_pos; + setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--'); + + const cv = data.cmd_vel || {}; + const lin = cv.linear || {}; + const ang = cv.angular || {}; + setText('cv-vx', (lin.x ?? 0).toFixed(3)); + setText('cv-vy', (lin.y ?? 0).toFixed(3)); + setText('cv-yaw', (ang.z ?? 0).toFixed(3)); + + updateJointsGrid(robot); +} + +async function poll() { + try { + const res = await fetch('/api/state'); + const data = await res.json(); + applyState(data); + } catch (_) {} +} + +// ── WEB mode heartbeat ──────────────────────────────────────────────────────── +setInterval(() => { + if (currentMode === 'WEB' && !dragging) sendCmd(); +}, 50); + +// ── Event log ───────────────────────────────────────────────────────────────── +function appendEvent(kind, detail, cls) { + const el = $('events-log'); + if (!el) return; + const div = document.createElement('div'); + const t = new Date().toLocaleTimeString(); + div.innerHTML = `${t} ${kind} ${detail || ''}`; + el.appendChild(div); + while (el.children.length > 200) el.removeChild(el.firstChild); + el.scrollTop = el.scrollHeight; +} + +// ── Init ────────────────────────────────────────────────────────────────────── +initJointsGrid(); +setInterval(poll, 100); +appendEvent('READY', '页面已加载，等待 Nano 连接', 'ok'); diff --git a/05_software/real/sim2real_ros2/tools/win_web_debug/static/index.html b/05_software/real/sim2real_ros2/tools/win_web_debug/static/index.html new file mode 100644 index 0000000..8b8aead --- /dev/null +++ b/05_software/real/sim2real_ros2/tools/win_web_debug/static/index.html @@ -0,0 +1,98 @@ + + + + + + sim2real ROS2 控制台 + + + +
+
+

sim2real ROS2

+ DISCONNECTED +
+
+ 控制模式 + + + + +
+ +
+
+ +
+
+ +
+
+

运行状态

+
target_source--
+
runtime_released--
+
release_alpha--
+
zero_command--
+
estop--
+
mux--
+
+ +
+

IMU & 里程计

+
IMU fresh--
+
IMU age ms--
+
projected_gravity--
+
holdover--
+
odom age ms--
+
odom local pos--
+
+ +
+

关节状态 (16轴)

+
+
+
+ +
+
+

Web 手动控制

+
+
+

拖动控制前后(vx)和横移(vy)，松开归零

+
+
+
+ vx + + 0.00 +
+
+ vy + + 0.00 +
+
+ yaw + + 0.00 +
+
+
vx=0.00 vy=0.00 yaw=0.00
+
+ +
+

当前输出 /cmd_vel

+
linear.x--
+
linear.y--
+
angular.z--
+
+ +
+

事件流

+
+
+
+ + + + diff --git a/05_software/real/sim2real_ros2/tools/win_web_debug/static/style.css b/05_software/real/sim2real_ros2/tools/win_web_debug/static/style.css new file mode 100644 index 0000000..3606895 --- /dev/null +++ b/05_software/real/sim2real_ros2/tools/win_web_debug/static/style.css @@ -0,0 +1,263 @@ +/* sim2real ROS2 Web Debug — Apple Glass Design */ +:root { + --bg-primary: #000000; + --glass-bg: rgba(20, 20, 22, 0.65); + --glass-border: rgba(255, 255, 255, 0.12); + --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25); + --text-primary: #ffffff; + --text-secondary: #ebebf5; + --text-tertiary: #8e8e93; + --accent: #0a84ff; + --accent-hover: #409cff; + --success: #30d158; + --warning: #ffd60a; + --danger: #ff453a; + --blur-amount: 24px; + --saturation: 180%; + --spring: cubic-bezier(0.4, 0, 0.2, 1); + --panel-radius: 16px; + --font-family: -apple-system, BlinkMacSystemFont, "SF Pro Display", "PingFang SC", sans-serif; +} + +* { box-sizing: border-box; margin: 0; padding: 0; } + +body { + font-family: var(--font-family); + background: radial-gradient(circle at top left, #1a1a24 0%, #000000 100%); + color: var(--text-primary); + -webkit-font-smoothing: antialiased; + min-height: 100vh; + overflow-x: hidden; +} + +.glass-panel { + background: var(--glass-bg); + backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation)); + -webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation)); + border: 0.5px solid var(--glass-border); + box-shadow: var(--glass-shadow); + z-index: 50; +} + +/* Top Bar */ +.top-bar { + position: fixed; + top: 16px; + left: 50%; + transform: translateX(-50%); + display: flex; + align-items: center; + justify-content: space-between; + padding: 8px 16px; + border-radius: 24px; + width: 96%; + max-width: 1400px; + gap: 16px; +} +.top-bar-left, .top-bar-center, .top-bar-right { + display: flex; + align-items: center; + gap: 10px; +} +.top-bar-center { flex: 1; justify-content: center; } +.top-bar h1 { + font-size: 16px; + font-weight: 600; + background: linear-gradient(45deg, #fff, #8e8e93); + -webkit-background-clip: text; + -webkit-text-fill-color: transparent; +} +.divider { width: 1px; height: 24px; background: var(--glass-border); margin: 0 4px; } + +/* Stage badge */ +.stage { + padding: 4px 10px; + border-radius: 12px; + font-size: 11px; + font-weight: 700; + text-transform: uppercase; + letter-spacing: 0.5px; + background: rgba(255,255,255,0.1); + color: var(--text-secondary); +} +.stage.DISCONNECTED { background: rgba(142,142,147,0.3); color: #aaa; } +.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; } +.stage.REMOTE { background: rgba(48,209,88,0.3); color: #8deda7; } +.stage.WEB { background: rgba(0,122,255,0.3); color: #82c4ff; } +.stage.NAV { background: rgba(255,214,10,0.3); color: #ffe680; } +.stage.DISABLED { background: rgba(142,142,147,0.25); color: #aaa; } +.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); } + +/* Buttons */ +.btn { + background: rgba(255,255,255,0.08); + border: 1px solid rgba(255,255,255,0.1); + border-radius: 8px; + color: var(--text-primary); + font-size: 12px; + font-weight: 500; + padding: 6px 12px; + cursor: pointer; + transition: all 0.2s var(--spring); + font-family: inherit; +} +.btn:hover:not(:disabled) { background: rgba(255,255,255,0.15); transform: translateY(-1px); } +.btn:active:not(:disabled) { transform: translateY(1px); } +.btn:disabled { opacity: 0.5; cursor: not-allowed; } +.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; } +.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; } +.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; } +.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; } +.btn.active-mode { box-shadow: 0 0 0 2px white; } + +.label { font-size: 11px; color: var(--text-tertiary); } + +/* Side panels */ +.side-panel { + position: fixed; + top: 80px; + bottom: 20px; + width: 340px; + border-radius: var(--panel-radius); + display: flex; + flex-direction: column; + overflow: hidden; +} +.left-panel { left: 2%; } +.right-panel { right: 2%; } +.flex-1 { flex: 1; min-height: 0; overflow: hidden; display: flex; flex-direction: column; } + +.panel-section { + padding: 14px 16px; + border-bottom: 0.5px solid var(--glass-border); +} +.panel-section:last-child { border-bottom: none; } + +.panel-title { + font-size: 11px; + font-weight: 700; + color: var(--text-tertiary); + text-transform: uppercase; + letter-spacing: 0.5px; + margin-bottom: 10px; +} + +/* Diag rows */ +.diag-row { + display: flex; + justify-content: space-between; + align-items: center; + padding: 4px 6px; + background: rgba(0,0,0,0.2); + border-radius: 4px; + margin-bottom: 3px; +} +.diag-label { font-size: 11px; color: var(--text-tertiary); } +.diag-value { font-size: 11px; font-family: monospace; color: var(--text-primary); font-weight: 600; } +.diag-value.ok { color: var(--success); } +.diag-value.warn { color: var(--warning); } +.diag-value.bad { color: var(--danger); } +.diag-value.active { color: #82c4ff; } + +/* Joints grid */ +.motors-grid-list { + display: flex; + flex-direction: column; + gap: 2px; + overflow-y: auto; + flex: 1; +} +.motor-row { + display: flex; + align-items: center; + gap: 4px; + padding: 3px 6px; + background: rgba(0,0,0,0.25); + border-radius: 5px; +} +.motor-row .name { font-size: 10px; color: var(--text-secondary); width: 72px; font-family: monospace; flex-shrink: 0; } +.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; flex: 1; } +.motor-row .val.pos { color: #0a84ff; } +.motor-row .val.vel { color: #30d158; } +.motor-row .val.tau { color: #ff9f0a; } +.motor-row .stale { font-size: 9px; width: 8px; flex-shrink: 0; } + +/* Joystick */ +.joystick-area { display: flex; flex-direction: column; align-items: center; gap: 8px; margin-bottom: 12px; } +.joystick { + position: relative; + width: 180px; + height: 180px; + border-radius: 50%; + background: radial-gradient(circle, rgba(10,132,255,0.2), rgba(10,132,255,0.05)); + border: 1px solid rgba(10,132,255,0.3); + touch-action: none; + flex-shrink: 0; +} +#stick { + position: absolute; + left: 50%; top: 50%; + width: 56px; height: 56px; + border-radius: 50%; + background: linear-gradient(135deg, #30d158, #0a84ff); + transform: translate(-50%, -50%); + box-shadow: 0 8px 24px rgba(0,0,0,0.4); +} + +/* Sliders */ +.slider-group { display: flex; flex-direction: column; gap: 8px; } +.slider-row { display: flex; align-items: center; gap: 8px; } +.slider-label { font-size: 11px; color: var(--text-tertiary); width: 28px; font-family: monospace; } +.slider-val { font-size: 11px; color: var(--accent); font-family: monospace; width: 38px; text-align: right; } +.glass-slider { + flex: 1; + -webkit-appearance: none; + height: 4px; + border-radius: 2px; + background: rgba(255,255,255,0.2); + outline: none; +} +.glass-slider::-webkit-slider-thumb { + -webkit-appearance: none; + width: 14px; height: 14px; + border-radius: 50%; + background: white; + cursor: pointer; + box-shadow: 0 2px 4px rgba(0,0,0,0.5); +} +.cmd-display { + margin-top: 8px; + padding: 6px 10px; + background: rgba(0,0,0,0.3); + border-radius: 6px; + font-family: monospace; + font-size: 12px; + color: var(--accent); + text-align: center; +} +.hint { font-size: 11px; color: var(--text-tertiary); text-align: center; } + +/* Log */ +.log-section { flex: 1; overflow: hidden; display: flex; flex-direction: column; } +.log { + flex: 1; + background: rgba(0,0,0,0.4); + border: 1px solid rgba(255,255,255,0.05); + border-radius: 6px; + padding: 8px; + font-family: monospace; + font-size: 11px; + color: var(--text-secondary); + overflow-y: auto; +} +.log div { margin-bottom: 2px; line-height: 1.4; } +.ev-t { color: var(--text-tertiary); margin-right: 4px; } +.ev-ok { color: var(--success); } +.ev-warn { color: var(--warning); } +.ev-bad { color: var(--danger); } + +@media (max-width: 960px) { + .side-panel { position: relative; top: auto; width: 100%; left: 0; right: 0; border-radius: 0; height: auto; } + .top-bar { width: 100%; border-radius: 0; top: 0; } + body { padding-top: 56px; } +} diff --git a/05_software/real/sim2real_ros2_v3/.gitignore b/05_software/real/sim2real_ros2_v3/.gitignore new file mode 100644 index 0000000..2641ed6 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/.gitignore @@ -0,0 +1,14 @@ +build/ +install/ +log/ +logs_v2_web/ +map/load/ +src/odin_ros_driver/log/ +src/odin_ros_driver/recorddata/ +src/odin_ros_driver/image/ +*.bak_* +__pycache__/ +*.py[cod] +.colcon/ +.vscode/ +compile_commands.json diff --git a/05_software/real/sim2real_ros2_v3/DEPLOYMENT_GUIDE.md b/05_software/real/sim2real_ros2_v3/DEPLOYMENT_GUIDE.md new file mode 100644 index 0000000..b2d5d38 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/DEPLOYMENT_GUIDE.md @@ -0,0 +1,177 @@ +# ROS2 C++ Sim2Real 运动控制栈 - 部署指南 + +本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。 + +--- + +## 1. 前提条件与环境 + +### 硬件 +* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。 +* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。 +* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。 + +### 主机依赖 +* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。 +* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。 +* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。 +* **库与 ROS2 包**： + * `libyaml-cpp-dev` + * `libeigen3-dev` + * `libusb-1.0-0-dev`（Odin USB 传感器通信） + * `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理） + * `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器） + * `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用） + * `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理） + * `can-utils`（SocketCAN 验证工具） + +--- + +## 2. 本地编译与部署 + +按以下步骤在主机系统上编译运行整个栈： + +### 步骤 1：安装系统依赖 +```bash +sudo apt-get update +sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \ + libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \ + ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \ + ros-humble-cv-bridge ros-humble-pcl-conversions +``` + +### 步骤 2：下载 ONNXRuntime C++ SDK +策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录： + +```bash +# 创建目录 +sudo mkdir -p /opt/onnxruntime +cd /opt + +# 针对 Jetson Orin (ARM64 / aarch64)： +sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz +sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime + +# 或标准桌面仿真 (x86_64 / amd64)： +# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz +# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime +``` + +导出 CMake 辅助变量： +```bash +export ONNXRUNTIME_DIR=/opt/onnxruntime +``` + +### 步骤 3：构建工作区 +进入包含 `src/` 的本包根目录，运行 `colcon`： +```bash +colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +### 步骤 4：配置 SocketCAN 接口 +启动前，以 1 Mbps 波特率激活 CAN 接口： +```bash +sudo ip link set can0 up type can bitrate 1000000 +sudo ip link set can1 up type can bitrate 1000000 +``` +使用 `ifconfig` 或 `ip link` 验证接口已启动。 + +### 步骤 5：启动节点 +使启动脚本可执行并运行： +```bash +chmod +x start_sim2real.sh +./start_sim2real.sh +``` + +--- + +## 3. Docker 部署（推荐） + +强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。 + +### 步骤 1：构建镜像 +确保在 `sim2real_ros2_v3` 目录中（包含 `Dockerfile`）： +```bash +# 使用标准 docker build： +docker build -t sim2real_ros2:latest . + +# 或使用 Docker Compose： +docker compose build +``` + +### 步骤 2：运行容器 +对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度： + +```bash +# 选项 A：手动运行 +docker run -it \ + --network host \ + --privileged \ + --cap-add=sys_nice \ + --volume=/dev:/dev \ + --shm-size=2g \ + --name sim2real_ros2_run \ + sim2real_ros2:latest + +# 选项 B：通过 Docker Compose 运行（最简单） +docker compose up -d +``` + +--- + +## 4. 系统拓扑与话题 + +控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互： + +* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。 +* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。 +* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。 +* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。 +* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。 +* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。 + +### TF 树 +``` +odom ──→ base_link （由 odom_relay_node 广播） +map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式） +``` + +--- + +## 5. 集成 ROS 2 导航与传感器驱动 + +### USB 设备权限（Odin 传感器） +要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则： + +```bash +# 1. 添加 udev 规则 +echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules + +# 2. 重新加载 udev 规则并重新插拔传感器 +sudo udevadm control --reload +sudo udevadm trigger +``` + +### 集成启动参数 +统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈： + +* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。 +* `launch_nav2`（默认：`false`）：按需启动 ROS2 Navigation2；比赛默认使用 `simple_nav_node.py` 的路线跟踪。 + +#### 1. 完整真实硬件闭环（默认） +启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true +``` + +#### 2. Dry-Run / 仿真航点测试 +在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true +``` + +#### 3. 仅运动控制（无导航） +禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false +``` diff --git a/05_software/real/sim2real_ros2_v3/Dockerfile b/05_software/real/sim2real_ros2_v3/Dockerfile new file mode 100644 index 0000000..5753fa4 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/Dockerfile @@ -0,0 +1,79 @@ +# 使用 ROS2 官方 Humble 基础镜像 +FROM ros:humble-ros-base-jammy + +ENV DEBIAN_FRONTEND=noninteractive + +# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库 +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + can-utils \ + libyaml-cpp-dev \ + libeigen3-dev \ + libusb-1.0-0-dev \ + libpcl-dev \ + libopencv-dev \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + ros-humble-pointcloud-to-laserscan \ + ros-humble-cv-bridge \ + ros-humble-pcl-conversions \ + wget \ + tar \ + python3-pip \ + && rm -rf /var/lib/apt/lists/* + +# ============================================================================ +# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速 +# ============================================================================ +# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP） +# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用） +WORKDIR /opt +RUN ARCH=$(uname -m) && \ + if [ "$ARCH" = "aarch64" ]; then \ + echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \ + pip3 install --no-cache-dir onnxruntime-gpu && \ + SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \ + mkdir -p onnxruntime/include onnxruntime/lib && \ + cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \ + cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \ + echo "[ONNX] CUDA ONNX Runtime installed."; \ + else \ + echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \ + wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \ + tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \ + mv onnxruntime-linux-x64-1.16.3 onnxruntime && \ + rm onnxruntime-linux-x64-1.16.3.tgz; \ + fi + +ENV ONNXRUNTIME_DIR=/opt/onnxruntime + +# 创建工作空间，将所有 C++ 源码包拷入 +WORKDIR /sim2real_ws/src +COPY src/sim2real_bringup sim2real_bringup +COPY src/sim2real_common sim2real_common +COPY src/sim2real_hw sim2real_hw +COPY src/sim2real_interfaces sim2real_interfaces +COPY src/sim2real_runtime sim2real_runtime +COPY src/odin_ros_driver odin_ros_driver +COPY src/sim2real_nav2 sim2real_nav2 + +# 拷贝策略文件与运行脚本 +WORKDIR /sim2real_ws +COPY policies policies +COPY map map +COPY start_sim2real.sh start_sim2real.sh +RUN chmod +x start_sim2real.sh + +# 编译 ROS2 工作空间 +SHELL ["/bin/bash", "-c"] +RUN source /opt/ros/humble/setup.bash && \ + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release + +# 拷贝 Docker 入口脚本并设置 +COPY docker_entrypoint.sh /docker_entrypoint.sh +RUN chmod +x /docker_entrypoint.sh + +ENTRYPOINT ["/docker_entrypoint.sh"] +CMD ["./start_sim2real.sh"] diff --git a/05_software/real/sim2real_ros2_v3/README.md b/05_software/real/sim2real_ros2_v3/README.md new file mode 100644 index 0000000..77a1743 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/README.md @@ -0,0 +1,106 @@ +# ROS 2 Sim2Real v3：最终比赛版 + +本目录由原始 `sim2real_ros2_v2(last_not_slalom_1050)` 整理而来，正式作为 ROS 2/C++ 第三版（v3），对应 RC_WheelLeg 在 RoboCon 仿生足式障碍赛使用的最终部署栈。`1050` 是比赛得分，不是模型编号；比赛 Rough 策略为 `model_6800.onnx`。 + +该里程碑对应重排主线的 `v1.0.0`，是 1050 分比赛最终版本。训练架构和策略来源见 `v0.6.0`，比赛 Rough 模型首次归档见 `v0.8.0`，导航打点与路线演进见 `v0.8.1`，ROS 2 迁移过程见 `v0.10.0`～`v0.12.0`。 + +当前 `main` 的 `v1.0.1` 只是在 `v1.0.0` 基础上补充比赛机器人图片和比赛视频，本目录代码仍以 `v1.0.0` 为比赛部署基线。 + +## 系统闭环 + +```text +Odin IMU / Odom ──> hardware bridge ──> RuntimeState + | +导航 / 遥控 / 屏幕 ──> cmd mux ──> policy runtime (50 Hz) + | + RuntimeTarget + | + hardware bridge / CAN (200 Hz) +``` + +核心约束： + +- 53 维策略观测、16 维动作输出。 +- Rough：`model_6800`，优先 TensorRT，失败时回退 ONNX Runtime。 +- Wall：`model_84`，同样保留 TensorRT 与 ONNX 两种文件。 +- Crawl：比赛配置使用解析 IK，不加载 Crawl RL 权重。 +- 默认站姿：髋俯仰 `0.550`、膝关节 `-1.125`。 +- 默认命令源：`NAV`；默认定位模式：`relocal`。 + +## 目录 + +```text +sim2real_ros2_v3/ +├─ src/ +│ ├─ sim2real_interfaces/ # RuntimeState / RuntimeTarget 消息 +│ ├─ sim2real_common/ # 部署契约、滤波、平衡和安全监控 +│ ├─ sim2real_hw/ # SocketCAN、IMU 和 200 Hz 电机热路径 +│ ├─ sim2real_runtime/ # 策略、命令仲裁、导航、Web API +│ ├─ sim2real_nav2/ # Nav2 配置入口 +│ ├─ sim2real_bringup/ # 统一参数和启动文件 +│ └─ odin_ros_driver/ # Odin ROS 驱动（Apache-2.0） +├─ policies/ # 比赛实际使用的 Rough / Wall 模型 +├─ map/ # 比赛路线和抽样 PCD +├─ screen/ # Orin 800×600 触控面板 +├─ docs/ # 架构、遥控、Web 和迁移说明 +├─ Dockerfile +└─ start_sim2real.sh +``` + +## 构建与运行 + +目标环境是 Ubuntu 22.04、ROS 2 Humble 和 Jetson Orin。系统依赖和 Docker 流程见 [`DEPLOYMENT_GUIDE.md`](DEPLOYMENT_GUIDE.md)。 + +```bash +cd 05_software/real/sim2real_ros2_v3 +colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release +./start_sim2real.sh +``` + +运行参数和模型/路线均使用工作区根目录相对路径，因此应从本目录启动。常用启动覆盖： + +```bash +# 纯里程计模式，不等待 Odin 重定位地图 +./start_sim2real.sh localization_mode:=odom \ + odin_config_file:=src/odin_ros_driver/config/control_command_odom.yaml + +# 禁止驱动，仅做软件链路检查 +./start_sim2real.sh launch_driver:=false launch_remote:=false +``` + +## 必须补充的部署资产 + +最终源目录配置引用了 Odin `map/1hao.bin`，但工作区备份中不存在这个文件；全盘检索也未找到同名文件。为避免用来源不明的 `.bin` 冒充比赛地图，本仓库不伪造该资产。 + +使用 `relocal` 前必须： + +1. 从比赛 Orin 或 Odin 建图备份取得真实 `1hao.bin`。 +2. 修改 `src/odin_ros_driver/config/control_command_relocal.yaml` 中的 `relocalization_map_abs_path` 为目标机绝对路径。 +3. 核对文件哈希并在发布说明中补充来源。 + +缺少该文件时请使用 `localization_mode:=odom`，不要宣称重定位闭环已复现。地图和路线边界见 [`map/README.md`](map/README.md)。 + +## 归档边界 + +已保留： + +- 最终六个 ROS 2 包、Odin 驱动源码、比赛设备标定参数和预编译 SDK 静态库。 +- 最终 Rough/Wall ONNX 与比赛机 TensorRT engine。 +- 五份最终工程路线、1 号场地抽样 PCD、屏幕 UI 和启动脚本。 +- Odin 驱动 Apache-2.0 许可证。 + +未保留： + +- 嵌套 `.git`、`__pycache__`、日志、备份、构建/安装目录。 +- 未被比赛配置引用的候选模型与候选 TensorRT engine。 +- 开发计划、任务草稿、重复地图工具和运行时轨迹。 +- 原备份中大小为 0 的浏览器静态页面；HTTP JSON API 和屏幕 UI 源码仍保留。 + +TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关；其他机器应从同名 ONNX 重新生成，不应默认复用比赛 engine。模型哈希见 [`policies/README.md`](policies/README.md)。 + +## 安全与开源状态 + +- 真机运行前必须架空轮组验证 CAN 映射、方向、零位、急停和限幅。 +- `deployment_contract.hpp` 是电机映射和动作缩放真值源；参考 YAML 不会自动修改 C++ 契约。 +- 自研 ROS 包的 `package.xml` 仍保留原工程的 `Proprietary` 字段。迁移到 GitHub 公共开源前，需要由项目负责人选择许可证并统一修改；本次整理不代替权利人作许可证决定。 +- 当前 Windows 环境只能做静态检查，不能证明 ROS 2、SocketCAN、Odin SDK 或 TensorRT 真机运行成功。 diff --git a/05_software/real/sim2real_ros2_v3/docker-compose.yml b/05_software/real/sim2real_ros2_v3/docker-compose.yml new file mode 100644 index 0000000..1d2e414 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docker-compose.yml @@ -0,0 +1,22 @@ +version: '3.8' + +services: + sim2real_ros2: + build: + context: . + dockerfile: Dockerfile + container_name: sim2real_ros2_node + runtime: nvidia + network_mode: host + privileged: true + stdin_open: true + tty: true + environment: + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=compute,utility + cap_add: + - SYS_NICE + shm_size: '2gb' + volumes: + - /dev:/dev + restart: unless-stopped diff --git a/05_software/real/sim2real_ros2_v3/docker_entrypoint.sh b/05_software/real/sim2real_ros2_v3/docker_entrypoint.sh new file mode 100644 index 0000000..c4e1054 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docker_entrypoint.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +# Source ROS2 Humble environment +source /opt/ros/humble/setup.bash + +# Source workspace install setup if compiled +if [ -f "/sim2real_ws/install/setup.bash" ]; then + source /sim2real_ws/install/setup.bash +fi + +exec "$@" diff --git a/05_software/real/sim2real_ros2_v3/docs/ARCHITECTURE.md b/05_software/real/sim2real_ros2_v3/docs/ARCHITECTURE.md new file mode 100644 index 0000000..9b02b99 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docs/ARCHITECTURE.md @@ -0,0 +1,102 @@ +# sim2real_ros2_v3 架构说明 + +## 设计目标 + +- 保留已验证的 RL 部署契约不变 +- 将低延迟循环从 Python 迁移至 C++ +- 暴露标准 ROS 2 接口用于导航和系统集成 +- 保持安全边界独立于策略正确性 + +## 各包职责 + +### `sim2real_interfaces`（接口消息） + +定义最小化的运行时消息： + +- `RuntimeState` + 硬件桥接发布的归一化运行时状态快照 +- `RuntimeTarget` + 策略运行时发送至硬件桥接的最新策略目标 + +### `sim2real_common`（共享常量） + +存储编译期常量和部署契约辅助： + +- 观测维度和字段布局 +- 动作维度和轮子索引 +- 关节顺序和默认站姿 +- 动作缩放因子和默认循环频率 +- Mahony 姿态滤波器 +- 站立平衡控制器 +- 安全监控器（SafetyMonitor / RuntimeGuard） + +### `sim2real_hw`（硬件桥接） + +拥有硬件侧执行循环和安全边界： + +- RobStride CAN 收发 +- IMU 与 Odin 状态采集 +- 电机丢帧检测与保活逻辑（holdover） +- 看门狗与阻尼刹车 +- 发布 `RuntimeState` +- 订阅 `RuntimeTarget` +- 订阅 `/odom` 里程计数据 + +目标热路径： + +- 以 `200Hz` 频率读取状态 +- 应用最新安全目标 +- 超时或安全违规时立即停机 + +### `sim2real_runtime`（策略运行时） + +拥有策略侧执行： + +- 订阅 `RuntimeState` +- 按当前部署契约精确构建 `53D` 观测 +- 以 `50Hz` 运行 ONNXRuntime 推理 +- 对 raw_action 做 `[-10, 10]` 安全裁剪 +- 发布 `RuntimeTarget` +- 仲裁命令来源：estop > safety_hold > startup > navigation > web + +同时包含： +- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF + +### `sim2real_nav2`（导航配置） + +拥有： + +- Nav2 参数文件（planner、controller、costmap、AMCL、behavior） +- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan） + +### `sim2real_bringup`（启动管理） + +拥有： + +- 参数文件 +- 启动组合 +- 运行时模式选择 +- 集成 odin_ros_driver、sim2real_nav2 的条件启动 + +## 迁移规则 + +1. 优化之前先冻结当前契约 +2. 先迁移传输和循环结构，再调整控制算法 +3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用 +4. 按段测量延迟： + - 观测延迟 + - 策略推理延迟 + - 目标传输延迟 + - 执行器响应延迟 + +## 首个里程碑 + +首个里程碑不是"机器人在 ROS 2 下行走"，而是： + +1. `sim2real_hw` 发布稳定的 `RuntimeState` +2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测 +3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget` +4. `sim2real_hw` 消费最新目标并执行超时刹车 +5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约 + +> ✅ 以上里程碑已全部完成。 diff --git a/05_software/real/sim2real_ros2_v3/docs/MIGRATION_PLAN.md b/05_software/real/sim2real_ros2_v3/docs/MIGRATION_PLAN.md new file mode 100644 index 0000000..d39e093 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docs/MIGRATION_PLAN.md @@ -0,0 +1,80 @@ +# 迁移计划 + +## Phase 1: 硬件核心迁移 ✅ 已完成 + +将当前高频热路径从 Python 迁出。 + +吸收的源文件： + +- `sim2real/interface/motor_driver.py` +- `sim2real/interface/motor_mapping.py` +- `sim2real/interface/imu_client.py` +- `sim2real/safety/runtime_guard.py` +- `sim2real/web/session.py` + +交付物： + +- C++ SocketCAN 电机总线封装 +- C++ 状态缓存 +- target 超时保活（timeout hold） +- 阻尼刹车 / 急停通路 +- 发布 `RuntimeState` + +## Phase 2: 策略运行时迁移 ✅ 已完成 + +吸收的源文件： + +- `sim2real/policy/policy_runner.py` +- `sim2real/interface/real_io.py` +- `sim2real/web/session.py` + +交付物： + +- 精确的 `53D` 观测构造器 +- ONNXRuntime C++ 推理封装 +- `50Hz` 策略定时器 +- 命令平滑与来源仲裁 +- raw_action `[-10, 10]` 安全裁剪 +- 发布 `RuntimeTarget` + +## Phase 3: ROS 2 系统集成 ✅ 已完成 + +参考的源项目： + +- `00_ reference/odin_ros_driver` +- `00_ reference/EDULITE_A3/el_a3_ros` +- `00_ reference/rl_sar` + +交付物： + +- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped） +- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link） +- 诊断话题 +- rosbag/foxglove 可观测性 + +## Phase 4: 导航集成 ✅ 已完成 + +目标： + +- 导航通过 ROS 2 发送身体速度指令 +- RL 运行时保持为 locomotion 控制器 +- 看门狗和安全边界始终在导航之下 + +规则： + +- 导航绝不直接写电机指令 +- 策略契约在重新训练前保持不变 +- 任何新增历史项或里程计项必须版本化 + +## 当前状态 + +所有 4 个 Phase 已全部完成。以下为已实现的关键组件： + +| 组件 | 节点 | 说明 | +|------|------|------| +| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 | +| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip | +| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF | +| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle | +| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 | +| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) | diff --git a/05_software/real/sim2real_ros2_v3/docs/REMOTE_CONTROL_USAGE.md b/05_software/real/sim2real_ros2_v3/docs/REMOTE_CONTROL_USAGE.md new file mode 100644 index 0000000..eef4cde --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docs/REMOTE_CONTROL_USAGE.md @@ -0,0 +1,499 @@ +# sim2real_ros2_v3 遥控器调用说明 + +本文档说明如何在 `sim2real_ros2_v3` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。 + +## 1. 当前接入关系 + +遥控器节点位于： + +```text +src/sim2real_runtime/src/remote_uart_node.py +``` + +该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题： + +| 输入 | 输出 | 作用 | +|---|---|---| +| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 | +| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 | + +策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。 + +## 2. 通道映射 + +通道映射与本仓库第一代 Python Sim2Real 实现中的遥控器配置保持一致。 + +| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 | +|---|---|---|---:| +| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` | +| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` | +| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` | +| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - | + +默认方向反转配置： + +| 参数 | 默认值 | 含义 | +|---|---:|---| +| `remote_invert_vx` | `true` | 反转前后方向 | +| `remote_invert_vy` | `false` | 不反转横移方向 | +| `remote_invert_yaw` | `true` | 反转转向方向 | + +## 3. 参数位置 + +遥控器参数在： + +```text +src/sim2real_bringup/config/runtime.yaml +``` + +当前默认参数： + +```yaml +remote_enabled: true +remote_port: "/dev/ttyACM0" +remote_baudrate: 100000 +remote_timeout: 0.02 +remote_axis_deadzone: 50 +remote_active_threshold: 50 +remote_axis_full_scale: 660.0 +remote_max_vx: 0.8 +remote_max_vy: 0.3 +remote_max_yaw_rate: 0.5 +remote_invert_vx: true +remote_invert_vy: false +remote_invert_yaw: true +remote_publish_inactive_zero: true +remote_estop_latch: true +remote_estop_channel: 7 +remote_estop_level: "high" +remote_estop_debounce_frames: 3 +remote_estop_require_remote_mode: true +remote_poll_hz: 50.0 +``` + +如果遥控器串口不是 `/dev/ttyACM0`，需要修改： + +```yaml +remote_port: "/dev/ttyUSB0" +``` + +或改成实际设备路径。 + +## 4. 启动前检查 + +### 4.1 确认串口存在 + +```bash +ls /dev/ttyACM* /dev/ttyUSB* +``` + +如果使用默认配置，应能看到： + +```bash +/dev/ttyACM0 +``` + +### 4.2 确认串口权限 + +如果节点提示串口权限不足，可以临时执行： + +```bash +sudo chmod 666 /dev/ttyACM0 +``` + +更推荐的长期方式是把当前用户加入 `dialout` 组： + +```bash +sudo usermod -aG dialout $USER +``` + +然后重新登录。 + +### 4.3 确认 Python serial 依赖 + +节点依赖 `pyserial`。如果系统没有安装： + +```bash +sudo apt update +sudo apt install -y python3-serial +``` + +## 5. 构建 + +如果刚修改过代码或参数，建议重新构建相关包： + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install +``` + +构建完成后 source 环境： + +```bash +source install/setup.bash +``` + +确认可执行节点存在： + +```bash +ros2 pkg executables sim2real_runtime +``` + +应包含： + +```text +sim2real_runtime remote_uart_node.py +``` + +## 6. 推荐启动方式 + +### 6.1 启动完整系统，不启动 Nav2 + +这是你当前常用方式： + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。 + +等价完整写法： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true +``` + +### 6.2 不启动遥控器 + +如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false +``` + +## 7. 单独启动遥控器节点 + +如果系统已经在运行，只想单独测试遥控器节点： + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml +``` + +如果要临时指定串口： + +```bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args \ + --params-file src/sim2real_bringup/config/runtime.yaml \ + -p remote_port:=/dev/ttyUSB0 +``` + +## 8. 执行后会产生什么效果 + +启动以下命令后： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +系统会产生以下效果。 + +### 8.1 启动硬件桥接节点 + +节点： + +```text +/sim2real_hw_node +``` + +效果： + +1. 打开 `can0` 和 `can1`。 +2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。 +3. 设置电机 MIT 模式。 +4. 设置电机速度限制和力矩限制。 +5. 以 `200Hz` 运行硬件读写循环。 +6. 发布 `/runtime/state`。 +7. 订阅 `/runtime/target` 执行策略目标。 + +### 8.2 启动策略运行节点 + +节点： + +```text +/sim2real_runtime_node +``` + +效果： + +1. 加载 ONNX 策略模型。 +2. 订阅 `/runtime/state`。 +3. 订阅 `/cmd_vel`。 +4. 订阅 `/safety/estop`。 +5. 执行启动站立流程： + - `boot_hold` + - `startup_soft_hold` + - `startup_hold` + - `runtime_zero_hold` + - `runtime_policy` +6. 以 `50Hz` 发布 `/runtime/target`。 + +### 8.3 启动遥控器节点 + +节点： + +```text +/sim2real_remote_uart_node +``` + +效果： + +1. 打开默认串口 `/dev/ttyACM0`。 +2. 以 `50Hz` 轮询 SBUS 数据。 +3. 遥控器摇杆居中时持续发布零速度： + +```text +/cmd_vel: + linear.x = 0.0 + linear.y = 0.0 + angular.z = 0.0 +``` + +4. 推动遥控器时发布非零速度，例如： + +```text +/cmd_vel: + linear.x = vx + linear.y = vy + angular.z = yaw +``` + +5. 当 CH7 打到高位时发布： + +```text +/safety/estop: true +``` + +由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。 + +### 8.4 机器人行为效果 + +正常启动后，机器人不会立即按策略行走，而是按阶段执行： + +1. 电机使能。 +2. 读取当前关节位置。 +3. 软保持当前姿态。 +4. 平滑过渡到默认站立姿态。 +5. 稳定后进入 runtime。 +6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。 +7. 遥控器有输入时进入策略控制，即 `runtime_policy`。 + +也就是说： + +| 遥控器状态 | 机器人效果 | +|---|---| +| 摇杆居中 | 站立保持，不主动行走 | +| CH2 前后推动 | 前进/后退 | +| CH4 左右推动 | 横向移动 | +| CH1 左右推动 | 原地转向 | +| CH7 高位 | 软件急停，进入安全刹车 | + +## 9. 如何确认遥控器已经生效 + +### 9.1 查看节点是否存在 + +```bash +ros2 node list +``` + +应看到： + +```text +/sim2real_remote_uart_node +/sim2real_runtime_node +/sim2real_hw_node +``` + +### 9.2 查看 `/cmd_vel` + +```bash +ros2 topic echo /cmd_vel +``` + +摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。 + +### 9.3 查看 `/safety/estop` + +```bash +ros2 topic echo /safety/estop +``` + +CH7 高位时应看到： + +```yaml +data: true +``` + +### 9.4 查看策略目标阶段 + +```bash +ros2 topic echo /runtime/target --field target_source +``` + +常见输出含义： + +| `target_source` | 含义 | +|---|---| +| `boot_hold` | 刚启动，保持初始姿态 | +| `startup_soft_hold` | 启动软保持 | +| `startup_hold` | 正在站立或站立后保持 | +| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 | +| `runtime_policy` | 遥控器有输入，策略已经介入 | +| `safety_brake` | 安全刹车 | +| `timeout_hold` | 目标超时，硬件保持默认姿态 | + +### 9.5 查看完整目标状态 + +```bash +ros2 topic echo --once /runtime/target +``` + +重点关注字段： + +```yaml +target_source: +zero_command: +runtime_released: +release_alpha: +command: +raw_command: +``` + +如果遥控器摇杆有输入，通常会看到： + +```yaml +target_source: runtime_policy +zero_command: false +runtime_released: true +release_alpha: 1.0 +``` + +## 10. 常见问题 + +### 10.1 启动后提示无法打开串口 + +可能原因： + +1. 串口路径不对。 +2. 权限不足。 +3. 设备没有插好。 +4. 设备被其他程序占用。 + +检查： + +```bash +ls /dev/ttyACM* /dev/ttyUSB* +``` + +修改 `runtime.yaml`： + +```yaml +remote_port: "/dev/ttyUSB0" +``` + +### 10.2 `/cmd_vel` 没有变化 + +检查： + +```bash +ros2 node list +ros2 topic echo /cmd_vel +``` + +如果节点存在但无变化，可能是： + +1. 遥控器没有输出 SBUS。 +2. 串口波特率不对。 +3. SBUS 接线错误。 +4. 遥控器通道未校准。 +5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。 + +### 10.3 摇杆方向反了 + +修改： + +```yaml +remote_invert_vx: true +remote_invert_vy: false +remote_invert_yaw: true +``` + +例如前后方向反了，就切换： + +```yaml +remote_invert_vx: false +``` + +### 10.4 急停后不恢复 + +当前配置： + +```yaml +remote_estop_latch: true +``` + +这表示急停锁存。触发后建议： + +1. 先确认机器人物理安全。 +2. 停止 launch。 +3. 将 CH7 打回安全位置。 +4. 重新启动系统。 + +如果需要非锁存模式，可以改为： + +```yaml +remote_estop_latch: false +``` + +但实机调试时更建议使用锁存模式。 + +## 11. 快速验证命令清单 + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +source install/setup.bash + +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +另开终端： + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +source install/setup.bash + +ros2 node list +ros2 topic echo /cmd_vel +ros2 topic echo /runtime/target --field target_source +``` + +如果只测遥控器，不启动电机系统： + +```bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml +``` + +另开终端： + +```bash +ros2 topic echo /cmd_vel +ros2 topic echo /safety/estop +``` diff --git a/05_software/real/sim2real_ros2_v3/docs/WEB_DEBUG_USAGE.md b/05_software/real/sim2real_ros2_v3/docs/WEB_DEBUG_USAGE.md new file mode 100644 index 0000000..17f54c4 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/docs/WEB_DEBUG_USAGE.md @@ -0,0 +1,246 @@ +# sim2real_ros2_v3 Web UDP 调试说明 + +本文档说明本次新增的最小 Web 调试链路。 + +## 1. 架构 + +```text +Windows 本地浏览器/HTTP 服务 + | + | UDP JSON + v +Nano: sim2real_web_udp_bridge_node.py + | + | ROS 2 topics + v +sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node +``` + +Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。 + +## 2. 新增 ROS2 节点 + +### `remote_uart_node.py` + +遥控器节点现在发布： + +```text +/cmd_vel_remote +``` + +不再直接发布 `/cmd_vel`。 + +通道触发阈值改为： + +```yaml +remote_axis_deadzone: 40 +remote_active_threshold: 40 +``` + +只有通道归一化值绝对值大于 `40` 才认为是有效输入。 + +### `cmd_mux_node.py` + +输入： + +```text +/cmd_vel_remote +/cmd_vel_web +/cmd_vel_nav +/control/mode +/remote/enabled +/web/enabled +/nav/enabled +/safety/estop +``` + +输出： + +```text +/cmd_vel +/control/mode_state +/control/mux_status +``` + +控制模式： + +```text +DISABLED +REMOTE +WEB +NAV +``` + +急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。 + +### `web_udp_bridge_node.py` + +Nano 端 UDP 监听： + +```text +0.0.0.0:15000 +``` + +发布： + +```text +/cmd_vel_web +/safety/estop +/control/mode +/web/enabled +/remote/enabled +/nav/enabled +``` + +订阅并回传状态： + +```text +/runtime/state +/runtime/target +/cmd_vel +/safety/estop +/control/mode_state +/control/mux_status +``` + +## 3. Nano 启动 + +```bash +cd /path/to/sim2real_ros2_v3 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +默认会启动： + +```text +sim2real_remote_uart_node +sim2real_cmd_mux_node +sim2real_web_udp_bridge_node +``` + +如果不想启动 Web UDP bridge： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false +``` + +## 4. Windows 本地 Web 启动 + +把目录复制到 Windows 或通过共享目录访问： + +```text +tools/win_web_debug +``` + +在 Windows 上安装 Python 3 后运行： + +```bash +python server.py --nano-host --http-port 8088 --udp-port 15001 +``` + +浏览器打开： + +```text +http://127.0.0.1:8088 +``` + +## 5. UDP 命令格式 + +### 切换模式 + +```json +{"type":"mode","mode":"REMOTE"} +``` + +```json +{"type":"mode","mode":"WEB"} +``` + +```json +{"type":"mode","mode":"DISABLED"} +``` + +### Web 速度控制 + +```json +{ + "type": "cmd_vel", + "linear": {"x": 0.2, "y": 0.0, "z": 0.0}, + "angular": {"x": 0.0, "y": 0.0, "z": 0.1} +} +``` + +Nano 端会再次限幅： + +```text +vx <= ±0.8 m/s +vy <= ±0.3 m/s +yaw <= ±0.5 rad/s +``` + +### 零速度 + +```json +{"type":"zero"} +``` + +### 软急停 + +```json +{"type":"estop","data":true} +``` + +## 6. 安全保护 + +当前最小版本已经包含： + +1. 遥控器误触发阈值：`40`。 +2. 遥控器/Web/Nav 互斥控制模式。 +3. `cmd_mux` 二次限幅。 +4. `cmd_mux` 加速度限制。 +5. Web UDP 超时自动发布零速度。 +6. 急停优先级最高。 +7. Web 页面切换到 `WEB` 模式需要确认。 +8. Web 松开虚拟摇杆会自动发送零速度。 + +建议实机调试流程： + +1. 先点击 `DISABLED`。 +2. 确认 `/cmd_vel` 为零。 +3. 如果使用遥控器，点击 `REMOTE`。 +4. 如果使用 Web，点击 `WEB` 并确认周围安全。 +5. 一旦异常，立即点击 `软急停`。 + +## 7. 验证命令 + +查看最终输出速度： + +```bash +ros2 topic echo /cmd_vel +``` + +查看遥控器输入： + +```bash +ros2 topic echo /cmd_vel_remote +``` + +查看 Web 输入： + +```bash +ros2 topic echo /cmd_vel_web +``` + +查看当前仲裁模式： + +```bash +ros2 topic echo /control/mode_state +``` + +查看策略状态： + +```bash +ros2 topic echo /runtime/target --field target_source +``` diff --git a/05_software/real/sim2real_ros2/docs/competition_yaw_configuration.md b/05_software/real/sim2real_ros2_v3/docs/competition_yaw_configuration.md similarity index 97% rename from 05_software/real/sim2real_ros2/docs/competition_yaw_configuration.md rename to 05_software/real/sim2real_ros2_v3/docs/competition_yaw_configuration.md index f8454ce..a40533c 100644 --- a/05_software/real/sim2real_ros2/docs/competition_yaw_configuration.md +++ b/05_software/real/sim2real_ros2_v3/docs/competition_yaw_configuration.md @@ -115,7 +115,7 @@ ros2 param get /sim2real_simple_nav_node nav_goal_yaw_tolerance_deg # 应该显示: 8.0 # 3. 加载路线 -# 使用: sim2real_ros2_v2_ooo/map/routes/points_nav1007_optimized.json +# 当前归档默认使用: sim2real_ros2_v3/map/routes/1hao_reall.json # 4. 监控 ros2 topic echo /cmd_vel_nav diff --git a/05_software/real/sim2real_ros2/map/1hao.pcd b/05_software/real/sim2real_ros2_v3/map/1hao.pcd similarity index 100% rename from 05_software/real/sim2real_ros2/map/1hao.pcd rename to 05_software/real/sim2real_ros2_v3/map/1hao.pcd diff --git a/05_software/real/sim2real_ros2/map/README.md b/05_software/real/sim2real_ros2_v3/map/README.md similarity index 100% rename from 05_software/real/sim2real_ros2/map/README.md rename to 05_software/real/sim2real_ros2_v3/map/README.md diff --git a/05_software/real/sim2real_ros2/map/routes/1hao_jianyi.json b/05_software/real/sim2real_ros2_v3/map/routes/1hao_jianyi.json similarity index 100% rename from 05_software/real/sim2real_ros2/map/routes/1hao_jianyi.json rename to 05_software/real/sim2real_ros2_v3/map/routes/1hao_jianyi.json diff --git a/05_software/real/sim2real_ros2/map/routes/1hao_not_raogan.json b/05_software/real/sim2real_ros2_v3/map/routes/1hao_not_raogan.json similarity index 100% rename from 05_software/real/sim2real_ros2/map/routes/1hao_not_raogan.json rename to 05_software/real/sim2real_ros2_v3/map/routes/1hao_not_raogan.json diff --git a/05_software/real/sim2real_ros2/map/routes/1hao_reall.json b/05_software/real/sim2real_ros2_v3/map/routes/1hao_reall.json similarity index 100% rename from 05_software/real/sim2real_ros2/map/routes/1hao_reall.json rename to 05_software/real/sim2real_ros2_v3/map/routes/1hao_reall.json diff --git a/05_software/real/sim2real_ros2/map/routes/2hao_nnewnnn.json b/05_software/real/sim2real_ros2_v3/map/routes/2hao_nnewnnn.json similarity index 100% rename from 05_software/real/sim2real_ros2/map/routes/2hao_nnewnnn.json rename to 05_software/real/sim2real_ros2_v3/map/routes/2hao_nnewnnn.json diff --git a/05_software/real/sim2real_ros2/map/routes/2hao_not_raogan.json b/05_software/real/sim2real_ros2_v3/map/routes/2hao_not_raogan.json similarity index 100% rename from 05_software/real/sim2real_ros2/map/routes/2hao_not_raogan.json rename to 05_software/real/sim2real_ros2_v3/map/routes/2hao_not_raogan.json diff --git a/05_software/real/sim2real_ros2/policies/README.md b/05_software/real/sim2real_ros2_v3/policies/README.md similarity index 100% rename from 05_software/real/sim2real_ros2/policies/README.md rename to 05_software/real/sim2real_ros2_v3/policies/README.md diff --git a/05_software/real/sim2real_ros2/policies/model_6800.onnx b/05_software/real/sim2real_ros2_v3/policies/model_6800.onnx similarity index 100% rename from 05_software/real/sim2real_ros2/policies/model_6800.onnx rename to 05_software/real/sim2real_ros2_v3/policies/model_6800.onnx diff --git a/05_software/real/sim2real_ros2/policies/model_6800_fp16.engine b/05_software/real/sim2real_ros2_v3/policies/model_6800_fp16.engine similarity index 100% rename from 05_software/real/sim2real_ros2/policies/model_6800_fp16.engine rename to 05_software/real/sim2real_ros2_v3/policies/model_6800_fp16.engine diff --git a/05_software/real/sim2real_ros2/policies/model_84.onnx b/05_software/real/sim2real_ros2_v3/policies/model_84.onnx similarity index 100% rename from 05_software/real/sim2real_ros2/policies/model_84.onnx rename to 05_software/real/sim2real_ros2_v3/policies/model_84.onnx diff --git a/05_software/real/sim2real_ros2/policies/model_84_fp16.engine b/05_software/real/sim2real_ros2_v3/policies/model_84_fp16.engine similarity index 100% rename from 05_software/real/sim2real_ros2/policies/model_84_fp16.engine rename to 05_software/real/sim2real_ros2_v3/policies/model_84_fp16.engine diff --git a/05_software/real/sim2real_ros2/screen/README.md b/05_software/real/sim2real_ros2_v3/screen/README.md similarity index 95% rename from 05_software/real/sim2real_ros2/screen/README.md rename to 05_software/real/sim2real_ros2_v3/screen/README.md index 9b9005b..d5f9141 100644 --- a/05_software/real/sim2real_ros2/screen/README.md +++ b/05_software/real/sim2real_ros2_v3/screen/README.md @@ -3,7 +3,7 @@ `fullscreen_quit.py` 是比赛 Orin 外接 `800×600` 屏幕使用的控制面板，通过本机 `http://127.0.0.1:18080/api/*` 调用 ROS 2 Web bridge，不建立第二套控制协议。 ```bash -cd +cd DISPLAY=:0 python3 screen/fullscreen_quit.py ``` diff --git a/05_software/real/sim2real_ros2/screen/check_display.py b/05_software/real/sim2real_ros2_v3/screen/check_display.py similarity index 100% rename from 05_software/real/sim2real_ros2/screen/check_display.py rename to 05_software/real/sim2real_ros2_v3/screen/check_display.py diff --git a/05_software/real/sim2real_ros2/screen/fullscreen_quit.py b/05_software/real/sim2real_ros2_v3/screen/fullscreen_quit.py similarity index 100% rename from 05_software/real/sim2real_ros2/screen/fullscreen_quit.py rename to 05_software/real/sim2real_ros2_v3/screen/fullscreen_quit.py diff --git a/05_software/real/sim2real_ros2/screen/install_autostart.sh b/05_software/real/sim2real_ros2_v3/screen/install_autostart.sh similarity index 100% rename from 05_software/real/sim2real_ros2/screen/install_autostart.sh rename to 05_software/real/sim2real_ros2_v3/screen/install_autostart.sh diff --git a/05_software/real/sim2real_ros2/screen/install_boot_service.sh b/05_software/real/sim2real_ros2_v3/screen/install_boot_service.sh similarity index 100% rename from 05_software/real/sim2real_ros2/screen/install_boot_service.sh rename to 05_software/real/sim2real_ros2_v3/screen/install_boot_service.sh diff --git a/05_software/real/sim2real_ros2/screen/run_boot_screen_service.sh b/05_software/real/sim2real_ros2_v3/screen/run_boot_screen_service.sh similarity index 100% rename from 05_software/real/sim2real_ros2/screen/run_boot_screen_service.sh rename to 05_software/real/sim2real_ros2_v3/screen/run_boot_screen_service.sh diff --git a/05_software/real/sim2real_ros2/screen/run_fullscreen_quit.sh b/05_software/real/sim2real_ros2_v3/screen/run_fullscreen_quit.sh similarity index 100% rename from 05_software/real/sim2real_ros2/screen/run_fullscreen_quit.sh rename to 05_software/real/sim2real_ros2_v3/screen/run_fullscreen_quit.sh diff --git a/05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview.png b/05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview.png similarity index 100% rename from 05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview.png rename to 05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview.png diff --git a/05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_current.png b/05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_current.png similarity index 100% rename from 05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_current.png rename to 05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_current.png diff --git a/05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_default_relocal.png b/05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_default_relocal.png similarity index 100% rename from 05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_default_relocal.png rename to 05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_default_relocal.png diff --git a/05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_loc_main_button.png b/05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_loc_main_button.png similarity index 100% rename from 05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_loc_main_button.png rename to 05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_loc_main_button.png diff --git a/05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_odom_re_rows.png b/05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_odom_re_rows.png similarity index 100% rename from 05_software/real/sim2real_ros2/screen/sim2real_screen_ui_preview_odom_re_rows.png rename to 05_software/real/sim2real_ros2_v3/screen/sim2real_screen_ui_preview_odom_re_rows.png diff --git a/05_software/real/sim2real_ros2/screen/uninstall_boot_service.sh b/05_software/real/sim2real_ros2_v3/screen/uninstall_boot_service.sh similarity index 100% rename from 05_software/real/sim2real_ros2/screen/uninstall_boot_service.sh rename to 05_software/real/sim2real_ros2_v3/screen/uninstall_boot_service.sh diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/.gitignore b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/.gitignore similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/.gitignore rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/.gitignore diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/CHANGELOG.md b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/CHANGELOG.md similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/CHANGELOG.md rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/CHANGELOG.md diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/CMakeLists.txt similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/CMakeLists.txt rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/CMakeLists.txt diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/LICENSE b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/LICENSE similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/LICENSE rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/LICENSE diff --git a/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/README.md b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/README.md new file mode 100644 index 0000000..f099caa --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/README.md @@ -0,0 +1,821 @@ +# Odin_ROS_Driver Readme + +ROS driver suite for Odin sensor modules (Manifold Tech Ltd.) + +Odin1 wiki: https://manifoldtechltd.github.io/wiki/Odin1/Cover.html + +## Odin_ROS_Driver + +Compatibility: + +● ROS 1(LTS Release: Noetic recommended) + +● ROS 2(LTS Release: Humble recommended) + +## Important Notice: + +This driver package provides core functionality for point cloud SLAM applications and targets specific use cases. It is intended exclusively for technical professionals conducting secondary development. End users must perform scenario-specific optimization and custom development to align with operational requirements in practical deployment environments. + +## 1. Version + +Current version: v0.12.0 + +Required device firmware version: v0.12.0 + +## 2. Preparation + +### 2.1 OS Requirement + +● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy; + +● Ubuntu 22.04 for ROS2 Humble; + +● Ubuntu 18.04 is currently not supported; + +● Ubuntu 24.04 is not officially supported but may work with some modifications. + +### 2.2 Dependencies + +● Opencv >= 4.2.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed) + +● yaml-cpp + +● thread + +● OpenSSL + +● Eigen3 + +### 2.3 Dependencies Install + +#### 2.3.1 System +```shell +sudo apt update +sudo apt-get install build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev +``` + +#### 2.3.2 yaml-cpp +```shell +sudo apt update +sudo apt install -y libyaml-cpp-dev +``` + +#### 2.3.3 libusb +```shell +sudo apt update +sudo apt install -y libusb-1.0-0-dev +``` + +#### 2.3.4 opencv +```shell +sudo apt update +sudo apt-get install libopencv-dev +``` + +#### 2.3.4 ROS install + +For ROS Noetic installation, please refer to: +[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation) + +For ROS2 Foxy installation, please refer to: +[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html) + +For ROS2 Humble installation, please refer to: +[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) + +## 3. Preparation + +### 3.1 Create Udev rules +```shell +sudo vim /etc/udev/rules.d/99-odin-usb.rules +``` +Add the following content to the 99-odin-usb.rules file +```shell +SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev" +``` +Reload rules and reinsert devices +```shell +sudo udevadm control --reload +sudo udevadm trigger +``` +### 3.2 OS Requirement +```shell +git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver +``` +Note: +Please clone the source code into the "[ros_workspace]/src/" folder, otherwise compilation errors will occur. + +### 3.3 make + +#### 3.3.1 ROS1 (Noetic for example): + +```shell +source /opt/ros/noetic/setup.bash +./script/build_ros.sh +``` + +#### 3.3.2 ROS2 (Foxy for example): + +```shell +source /opt/ros/foxy/setup.bash +./script/build_ros2.sh +``` + +### 3.4 run: + +#### 3.4.1 ROS1 (Noetic for example): + +```shell +source [ros_workspace]/devel/setup.bash +roslaunch odin_ros_driver [launch file] +``` +● odin_ros_driver: package name; + +● launch file: launch file; + +● ros_workspace: User's ROS environment workspace; +```shell +roslaunch odin_ros_driver odin1_ros1.launch +``` +#### 3.4.2 ROS2 (Foxy for example): + +```shell +source [ros2_workspace]/install/setup.bash +ros2 launch odin_ros_driver [launch file] +``` +● odin_ros_driver: package name; + +● launch file: launch file; + +● ros2_workspace: User's ROS2 environment workspace; + +ROS2 Demo Launch Instructions: +```shell +ros2 launch odin_ros_driver odin1_ros2.launch.py +``` + +### 3.5 Operation Mode: + +The operation mode can be configured via the `custom_map_mode` parameter in config/control_command.yaml. + +#### Odometry mode + +Set `custom_map_mode = 0` to enable odometry mode. In this mode, the map frame and odom frame share the same pose. + +If the odom data is found to drift, the script command "./set_param.sh algo_reset 1" can be used to dynamically reset the algorithm. + +#### SLAM mode + +Set `custom_map_mode = 1` to enable slam mode. This mode provides a complete SLAM system that builds upon the Odometry Mode by adding **loop closure detection** and **map saving** capabilities. + +After launching the driver, odin1 will automatically perform mapping and cache map data. When the scene capture is complete, users need to execute `./set_param.sh save_map 1` in the driver's source directory to save all map data collected since the program started. The map will be saved to the location specified by the `mapping_result_dest_dir` and `mapping_result_file_name` parameters in config/control_command.yaml. If these parameters are not specified, default values will be used. + +After the initial save, you can execute the command again to save a new map. Each save operation will generate a new map file. (Please allow at least 5 seconds between consecutive save operations) + +The map origin corresponds to the odom coordinate system's origin at the program's startup. + +##### Relocalization mode + +To enable relocalization, set `custom_map_mode = 2` and specify the absolute path to the pre-built map using the `relocalization_map_abs_path` parameter in config/control_command.yaml. + +Once launched, odin1 will initiate the relocalization process based on the current viewpoint and the specified map. To ensure a high success rate, it is recommended to starting within 1 meter ±10 degrees of the original position and orientation from the SLAM trajectory. + +Note that relocalization performance is highly environment-dependent. In highly distinctive scenes, successful matching may occur even beyond the 1m/10° range, while other environments may require more stringent conditions. We advise testing in your target environment to determine practical tolerances. + +If relocalization fails initially, the system will temporarily operate in a fallback SLAM mode (map saving is disabled in this state). During this time, you can freely move odin1. It will continue relocalization attempts in the background. Once successful, the TF between map and odom frames will be published. (Tip: Gently shaking or moving the device after initialization can help improve relocalization accuracy.) + +The following topics are published in the odom frame: `/odin1/cloud_slam, /odin1/odom, /odin1/highodom and /odin1/path`. To obtain these in the map frame, apply the TF from odom frame to map frame. + +## 4. File structure and data format +### 4.1 File structure +```shell +Odin_ROS_Driver/ // ROS1/ROS2 driver package + 3rdparty/ // Third-party libraries + src/ + host_sdk_sample.cpp // Example source code + yaml_parser.cpp // Source code for reading yaml parameters + rawCloudRender.cpp // Source code for RenderCloud + depth_image_ros_node.cpp //depth_image_ros_node + depth_image_ros2_node.cpp //depth_image_ros2_node + pcd2depth_ros.cpp //Source code for pcd2depth_ros + pcd2depth_ros2.cpp //Source code for pcd2depth_ros2 + pointcloud_depth_converter.cpp //Source code for pointcloud_depth_converter + cloud_reprojection_ros.cpp //Source code for cloud reprojection node (ROS1/ROS2) + cloud_reprojector.cpp //Core logic for cloud reprojection + lib/ + liblydHostApi_amd.a // Static library for AMD platform + liblydHostApi_arm.a // Static library for ARM platform + include/ + host_sdk_sample.h // Example header file + lidar_api_type.h // API data structure header file + lidar_api.h // API function declarations + yaml_parser.h // Parameter file reading header file + rawCloudRender.h // API about RenderCloud + data_logger.h // LOG about save_data + depth_image_ros_node.hpp // depth_image_ros_node + depth_image_ros2_node.hpp // depth_image_ros2_node + pointcloud_depth_converter.hpp // pointcloud_depth_convert + cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node (ROS1/ROS2) + cloud_reprojector.hpp // Core class for cloud reprojection + config/ + control_command.yaml // Control parameter file for driver + calib.yaml // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver + launch_ROS1/ + odin1_ros1.launch // ROS1 launch file + launch_ROS2/ + odin1_ros2.launch.py // ROS2 launch file + script/ + build_ros1.sh // Installation script for ROS1 + build_ros2.sh // Installation script for ROS2 + recorddata/ // holds recorded data that can import into MindCloud + log/ // holds log files + Driver_{timestamp}/ // holds all log folders for each time driver started + Conn_{timestamp}/ // holds all log files for each odin1 device connection + dev_status.csv // device status log file + README.md // Usage instructions + CMakeLists.txt // CMake build file + License // License file +``` +### 4.2 File structure +| Launch File Name | Description | +|--------------------------|-------------| +| odin1_ros1.launch | Launch file for ROS1 - Odin1 Basic Operations Demo | +| odin1_ros2.launch.py | Launch file for ROS2 - Odin1 Basic Operations Demo | + + +### 4.3 ROS topics +Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters: + +| Topic |control_command.yaml | Detailed Description | +|---------------------|----------------------|----------------------| +| odin1/imu | sendimu | Imu Topic | +| odin1/image | sendrgb | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format | +| odin1/image_undistort | sendrgbundistort | undistorted RGB Camera Topic, processed with calib.yaml from device | +| odin1/image/compressed | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device | +| odin1/cloud_raw | senddtof | Raw_Cloud Topic | +| odin1/cloud_render | sendcloudrender | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device | +| odin1/cloud_slam | sendcloudslam | Slam_PointCloud Topic | +| odin1/odometry | sendodom | Odom Topic | +| odin1/odometry_high | sendodom | high frequency Odom Topic | +| odin1/path | showpath | Odom Path Topic | +| tf | sendodom | tf tree Topic | +| odin1/depth_img_competetion | senddepth | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. | +| odin1/depth_img_competetion_cloud | senddepth | Dense Depth_Cloud Topic. Demo, high computing power required | +| odin1/reprojected_image | sendreprojection | Reprojected cloud to image Topic. Projects cloud_slam to camera image using odometry. Processed on host device. | + +### 4.4 Data format + +1. The raw point cloud (cloud_raw) has the following fields: +``` +float32 x // X axis, in meters +float32 y // Y axis, in meters +float32 z // Z axis, in meters +uint8 intensity // Reflectivity, range 0–255 +uint16 confidence // Point confidence, actual value range from 0 to around 1300 in typical scene, higher value means more reliable. Recommanded filtering threshold is 30-35, should be adjusted accordingly. +float32 offset_time // Time offset relative to the base timestamp unit: s +``` + +To work with this custom format in PCL, first define the point type: +```cpp +/*** LS ***/ +namespace ls_ros { + struct EIGEN_ALIGN16 Point { + float x; + float y; + float z; + uint8_t intensity; + uint16_t confidence; + float offset_time; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace ls_ros + +POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (uint8_t, intensity, intensity) + (uint16_t, confidence, confidence) + (float offset_time , offset_time) +) +``` +Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL point cloud: +``` +pcl::PointCloud ls_cloud; +pcl::fromROSMsg(*msg, ls_cloud); +``` + +2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields: +``` +float32 x // X axis, in meters +float32 y // Y axis, in meters +float32 z // Z axis, in meters +float32 rgb // RGB value +``` + +### 4.5 Other functionalities + +|control_command.yaml | Detailed Description | +|-----------------------|----------------------| +| use_host_ros_time | Time synchronization mode: 0 - use odin internal system time as data timestamp (typical and recommended); 1 - use host ROS time upon receive (not recommended for most users); 2 - align odin1 time to host time via NTP-like synchronization, timestamp is the sensor data reception time on host time axis. | +| strict_usb3.0_check | Strict USB3.0 check, if off, allow connection even if usb connection is below usb 3.0 | +| recorddata | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. The per-frame timestamps written into the recorded files (IMU / image / point cloud / pose / rotate) follow the same alignment policy as `use_host_ros_time`, so under NTP mode (`use_host_ros_time=1` or `2`) the recorded timestamps are NTP-aligned host time instead of odin1 boot time.
录制文件 (IMU / 图像 / 点云 / Pose / Rotate) 中每帧的时间戳与 `use_host_ros_time` 采用相同对齐策略：在 NTP 模式 (`use_host_ros_time=1` 或 `2`) 下，录制时间戳为 NTP 对齐后的主机时间，而非 odin1 开机时间。 | +| devstatuslog | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. | +| showcamerapose | Display Camera Pose and Field of View. | +| custom_map_mode | Operation Modes: Mode 0 - Odometry mode: The map frame and odom frame share the same pose. Mode 1 - Mapping (with loop closure) mode: This mode supports map saving. Mode 2 - Relocalization mode: Requires specifying the absolute path to the map file. After successful relocalization, it will output the TF relationship between the map and odom frames.| +| custom_init_pos | Initialization Position (currently unused). | +| relocalization_map_abs_path | Absolute Path to Map File: Used for relocalization mode. | +| mapping_result_dest_dir and mapping_result_file_name| Path and Name for Saving Maps in Mapping Mode: If not specified, default values will be used. | + +### 4.6 Runtime AE/AWB Tuning via ROS Service / 通过 ROS Service 在线调节 AE/AWB + +The driver hosts four ROS services that let a side terminal tune the +camera's auto exposure (AE) and auto white balance (AWB) at runtime, +while the main data streams keep flowing. The same SDK call is shared +with the driver's main control path and serialised by an internal +mutex, so it is safe to invoke these services concurrently with normal +operation. + +驱动启动后会注册 4 个 ROS Service，允许在不重启 driver 的前提下，从另一个终端动态调节 +相机的自动曝光（AE）和自动白平衡（AWB）。底层 SDK 调用与驱动主控制路径共享同一把 +互斥锁，因此可以与正常数据流并发调用。 + +**Service list / Service 一览** + +| Service name | Type / 类型 | Purpose / 用途 | +|---|---|---| +| `/odin1/get_ae` | `odin_ros_driver/srv/GetAe` | Query current AE status / 查询当前 AE 状态 | +| `/odin1/get_awb` | `odin_ros_driver/srv/GetAwb` | Query current AWB status / 查询当前 AWB 状态 | +| `/odin1/set_ae` | `odin_ros_driver/srv/SetAe` | Set AE mode and (manual) exposure / gain / 设置 AE 模式和手动曝光/增益 | +| `/odin1/set_awb` | `odin_ros_driver/srv/SetAwb` | Set AWB mode and (manual) R/B gain / 设置 AWB 模式和手动 R/B 增益 | + +#### 4.6.1 Request fields, ranges, physical meaning / 请求字段、范围与物理含义 + +**`SetAe.Request`** + +| Field | Range / 范围 | Meaning / 含义 | +|---|---|---| +| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AE loop, the two floats below are ignored / 设备自动调 AE，下方参数被忽略
`1` = device locks AE and applies the provided values / 设备锁 AE 并应用提供的值 | +| `exposure_time` | `0.0001` ~ `0.033` s (manual only / 仅手动模式) | Sensor exposure time per frame. Longer = brighter but more motion blur / 每帧传感器曝光时间。越长越亮但运动模糊增大 | +| `gain` | `1.0` ~ `64.0` (manual only / 仅手动模式) | Analog gain. Higher = brighter output but worse SNR / 模拟增益。越大越亮但信噪比越差 | + +**`SetAwb.Request`** + +| Field | Range / 范围 | Meaning / 含义 | +|---|---|---| +| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AWB loop / 设备自动 AWB
`1` = device locks AWB and applies provided gains / 设备锁定 AWB 并应用所给增益 | +| `rgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | R channel gain. Higher `rgain` vs `bgain` shifts the image warm (yellow/red) / R 通道增益，相对 bgain 越大，画面越偏暖 | +| `bgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | B channel gain. Higher `bgain` vs `rgain` shifts the image cool (blue) / B 通道增益，相对 rgain 越大，画面越偏冷 | + +> Gr / Gb channels are fixed to 1.0 by the device and are not adjustable. +> Gr / Gb 通道被设备固定为 1.0，不可调节。 + +#### 4.6.2 Response fields / 响应字段 + +All four services return a `success` (bool) and `rc` (int32). Get +services additionally return the queried state. +4 个 Service 都返回 `success` (bool) 与 `rc` (int32)。Get 类还会返回查询到的状态字段。 + +**`GetAe.Response`** + +| Field | Typical range / 典型范围 | Meaning / 含义 | +|---|---|---| +| `exposure_time` | `0.0001`~`0.033` s | Current exposure / 当前曝光时间 | +| `gain` | `1.0`~`64.0` | Current analog gain / 当前模拟增益 | +| `iso` | `100`~`6400` | Equivalent ISO / 等效 ISO | +| `brightness` | `0`~`255` | Average frame brightness / 平均帧亮度 | +| `is_converged` | `0` or `1` | `1` = AE settled / AE 已收敛 | +| `env_lv` | `0`~`15` | Ambient luminance index, higher = brighter / 环境光强度指数，越大越亮 | +| `fps` | `~10` / `~14.5` / `~29` | Current frame rate / 当前帧率 | + +**`GetAwb.Response`** + +| Field | Typical range / 典型范围 | Meaning / 含义 | +|---|---|---| +| `rgain` / `bgain` | `0.1`~`4.0` | R / B channel gain / R / B 通道增益 | +| `grgain` / `gbgain` | `1.0` (fixed / 固定) | Gr / Gb gain, device-fixed / Gr / Gb 增益，设备固定 | +| `cct` | `2500`~`8000` K | Correlated color temperature / 相关色温 | +| `ccri` | `-50`~`50` | Color temp deviation index, 0 = on Planckian locus / 色温偏离指数，0 表示在普朗克轨迹上 | +| `is_converged` | `0` or `1` | `1` = AWB settled / AWB 已收敛 | + +#### 4.6.3 `rc` return code / `rc` 返回码 + +| `rc` | Meaning / 含义 | +|---|---| +| `0` | Success / 成功 | +| `400` | Device payload too short / 设备载荷过短 | +| `401` | Device opcode not supported / 设备不支持该 opcode | +| `402` | Device parameter length wrong / 参数长度错误 | +| `403` | **Parameter out of range** / 参数越界 — most common when manual values exceed the table above / 手动值超出上表范围时最常见 | +| `404` | Device-side socket error / 设备端 socket 错误 | +| `405` | Device-side `ae_control` did not respond / 设备端 `ae_control` 无应答（确认 lydapp 已运行） | +| `255` (`0xFF`) | Unknown opcode reported by ae_control / ae_control 报未知 opcode | +| `-1` | SDK not initialised / SDK 未初始化 | +| `-2` ~ `-5` | USB transfer / timeout / malformed reply / USB 传输异常、超时、应答畸形 | +| `-100` | **Driver has not opened the device yet** / driver 还未打开设备，请等设备连接成功 | + +#### 4.6.4 Usage examples / 调用示例 + +ROS2 (Humble) — start the driver in one terminal, then in a side terminal: +ROS2（Humble）—— 在一个终端启动 driver，在另一个终端： + +```bash +source install/setup.bash + +# Query current state / 查询当前状态 +ros2 service call /odin1/get_ae odin_ros_driver/srv/GetAe +ros2 service call /odin1/get_awb odin_ros_driver/srv/GetAwb + +# Set AE to AUTO / 设置 AE 为自动 +ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}" + +# Set AE to MANUAL with 10 ms exposure and gain 4.0 +# 设置 AE 为手动，10 毫秒曝光，增益 4.0 +ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe \ + "{mode: 1, exposure_time: 0.010, gain: 4.0}" + +# Set AWB to MANUAL with rgain=1.5, bgain=2.0 +# 设置 AWB 为手动，rgain=1.5、bgain=2.0 +ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb \ + "{mode: 1, rgain: 1.5, bgain: 2.0}" + +# Restore AUTO / 一键回自动 +ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}" +ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb "{mode: 0}" + +# Inspect srv definition / 查看 srv 完整定义 +ros2 interface show odin_ros_driver/srv/SetAe +``` + +ROS1 (Noetic) — start the driver, then in a side terminal: +ROS1（Noetic）—— 启动 driver 后，新开终端： + +```bash +source devel/setup.bash + +# Query / 查询 +rosservice call /odin1/get_ae +rosservice call /odin1/get_awb + +# Set AE manual / 设置 AE 手动 +rosservice call /odin1/set_ae "{mode: 1, exposure_time: 0.010, gain: 4.0}" + +# Set AWB manual / 设置 AWB 手动 +rosservice call /odin1/set_awb "{mode: 1, rgain: 1.5, bgain: 2.0}" + +# Restore AUTO (ROS1 requires all fields to be present) +# 一键回自动（ROS1 要求填齐全部字段） +rosservice call /odin1/set_ae "{mode: 0, exposure_time: 0.0, gain: 0.0}" +rosservice call /odin1/set_awb "{mode: 0, rgain: 0.0, bgain: 0.0}" + +# Inspect srv definition / 查看 srv 完整定义 +rossrv show odin_ros_driver/SetAe +``` + +#### 4.6.5 Recommended starting points by scene / 不同场景推荐起步参数 + +**AE (`exposure_time`, `gain`)** + +| Scene / 场景 | `exposure_time` | `gain` | +|---|---|---| +| Bright outdoor / 明亮室外 | `0.001` ~ `0.005` s | `1.0` ~ `2.0` | +| Normal indoor / 普通室内 | `0.008` ~ `0.015` s | `2.0` ~ `8.0` | +| Dim light / 暗光环境 | `0.020` ~ `0.030` s | `8.0` ~ `32.0` | +| Very dark / 极暗 | `0.033` s | `32.0` ~ `64.0` | + +**AWB (`rgain`, `bgain`)** + +| Target tone / 目标色调 | `rgain` | `bgain` | +|---|---|---| +| Warm (tungsten, sunset) / 暖（钨丝灯、夕阳） | `2.0` ~ `2.5` | `1.0` ~ `1.2` | +| Neutral (D65 daylight) / 中性（D65 日光） | `1.5` ~ `1.7` | `1.8` ~ `2.0` | +| Cool (cloudy, fluorescent) / 冷（阴天、荧光） | `1.2` ~ `1.4` | `2.2` ~ `2.6` | +| Very cool / 极冷 | `1.0` | `3.0` ~ `4.0` | + +#### 4.6.6 Caveats / 注意事项 + +- The service blocks for up to ~10 s waiting for the device to reply; + typical latency is tens of milliseconds. + Service 最长阻塞约 10 秒等设备应答；正常几十毫秒返回。 +- Manual mode is **not** persisted across driver / device restart; + it falls back to AUTO on each new connection. + 手动模式**不会**跨重启保留；每次重连默认回到 AUTO。 +- `rc = -100` means the driver has not yet opened the device. + Wait until the driver logs `device connected` before calling. + 返回 `rc = -100` 表示 driver 还没打开设备，等到 driver 日志显示 `device connected` 再调用。 +- The effective maximum `exposure_time` is bounded by the frame + period `1 / fps`. With `dtof_fps = 290` (29 Hz, period ~34 ms) + the upper limit 0.033 s is already at the frame boundary. + 最大可用 `exposure_time` 受帧周期 `1/fps` 限制。在 `dtof_fps = 290`（29 Hz、周期 ~34 ms）下，上限 0.033 s 已经贴到帧边界。 + +## 5. FAQ +### 5.1 Segmentation fault upon re-launching host SDK +**Error Message** +No device connected after 60 seconds + +**Solution** +1. Please power on Odin module again # Disconnect and reconnect odin power + +2. Reinitialize Odin SDK # Execute SDK after device reboot + + +### 5.2 Library binding failure during compilation + +**Error Message** +ld: cannot find -llydHostApi or symbol lookup errors + +**Resolution** + +1. Clean previous build artifacts + +ROS1 +```shell +rm -rf devel/ build/ +``` +ROS2 +```shell +rm -rf devel/ install/ log/ +``` +2. Re-run script installation + +### 5.3 Docker GUI passthrough failure + +**Error Message** +Unable to open X display or No protocol specified + +**Resolution** +```shell +xhost + #This command enables graphical passthrough to Docker containers +``` + +### 5.4 ROS driver exit with get version failed error + +**Error Message** +```shell +: get device version fail. +get version failed. +``` + +**Resolution** + +Device firmware version is too low, please update to latest version. + + +### 5.5 RVIZ has not responded for a long time + +**Error Message** +Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection... + +**Resolution** + +Please power on Odin module again + +### 5.6 Device not responding + +**Error Message** +Missed ok response from device,probably wrong interaction procedure. + +**Resolution** + +Please adopt the solution mentioned in 5.1 + +### 5.7 Device has no external calibration file + +**Error Message** +ERROR：Missing camera node 'cam_0' + +**Resolution** + +Please plug and unplug the USB again + +### 5.8 ROS Driver report device disconnected immediately after stream started + +**Error Message** + +```shell +Device ready and streams activated +Device detaching... +Wating for device reconnection... +Device disconnected, waiting for reconnection... +``` + +**Reason** + +Mostly common on ros2 environment and connected to complex network environment, such as office wifi & ethernet. ROS2 default to broadcast, and complex network environment will cause ros2 publish to block, leading to device disconnection. + +**Resolution** + +If cross-device communication is not required, please restrict ros2 to localhost only with: +```shell +export ROS_LOCALHOST_ONLY=1 +``` + +If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended. + +### 5.9 ROS Driver died immediately after stream started + +**Error Message** + +```shell +Device ready and streams activated +[host_sdk_sample-2] process has died ...... +``` + +**Test** + +Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system. + +**Resolution** + +Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again. + +### 5.10 ROS Driver printing "TF_OLD_DATA ignoring data" warning + +**Error Message** + +```shell +[rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame odin1_base_link at time 20.547632 according to authority Authority undetectable +[rviz2-3] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained +[rviz2-3] at line 294 in ./src/buffer_core.cpp +``` + +**Reason** + +This is a ros & rviz feature to warn user that some tf data is being ignored due to timestamp conflicts. It happens when user keeps ros driver running and power-cycles odin device, which cause odin's internal system time being reset and now data timestamps conflicts with old data recieved by rviz during last run. + +**Resolution** + +There's a reset button on bottom of rviz gui. Click on this button will reset rviz's internal state and stop the warning. + +### 5.11 ROS Driver printing "unknown cmd code: xx" error + +**Error Message** + +```shell +: unknow command code 21. +``` + +**Reason** + +This is due to ros driver version mismatch with device firmware version, resulting in ros driver unable to decode new data added in newer firmware. + +**Resolution** + +Please make sure you are using most up-to-date ros driver and device firmware. + +### 5.12 USB device access error (LIBUSB_ERROR_BUSY or LIBUSB_ERROR_ACCESS) + +**Error Message** + +```shell +libusb: error [udev_hotplug_event] ignoring udev action bind +LIBUSB_ERROR_BUSY +``` + +or + +```shell +libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13 +LIBUSB_ERROR_ACCESS +``` + +**Reason** + +- **LIBUSB_ERROR_BUSY**: Another process is already using the USB device. This commonly happens when multiple instances of the ROS driver are running, or another application (such as a previous crashed instance) still holds the device handle. + +- **LIBUSB_ERROR_ACCESS**: The current user does not have permission to access the USB device. This is typically caused by missing udev rules or insufficient user privileges. + +**Resolution** + +For **LIBUSB_ERROR_BUSY**: + +1. Check if another instance of the driver is running: +```shell +ps aux | grep host_sdk_sample +``` + +2. Kill any existing instances: +```shell +killall host_sdk_sample +``` + +3. If the issue persists, unplug and replug the USB device to reset the device state. + +For **LIBUSB_ERROR_ACCESS**: + +1. Add udev rules for the device. Create a file `/etc/udev/rules.d/99-odin.rules` with the following content: +```shell +SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev" +``` + +2. Reload udev rules: +```shell +sudo udevadm control --reload-rules +sudo udevadm trigger +``` + +3. Alternatively, run the driver with sudo (not recommended for production): +```shell +sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py +``` + +4. Make sure your user is in the `plugdev` group: +```shell +sudo usermod -aG plugdev $USER +``` +Then log out and log back in for the group change to take effect. + +### 5.13 ros2 bag drops high-frequency topics (IMU / odometry_highfreq) / ros2 bag 录制丢失高频话题（IMU / odometry_highfreq） + +**Symptom / 现象** + +When recording with `ros2 bag record`, low-frequency topics (cloud, image, odometry, wiwc) are intact, but `/odin1/imu` (400 Hz) and `/odin1/odometry_highfreq` (400 Hz) show missing samples — analysis scripts report inter-message intervals that are 2× or more of the expected period, while no drop is reported on the SDK side or by an online subscriber such as `ros2 topic hz`. + +使用 `ros2 bag record` 录制时，低频话题（cloud、image、odometry、wiwc）完整无丢，但 `/odin1/imu`（400 Hz）和 `/odin1/odometry_highfreq`（400 Hz）会出现丢帧——分析脚本上看到消息间隔达到正常周期的 2 倍以上，而 SDK 侧不报丢，独立的 `ros2 topic hz` 订阅者也看不到丢。 + +**Reason / 原因** + +The driver publishes `/odin1/imu` and `/odin1/odometry_highfreq` with `RELIABLE` QoS. By default `ros2 bag record` subscribes with `history = keep_last`, `depth = 10`, which only buffers ~25 ms of samples at 400 Hz. Whenever the recorder is briefly delayed (disk flush, mcap/sqlite chunk write, scheduler jitter), its subscription queue overflows and DDS silently drops the oldest samples on the **subscriber side**. The SDK and publisher are unaffected, which is why no drop appears in the driver logs or in `ros2 topic hz`. + +驱动以 `RELIABLE` QoS 发布 `/odin1/imu` 与 `/odin1/odometry_highfreq`。`ros2 bag record` 默认订阅使用 `history = keep_last`、`depth = 10`，在 400 Hz 下只能缓冲约 25 ms。一旦录制端有短暂阻塞（落盘 flush、mcap/sqlite chunk 写入、调度抖动），订阅队列就会溢出，DDS 在**订阅端**静默丢掉最旧的样本。SDK 与 publisher 不受影响，因此驱动日志和 `ros2 topic hz` 都看不到丢。 + +**Resolution / 解决方案** + +Use the provided QoS override file `script/rosbag2_qos.yaml` to raise the subscriber-side queue depth on the recorder for the two high-rate topics: + +使用本仓库提供的 QoS 配置 `script/rosbag2_qos.yaml`，把高频话题的录制订阅 depth 拉大： + +```yaml +# script/rosbag2_qos.yaml +/odin1/imu: + reliability: reliable + history: keep_last + depth: 4000 + +/odin1/odometry_highfreq: + reliability: reliable + history: keep_last + depth: 4000 +``` + +Apply it when recording / 录制时通过 `--qos-profile-overrides-path` 应用： + +```shell +ros2 bag record -a \ + --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ + -o my_bag +``` + +Or only the high-rate topics / 也可以只录制高频话题： + +```shell +ros2 bag record \ + --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ + -o my_bag \ + /odin1/imu /odin1/odometry_highfreq /odin1/odometry /odin1/wiwc /odin1/cloud_raw +``` + +**Optional further tuning / 可选的进一步优化** + +If drops still occur after applying the override (typically on slower disks), try the following in addition / 套用上述 override 后仍有丢包时（通常发生在慢盘上），可叠加以下措施： + +```shell +# Use mcap backend with a larger internal cache (faster than sqlite3). +# 使用 mcap 后端 + 更大的内部缓存（比 sqlite3 快）。 +ros2 bag record -s mcap --max-cache-size 1073741824 \ + --qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \ + -o my_bag \ + /odin1/imu /odin1/odometry_highfreq ... + +# Enlarge kernel UDP socket buffers (the most common hidden bottleneck for +# 400 Hz RELIABLE traffic, default is only 208 KB). +# 放大内核 UDP socket buffer（400 Hz RELIABLE 流量最常见的隐藏瓶颈，默认仅 208 KB）。 +sudo sysctl -w net.core.rmem_max=33554432 +sudo sysctl -w net.core.wmem_max=33554432 +``` + +**Does ROS1 have the same problem? / ROS1 是否存在同样的问题？** + +No. ROS1 uses TCP-based publish/subscribe with a single `queue_size` parameter on each side, and has no QoS profile mismatch between publisher and subscriber. The ROS1 publisher path in this driver already sizes the IMU and `odometry_highfreq` publishers to `queue_size = 4000` (`include/host_sdk_sample.h`, see `initialize_publishers` ROS1 branch), and `rosbag record` uses TCP transport which is reliable by construction. As a result this specific drop pattern does not occur under ROS1; no additional configuration is required. + +不存在。ROS1 使用基于 TCP 的发布/订阅，发布端与订阅端各自只有一个 `queue_size` 参数，不存在 ROS2 那种 QoS profile 不匹配的问题。本驱动 ROS1 路径已经把 IMU 与 `odometry_highfreq` 的发布队列设置为 `queue_size = 4000`（见 `include/host_sdk_sample.h` 中 `initialize_publishers` 的 ROS1 分支），并且 `rosbag record` 使用 TCP 传输本身即可靠传递。因此在 ROS1 下不会出现该丢帧现象，也不需要额外配置。 + +## 6. Contact Information​​ + +You can contact our support through support@manifoldtech.cn + +To help diagnose the issue, please provide the following details to our FAE engineer: + +1. Current firmware version​​ +```shell +[device_version_capture]: ros_driver_version: [Version Number] +``` +2. Photos of power adapter and converter cable​​ in use. + +3. Does the issue happen occasionally or consistently? + +4. Provide images of the problem scenario. + +5. Did the troubleshooting methods in ​​Section V​​ resolve the issue? + +6. Expected timeline for issue resolution. diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/RELOCALIZATION_GUIDE.md b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/RELOCALIZATION_GUIDE.md similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/RELOCALIZATION_GUIDE.md rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/RELOCALIZATION_GUIDE.md diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/calib.yaml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/calib.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/calib.yaml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/calib.yaml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command.yaml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command.yaml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command.yaml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command_odom.yaml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command_odom.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command_odom.yaml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command_odom.yaml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command_relocal.yaml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command_relocal.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/control_command_relocal.yaml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/control_command_relocal.yaml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/odin_ros.rviz b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/odin_ros.rviz similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/odin_ros.rviz rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/odin_ros.rviz diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/config/odin_ros2.rviz b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/odin_ros2.rviz similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/config/odin_ros2.rviz rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/config/odin_ros2.rviz diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/camera_pose_visualization.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/camera_pose_visualization.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/camera_pose_visualization.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/camera_pose_visualization.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/cloud_reprojection_ros_node.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/cloud_reprojection_ros_node.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/cloud_reprojection_ros_node.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/cloud_reprojection_ros_node.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/cloud_reprojector.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/cloud_reprojector.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/cloud_reprojector.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/cloud_reprojector.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/data_logger.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/data_logger.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/data_logger.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/data_logger.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/depth_image_ros2_node.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/depth_image_ros2_node.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/depth_image_ros2_node.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/depth_image_ros2_node.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/depth_image_ros_node.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/depth_image_ros_node.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/depth_image_ros_node.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/depth_image_ros_node.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/host_sdk_sample.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/host_sdk_sample.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/host_sdk_sample.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/host_sdk_sample.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/image_overlay_node.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/image_overlay_node.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/image_overlay_node.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/image_overlay_node.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/lidar_api.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/lidar_api.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/lidar_api.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/lidar_api.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/lidar_api_type.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/lidar_api_type.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/lidar_api_type.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/lidar_api_type.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/pointcloud_depth_converter.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/pointcloud_depth_converter.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/pointcloud_depth_converter.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/pointcloud_depth_converter.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/polynomial_camera.hpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/polynomial_camera.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/polynomial_camera.hpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/polynomial_camera.hpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/rawCloudRender.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/rawCloudRender.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/rawCloudRender.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/rawCloudRender.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/include/yaml_parser.h b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/yaml_parser.h similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/include/yaml_parser.h rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/include/yaml_parser.h diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/launch_ROS1/odin1_ros1.launch b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/launch_ROS1/odin1_ros1.launch similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/launch_ROS1/odin1_ros1.launch rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/launch_ROS1/odin1_ros1.launch diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/launch_ROS2/odin1_ros2.launch.py b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/launch_ROS2/odin1_ros2.launch.py similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/launch_ROS2/odin1_ros2.launch.py rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/launch_ROS2/odin1_ros2.launch.py diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_amd.a b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/liblydHostApi_amd.a similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_amd.a rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/liblydHostApi_amd.a diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_arm.a b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/liblydHostApi_arm.a similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_arm.a rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/liblydHostApi_arm.a diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/package.xml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package.xml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/package.xml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package.xml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/package_ros1.xml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package_ros1.xml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/package_ros1.xml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package_ros1.xml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/package_ros2.xml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package_ros2.xml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/package_ros2.xml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/package_ros2.xml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/script/build_ros.sh b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/build_ros.sh similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/script/build_ros.sh rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/build_ros.sh diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/script/build_ros2.sh b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/build_ros2.sh similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/script/build_ros2.sh rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/build_ros2.sh diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/script/rosbag2_qos.yaml b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/rosbag2_qos.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/script/rosbag2_qos.yaml rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/script/rosbag2_qos.yaml diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/set_param.sh b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/set_param.sh similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/set_param.sh rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/set_param.sh diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/camera_pose_visualization.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/camera_pose_visualization.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/camera_pose_visualization.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/camera_pose_visualization.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/cloud_reprojection_ros.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/cloud_reprojection_ros.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/cloud_reprojection_ros.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/cloud_reprojection_ros.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/cloud_reprojector.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/cloud_reprojector.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/cloud_reprojector.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/cloud_reprojector.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/depth_image_ros2_node.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/depth_image_ros2_node.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/depth_image_ros2_node.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/depth_image_ros2_node.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/depth_image_ros_node.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/depth_image_ros_node.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/depth_image_ros_node.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/depth_image_ros_node.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/host_sdk_sample.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/host_sdk_sample.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/host_sdk_sample.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/host_sdk_sample.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/image_overlay_node.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/image_overlay_node.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/image_overlay_node.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/image_overlay_node.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/pcd2depth_ros.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pcd2depth_ros.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/pcd2depth_ros.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pcd2depth_ros.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/pcd2depth_ros2.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pcd2depth_ros2.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/pcd2depth_ros2.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pcd2depth_ros2.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/pointcloud_depth_converter.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pointcloud_depth_converter.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/pointcloud_depth_converter.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/pointcloud_depth_converter.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/rawCloudRender.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/rawCloudRender.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/rawCloudRender.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/rawCloudRender.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/src/yaml_parser.cpp b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/yaml_parser.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/src/yaml_parser.cpp rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/src/yaml_parser.cpp diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/srv/GetAe.srv b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/GetAe.srv similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/srv/GetAe.srv rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/GetAe.srv diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/srv/GetAwb.srv b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/GetAwb.srv similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/srv/GetAwb.srv rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/GetAwb.srv diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/srv/SetAe.srv b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/SetAe.srv similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/srv/SetAe.srv rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/SetAe.srv diff --git a/05_software/real/sim2real_ros2/src/odin_ros_driver/srv/SetAwb.srv b/05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/SetAwb.srv similarity index 100% rename from 05_software/real/sim2real_ros2/src/odin_ros_driver/srv/SetAwb.srv rename to 05_software/real/sim2real_ros2_v3/src/odin_ros_driver/srv/SetAwb.srv diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/CMakeLists.txt new file mode 100644 index 0000000..260fc4a --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_bringup) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/deployment_contract.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/deployment_contract.yaml new file mode 100644 index 0000000..cd931ee --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/deployment_contract.yaml @@ -0,0 +1,103 @@ +# ============================================================================= +# 部署契约参考文件（仅供参考，C++ 代码不读取此文件） +# ============================================================================= +# +# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在 +# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。 +# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！ +# 如需修改部署参数，请同步更新 .hpp 文件和本文件。 +# +# ============================================================================= + +model: + path: "policies/model_rough.onnx" + source_pt: "policies/model_rough.pt" + backend: "onnxruntime" + obs_dim: 53 + action_dim: 16 + clip_obs: 100.0 + +observation: + terms: + - {name: base_ang_vel, dim: 3, scale: 0.25} + - {name: projected_gravity, dim: 3} + - {name: command, dim: 3} + - {name: joint_pos_rel, dim: 12} + - {name: joint_vel_rel, dim: 12, scale: 0.05} + - {name: wheel_vel, dim: 4, scale: 0.05} + - {name: last_actions, dim: 16} + +action: + joint_order: + - fl_hip_abduction + - fl_hip_pitch + - fl_knee + - fr_hip_abduction + - fr_hip_pitch + - fr_knee + - rl_hip_abduction + - rl_hip_pitch + - rl_knee + - rr_hip_abduction + - rr_hip_pitch + - rr_knee + - fl_wheel + - fr_wheel + - rl_wheel + - rr_wheel + wheel_indices: [12, 13, 14, 15] + scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0] + default_dof_pos: [0.0, 0.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: + fl_hip_abduction: [1, 1] + fl_hip_pitch: [1, 2] + fl_knee: [1, 3] + fl_wheel: [1, 4] + fr_hip_abduction: [1, 5] + fr_hip_pitch: [1, 6] + fr_knee: [1, 7] + fr_wheel: [1, 8] + rl_hip_abduction: [2, 1] + rl_hip_pitch: [2, 2] + rl_knee: [2, 3] + rl_wheel: [2, 4] + rr_hip_abduction: [2, 5] + rr_hip_pitch: [2, 6] + rr_knee: [2, 7] + rr_wheel: [2, 8] + direction_map: + fl_hip_abduction: -1 + fl_hip_pitch: -1 + fl_knee: -1 + fl_wheel: -1 + fr_hip_abduction: -1 + fr_hip_pitch: 1 + fr_knee: 1 + fr_wheel: 1 + rl_hip_abduction: 1 + rl_hip_pitch: -1 + rl_knee: -1 + rl_wheel: -1 + rr_hip_abduction: 1 + rr_hip_pitch: 1 + rr_knee: 1 + rr_wheel: 1 + zero_offset_map: + fl_hip_abduction: 0.003 + fl_hip_pitch: 0.030 + fl_knee: 0.028 + fl_wheel: 0.0 + fr_hip_abduction: 0.004 + fr_hip_pitch: 0.038 + fr_knee: 0.011 + fr_wheel: 0.0 + rl_hip_abduction: 0.019 + rl_hip_pitch: -0.034 + rl_knee: 0.025 + rl_wheel: 0.0 + rr_hip_abduction: -0.001 + rr_hip_pitch: 0.039 + rr_knee: 0.018 + rr_wheel: 0.0 diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/runtime.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/runtime.yaml new file mode 100644 index 0000000..cb6e208 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/config/runtime.yaml @@ -0,0 +1,304 @@ +/**: + ros__parameters: + policy_hz: 50.0 + motor_hz: 200.0 + status_hz: 10.0 + target_timeout_ms: 150.0 + event_log_dir: "logs_v2_web" + model_engine_path: policies/model_6800_fp16.engine + prefer_tensorrt: true + model_path: policies/model_6800.onnx + rough_model_engine_path: policies/model_6800_fp16.engine + crawl_model_path: policies/model_crawl.onnx # unused while crawl_backend is "ik" + crawl_model_engine_path: "" + wall_model_path: policies/model_84.onnx + wall_model_engine_path: policies/model_84_fp16.engine + rough_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0] + wall_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0] + crawl_backend: "ik" + crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0] + crawl_ik_wheel_linear_gain: 12.5 + crawl_ik_wheel_yaw_gain: 8.0 + crawl_ik_max_wheel_speed: 12.0 + crawl_ik_abduction_clip: 0.45 + crawl_ik_yaw_rate_kp: 0.5 + crawl_ik_imu_posture: false + crawl_ik_encoder_posture_kp: 0.0 + crawl_ik_encoder_posture_max: 0.03 + crawl_ik_encoder_guard: false + crawl_ik_encoder_guard_start: 0.28 + crawl_ik_encoder_guard_stop: 0.65 + crawl_ik_imu_guard: true + crawl_ik_imu_guard_start_deg: 12.0 + crawl_ik_imu_guard_stop_deg: 28.0 + model_switch_transition_s: 0.9 + model_switch_min_transition_s: 0.4 + model_switch_to_stand_transition_scale: 2.1 + model_switch_to_model_transition_scale: 2.4 + model_switch_stand_hold_s: 0.45 + model_switch_stand_max_err: 0.18 + model_switch_stand_max_vel: 0.8 + model_switch_release_scale: 1.0 + runtime_max_vx: 0.9 + runtime_max_vy: 0.5 + runtime_max_yaw_rate: 0.85 + debug_trace_enabled: true + debug_trace_decimation: 1 + use_cuda: true # enable CUDA Execution Provider on Orin Nano GPU + contract_file: deployment_contract.yaml + dry_run: false + can0_name: "can0" + can1_name: "can1" + imu_topic: "/odin1/imu" + odom_topic: "/odom" + + # Remote UART / SBUS parameters, aligned with the first-generation Python deployment + remote_enabled: false # true + remote_port: "/dev/ttyACM0" + remote_baudrate: 100000 + remote_timeout: 0.02 + remote_axis_deadzone: 40 + remote_active_threshold: 40 + remote_axis_full_scale: 660.0 + remote_max_vx: 0.9 + remote_max_vy: 0.5 + remote_max_yaw_rate: 0.85 + remote_invert_vx: true + remote_invert_vy: false + remote_invert_yaw: true + remote_publish_inactive_zero: true + remote_estop_latch: true + remote_poll_hz: 50.0 + remote_model_switch_enabled: true + remote_model_switch_channel: 10 + remote_model_switch_debounce_frames: 3 + remote_model_switch_rough_level: "low" + remote_model_switch_ik_level: "high" + + # Command mux parameters + cmd_mux_default_mode: "NAV" + cmd_mux_output_hz: 50.0 + cmd_mux_remote_timeout_ms: 250.0 + cmd_mux_web_timeout_ms: 300.0 + cmd_mux_nav_timeout_ms: 500.0 + cmd_mux_max_vx: 0.9 + cmd_mux_max_vy: 0.5 + cmd_mux_max_yaw_rate: 0.85 + cmd_mux_max_vx_acc: 1.0 + cmd_mux_max_vy_acc: 1.0 + cmd_mux_max_yaw_acc: 1.5 + cmd_mux_max_vx_decel: 2.0 + cmd_mux_max_vy_decel: 2.0 + cmd_mux_max_yaw_decel: 2.0 + # The rough locomotion policy has an approximately 0.2 m/s linear command dead zone. + # Skip that ineffective band on start-up, but still allow exact zero for braking/estop. + cmd_mux_linear_deadzone_epsilon: 0.05 + cmd_mux_yaw_deadzone_epsilon: 0.02 + cmd_mux_min_effective_vx: 0.22 + cmd_mux_min_effective_vy: 0.22 + cmd_mux_min_effective_yaw_rate: 0.0 + cmd_mux_deadzone_sources: "nav" + + # Windows/Nano Web UDP bridge parameters + web_bridge_enabled: true + web_http_host: "0.0.0.0" + web_http_port: 18080 + web_static_dir: "" + # Odom task actual path export; files can be opened by nav_tools over the PCD. + odom_trace_export_dir: "map/load" + web_udp_listen_host: "0.0.0.0" + web_udp_listen_port: 15000 + web_udp_remote_host: "" + web_udp_remote_port: 15001 + web_udp_state_hz: 20.0 + web_udp_cmd_timeout_ms: 300.0 + web_udp_max_packet_bytes: 8192 + web_udp_max_vx: 0.9 + web_udp_max_vy: 0.3 + web_udp_max_yaw_rate: 0.85 + web_udp_estop_on_timeout: false + + # Safety parameters + safety_enabled: true + max_target_offset: 2.4 + model_switch_max_target_offset: 1.8 + hard_target_offset: 3.0 + max_ang_vel: 30.0 + max_tilt_z: -0.3 + clip_to_brake: 0 + imu_age_warn_ms: 60.0 + imu_age_stop_ms: 500.0 + wheel_no_effect_command_threshold: 1.0 + wheel_no_effect_min_response_ratio: 0.20 + wheel_no_effect_velocity_epsilon: 0.25 + wheel_no_effect_max_temperature_c: 90.0 + wheel_no_effect_min_bus_voltage_v: 18.0 + wheel_no_effect_command_warmup_cycles: 12 + wheel_no_effect_trigger_cycles: 30 + wheel_no_effect_attempt_limit: 2 + wheel_no_effect_cooldown_ms: 1200 + wheel_recovery_verify_timeout_ms: 180 + wheel_no_effect_diag_freshness_ms: 350 + wheel_no_effect_diag_request_period_ms: 80 + leg_no_effect_position_error_threshold: 0.18 + leg_no_effect_velocity_epsilon: 0.12 + leg_no_effect_max_estimated_current_arms: 4.0 + leg_no_effect_max_abs_torque_nm: 5.0 + leg_no_effect_max_temperature_c: 100.0 + leg_no_effect_min_bus_voltage_v: 18.0 + leg_no_effect_command_warmup_cycles: 40 + leg_no_effect_trigger_cycles: 25 + leg_no_effect_attempt_limit: 2 + leg_no_effect_cooldown_ms: 1200 + leg_recovery_verify_timeout_ms: 220 + + # Policy alignment with the first-generation Python deployment + command_release_s: 0.35 + release_command_hold_s: 0.12 + release_posture_max_err: 0.35 + release_target_blend_s: 0.30 + clip_obs: 100.0 + hold_zero_command_pose: true + enable_zero_cmd_suppression: true + require_active_command_to_release: true + zero_cmd_use_yaw_rate: true + + # Simple navigation parameters + localization_mode: "relocal" # relocal: wait for Odin map/odom TF; odom: bridge map->odom fallback + nav_map_frame: "map" + nav_odom_frame: "odom" + nav_base_frame: "base_link" + nav_control_hz: 20.0 + nav_goal_tolerance: 0.20 + nav_yaw_stop_threshold: 0.80 + nav_max_vx: 0.90 + nav_max_vy: 0.50 + nav_max_wz: 0.85 + nav_kp_dist: 0.80 + nav_kp_yaw: 1.80 + nav_goal_exit_tolerance_margin: 0.08 + nav_goal_complete_stable_cycles: 2 + nav_final_align_kp_yaw_scale: 0.60 + nav_final_align_max_wz: 0.45 + nav_final_align_creep_speed: 0.05 + nav_goal_yaw_tolerance_deg: 12.0 + nav_astar_enabled: true + nav_astar_resolution: 0.10 + nav_astar_pcd_sample_step: 5 + nav_astar_allow_diagonal: true + nav_astar_smooth_enabled: true + nav_astar_corner_blend_dist: 0.20 + nav_astar_waypoint_reach_dist: 0.18 + nav_astar_lookahead_dist: 0.35 + nav_astar_snap_radius: 0.60 + nav_astar_max_expansions: 120000 + # Slalom is treated as a continuous path by simple_nav even if the route JSON + # was saved without precisionFollow/stableCycles/lookahead metadata. + nav_slalom_auto_precision_enabled: true + nav_slalom_auto_precision_force: true + nav_slalom_task_names: "slalom" + nav_slalom_stable_cycles: 0 + nav_slalom_lookahead: 0.35 + nav_slalom_yaw_rate_limit: 0.45 + nav_slalom_tolerance: 0.15 + nav_slalom_max_vx: 0.58 + nav_slalom_min_vx: 0.22 + nav_slalom_curvature_slowdown_enabled: true + nav_slalom_min_turn_speed_scale: 0.45 + # Execute waypoints marked slalomStraight as odometry-closed scripted moves. + nav_slalom_script_enabled: true + nav_slalom_script_start_tolerance: 0.22 + nav_slalom_script_pos_tolerance: 0.10 + nav_slalom_script_yaw_tolerance_deg: 5.0 + nav_slalom_script_drive_yaw_deadband_deg: 8.0 + nav_slalom_script_stable_cycles: 1 + nav_slalom_script_rotate_steps_enabled: false + nav_slalom_script_final_rotate_enabled: false + nav_slalom_script_require_yaw_at_step: false + nav_slalom_script_kp_dist: 1.00 + nav_slalom_script_kp_yaw: 1.20 + nav_slalom_script_max_vx: 0.58 + nav_slalom_script_max_vy: 0.50 + nav_slalom_script_max_wz: 0.50 + nav_slalom_script_min_cmd_linear: 0.22 + nav_slalom_script_min_cmd_angular: 0.20 + nav_slalom_script_min_cmd_epsilon: 0.05 + nav_slalom_script_min_step_distance: 0.02 + nav_slalom_script_yaw_gate_deg: 8.0 + nav_slalom_script_lateral_gate: 0.07 + nav_slalom_script_lateral_slow_gate: 0.15 + nav_slalom_script_lateral_creep_vx: 0.22 + nav_slalom_script_drive_yaw_source: "segment" + nav_slalom_script_segment_yaw_min_dist: 0.45 + nav_precision_lateral_control_enabled: true + nav_precision_lateral_kp: 0.80 + nav_precision_lateral_max_vy: 0.22 + # Lightweight DWA-style local safety layer over the route/avoid polygons. + nav_local_planner_enabled: true + nav_local_planner_tasks: "slalom" + nav_local_planner_precision_enabled: true + nav_local_planner_sim_time: 0.9 + nav_local_planner_sim_dt: 0.1 + nav_local_planner_v_samples: 5 + nav_local_planner_w_samples: 7 + nav_local_planner_vy_samples: 3 + nav_local_planner_obstacle_margin: 0.08 + nav_local_planner_recovery_clearance_epsilon: 0.005 + # 0.0 means auto: use the nav_tools body+wheel lateral footprint. + nav_local_planner_robot_radius: 0.0 + nav_local_planner_clearance_weight: 2.0 + nav_local_planner_path_weight: 2.0 + nav_local_planner_heading_weight: 0.7 + nav_local_planner_speed_weight: 0.3 + nav_local_planner_nominal_weight: 1.0 + nav_local_planner_min_vx: 0.22 + nav_slalom_script_safety_filter_enabled: true + nav_local_planner_use_astar_grid: false + nav_turn_in_place_enabled: true + nav_turn_in_place_enter_yaw_deg: 70.0 + nav_turn_in_place_exit_yaw_deg: 18.0 + nav_turn_in_place_max_wz: 0.80 + nav_pre_dock_enabled: true + nav_pre_dock_distance: 0.35 + nav_pre_dock_tolerance: 0.18 + nav_pre_dock_skip_within_goal_dist: 0.45 + nav_goals_file: "" + nav_missions_file: "" + # Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below. + nav_route_file: "" + # Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes. + # The web "odom" button uses the first waypoint of this route as the fixed odom fallback start pose. + nav_route_task_file: map/routes/1hao_reall.json + nav_route_auto_align_enabled: false + nav_route_rotation_offset_deg: 0.0 + nav_route_align_max_angle_deg: 6.0 + nav_route_align_angle_step_deg: 0.5 + nav_route_align_search_radius: 0.35 + nav_avoid_regions_enabled: true + nav_avoid_region_margin: 0.0 + # 0.0 means auto: use the nav_tools body+wheel lateral footprint for avoid-region inflation. + nav_avoid_footprint_radius: 0.0 + nav_robot_body_length: 0.356 + nav_robot_body_width: 0.235 + nav_robot_body_center_x: 0.1518 + nav_robot_origin_from_front: 0.105 + nav_robot_pose_hip: 0.550 + nav_robot_pose_knee: -1.125 + nav_robot_wheel_vis_length: 0.16 + nav_robot_wheel_vis_width: 0.055 + nav_robot_footprint_padding: 0.02 + odom_fallback_require_odom_fresh: true + odom_fallback_max_odom_age_ms: 500.0 + odom_fallback_block_existing_map_odom_tf: true + odom_fallback_tf_conflict_window_s: 1.0 + odom_fallback_tf_conflict_xy_tolerance: 0.05 + odom_fallback_tf_conflict_yaw_tolerance_deg: 2.0 + # Keep odom fallback running if Odin relocalizes mid-task; hand off after mission end or Exit odom. + odom_fallback_stop_on_external_tf: false + pcd_nav_file: map/1hao.pcd + pcd_floor_z_min: -1.6 + pcd_floor_z_max: 0.4 + pcd_sample_step: 25 + pcd_robot_radius: 0.18 + + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/launch/sim2real_system.launch.py b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/launch/sim2real_system.launch.py new file mode 100644 index 0000000..807d751 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/launch/sim2real_system.launch.py @@ -0,0 +1,173 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + runtime_params = ParameterFile( + PathJoinSubstitution([ + FindPackageShare("sim2real_bringup"), + "config", + "runtime.yaml", + ]), + allow_substs=True, + ) + + # Declare launch configurations + launch_driver_arg = DeclareLaunchArgument( + 'launch_driver', + default_value='true', + description='Whether to launch the odin_ros_driver sensor node' + ) + + launch_nav2_arg = DeclareLaunchArgument( + 'launch_nav2', + default_value='false', + description='Whether to launch the Nav2 navigation stack' + ) + + launch_remote_arg = DeclareLaunchArgument( + 'launch_remote', + default_value='true', + description='Whether to launch the SBUS UART remote control node' + ) + + launch_web_bridge_arg = DeclareLaunchArgument( + 'launch_web_bridge', + default_value='true', + description='Whether to launch the Windows/Nano UDP web debug bridge' + ) + + launch_simple_nav_arg = DeclareLaunchArgument( + 'launch_simple_nav', + default_value='true', + description='Whether to launch the simple waypoint navigation node' + ) + + localization_mode_arg = DeclareLaunchArgument( + 'localization_mode', + default_value='relocal', + description='Localization profile: odom uses bridge fallback; relocal waits for Odin map/odom TF' + ) + + odin_config_file_arg = DeclareLaunchArgument( + 'odin_config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('odin_ros_driver'), + 'config', + 'control_command_relocal.yaml', + ]), + description='Odin control config YAML for the selected localization profile' + ) + + event_log_dir_arg = DeclareLaunchArgument( + 'event_log_dir', + default_value=PythonExpression([ + "'logs_v2_web/run_' + __import__('datetime').datetime.now().strftime('%Y-%m-%d_%H-%M-%S_%f')[:-3]" + ]), + description='Per-run event log directory' + ) + + # Include odin_ros_driver launch + driver_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('odin_ros_driver'), + 'launch', + 'odin1_ros2.launch.py' + ]) + ), + launch_arguments={ + 'launch_rviz': 'false', + 'config_file': LaunchConfiguration('odin_config_file'), + }.items(), + condition=IfCondition(LaunchConfiguration('launch_driver')) + ) + + # Include sim2real_nav2 launch + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('sim2real_nav2'), + 'launch', + 'nav2.launch.py' + ]) + ), + condition=IfCondition(LaunchConfiguration('launch_nav2')) + ) + + return LaunchDescription([ + launch_driver_arg, + launch_nav2_arg, + launch_remote_arg, + launch_web_bridge_arg, + launch_simple_nav_arg, + localization_mode_arg, + odin_config_file_arg, + event_log_dir_arg, + Node( + package="sim2real_hw", + executable="sim2real_hw_node", + name="sim2real_hw_node", + output="screen", + parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}], + ), + Node( + package="sim2real_runtime", + executable="sim2real_runtime_node", + name="sim2real_runtime_node", + output="screen", + parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}], + ), + Node( + package="sim2real_runtime", + executable="cmd_mux_node.py", + name="sim2real_cmd_mux_node", + output="screen", + parameters=[runtime_params], + ), + Node( + package="sim2real_runtime", + executable="web_udp_bridge_node.py", + name="sim2real_web_udp_bridge_node", + output="screen", + parameters=[runtime_params, {"localization_mode": LaunchConfiguration("localization_mode")}], + condition=IfCondition(LaunchConfiguration('launch_web_bridge')), + ), + Node( + package="sim2real_runtime", + executable="remote_uart_node.py", + name="sim2real_remote_uart_node", + output="screen", + parameters=[runtime_params], + condition=IfCondition(LaunchConfiguration('launch_remote')), + ), + Node( + package="sim2real_runtime", + executable="simple_nav_node.py", + name="sim2real_simple_nav_node", + output="screen", + parameters=[runtime_params], + condition=IfCondition(LaunchConfiguration('launch_simple_nav')), + ), + Node( + package="sim2real_runtime", + executable="odom_relay_node", + name="odom_relay_node", + output="screen", + parameters=[{ + "odom_input_topic": "/odin1/odometry", + "odom_output_topic": "/odom", + "base_frame": "base_link", + "publish_tf": True, + }], + condition=IfCondition(LaunchConfiguration('launch_driver')), + ), + driver_launch, + nav2_launch, + ]) diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/package.xml new file mode 100644 index 0000000..990dda7 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_bringup/package.xml @@ -0,0 +1,24 @@ + + + sim2real_bringup + 0.1.0 + Launch and configuration package for sim2real_ros2. + todo + Proprietary + + ament_cmake + + launch + launch_ros + sim2real_common + sim2real_hw + sim2real_interfaces + sim2real_runtime + sim2real_nav2 + odin_ros_driver + tf2_ros + + + ament_cmake + + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_common/CMakeLists.txt new file mode 100644 index 0000000..79683b9 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_common) + +find_package(ament_cmake REQUIRED) + +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories(${PROJECT_NAME} INTERFACE + $ + $ +) +target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_package() diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/deployment_contract.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/deployment_contract.hpp new file mode 100644 index 0000000..eb8cee7 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/deployment_contract.hpp @@ -0,0 +1,94 @@ +#pragma once + +#include +#include + +namespace sim2real_common +{ + +struct DeploymentContract +{ + static constexpr std::size_t kObsDim = 53; + static constexpr std::size_t kActionDim = 16; + static constexpr std::size_t kLegJointCount = 12; + static constexpr std::size_t kWheelCount = 4; + static constexpr double kPolicyHz = 50.0; + static constexpr double kMotorHz = 200.0; + static constexpr double kStatusHz = 10.0; + + static constexpr std::array kWheelIndices = {12, 13, 14, 15}; + static constexpr float kLegKp = 50.0f; + static constexpr float kLegKd = 1.5f; + static constexpr float kLegHoldKp = kLegKp; + static constexpr float kLegHoldKd = kLegKd; + static constexpr float kWheelKd = 1.0f; + + static constexpr std::array kCanBusMap = { + 1, 1, 1, // fl legs + 1, 1, 1, // fr legs + 2, 2, 2, // rl legs + 2, 2, 2, // rr legs + 1, 1, 2, 2 // wheels: fl, fr, rl, rr + }; + + static constexpr std::array kCanIdMap = { + 1, 2, 3, // fl legs + 5, 6, 7, // fr legs + 1, 2, 3, // rl legs + 5, 6, 7, // rr legs + 4, 8, 4, 8 // wheels: fl, fr, rl, rr + }; + + static constexpr std::array kDirectionMap = { + -1.0f, -1.0f, -1.0f, // fl + -1.0f, 1.0f, 1.0f, // fr + 1.0f, -1.0f, -1.0f, // rl + 1.0f, 1.0f, 1.0f, // rr + -1.0f, 1.0f, -1.0f, 1.0f // wheels + }; + + static constexpr std::array kZeroOffsetMap = { + 0.003f, 0.030f, 0.028f, // fl + 0.004f, 0.038f, 0.011f, // fr + 0.019f, -0.034f, 0.025f, // rl + -0.001f, 0.039f, 0.018f, // rr + 0.000f, 0.000f, 0.000f, 0.000f // wheels + }; + + static constexpr std::array kActionScale = { + 0.125f, 0.25f, 0.25f, + 0.125f, 0.25f, 0.25f, + 0.125f, 0.25f, 0.25f, + 0.125f, 0.25f, 0.25f, + 5.0f, 5.0f, 5.0f, 5.0f + }; + + static constexpr std::array kDefaultDofPos = { + 0.0f, 0.550f, -1.125f, + 0.0f, 0.550f, -1.125f, + 0.0f, 0.550f, -1.125f, + 0.0f, 0.550f, -1.125f, + 0.0f, 0.0f, 0.0f, 0.0f + }; +}; + +static constexpr std::array kJointLabels = { + "fl_hip_abduction", + "fl_hip_pitch", + "fl_knee", + "fr_hip_abduction", + "fr_hip_pitch", + "fr_knee", + "rl_hip_abduction", + "rl_hip_pitch", + "rl_knee", + "rr_hip_abduction", + "rr_hip_pitch", + "rr_knee", + "fl_wheel", + "fr_wheel", + "rl_wheel", + "rr_wheel" +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/event_logger.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/event_logger.hpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/event_logger.hpp rename to 05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/event_logger.hpp diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/low_pass_filter.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/low_pass_filter.hpp new file mode 100644 index 0000000..b05e187 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/low_pass_filter.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace sim2real_common +{ + +class LowPassFilter +{ +public: + LowPassFilter(double cutoff_freq, double dt, std::size_t dim) + : dim_(dim), initialized_(false) + { + alpha_ = static_cast(1.0 - std::exp(-2.0 * M_PI * cutoff_freq * dt)); + y_prev_.resize(dim, 0.0f); + } + + void filter(const float* x, float* y) + { + if (!initialized_) { + for (std::size_t i = 0; i < dim_; ++i) { + y_prev_[i] = x[i]; + } + initialized_ = true; + } + for (std::size_t i = 0; i < dim_; ++i) { + y[i] = alpha_ * x[i] + (1.0f - alpha_) * y_prev_[i]; + y_prev_[i] = y[i]; + } + } + + void filter(const std::vector& x, std::vector& y) + { + filter(x.data(), y.data()); + } + + void reset() + { + initialized_ = false; + } + +private: + float alpha_; + std::size_t dim_; + bool initialized_; + std::vector y_prev_; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/mahony_filter.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/mahony_filter.hpp new file mode 100644 index 0000000..8d9b964 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/mahony_filter.hpp @@ -0,0 +1,152 @@ +#pragma once + +#include +#include +#include + +namespace sim2real_common +{ + +// Helper to calculate gravity orientation from quaternion [w, x, y, z] +inline std::array get_gravity_orientation(const std::array& quat_wxyz) +{ + float qw = quat_wxyz[0]; + float qx = quat_wxyz[1]; + float qy = quat_wxyz[2]; + float qz = quat_wxyz[3]; + + float gx = 2.0f * (-qz * qx + qw * qy); + float gy = -2.0f * (qz * qy + qw * qx); + float gz = 1.0f - 2.0f * (qw * qw + qz * qz); + return {gx, gy, gz}; +} + +// Helper to create quaternion from acceleration vector +inline std::array quat_from_accel(const std::array& accel) +{ + float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]); + if (norm_a < 1e-9f) { + return {1.0f, 0.0f, 0.0f, 0.0f}; + } + + float ax = accel[0] / norm_a; + float ay = accel[1] / norm_a; + float az = accel[2] / norm_a; + + // Ref gravity vector is [0.0, 0.0, 1.0] + float cross_x = -ay; + float cross_y = ax; + float cross_z = 0.0f; + float dot = az; + + if (dot < -0.999999f) { + return {0.0f, 1.0f, 0.0f, 0.0f}; + } + + float s = std::sqrt((1.0f + dot) * 2.0f); + std::array q = { + s * 0.5f, + cross_x / s, + cross_y / s, + cross_z / s + }; + + float norm_q = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + if (norm_q < 1e-9f) { + return {1.0f, 0.0f, 0.0f, 0.0f}; + } + q[0] /= norm_q; + q[1] /= norm_q; + q[2] /= norm_q; + q[3] /= norm_q; + + return q; +} + +class MahonyFilter +{ +public: + MahonyFilter(float kp = 2.0f, float ki = 0.0f) + : kp_(kp), ki_(ki) + { + q_ = {1.0f, 0.0f, 0.0f, 0.0f}; + e_int_ = {0.0f, 0.0f, 0.0f}; + } + + void reset_with_accel(const std::array& accel) + { + q_ = quat_from_accel(accel); + e_int_ = {0.0f, 0.0f, 0.0f}; + } + + std::array update(const std::array& accel, const std::array& gyro, float dt) + { + float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]); + std::array gyro_corr = gyro; + + if (norm_a > 1e-6f) { + float ax = accel[0] / norm_a; + float ay = accel[1] / norm_a; + float az = accel[2] / norm_a; + + float qw = q_[0]; + float qx = q_[1]; + float qy = q_[2]; + float qz = q_[3]; + + float vx = 2.0f * (qx * qz - qw * qy); + float vy = 2.0f * (qw * qx + qy * qz); + float vz = qw * qw - qx * qx - qy * qy + qz * qz; + + // Error = cross(a, v) + float ex = ay * vz - az * vy; + float ey = az * vx - ax * vz; + float ez = ax * vy - ay * vx; + + if (ki_ > 0.0f) { + e_int_[0] += ex * dt; + e_int_[1] += ey * dt; + e_int_[2] += ez * dt; + } else { + e_int_ = {0.0f, 0.0f, 0.0f}; + } + + gyro_corr[0] += kp_ * ex + ki_ * e_int_[0]; + gyro_corr[1] += kp_ * ey + ki_ * e_int_[1]; + gyro_corr[2] += kp_ * ez + ki_ * e_int_[2]; + } + + float qw = q_[0]; + float qx = q_[1]; + float qy = q_[2]; + float qz = q_[3]; + + float q_dot_w = 0.5f * (-qx * gyro_corr[0] - qy * gyro_corr[1] - qz * gyro_corr[2]); + float q_dot_x = 0.5f * ( qw * gyro_corr[0] + qy * gyro_corr[2] - qz * gyro_corr[1]); + float q_dot_y = 0.5f * ( qw * gyro_corr[1] - qx * gyro_corr[2] + qz * gyro_corr[0]); + float q_dot_z = 0.5f * ( qw * gyro_corr[2] + qx * gyro_corr[1] - qy * gyro_corr[0]); + + q_[0] += q_dot_w * dt; + q_[1] += q_dot_x * dt; + q_[2] += q_dot_y * dt; + q_[3] += q_dot_z * dt; + + float norm_q = std::sqrt(q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3]) + 1e-9f; + q_[0] /= norm_q; + q_[1] /= norm_q; + q_[2] /= norm_q; + q_[3] /= norm_q; + + return q_; + } + + const std::array& get_q() const { return q_; } + +private: + float kp_; + float ki_; + std::array q_; + std::array e_int_; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/runtime_guard.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/runtime_guard.hpp new file mode 100644 index 0000000..5abe453 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/runtime_guard.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include +#include +#include +#include + +namespace sim2real_common +{ + +enum class GuardLevel : int { + OK = 0, + WARN = 1, + STOP = 2 +}; + +struct GuardDecision { + GuardLevel level{GuardLevel::OK}; + std::string reason; +}; + +class RuntimeGuard { +public: + RuntimeGuard( + float max_ang_vel = 12.0f, + float max_tilt_z = -0.30f, + float imu_age_warn_ms = 60.0f, + float imu_age_stop_ms = 200.0f) + : max_ang_vel_(max_ang_vel), + max_tilt_z_(max_tilt_z), + imu_age_warn_ms_(imu_age_warn_ms), + imu_age_stop_ms_(imu_age_stop_ms) + {} + + GuardDecision check( + const std::array& imu_gyro, + const std::array& projected_gravity, + float imu_age_ms, + bool estop_triggered, + const std::vector& extra_vals = {}) + { + GuardDecision decision; + + // 1) user E-stop + if (estop_triggered) { + decision.level = GuardLevel::STOP; + decision.reason = "user E-stop"; + return decision; + } + + // 2) NaN/Inf check + for (float v : imu_gyro) { + if (std::isnan(v) || std::isinf(v)) { + decision.level = GuardLevel::STOP; + decision.reason = "NaN/Inf detected in imu_gyro"; + return decision; + } + } + for (float v : projected_gravity) { + if (std::isnan(v) || std::isinf(v)) { + decision.level = GuardLevel::STOP; + decision.reason = "NaN/Inf detected in projected_gravity"; + return decision; + } + } + for (float v : extra_vals) { + if (std::isnan(v) || std::isinf(v)) { + decision.level = GuardLevel::STOP; + decision.reason = "NaN/Inf detected in checked values"; + return decision; + } + } + + // 3) IMU stale + if (imu_age_ms > imu_age_stop_ms_) { + decision.level = GuardLevel::STOP; + decision.reason = "IMU stale " + std::to_string(imu_age_ms) + "ms"; + return decision; + } + bool warned_imu = (imu_age_ms > imu_age_warn_ms_); + + // 4) Tilt check + if (projected_gravity[2] > max_tilt_z_) { + decision.level = GuardLevel::STOP; + decision.reason = "tilt: g_z=" + std::to_string(projected_gravity[2]); + return decision; + } + + // 5) Angular velocity check + float ang_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]); + if (ang_norm > max_ang_vel_) { + decision.level = GuardLevel::STOP; + decision.reason = "ang_vel overflow: |w|=" + std::to_string(ang_norm); + return decision; + } + + if (warned_imu) { + decision.level = GuardLevel::WARN; + decision.reason = "IMU age " + std::to_string(imu_age_ms) + "ms"; + return decision; + } + + decision.level = GuardLevel::OK; + return decision; + } + +private: + float max_ang_vel_; + float max_tilt_z_; + float imu_age_warn_ms_; + float imu_age_stop_ms_; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/safety_monitor.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/safety_monitor.hpp new file mode 100644 index 0000000..628f87f --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/safety_monitor.hpp @@ -0,0 +1,119 @@ +#pragma once + +#include +#include +#include +#include + +namespace sim2real_common +{ + +enum class SafetyLevel : int { + NORMAL = 0, + CLIP = 1, + BRAKE = 2, + ESTOP = 3 +}; + +struct SafetyDecision { + SafetyLevel level{SafetyLevel::NORMAL}; + std::string message; + std::array clipped_target{}; +}; + +class SafetyMonitor { +public: + SafetyMonitor( + float max_target_offset = 0.6f, + float max_ang_vel = 10.0f, + float max_tilt_z = -0.3f, + int clip_to_brake = 0, + float hard_target_offset = 1.2f) + : max_target_offset_(max_target_offset), + max_ang_vel_(max_ang_vel), + max_tilt_z_(max_tilt_z), + clip_to_brake_(clip_to_brake), + hard_target_offset_(hard_target_offset), + consecutive_clips_(0) + {} + + SafetyDecision check( + const std::array& target_pose, + const std::array& default_pose, + const std::array& imu_gyro, + const std::array& projected_gravity, + bool estop_triggered) + { + SafetyDecision decision; + decision.clipped_target = target_pose; + + if (estop_triggered) { + decision.level = SafetyLevel::ESTOP; + decision.message = "user E-stop"; + return decision; + } + + // Tilt check (g_z should be ~ -1.0, if it is > max_tilt_z e.g. -0.3, it is tilted) + if (projected_gravity[2] > max_tilt_z_) { + decision.level = SafetyLevel::BRAKE; + decision.message = "tilt detected: g_z=" + std::to_string(projected_gravity[2]); + return decision; + } + + // Angular velocity norm check + float ang_vel_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]); + if (ang_vel_norm > max_ang_vel_) { + decision.level = SafetyLevel::BRAKE; + decision.message = "angular velocity overflow: |w|=" + std::to_string(ang_vel_norm); + return decision; + } + + // Offset check + bool needs_clip = false; + float max_offset = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { // check leg joint offsets from default pose + float offset = target_pose[i] - default_pose[i]; + max_offset = std::max(max_offset, std::abs(offset)); + if (std::abs(offset) > max_target_offset_) { + needs_clip = true; + float clipped_val = std::clamp(offset, -max_target_offset_, max_target_offset_); + decision.clipped_target[i] = default_pose[i] + clipped_val; + } + } + + if (needs_clip) { + consecutive_clips_++; + if (hard_target_offset_ > 0.0f && max_offset > hard_target_offset_) { + decision.level = SafetyLevel::BRAKE; + decision.message = "target leg offset exceeds hard limit: " + std::to_string(max_offset); + return decision; + } + if (clip_to_brake_ > 0 && consecutive_clips_ >= clip_to_brake_) { + decision.level = SafetyLevel::BRAKE; + decision.message = "clipped " + std::to_string(consecutive_clips_) + " frames in a row"; + return decision; + } + decision.level = SafetyLevel::CLIP; + decision.message = "target leg offset out of range"; + return decision; + } + + consecutive_clips_ = 0; + decision.level = SafetyLevel::NORMAL; + return decision; + } + + void reset() { + consecutive_clips_ = 0; + } + +private: + float max_target_offset_; + float max_ang_vel_; + float max_tilt_z_; + int clip_to_brake_; + float hard_target_offset_; + int consecutive_clips_; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp new file mode 100644 index 0000000..c696572 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp @@ -0,0 +1,107 @@ +#pragma once + +#include +#include +#include + +#include "sim2real_common/deployment_contract.hpp" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace sim2real_common +{ + +class StandBalanceController +{ +public: + StandBalanceController(double control_dt = 0.02) + : control_dt_(control_dt) + { + reset(); + } + + void setNominalLegPose(float hip_pitch, float knee) + { + nominal_hip_pitch_ = hip_pitch; + nominal_knee_ = knee; + } + + void reset() + { + stable_time_ = 0.0f; + } + + std::array computeTarget( + const std::array& projected_gravity, + const std::array& imu_gyro, + const std::array& cmd) + { + const float hip_base = nominal_hip_pitch_; + const float knee_base = nominal_knee_; + + float roll = 0.0f; + float pitch = 0.0f; + estimateRollPitch(projected_gravity, roll, pitch); + + float roll_rate = imu_gyro[0]; + float pitch_rate = imu_gyro[1]; + + float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate; + + float lateral_lean = lateral_lean_gain_ * cmd[1]; + + std::array target{}; + for (int leg_idx = 0; leg_idx < 4; ++leg_idx) { + float side = (leg_idx == 0 || leg_idx == 2) ? 1.0f : -1.0f; + + target[leg_idx * 3 + 0] = std::clamp(side * roll_corr + lateral_lean, -hip_abduction_clip_, hip_abduction_clip_); + target[leg_idx * 3 + 1] = std::clamp(hip_base, hip_pitch_clip_[0], hip_pitch_clip_[1]); + target[leg_idx * 3 + 2] = std::clamp(knee_base, knee_clip_[0], knee_clip_[1]); + } + // wheels 0 + target[12] = target[13] = target[14] = target[15] = 0.0f; + + bool stable = (std::abs(roll * 180.0f / static_cast(M_PI)) <= stable_roll_deg_) && + (std::abs(pitch * 180.0f / static_cast(M_PI)) <= stable_pitch_deg_) && + (std::max(std::abs(roll_rate * 180.0f / static_cast(M_PI)), std::abs(pitch_rate * 180.0f / static_cast(M_PI))) <= stable_gyro_deg_s_); + + stable_time_ = stable ? (stable_time_ + static_cast(control_dt_)) : 0.0f; + + return target; + } + + bool isStable() const + { + return stable_time_ >= enter_hold_s_; + } + +private: + void estimateRollPitch(const std::array& projected_gravity, float& roll, float& pitch) + { + float gx = projected_gravity[0]; + float gy = projected_gravity[1]; + float gz = projected_gravity[2]; + roll = std::atan2(-gy, std::max(1e-6f, -gz)); + pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz))); + } + + double control_dt_; + float nominal_hip_pitch_{DeploymentContract::kDefaultDofPos[1]}; + float nominal_knee_{DeploymentContract::kDefaultDofPos[2]}; + float kp_roll_{0.85f}; + float kd_roll_rate_{0.03f}; + float lateral_lean_gain_{0.0f}; + float hip_abduction_clip_{0.45f}; + std::array hip_pitch_clip_{-1.0f, 2.5f}; + std::array knee_clip_{-2.6f, -0.3f}; + float stable_roll_deg_{6.0f}; + float stable_pitch_deg_{8.0f}; + float stable_gyro_deg_s_{45.0f}; + float enter_hold_s_{1.0f}; + + float stable_time_{0.0f}; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_common/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_common/package.xml new file mode 100644 index 0000000..da27c1e --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_common/package.xml @@ -0,0 +1,10 @@ + + + sim2real_common + 0.1.0 + Shared constants and deployment contract helpers for sim2real_ros2. + todo + Proprietary + + ament_cmake + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_hw/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/CMakeLists.txt new file mode 100644 index 0000000..d1b38b5 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_hw) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sim2real_common REQUIRED) +find_package(sim2real_interfaces REQUIRED) + +add_executable(sim2real_hw_node + src/hardware_bridge_node.cpp +) + +target_include_directories(sim2real_hw_node PRIVATE include) +target_compile_features(sim2real_hw_node PRIVATE cxx_std_17) +ament_target_dependencies(sim2real_hw_node + rclcpp + sensor_msgs + std_msgs + nav_msgs + sim2real_common + sim2real_interfaces +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS sim2real_hw_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp new file mode 100644 index 0000000..7612333 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp @@ -0,0 +1,323 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "sim2real_interfaces/msg/runtime_state.hpp" +#include "sim2real_interfaces/msg/runtime_target.hpp" +#include "sim2real_common/event_logger.hpp" +#include "sim2real_common/low_pass_filter.hpp" +#include "sim2real_common/mahony_filter.hpp" +#include "sim2real_common/safety_monitor.hpp" +#include "sim2real_common/runtime_guard.hpp" + +struct can_frame; + +namespace sim2real_hw +{ + +enum class RecoveryKind +{ + None, + Stale, + NoEffect +}; + +enum class RecoveryStage +{ + Idle, + AwaitInitFeedback, + AwaitEffectVerification +}; + +enum class ActiveModelMode +{ + Rough, + Crawl, + Wall +}; + +struct MotorConfig +{ + int bus; // 1 or 2 + int id; // motor CAN id + float direction; + float offset; +}; + +struct MotorStateInternal +{ + float position{0.0f}; + float velocity{0.0f}; + float torque{0.0f}; + float temperature{0.0f}; + float bus_voltage{0.0f}; + float estimated_current_arms{0.0f}; + float last_command_sim{0.0f}; + std::uint32_t update_count{0}; + std::uint32_t stale_count{0}; + std::uint32_t command_active_count{0}; + std::uint32_t no_effect_count{0}; + std::uint16_t fault_code{0}; + std::uint16_t fault_detail_1{0}; + std::uint16_t fault_detail_2{0}; + // Hold-over state + float last_valid_pos{0.0f}; + float last_valid_vel{0.0f}; + float last_valid_torque{0.0f}; + std::uint32_t prev_update_count{0}; + bool has_valid_data{false}; + bool has_bus_voltage{false}; + bool has_fault_snapshot{false}; + bool stale_reported{false}; + bool recovered_reported{false}; + bool disable_reported{false}; + bool command_effect_monitoring_active{false}; + bool no_effect_reported{false}; + bool high_temp_reported{false}; + bool high_current_reported{false}; + bool high_voltage_reported{false}; + bool low_voltage_reported{false}; + bool fault_code_reported{false}; + bool init_confirmed{false}; + std::uint32_t init_attempt_count{0}; + std::uint32_t recovery_attempt_count{0}; + std::uint32_t no_effect_recovery_attempt_count{0}; + std::chrono::steady_clock::time_point last_recovery_attempt_time_{}; + std::chrono::steady_clock::time_point last_no_effect_recovery_attempt_time_{}; + std::chrono::steady_clock::time_point last_diag_snapshot_time_{}; + std::chrono::steady_clock::time_point last_diag_request_time_{}; + std::chrono::steady_clock::time_point recovery_stage_deadline_{}; + std::uint32_t recovery_start_update_count{0}; + std::uint32_t recovery_active_attempt_number{0}; + RecoveryKind recovery_kind{RecoveryKind::None}; + RecoveryStage recovery_stage{RecoveryStage::Idle}; + std::string recovery_trigger; + std::string last_power_event_reason; +}; + +class HardwareBridgeNode : public rclcpp::Node +{ +public: + HardwareBridgeNode(); + ~HardwareBridgeNode(); + +private: + void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg); + void onModelStatus(const std_msgs::msg::String::SharedPtr msg); + void onReadLoop(); + void onWriteLoop(); + void onImu(const sensor_msgs::msg::Imu::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + + bool initCan(const std::string& ifname, int& fd); + bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc); + bool readCanFrame(int fd, void* frame, int timeout_us); + + bool enableMotor(int fd, int motor_id); + bool disableMotor(int fd, int motor_id, bool 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); + bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque); + bool initializeMotor(std::size_t index, const std::string & reason, int max_attempts = 3); + bool initializeMotorsOnBus(int bus_id, const std::string & reason); + bool waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout); + void processCanFrame(const struct can_frame & frame, int bus_id); + void drainCanFrames(int fd, int bus_id, int timeout_us); + bool isLegMotor(std::size_t index) const; + bool isWheelMotor(std::size_t index) const; + bool motorHasBlockingFault(std::size_t index) const; + bool isNoEffectConditionPresent(std::size_t index) const; + std::uint32_t noEffectCommandWarmupCycles(std::size_t index) const; + std::uint32_t noEffectTriggerCycles(std::size_t index) const; + std::uint32_t noEffectAttemptLimit(std::size_t index) const; + std::uint32_t noEffectCooldownMs(std::size_t index) const; + std::uint32_t noEffectVerifyTimeoutMs(std::size_t index) const; + bool hasFreshNoEffectDiagnostics(std::size_t index) const; + void requestMotorDiagnostics(std::size_t index); + std::string classifyNoEffectSuspect(std::size_t index) const; + std::string buildNoEffectSummary(std::size_t index) const; + void updateMotorCommandTracking(std::size_t index, float sim_command, const std::string & target_source); + void updateNoEffectDetection(std::size_t index); + bool startMotorRecoverySequence( + std::size_t index, + const std::string & trigger, + RecoveryKind kind, + std::uint32_t attempt_number); + void processMotorRecoverySequence(std::size_t index); + void clearMotorRecoverySequence(std::size_t index); + bool shouldAttemptMotorRecovery(std::size_t index) const; + bool attemptMotorRecovery(std::size_t index, const std::string & trigger); + bool shouldAttemptNoEffectRecovery(std::size_t index) const; + bool attemptNoEffectRecovery(std::size_t index, const std::string & trigger); + const char * jointName(std::size_t index) const; + std::string motorTag(std::size_t index) const; + float estimateCurrentArms(float torque_nm) const; + std::string decodeFaultCode(std::uint16_t fault_code) const; + std::string decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const; + std::string buildMotorFaultSummary(std::size_t index) const; + std::string formatProtectionReason(const std::string & trigger, const std::string & reason) const; + void logProtectionEvent(const std::string & trigger, const std::string & reason, const std::string & action); + void logMotorPowerEvent(std::size_t index, const std::string & state, const std::string & reason); + void logMotorDiagnosticEvent(std::size_t index, const std::string & event, const std::string & reason, const char * level = "WARN"); + void updateMotorTelemetry(std::size_t index, float pos_sim, float vel_sim, float torque_sim, float temperature_c); + void handleParameterResponse(const struct can_frame & frame, int bus_id); + void updateMotorDiagnostics(std::size_t index); + void pollMotorDiagnostics(); + void finalizeRunSummary(); + + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr target_sub_; + rclcpp::Subscription::SharedPtr model_status_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr estop_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + rclcpp::TimerBase::SharedPtr read_timer_; + rclcpp::TimerBase::SharedPtr write_timer_; + rclcpp::CallbackGroup::SharedPtr motor_callback_group_; + rclcpp::CallbackGroup::SharedPtr sensor_callback_group_; + rclcpp::CallbackGroup::SharedPtr control_callback_group_; + + std::mutex target_mutex_; + std::array latest_target_{}; + std::array latest_raw_action_{}; + std::string latest_target_source_{"boot_hold"}; + rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME}; + std::array rough_default_dof_pos_{}; + std::array crawl_default_dof_pos_{}; + std::array wall_default_dof_pos_{}; + std::array active_default_dof_pos_{}; + ActiveModelMode active_model_mode_{ActiveModelMode::Rough}; + bool model_switch_active_{false}; + std::uint32_t target_sequence_{0}; + std::uint32_t state_sequence_{0}; + double target_timeout_ms_{150.0}; + + // SocketCAN file descriptors + int can0_fd_{-1}; + int can1_fd_{-1}; + std::string can0_name_{"can0"}; + std::string can1_name_{"can1"}; + + // CAN error recovery + static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit + int can0_error_count_{0}; + int can1_error_count_{0}; + bool reinitCan(const std::string& ifname, int& fd, int& error_count); + + // Hold-over constants + static constexpr std::uint32_t kHoldoverThreshold = 2; + static constexpr std::uint32_t kMotorDropReportThreshold = 40; + + // Motor configurations and states + std::array motors_; + std::array motor_states_; + + // IMU state + std::mutex imu_mutex_; + std::array imu_gyro_{}; + std::array imu_accel_{}; + std::array projected_gravity_{0.0f, 0.0f, -1.0f}; + bool imu_fresh_{false}; + rclcpp::Time last_imu_stamp_{0, 0, RCL_ROS_TIME}; + std::chrono::steady_clock::time_point last_imu_recv_time_{}; + bool has_received_imu_{false}; + std::array imu_gravity_sum_{0.0f, 0.0f, 0.0f}; + std::uint32_t imu_gravity_sample_count_{0}; + static constexpr std::uint32_t kImuGravityAlignSamples = 50; + + // Odom state + std::mutex odom_mutex_; + rclcpp::Time last_odom_stamp_{0, 0, RCL_ROS_TIME}; + std::array odom_pos_{}; + std::array odom_quat_wxyz_{1.0f, 0.0f, 0.0f, 0.0f}; + std::array odom_linear_vel_{}; + std::array odom_angular_vel_{}; + bool odom_fresh_{false}; + + // Filters and Estimators + std::unique_ptr lpf_legs_; + std::unique_ptr lpf_wheels_; + std::unique_ptr mahony_filter_; + std::unique_ptr safety_monitor_; + std::unique_ptr model_switch_safety_monitor_; + std::unique_ptr runtime_guard_; + std::atomic mahony_initialized_{false}; + rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time last_diag_poll_time_{0, 0, RCL_ROS_TIME}; + std::size_t diag_poll_motor_index_{0}; + + // Telemetry + std::uint32_t fresh_count_{0}; + std::uint32_t holdover_count_{0}; + std::uint32_t stale_max_{0}; + std::uint32_t holdover_events_total_{0}; + std::uint32_t protection_trigger_count_{0}; + std::uint32_t motor_drop_event_count_{0}; + std::uint32_t motor_recover_event_count_{0}; + std::uint32_t motor_fault_event_count_{0}; + bool timeout_hold_logged_{false}; + bool clip_active_logged_{false}; + + bool dry_run_{false}; + std::atomic estop_triggered_{false}; + std::atomic safety_enabled_{true}; + std::atomic safety_triggered_{false}; + std::string safety_reason_{""}; + sim2real_common::EventLogger event_logger_; + std::string run_log_dir_; + float motor_temp_warn_c_{100.0f}; + float motor_temp_fault_c_{135.0f}; + float motor_bus_overvoltage_v_{60.0f}; + float motor_bus_undervoltage_v_{12.0f}; + float motor_current_warn_arms_{10.5f}; + float motor_current_peak_arms_{14.0f}; + float motor_torque_warn_nm_{13.0f}; + float motor_torque_peak_nm_{17.0f}; + double diag_poll_period_s_{0.10}; + float wheel_no_effect_command_threshold_{1.0f}; + float wheel_no_effect_min_response_ratio_{0.20f}; + float wheel_no_effect_velocity_epsilon_{0.25f}; + float wheel_no_effect_max_temperature_c_{90.0f}; + float wheel_no_effect_min_bus_voltage_v_{18.0f}; + std::uint32_t wheel_no_effect_command_warmup_cycles_{12}; + std::uint32_t wheel_no_effect_trigger_cycles_{30}; + std::uint32_t wheel_no_effect_attempt_limit_{2}; + std::uint32_t wheel_no_effect_cooldown_ms_{1200}; + std::uint32_t wheel_recovery_verify_timeout_ms_{180}; + std::uint32_t wheel_no_effect_diag_freshness_ms_{350}; + std::uint32_t wheel_no_effect_diag_request_period_ms_{80}; + float leg_no_effect_position_error_threshold_{0.18f}; + float leg_no_effect_velocity_epsilon_{0.12f}; + float leg_no_effect_max_estimated_current_arms_{4.0f}; + float leg_no_effect_max_abs_torque_nm_{5.0f}; + float leg_no_effect_max_temperature_c_{100.0f}; + float leg_no_effect_min_bus_voltage_v_{18.0f}; + std::uint32_t leg_no_effect_command_warmup_cycles_{40}; + std::uint32_t leg_no_effect_trigger_cycles_{25}; + std::uint32_t leg_no_effect_attempt_limit_{2}; + std::uint32_t leg_no_effect_cooldown_ms_{1200}; + std::uint32_t leg_recovery_verify_timeout_ms_{220}; + + void onEstop(const std_msgs::msg::Bool::SharedPtr msg); + void logEvent( + const std::string & level, + const std::string & event, + const std::string & message); +}; + +} // namespace sim2real_hw diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_hw/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/package.xml new file mode 100644 index 0000000..acbe32f --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/package.xml @@ -0,0 +1,17 @@ + + + sim2real_hw + 0.1.0 + Hardware bridge and safety boundary for sim2real_ros2. + todo + Proprietary + + ament_cmake + + rclcpp + sensor_msgs + std_msgs + nav_msgs + sim2real_common + sim2real_interfaces + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_hw/src/hardware_bridge_node.cpp b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/src/hardware_bridge_node.cpp new file mode 100644 index 0000000..0331bf2 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_hw/src/hardware_bridge_node.cpp @@ -0,0 +1,2097 @@ +#include "sim2real_hw/hardware_bridge_node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "sim2real_common/deployment_contract.hpp" + +using namespace std::chrono_literals; + +namespace sim2real_hw +{ + +// Protocol constants +const std::uint32_t COMM_ENABLE = 3; +const std::uint32_t COMM_DISABLE = 4; +const std::uint32_t COMM_WRITE_PARAMETER = 18; +const std::uint32_t COMM_READ_PARAMETER = 17; +const std::uint32_t COMM_OPERATION_CONTROL = 1; +const std::uint32_t COMM_SET_ZERO_POSITION = 6; +const std::uint16_t PARAM_MODE = 0x7005; +const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017; +const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B; +const std::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; +const std::uint16_t PARAM_DRV_FAULT_DETAIL_2 = 0x3025; +const std::uint8_t HOST_ID = 0xFD; +constexpr int kMotorInitRetrySleepMs = 15; +constexpr int kMotorInitConfirmTimeoutMs = 120; +constexpr int kMotorRecoveryCooldownMs = 500; +constexpr std::uint32_t kMotorRecoveryTriggerStaleCount = 10; +constexpr std::uint32_t kMotorRecoveryAttemptLimit = 3; +inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val) +{ + buf[0] = (val >> 8) & 0xFF; + buf[1] = val & 0xFF; +} + +inline float nearest_periodic(float val, float ref) +{ + float diff = val - ref; + float wrapped = diff - 2.0f * static_cast(M_PI) * std::floor((diff + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + return ref + wrapped; +} + +std::string extractJsonStringField(const std::string & payload, const std::string & key) +{ + const std::string needle = "\"" + key + "\":\""; + const std::size_t start = payload.find(needle); + if (start == std::string::npos) { + return {}; + } + + const std::size_t value_start = start + needle.size(); + const std::size_t value_end = payload.find('"', value_start); + if (value_end == std::string::npos) { + return {}; + } + + return payload.substr(value_start, value_end - value_start); +} + +std::string toLowerCopy(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { + return static_cast(std::tolower(c)); + }); + return value; +} + +HardwareBridgeNode::HardwareBridgeNode() +: Node("sim2real_hw_node") +{ + // 1. Declare and get parameters + const std::string event_log_dir = declare_parameter( + "event_log_dir", "logs_v2_web"); + run_log_dir_ = event_log_dir; + event_logger_.configure(event_log_dir, "sim2real_hw_events"); + target_timeout_ms_ = declare_parameter("target_timeout_ms", 150.0); + can0_name_ = declare_parameter("can0_name", "can0"); + can1_name_ = declare_parameter("can1_name", "can1"); + dry_run_ = declare_parameter("dry_run", true); // Default to dry-run for safety + const std::vector default_rough_pose( + sim2real_common::DeploymentContract::kDefaultDofPos.begin(), + sim2real_common::DeploymentContract::kDefaultDofPos.end()); + const std::vector configured_rough_pose = declare_parameter>( + "rough_default_dof_pos", default_rough_pose); + if (configured_rough_pose.size() == rough_default_dof_pos_.size()) { + for (std::size_t i = 0; i < rough_default_dof_pos_.size(); ++i) { + rough_default_dof_pos_[i] = static_cast(configured_rough_pose[i]); + } + } else { + rough_default_dof_pos_ = sim2real_common::DeploymentContract::kDefaultDofPos; + RCLCPP_WARN( + get_logger(), + "Parameter rough_default_dof_pos has %zu entries, expected 16. Falling back to deployment default pose.", + configured_rough_pose.size()); + } + active_default_dof_pos_ = rough_default_dof_pos_; + const std::vector default_crawl_pose = declare_parameter>( + "crawl_default_dof_pos", + std::vector{ + 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 + }); + if (default_crawl_pose.size() == crawl_default_dof_pos_.size()) { + for (std::size_t i = 0; i < crawl_default_dof_pos_.size(); ++i) { + crawl_default_dof_pos_[i] = static_cast(default_crawl_pose[i]); + } + } else { + crawl_default_dof_pos_ = { + 0.2f, 1.697f, -2.650f, + -0.2f, 1.697f, -2.650f, + 0.2f, 1.697f, -2.650f, + -0.2f, 1.697f, -2.650f, + 0.0f, 0.0f, 0.0f, 0.0f + }; + RCLCPP_WARN( + get_logger(), + "Parameter crawl_default_dof_pos has %zu entries, expected 16. Falling back to configured crawl pose.", + default_crawl_pose.size()); + } + const std::vector default_wall_pose_param( + rough_default_dof_pos_.begin(), rough_default_dof_pos_.end()); + const std::vector default_wall_pose = declare_parameter>( + "wall_default_dof_pos", default_wall_pose_param); + if (default_wall_pose.size() == wall_default_dof_pos_.size()) { + for (std::size_t i = 0; i < wall_default_dof_pos_.size(); ++i) { + wall_default_dof_pos_[i] = static_cast(default_wall_pose[i]); + } + } else { + wall_default_dof_pos_ = rough_default_dof_pos_; + RCLCPP_WARN( + get_logger(), + "Parameter wall_default_dof_pos has %zu entries, expected 16. Falling back to rough default pose.", + default_wall_pose.size()); + } + + // Safety parameters + safety_enabled_ = declare_parameter("safety_enabled", true); + double max_target_offset = declare_parameter("max_target_offset", 0.6); + double model_switch_max_target_offset = declare_parameter( + "model_switch_max_target_offset", std::max(max_target_offset, 1.8)); + double hard_target_offset = declare_parameter("hard_target_offset", 1.2); + double max_ang_vel = declare_parameter("max_ang_vel", 10.0); + double max_tilt_z = declare_parameter("max_tilt_z", -0.3); + int clip_to_brake = declare_parameter("clip_to_brake", 0); + double imu_age_warn_ms = declare_parameter("imu_age_warn_ms", 60.0); + double imu_age_stop_ms = declare_parameter("imu_age_stop_ms", 200.0); + motor_temp_warn_c_ = static_cast(declare_parameter("motor_temp_warn_c", 100.0)); + motor_temp_fault_c_ = static_cast(declare_parameter("motor_temp_fault_c", 135.0)); + motor_bus_overvoltage_v_ = static_cast(declare_parameter("motor_bus_overvoltage_v", 60.0)); + motor_bus_undervoltage_v_ = static_cast(declare_parameter("motor_bus_undervoltage_v", 12.0)); + motor_current_warn_arms_ = static_cast(declare_parameter("motor_current_warn_arms", 10.5)); + motor_current_peak_arms_ = static_cast(declare_parameter("motor_current_peak_arms", 14.0)); + motor_torque_warn_nm_ = static_cast(declare_parameter("motor_torque_warn_nm", 13.0)); + motor_torque_peak_nm_ = static_cast(declare_parameter("motor_torque_peak_nm", 17.0)); + diag_poll_period_s_ = declare_parameter("motor_diag_poll_period_s", 0.10); + wheel_no_effect_command_threshold_ = static_cast( + declare_parameter("wheel_no_effect_command_threshold", 1.0)); + wheel_no_effect_min_response_ratio_ = static_cast( + declare_parameter("wheel_no_effect_min_response_ratio", 0.20)); + wheel_no_effect_velocity_epsilon_ = static_cast( + declare_parameter("wheel_no_effect_velocity_epsilon", 0.25)); + wheel_no_effect_max_temperature_c_ = static_cast( + declare_parameter("wheel_no_effect_max_temperature_c", 90.0)); + wheel_no_effect_min_bus_voltage_v_ = static_cast( + declare_parameter("wheel_no_effect_min_bus_voltage_v", 18.0)); + wheel_no_effect_command_warmup_cycles_ = static_cast( + declare_parameter("wheel_no_effect_command_warmup_cycles", 12)); + wheel_no_effect_trigger_cycles_ = static_cast( + declare_parameter("wheel_no_effect_trigger_cycles", 30)); + wheel_no_effect_attempt_limit_ = static_cast( + declare_parameter("wheel_no_effect_attempt_limit", 2)); + wheel_no_effect_cooldown_ms_ = static_cast( + declare_parameter("wheel_no_effect_cooldown_ms", 1200)); + wheel_recovery_verify_timeout_ms_ = static_cast( + declare_parameter("wheel_recovery_verify_timeout_ms", 180)); + wheel_no_effect_diag_freshness_ms_ = static_cast( + declare_parameter("wheel_no_effect_diag_freshness_ms", 350)); + wheel_no_effect_diag_request_period_ms_ = static_cast( + declare_parameter("wheel_no_effect_diag_request_period_ms", 80)); + leg_no_effect_position_error_threshold_ = static_cast( + declare_parameter("leg_no_effect_position_error_threshold", 0.18)); + leg_no_effect_velocity_epsilon_ = static_cast( + declare_parameter("leg_no_effect_velocity_epsilon", 0.12)); + leg_no_effect_max_estimated_current_arms_ = static_cast( + declare_parameter("leg_no_effect_max_estimated_current_arms", 4.0)); + leg_no_effect_max_abs_torque_nm_ = static_cast( + declare_parameter("leg_no_effect_max_abs_torque_nm", 5.0)); + leg_no_effect_max_temperature_c_ = static_cast( + declare_parameter("leg_no_effect_max_temperature_c", 100.0)); + leg_no_effect_min_bus_voltage_v_ = static_cast( + declare_parameter("leg_no_effect_min_bus_voltage_v", 18.0)); + leg_no_effect_command_warmup_cycles_ = static_cast( + declare_parameter("leg_no_effect_command_warmup_cycles", 40)); + leg_no_effect_trigger_cycles_ = static_cast( + declare_parameter("leg_no_effect_trigger_cycles", 25)); + leg_no_effect_attempt_limit_ = static_cast( + declare_parameter("leg_no_effect_attempt_limit", 2)); + leg_no_effect_cooldown_ms_ = static_cast( + declare_parameter("leg_no_effect_cooldown_ms", 1200)); + leg_recovery_verify_timeout_ms_ = static_cast( + declare_parameter("leg_recovery_verify_timeout_ms", 220)); + + RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false"); + RCLCPP_INFO(get_logger(), "Event log file: %s", event_logger_.componentLogPath().c_str()); + RCLCPP_INFO(get_logger(), "Run log directory: %s", run_log_dir_.c_str()); + logEvent("INFO", "node_start", dry_run_ ? "Hardware bridge node started in dry-run mode." : "Hardware bridge node started."); + if (safety_enabled_) { + RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel); + } else { + RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!"); + } + + // 2. Set up logical motors mapping matching contract + // Mapping index in array matches joint ordering in kJointLabels + for (std::size_t i = 0; i < 16; ++i) { + motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i]; + motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i]; + motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i]; + motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i]; + } + + // 3. Initialize filters & safety monitors + lpf_legs_ = std::make_unique(5.0, 0.005, 12); + lpf_wheels_ = std::make_unique(15.0, 0.005, 4); + mahony_filter_ = std::make_unique(2.0f, 0.0f); + + safety_monitor_ = std::make_unique( + static_cast(max_target_offset), + static_cast(max_ang_vel), + static_cast(max_tilt_z), + clip_to_brake, + static_cast(hard_target_offset) + ); + model_switch_safety_monitor_ = std::make_unique( + static_cast(model_switch_max_target_offset), + static_cast(max_ang_vel), + static_cast(max_tilt_z), + clip_to_brake, + static_cast(hard_target_offset) + ); + + runtime_guard_ = std::make_unique( + static_cast(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop + static_cast(max_tilt_z), + static_cast(imu_age_warn_ms), + static_cast(imu_age_stop_ms) + ); + + // 4. Initialize CAN sockets if not in dry-run + if (!dry_run_) { + if (!initCan(can0_name_, can0_fd_) || !initCan(can1_name_, can1_fd_)) { + RCLCPP_ERROR(get_logger(), "CAN initialization failed! Falling back to dry-run."); + dry_run_ = true; + } + } + + // 5. Initialize motor target states + latest_target_ = rough_default_dof_pos_; + latest_raw_action_.fill(0.0f); + + // 6. Set up ROS publishers & subscriptions + motor_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + sensor_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + control_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions sensor_sub_options; + sensor_sub_options.callback_group = sensor_callback_group_; + rclcpp::SubscriptionOptions control_sub_options; + control_sub_options.callback_group = control_callback_group_; + + state_pub_ = create_publisher("runtime/state", 10); + target_sub_ = create_subscription( + "runtime/target", 10, + std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1), + control_sub_options); + model_status_sub_ = create_subscription( + "runtime/model_status", 10, + std::bind(&HardwareBridgeNode::onModelStatus, this, std::placeholders::_1), + control_sub_options); + std::string imu_topic = declare_parameter("imu_topic", "/odin1/imu"); + imu_sub_ = create_subscription( + imu_topic, 10, + std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1), + sensor_sub_options); + estop_sub_ = create_subscription( + "/safety/estop", 10, + std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1), + control_sub_options); + + // Odom subscription + std::string odom_topic = declare_parameter("odom_topic", "/odom"); + odom_sub_ = create_subscription( + odom_topic, 10, + std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1), + sensor_sub_options); + + // 7. Enable motors on total startup + if (!dry_run_) { + RCLCPP_INFO(get_logger(), "Enabling RobStride motors..."); + for (std::size_t i = 0; i < 16; ++i) { + initializeMotor(i, "startup_init"); + } + } + + // 8. Timers at 200Hz (5ms) + read_timer_ = create_wall_timer( + 5ms, std::bind(&HardwareBridgeNode::onReadLoop, this), motor_callback_group_); + write_timer_ = create_wall_timer( + 5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this), motor_callback_group_); +} + +HardwareBridgeNode::~HardwareBridgeNode() +{ + if (!dry_run_) { + RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown..."); + for (std::size_t i = 0; i < 16; ++i) { + int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; + logMotorPowerEvent(i, "disabled", "node shutdown requested motor disable"); + disableMotor(fd, motors_[i].id); + } + if (can0_fd_ >= 0) { + if (::close(can0_fd_) < 0) { + RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno)); + } + } + if (can1_fd_ >= 0) { + if (::close(can1_fd_) < 0) { + RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno)); + } + } + } + logEvent("INFO", "node_stop", "Hardware bridge node stopped."); + finalizeRunSummary(); +} + +void HardwareBridgeNode::logEvent( + const std::string & level, + const std::string & event, + const std::string & message) +{ + event_logger_.log(level, "sim2real_hw_node", event, message); +} + +const char * HardwareBridgeNode::jointName(std::size_t index) const +{ + return sim2real_common::kJointLabels[index]; +} + +std::string HardwareBridgeNode::motorTag(std::size_t index) const +{ + std::ostringstream oss; + oss << jointName(index) + << "(index=" << index + << ",bus=" << motors_[index].bus + << ",id=" << motors_[index].id + << ")"; + return oss.str(); +} + +float HardwareBridgeNode::estimateCurrentArms(float torque_nm) const +{ + constexpr float kTorqueConstantNmPerArms = 1.22f; + return std::abs(torque_nm) / kTorqueConstantNmPerArms; +} + +std::string HardwareBridgeNode::decodeFaultCode(std::uint16_t fault_code) const +{ + if (fault_code == 0) { + return "none"; + } + + std::vector reasons; + if (fault_code & (1u << 14)) reasons.emplace_back("stall_or_overload"); + if (fault_code & (1u << 7)) reasons.emplace_back("encoder_not_calibrated"); + if (fault_code & (1u << 3)) reasons.emplace_back("bus_overvoltage"); + if (fault_code & (1u << 2)) reasons.emplace_back("bus_undervoltage"); + if (fault_code & (1u << 1)) reasons.emplace_back("driver_chip_fault"); + if (fault_code & (1u << 0)) reasons.emplace_back("overtemperature"); + if (reasons.empty()) reasons.emplace_back("unknown_fault_bits"); + + std::ostringstream oss; + for (std::size_t i = 0; i < reasons.size(); ++i) { + if (i > 0) { + oss << '|'; + } + oss << reasons[i]; + } + return oss.str(); +} + +std::string HardwareBridgeNode::decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const +{ + if (register_value == 0) { + return "none"; + } + + std::ostringstream oss; + if (register_index == 1) { + oss << "driver_fault_reg1=0x" << std::hex << register_value + << " (possible: mos_overcurrent_or_uvlo)"; + } else { + oss << "driver_fault_reg2=0x" << std::hex << register_value + << " (possible: gate_driver_short_or_half_bridge_damage)"; + } + return oss.str(); +} + +std::string HardwareBridgeNode::buildMotorFaultSummary(std::size_t index) const +{ + const auto & state = motor_states_[index]; + std::ostringstream oss; + oss << "motor=" << motorTag(index) + << ", fault_code=0x" << std::hex << state.fault_code << std::dec + << ", decoded_fault=" << decodeFaultCode(state.fault_code) + << ", temperature_c=" << state.temperature + << ", estimated_current_arms=" << state.estimated_current_arms; + if (state.has_bus_voltage) { + oss << ", bus_voltage_v=" << state.bus_voltage; + } + if (state.fault_detail_1 != 0) { + oss << ", " << decodeFaultDetailRegister(state.fault_detail_1, 1); + } + if (state.fault_detail_2 != 0) { + oss << ", " << decodeFaultDetailRegister(state.fault_detail_2, 2); + } + return oss.str(); +} + +std::string HardwareBridgeNode::formatProtectionReason( + const std::string & trigger, + const std::string & reason) const +{ + std::ostringstream oss; + oss << "trigger=" << trigger + << ", protection_action=safety_brake, reason=" << reason; + return oss.str(); +} + +void HardwareBridgeNode::logProtectionEvent( + const std::string & trigger, + const std::string & reason, + const std::string & action) +{ + protection_trigger_count_++; + std::ostringstream oss; + oss << "trigger=" << trigger + << ", protection_action=" << action + << ", reason=" << reason; + std::size_t suspect_index = motors_.size(); + float suspect_score = -1.0f; + for (std::size_t i = 0; i < motors_.size(); ++i) { + float score = 0.0f; + if (motor_states_[i].fault_code != 0 || motor_states_[i].fault_detail_1 != 0 || motor_states_[i].fault_detail_2 != 0) { + score += 100.0f; + } + score += motor_states_[i].temperature; + score += 2.0f * motor_states_[i].estimated_current_arms; + score += std::min(50.0f, static_cast(motor_states_[i].stale_count)); + if (score > suspect_score) { + suspect_score = score; + suspect_index = i; + } + } + if (suspect_index < motors_.size()) { + oss << ", suspect_motor={" << buildMotorFaultSummary(suspect_index) << "}"; + } + logEvent("ERROR", "protection_triggered", oss.str()); +} + +void HardwareBridgeNode::logMotorPowerEvent( + std::size_t index, + const std::string & state, + const std::string & reason) +{ + std::ostringstream oss; + oss << "motor=" << motorTag(index) + << ", state=" << state + << ", reason=" << reason; + logEvent(state == "recovered" ? "INFO" : "WARN", "motor_power_state", oss.str()); +} + +void HardwareBridgeNode::logMotorDiagnosticEvent( + std::size_t index, + const std::string & event, + const std::string & reason, + const char * level) +{ + std::ostringstream oss; + oss << "motor=" << motorTag(index) << ", " << reason; + logEvent(level, event, oss.str()); +} + +void HardwareBridgeNode::finalizeRunSummary() +{ + std::ostringstream oss; + oss << "run_dir=" << run_log_dir_ + << ", protection_trigger_count=" << protection_trigger_count_ + << ", motor_drop_event_count=" << motor_drop_event_count_ + << ", motor_recover_event_count=" << motor_recover_event_count_ + << ", motor_fault_event_count=" << motor_fault_event_count_ + << ", holdover_events_total=" << holdover_events_total_ + << ", final_safety_triggered=" << (safety_triggered_ ? "true" : "false"); + if (!safety_reason_.empty()) { + oss << ", final_safety_reason=" << safety_reason_; + } + event_logger_.logSummary("sim2real_hw_node", oss.str()); +} + +void HardwareBridgeNode::updateMotorTelemetry( + std::size_t index, + float pos_sim, + float vel_sim, + float torque_sim, + float temperature_c) +{ + auto & state = motor_states_[index]; + state.position = pos_sim; + state.velocity = vel_sim; + state.torque = torque_sim; + state.temperature = temperature_c; + state.estimated_current_arms = estimateCurrentArms(torque_sim); + state.update_count++; + state.stale_count = 0; + state.last_valid_pos = pos_sim; + state.last_valid_vel = vel_sim; + state.last_valid_torque = torque_sim; + state.has_valid_data = true; + state.init_confirmed = true; + state.recovery_attempt_count = 0; +} + +bool HardwareBridgeNode::isWheelMotor(std::size_t index) const +{ + return index >= sim2real_common::DeploymentContract::kLegJointCount; +} + +bool HardwareBridgeNode::isLegMotor(std::size_t index) const +{ + return index < sim2real_common::DeploymentContract::kLegJointCount; +} + +bool HardwareBridgeNode::motorHasBlockingFault(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { + return true; + } + const float max_temperature = isWheelMotor(index) + ? wheel_no_effect_max_temperature_c_ + : leg_no_effect_max_temperature_c_; + if (state.temperature >= max_temperature) { + return true; + } + const float min_bus_voltage = isWheelMotor(index) + ? wheel_no_effect_min_bus_voltage_v_ + : leg_no_effect_min_bus_voltage_v_; + if (state.has_bus_voltage && state.bus_voltage <= min_bus_voltage) { + return true; + } + return false; +} + +std::uint32_t HardwareBridgeNode::noEffectCommandWarmupCycles(std::size_t index) const +{ + return isWheelMotor(index) ? wheel_no_effect_command_warmup_cycles_ : leg_no_effect_command_warmup_cycles_; +} + +std::uint32_t HardwareBridgeNode::noEffectTriggerCycles(std::size_t index) const +{ + return isWheelMotor(index) ? wheel_no_effect_trigger_cycles_ : leg_no_effect_trigger_cycles_; +} + +std::uint32_t HardwareBridgeNode::noEffectAttemptLimit(std::size_t index) const +{ + return isWheelMotor(index) ? wheel_no_effect_attempt_limit_ : leg_no_effect_attempt_limit_; +} + +std::uint32_t HardwareBridgeNode::noEffectCooldownMs(std::size_t index) const +{ + return isWheelMotor(index) ? wheel_no_effect_cooldown_ms_ : leg_no_effect_cooldown_ms_; +} + +std::uint32_t HardwareBridgeNode::noEffectVerifyTimeoutMs(std::size_t index) const +{ + return isWheelMotor(index) ? wheel_recovery_verify_timeout_ms_ : leg_recovery_verify_timeout_ms_; +} + +bool HardwareBridgeNode::hasFreshNoEffectDiagnostics(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (!state.has_bus_voltage || !state.has_fault_snapshot) { + return false; + } + if (state.last_diag_snapshot_time_.time_since_epoch().count() == 0) { + return false; + } + const auto age = std::chrono::steady_clock::now() - state.last_diag_snapshot_time_; + return age <= std::chrono::milliseconds(wheel_no_effect_diag_freshness_ms_); +} + +void HardwareBridgeNode::requestMotorDiagnostics(std::size_t index) +{ + if (dry_run_ || index >= motors_.size()) { + return; + } + + auto & state = motor_states_[index]; + const auto now_tp = std::chrono::steady_clock::now(); + if (state.last_diag_request_time_.time_since_epoch().count() != 0) { + const auto since_last = now_tp - state.last_diag_request_time_; + if (since_last < std::chrono::milliseconds(wheel_no_effect_diag_request_period_ms_)) { + return; + } + } + + const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; + readParameter(fd, motors_[index].id, PARAM_VBUS); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_1); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_2); + state.last_diag_request_time_ = now_tp; +} + +std::string HardwareBridgeNode::classifyNoEffectSuspect(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { + return "suspect_driver_fault_or_protection"; + } + const float min_bus_voltage = isWheelMotor(index) + ? wheel_no_effect_min_bus_voltage_v_ + : leg_no_effect_min_bus_voltage_v_; + if (state.has_bus_voltage && state.bus_voltage <= min_bus_voltage) { + return "suspect_low_voltage"; + } + const float max_temperature = isWheelMotor(index) + ? wheel_no_effect_max_temperature_c_ + : leg_no_effect_max_temperature_c_; + if (state.temperature >= max_temperature) { + return "suspect_overtemp"; + } + if (state.estimated_current_arms >= motor_current_warn_arms_ || + std::abs(state.torque) >= motor_torque_warn_nm_) { + return "suspect_mechanical_stall_or_overload"; + } + if (isLegMotor(index)) { + return "suspect_enable_or_position_loop_drop"; + } + return "suspect_enable_or_mode_drop"; +} + +std::string HardwareBridgeNode::buildNoEffectSummary(std::size_t index) const +{ + const auto & state = motor_states_[index]; + const float commanded = std::abs(state.last_command_sim); + const float actual = std::abs(state.velocity); + const float response_ratio = isWheelMotor(index) && commanded > 1.0e-4f ? actual / commanded : 1.0f; + const float position_error = std::abs(state.last_command_sim - state.position); + + std::ostringstream oss; + oss << "suspect=" << classifyNoEffectSuspect(index) + << ", command=" << state.last_command_sim + << ", position=" << state.position + << ", position_error=" << position_error + << ", feedback_vel=" << state.velocity + << ", response_ratio=" << response_ratio + << ", torque_nm=" << state.torque + << ", estimated_current_arms=" << state.estimated_current_arms + << ", temperature_c=" << state.temperature; + if (state.has_bus_voltage) { + oss << ", bus_voltage_v=" << state.bus_voltage; + } + if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { + oss << ", fault_summary={" << buildMotorFaultSummary(index) << "}"; + } + return oss.str(); +} + +void HardwareBridgeNode::updateMotorCommandTracking( + std::size_t index, + float sim_command, + const std::string & target_source) +{ + auto & state = motor_states_[index]; + state.last_command_sim = sim_command; + + if (target_source == "safety_brake" || target_source == "safety_estop") { + state.command_active_count = 0; + state.no_effect_count = 0; + state.no_effect_recovery_attempt_count = 0; + state.command_effect_monitoring_active = false; + state.no_effect_reported = false; + return; + } + + bool command_active = false; + if (isWheelMotor(index)) { + command_active = std::abs(sim_command) >= wheel_no_effect_command_threshold_; + } else { + command_active = std::abs(sim_command - state.position) >= leg_no_effect_position_error_threshold_; + } + + if (command_active) { + state.command_active_count++; + state.command_effect_monitoring_active = true; + } else { + state.command_active_count = 0; + state.no_effect_count = 0; + state.no_effect_recovery_attempt_count = 0; + state.command_effect_monitoring_active = false; + state.no_effect_reported = false; + } +} + +void HardwareBridgeNode::updateNoEffectDetection(std::size_t index) +{ + auto & state = motor_states_[index]; + if (!state.has_valid_data || state.stale_count > 0) { + state.no_effect_count = 0; + state.command_effect_monitoring_active = false; + state.no_effect_reported = false; + return; + } + + if (!state.command_effect_monitoring_active || + state.command_active_count < noEffectCommandWarmupCycles(index)) { + state.no_effect_count = 0; + state.no_effect_reported = false; + return; + } + + if (!hasFreshNoEffectDiagnostics(index)) { + requestMotorDiagnostics(index); + state.no_effect_count = 0; + return; + } + + if (motorHasBlockingFault(index)) { + state.no_effect_count = 0; + return; + } + + if (isNoEffectConditionPresent(index)) { + state.no_effect_count++; + if (!state.no_effect_reported && + state.no_effect_count >= noEffectTriggerCycles(index)) { + state.no_effect_reported = true; + std::ostringstream oss; + oss << "trigger=" << (isWheelMotor(index) ? "wheel_no_effect" : "leg_no_effect") + << ", no_effect_count=" << state.no_effect_count + << ", " << buildNoEffectSummary(index); + logMotorDiagnosticEvent(index, "motor_no_effect_detected", oss.str(), "WARN"); + } + } else { + state.no_effect_count = 0; + state.no_effect_recovery_attempt_count = 0; + state.no_effect_reported = false; + } +} + +bool HardwareBridgeNode::isNoEffectConditionPresent(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (isWheelMotor(index)) { + const float commanded = std::abs(state.last_command_sim); + const float actual = std::abs(state.velocity); + const float response_ratio = commanded > 1.0e-4f ? actual / commanded : 1.0f; + return actual <= wheel_no_effect_velocity_epsilon_ || + response_ratio < wheel_no_effect_min_response_ratio_; + } + + const float position_error = std::abs(state.last_command_sim - state.position); + const float actual_velocity = std::abs(state.velocity); + const float estimated_current = std::abs(state.estimated_current_arms); + const float measured_torque = std::abs(state.torque); + return position_error >= leg_no_effect_position_error_threshold_ && + actual_velocity <= leg_no_effect_velocity_epsilon_ && + estimated_current <= leg_no_effect_max_estimated_current_arms_ && + measured_torque <= leg_no_effect_max_abs_torque_nm_; +} + +void HardwareBridgeNode::handleParameterResponse(const struct can_frame & frame, int bus_id) +{ + const std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; + const int motor_id = extra_data & 0xFF; + const std::uint16_t param_id = static_cast((frame.data[1] << 8) | frame.data[0]); + + for (std::size_t i = 0; i < motors_.size(); ++i) { + if (motors_[i].bus != bus_id || motors_[i].id != motor_id) { + continue; + } + + auto & state = motor_states_[i]; + if (param_id == PARAM_VBUS) { + float vbus = 0.0f; + std::memcpy(&vbus, &frame.data[4], sizeof(float)); + state.bus_voltage = vbus; + state.has_bus_voltage = true; + state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); + } else if (param_id == PARAM_DRV_FAULT) { + const std::uint16_t fault = static_cast((frame.data[5] << 8) | frame.data[4]); + state.fault_code = fault; + state.has_fault_snapshot = true; + state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); + } else if (param_id == PARAM_DRV_FAULT_DETAIL_1) { + state.fault_detail_1 = static_cast((frame.data[5] << 8) | frame.data[4]); + state.has_fault_snapshot = true; + state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); + } else if (param_id == PARAM_DRV_FAULT_DETAIL_2) { + state.fault_detail_2 = static_cast((frame.data[5] << 8) | frame.data[4]); + state.has_fault_snapshot = true; + state.last_diag_snapshot_time_ = std::chrono::steady_clock::now(); + } + break; + } +} + +bool HardwareBridgeNode::waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + const std::uint32_t prev_update_count = motor_states_[index].update_count; + + while (std::chrono::steady_clock::now() < deadline) { + if (motor_states_[index].update_count > prev_update_count) { + motor_states_[index].init_confirmed = true; + return true; + } + + if (can0_fd_ >= 0) { + drainCanFrames(can0_fd_, 1, 1000); + } + if (can1_fd_ >= 0) { + drainCanFrames(can1_fd_, 2, 1000); + } + std::this_thread::sleep_for(2ms); + } + + return motor_states_[index].update_count > prev_update_count; +} + +bool HardwareBridgeNode::startMotorRecoverySequence( + std::size_t index, + const std::string & trigger, + RecoveryKind kind, + std::uint32_t attempt_number) +{ + if (dry_run_ || index >= motors_.size()) { + return false; + } + + auto & state = motor_states_[index]; + if (state.recovery_stage != RecoveryStage::Idle) { + return false; + } + + const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; + if (fd < 0) { + logMotorDiagnosticEvent(index, "motor_init_failed", + "reason=" + trigger + ", detail=invalid_can_fd", "ERROR"); + return false; + } + + bool step_ok = true; + 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", + "reason=" + trigger + ", attempt=1, sent_ok=false, feedback_confirmed=false", "ERROR"); + return false; + } + + state.recovery_kind = kind; + state.recovery_stage = RecoveryStage::AwaitInitFeedback; + state.recovery_start_update_count = state.update_count; + state.recovery_active_attempt_number = attempt_number; + state.recovery_trigger = trigger; + state.recovery_stage_deadline_ = + std::chrono::steady_clock::now() + std::chrono::milliseconds(kMotorInitConfirmTimeoutMs); + state.init_confirmed = false; + return true; +} + +void HardwareBridgeNode::clearMotorRecoverySequence(std::size_t index) +{ + auto & state = motor_states_[index]; + state.recovery_kind = RecoveryKind::None; + state.recovery_stage = RecoveryStage::Idle; + state.recovery_start_update_count = 0; + state.recovery_active_attempt_number = 0; + state.recovery_stage_deadline_ = std::chrono::steady_clock::time_point{}; + state.recovery_trigger.clear(); +} + +void HardwareBridgeNode::processMotorRecoverySequence(std::size_t index) +{ + auto & state = motor_states_[index]; + if (state.recovery_stage == RecoveryStage::Idle) { + return; + } + + const auto now_tp = std::chrono::steady_clock::now(); + if (state.recovery_stage == RecoveryStage::AwaitInitFeedback) { + if (state.update_count > state.recovery_start_update_count) { + state.init_confirmed = true; + if (state.recovery_kind == RecoveryKind::NoEffect) { + state.recovery_stage = RecoveryStage::AwaitEffectVerification; + state.recovery_stage_deadline_ = + now_tp + std::chrono::milliseconds(noEffectVerifyTimeoutMs(index)); + } else { + state.recovered_reported = true; + state.stale_reported = false; + logMotorPowerEvent(index, "recovered_after_reinit", + "trigger=" + state.recovery_trigger + ", recovery_attempt=" + + std::to_string(state.recovery_active_attempt_number)); + clearMotorRecoverySequence(index); + } + return; + } + + if (now_tp >= state.recovery_stage_deadline_) { + logMotorDiagnosticEvent(index, "motor_init_retry", + "reason=" + state.recovery_trigger + + ", attempt=" + std::to_string(state.recovery_active_attempt_number) + + ", sent_ok=true, feedback_confirmed=false", "ERROR"); + clearMotorRecoverySequence(index); + } + return; + } + + if (state.recovery_stage == RecoveryStage::AwaitEffectVerification) { + if (state.stale_count == 0 && !motorHasBlockingFault(index)) { + bool recovered = false; + if (isWheelMotor(index)) { + const float commanded = std::abs(state.last_command_sim); + const float actual = std::abs(state.velocity); + const float response_ratio = commanded > 1.0e-4f ? actual / commanded : 1.0f; + recovered = + commanded >= wheel_no_effect_command_threshold_ && + actual > wheel_no_effect_velocity_epsilon_ && + response_ratio >= wheel_no_effect_min_response_ratio_; + } else { + const float position_error = std::abs(state.last_command_sim - state.position); + recovered = position_error < (leg_no_effect_position_error_threshold_ * 0.5f); + } + if (recovered) { + state.no_effect_count = 0; + state.no_effect_reported = false; + state.command_active_count = 0; + state.no_effect_recovery_attempt_count = 0; + logMotorPowerEvent(index, "recovered_after_no_effect_reinit", + "trigger=" + state.recovery_trigger + ", recovery_attempt=" + + std::to_string(state.recovery_active_attempt_number)); + clearMotorRecoverySequence(index); + return; + } + } + + if (now_tp >= state.recovery_stage_deadline_) { + logMotorDiagnosticEvent(index, "motor_no_effect_recovery_failed", + "trigger=" + state.recovery_trigger + + ", no_effect_count=" + std::to_string(state.no_effect_count) + + ", recovery_attempt=" + std::to_string(state.recovery_active_attempt_number) + + ", " + buildNoEffectSummary(index), "ERROR"); + clearMotorRecoverySequence(index); + } + } +} + +bool HardwareBridgeNode::initializeMotor(std::size_t index, const std::string & reason, int max_attempts) +{ + if (dry_run_ || index >= motors_.size()) { + return true; + } + + auto & state = motor_states_[index]; + const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; + if (fd < 0) { + logMotorDiagnosticEvent(index, "motor_init_failed", + "reason=" + reason + ", detail=invalid_can_fd", "ERROR"); + return false; + } + + state.init_confirmed = false; + bool success = false; + for (int attempt = 1; attempt <= max_attempts; ++attempt) { + state.init_attempt_count++; + bool step_ok = true; + 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)); + } + + if (success) { + std::ostringstream oss; + oss << "reason=" << reason + << ", attempt=" << attempt + << ", init_attempt_count=" << state.init_attempt_count; + logMotorDiagnosticEvent(index, "motor_init_confirmed", oss.str(), "INFO"); + return true; + } + + std::ostringstream oss; + oss << "reason=" << reason + << ", attempt=" << attempt + << ", sent_ok=" << (step_ok ? "true" : "false") + << ", feedback_confirmed=" << (state.init_confirmed ? "true" : "false"); + logMotorDiagnosticEvent(index, "motor_init_retry", oss.str(), attempt == max_attempts ? "ERROR" : "WARN"); + std::this_thread::sleep_for(std::chrono::milliseconds(kMotorInitRetrySleepMs)); + } + + return false; +} + +bool HardwareBridgeNode::initializeMotorsOnBus(int bus_id, const std::string & reason) +{ + bool all_ok = true; + for (std::size_t i = 0; i < motors_.size(); ++i) { + if (motors_[i].bus != bus_id) { + continue; + } + const bool ok = initializeMotor(i, reason, 3); + all_ok = ok && all_ok; + } + return all_ok; +} + +void HardwareBridgeNode::processCanFrame(const struct can_frame & frame, int bus_id) +{ + if (!(frame.can_id & CAN_EFF_FLAG)) { + return; + } + + const std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F; + if (comm_type == 2) { + const std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF; + const int motor_id = extra_data & 0xFF; + + for (std::size_t i = 0; i < motors_.size(); ++i) { + if (motors_[i].bus != bus_id || motors_[i].id != motor_id) { + continue; + } + + std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1]; + std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3]; + std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5]; + std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7]; + + double pos_raw = (static_cast(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI); + double vel_raw = (static_cast(v_u16) / 32767.0 - 1.0) * 44.0; + double torque_raw = (static_cast(t_u16) / 32767.0 - 1.0) * 17.0; + + float pos_sim = (static_cast(pos_raw) - motors_[i].offset) / motors_[i].direction; + float vel_sim = static_cast(vel_raw) / motors_[i].direction; + float torque_sim = static_cast(torque_raw) / motors_[i].direction; + + if (i < 12) { + pos_sim = nearest_periodic(pos_sim, rough_default_dof_pos_[i]); + } + + updateMotorTelemetry(i, pos_sim, vel_sim, torque_sim, static_cast(temp_u16) * 0.1f); + updateMotorDiagnostics(i); + return; + } + } else if (comm_type == COMM_READ_PARAMETER) { + handleParameterResponse(frame, bus_id); + } +} + +void HardwareBridgeNode::drainCanFrames(int fd, int bus_id, int timeout_us) +{ + if (fd < 0) { + return; + } + + struct can_frame frame; + while (readCanFrame(fd, &frame, timeout_us)) { + processCanFrame(frame, bus_id); + timeout_us = 0; + } +} + +bool HardwareBridgeNode::shouldAttemptMotorRecovery(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (state.recovery_stage != RecoveryStage::Idle) { + return false; + } + if (!state.has_valid_data) { + return false; + } + if (state.stale_count < kMotorRecoveryTriggerStaleCount) { + return false; + } + if (state.recovery_attempt_count >= kMotorRecoveryAttemptLimit) { + return false; + } + if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { + return false; + } + + const auto now_tp = std::chrono::steady_clock::now(); + if (state.last_recovery_attempt_time_.time_since_epoch().count() != 0) { + const auto since_last = now_tp - state.last_recovery_attempt_time_; + if (since_last < std::chrono::milliseconds(kMotorRecoveryCooldownMs)) { + return false; + } + } + return true; +} + +bool HardwareBridgeNode::attemptMotorRecovery(std::size_t index, const std::string & trigger) +{ + if (!shouldAttemptMotorRecovery(index)) { + return false; + } + + auto & state = motor_states_[index]; + const std::uint32_t attempt_number = state.recovery_attempt_count + 1; + + std::ostringstream start_oss; + start_oss << "trigger=" << trigger + << ", stale_count=" << state.stale_count + << ", recovery_attempt=" << attempt_number; + logMotorDiagnosticEvent(index, "motor_recovery_attempt", start_oss.str(), + attempt_number >= kMotorRecoveryAttemptLimit ? "ERROR" : "WARN"); + + if (!startMotorRecoverySequence(index, trigger, RecoveryKind::Stale, attempt_number)) { + return false; + } + state.recovery_attempt_count = attempt_number; + state.last_recovery_attempt_time_ = std::chrono::steady_clock::now(); + return true; +} + +bool HardwareBridgeNode::shouldAttemptNoEffectRecovery(std::size_t index) const +{ + const auto & state = motor_states_[index]; + if (state.recovery_stage != RecoveryStage::Idle) { + return false; + } + if (!state.has_valid_data || state.stale_count > 0) { + return false; + } + if (!hasFreshNoEffectDiagnostics(index)) { + return false; + } + if (state.no_effect_count < noEffectTriggerCycles(index)) { + return false; + } + if (motorHasBlockingFault(index)) { + return false; + } + if (state.no_effect_recovery_attempt_count >= noEffectAttemptLimit(index)) { + return false; + } + + const auto now_tp = std::chrono::steady_clock::now(); + if (state.last_no_effect_recovery_attempt_time_.time_since_epoch().count() != 0) { + const auto since_last = now_tp - state.last_no_effect_recovery_attempt_time_; + if (since_last < std::chrono::milliseconds(noEffectCooldownMs(index))) { + return false; + } + } + return true; +} + +bool HardwareBridgeNode::attemptNoEffectRecovery(std::size_t index, const std::string & trigger) +{ + if (!shouldAttemptNoEffectRecovery(index)) { + return false; + } + + auto & state = motor_states_[index]; + const std::uint32_t attempt_number = state.no_effect_recovery_attempt_count + 1; + + std::ostringstream start_oss; + start_oss << "trigger=" << trigger + << ", no_effect_count=" << state.no_effect_count + << ", recovery_attempt=" << attempt_number + << ", " << buildNoEffectSummary(index); + logMotorDiagnosticEvent(index, "motor_no_effect_recovery_attempt", start_oss.str(), + attempt_number >= noEffectAttemptLimit(index) ? "ERROR" : "WARN"); + + if (!startMotorRecoverySequence(index, trigger, RecoveryKind::NoEffect, attempt_number)) { + return false; + } + state.no_effect_recovery_attempt_count = attempt_number; + state.last_no_effect_recovery_attempt_time_ = std::chrono::steady_clock::now(); + return true; +} + +void HardwareBridgeNode::updateMotorDiagnostics(std::size_t index) +{ + auto & state = motor_states_[index]; + + if (state.temperature >= motor_temp_warn_c_) { + if (!state.high_temp_reported) { + state.high_temp_reported = true; + motor_fault_event_count_++; + std::ostringstream oss; + oss << "temperature_c=" << state.temperature + << ", trigger=temperature_high" + << ", protection_hint=" << (state.temperature >= motor_temp_fault_c_ ? "motor_overtemperature_fault" : "thermal_warning") + << ", estimated_current_arms=" << state.estimated_current_arms; + logMotorDiagnosticEvent(index, "motor_temperature_alert", oss.str(), + state.temperature >= motor_temp_fault_c_ ? "ERROR" : "WARN"); + } + } else { + state.high_temp_reported = false; + } + + if (state.estimated_current_arms >= motor_current_warn_arms_ || std::abs(state.torque) >= motor_torque_warn_nm_) { + if (!state.high_current_reported) { + state.high_current_reported = true; + motor_fault_event_count_++; + std::ostringstream oss; + oss << "estimated_current_arms=" << state.estimated_current_arms + << ", torque_nm=" << state.torque + << ", trigger=" << (std::abs(state.torque) >= motor_torque_peak_nm_ || state.estimated_current_arms >= motor_current_peak_arms_ + ? "overload_peak" : "overload_warning") + << ", protection_hint=stall_or_overload_protection"; + logMotorDiagnosticEvent(index, "motor_overload_alert", oss.str(), + (std::abs(state.torque) >= motor_torque_peak_nm_ || state.estimated_current_arms >= motor_current_peak_arms_) ? "ERROR" : "WARN"); + } + } else { + state.high_current_reported = false; + } + + if (state.has_bus_voltage) { + if (state.bus_voltage >= motor_bus_overvoltage_v_) { + if (!state.high_voltage_reported) { + state.high_voltage_reported = true; + motor_fault_event_count_++; + logMotorDiagnosticEvent(index, "motor_bus_voltage_alert", + "bus_voltage_v=" + std::to_string(state.bus_voltage) + + ", trigger=bus_overvoltage, protection_hint=overvoltage_fault", "ERROR"); + } + } else { + state.high_voltage_reported = false; + } + + if (state.bus_voltage <= motor_bus_undervoltage_v_) { + if (!state.low_voltage_reported) { + state.low_voltage_reported = true; + motor_fault_event_count_++; + logMotorDiagnosticEvent(index, "motor_bus_voltage_alert", + "bus_voltage_v=" + std::to_string(state.bus_voltage) + + ", trigger=bus_undervoltage, protection_hint=undervoltage_fault", "ERROR"); + } + } else { + state.low_voltage_reported = false; + } + } + + if (state.fault_code != 0 || state.fault_detail_1 != 0 || state.fault_detail_2 != 0) { + if (!state.fault_code_reported) { + state.fault_code_reported = true; + motor_fault_event_count_++; + logMotorDiagnosticEvent(index, "motor_fault_code", buildMotorFaultSummary(index), "ERROR"); + } + } else { + state.fault_code_reported = false; + } +} + +void HardwareBridgeNode::pollMotorDiagnostics() +{ + if (dry_run_ || diag_poll_period_s_ <= 0.0) { + return; + } + + const auto now_time = now(); + if (last_diag_poll_time_.nanoseconds() > 0 && + (now_time - last_diag_poll_time_).seconds() < diag_poll_period_s_) { + return; + } + last_diag_poll_time_ = now_time; + + const std::size_t index = diag_poll_motor_index_ % motors_.size(); + const int fd = motors_[index].bus == 1 ? can0_fd_ : can1_fd_; + readParameter(fd, motors_[index].id, PARAM_VBUS); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_1); + readParameter(fd, motors_[index].id, PARAM_DRV_FAULT_DETAIL_2); + diag_poll_motor_index_ = (diag_poll_motor_index_ + 1) % motors_.size(); +} + +void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg) +{ + std::scoped_lock lock(target_mutex_); + latest_target_ = msg->target; + latest_raw_action_ = msg->raw_action; + latest_target_source_ = msg->target_source; + latest_target_stamp_ = rclcpp::Time(msg->stamp); + target_sequence_ = msg->sequence; +} + +void HardwareBridgeNode::onModelStatus(const std_msgs::msg::String::SharedPtr msg) +{ + const std::string current_model = toLowerCopy(extractJsonStringField(msg->data, "current_model")); + const std::string switch_state = toLowerCopy(extractJsonStringField(msg->data, "switch_state")); + + std::scoped_lock lock(target_mutex_); + model_switch_active_ = !switch_state.empty() && switch_state != "idle"; + if (current_model.empty()) { + return; + } + ActiveModelMode next_mode = ActiveModelMode::Rough; + if (current_model == "crawl" || current_model == "ik") { + next_mode = ActiveModelMode::Crawl; + } else if (current_model == "wall") { + next_mode = ActiveModelMode::Wall; + } + if (next_mode == active_model_mode_) { + return; + } + + active_model_mode_ = next_mode; + const char * mode_name = "rough"; + if (active_model_mode_ == ActiveModelMode::Crawl) { + active_default_dof_pos_ = crawl_default_dof_pos_; + mode_name = "ik/crawl"; + } else if (active_model_mode_ == ActiveModelMode::Wall) { + active_default_dof_pos_ = wall_default_dof_pos_; + mode_name = "wall"; + } else { + active_default_dof_pos_ = rough_default_dof_pos_; + } + RCLCPP_INFO( + get_logger(), + "Hardware active model reference updated to %s pose.", + mode_name); +} + +void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + std::scoped_lock lock(imu_mutex_); + + const float gyro_x = static_cast(msg->angular_velocity.x); + const float gyro_y = static_cast(msg->angular_velocity.y); + const float gyro_z = static_cast(msg->angular_velocity.z); + + const float accel_x = static_cast(msg->linear_acceleration.x); + const float accel_y = static_cast(msg->linear_acceleration.y); + const float accel_z = static_cast(msg->linear_acceleration.z); + + imu_gyro_ = {gyro_x, gyro_y, gyro_z}; + imu_accel_ = {accel_x, accel_y, accel_z}; + if (!mahony_initialized_.load(std::memory_order_relaxed) && + imu_gravity_sample_count_ < kImuGravityAlignSamples) { + imu_gravity_sum_[0] += accel_x; + imu_gravity_sum_[1] += accel_y; + imu_gravity_sum_[2] += accel_z; + imu_gravity_sample_count_++; + } + // Track both ROS header time and local receive time. The local steady clock + // is used for stale detection so scheduler jitter or device timestamp quirks + // don't falsely trip the runtime guard. + last_imu_stamp_ = rclcpp::Time(msg->header.stamp); + last_imu_recv_time_ = std::chrono::steady_clock::now(); + has_received_imu_ = true; + imu_fresh_ = true; +} + +void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg) +{ + std::scoped_lock lock(target_mutex_); + const bool was_estop = estop_triggered_; + estop_triggered_ = msg->data; + if (estop_triggered_) { + RCLCPP_WARN(get_logger(), "!!! E-stop received over /safety/estop !!!"); + logEvent("WARN", "estop_triggered", "E-stop received over /safety/estop."); + } else { + RCLCPP_INFO(get_logger(), "E-stop reset."); + logEvent("INFO", "estop_reset", "E-stop reset via /safety/estop."); + const bool user_estop_latch = + safety_reason_.find("user E-stop") != std::string::npos || + (was_estop && safety_reason_.empty()); + if (safety_triggered_ && user_estop_latch) { + safety_triggered_ = false; + safety_reason_.clear(); + clip_active_logged_ = false; + RCLCPP_INFO(get_logger(), "Cleared user E-stop safety latch."); + logEvent("INFO", "safety_latch_reset", "Cleared user E-stop safety latch."); + } + } +} + +void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::scoped_lock lock(odom_mutex_); + last_odom_stamp_ = rclcpp::Time(msg->header.stamp); + + odom_pos_[0] = static_cast(msg->pose.pose.position.x); + odom_pos_[1] = static_cast(msg->pose.pose.position.y); + odom_pos_[2] = static_cast(msg->pose.pose.position.z); + + odom_quat_wxyz_[0] = static_cast(msg->pose.pose.orientation.w); + odom_quat_wxyz_[1] = static_cast(msg->pose.pose.orientation.x); + odom_quat_wxyz_[2] = static_cast(msg->pose.pose.orientation.y); + odom_quat_wxyz_[3] = static_cast(msg->pose.pose.orientation.z); + + odom_linear_vel_[0] = static_cast(msg->twist.twist.linear.x); + odom_linear_vel_[1] = static_cast(msg->twist.twist.linear.y); + odom_linear_vel_[2] = static_cast(msg->twist.twist.linear.z); + + odom_angular_vel_[0] = static_cast(msg->twist.twist.angular.x); + odom_angular_vel_[1] = static_cast(msg->twist.twist.angular.y); + odom_angular_vel_[2] = static_cast(msg->twist.twist.angular.z); + + odom_fresh_ = true; +} + +void HardwareBridgeNode::onReadLoop() +{ + // 1. Process CAN messages (only if CAN is open) + if (!dry_run_) { + for (std::size_t i = 0; i < 16; ++i) { + motor_states_[i].stale_count++; + } + + drainCanFrames(can0_fd_, 1, 50); + drainCanFrames(can1_fd_, 2, 50); + + // Hold-over: apply last valid data for stale motors + for (std::size_t i = 0; i < 16; ++i) { + if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) { + motor_states_[i].position = motor_states_[i].last_valid_pos; + motor_states_[i].velocity = motor_states_[i].last_valid_vel; + motor_states_[i].torque = motor_states_[i].last_valid_torque; + holdover_events_total_++; + if (!motor_states_[i].stale_reported && + motor_states_[i].stale_count >= kMotorDropReportThreshold) { + motor_states_[i].stale_reported = true; + motor_states_[i].recovered_reported = false; + motor_states_[i].disable_reported = true; + motor_states_[i].last_power_event_reason = + "no status frame received, stale_count=" + std::to_string(motor_states_[i].stale_count); + motor_drop_event_count_++; + logMotorPowerEvent(i, "dropped_or_unresponsive", motor_states_[i].last_power_event_reason); + } + if (shouldAttemptMotorRecovery(i)) { + attemptMotorRecovery(i, "stale_feedback"); + } + } else if (motor_states_[i].stale_count == 0 && motor_states_[i].stale_reported) { + motor_states_[i].stale_reported = false; + if (!motor_states_[i].recovered_reported) { + motor_states_[i].recovered_reported = true; + motor_recover_event_count_++; + logMotorPowerEvent(i, "recovered", "status frame reception resumed"); + } + } + + processMotorRecoverySequence(i); + + if (motor_states_[i].stale_count == 0) { + updateNoEffectDetection(i); + if (shouldAttemptNoEffectRecovery(i)) { + attemptNoEffectRecovery(i, "wheel_no_effect"); + } + } + } + } + + pollMotorDiagnostics(); + + // 2. Fetch IMU data & update MahonyFilter + auto now_time = now(); + double dt = 0.005; + if (last_read_time_.nanoseconds() > 0) { + dt = (now_time - last_read_time_).seconds(); + if (dt <= 0.0 || dt > 0.5) { + dt = 0.005; + } + } + last_read_time_ = now_time; + + std::array gyro{}; + std::array accel{0.0f, 0.0f, 9.81f}; + bool imu_fresh = false; + double imu_age_ms = 0.0; + { + std::scoped_lock lock(imu_mutex_); + gyro = imu_gyro_; + accel = imu_accel_; + imu_fresh = imu_fresh_; + imu_fresh_ = false; + + if (has_received_imu_) { + const auto age = std::chrono::steady_clock::now() - last_imu_recv_time_; + imu_age_ms = std::chrono::duration(age).count(); + } else if (last_imu_stamp_.nanoseconds() > 0) { + const auto age_ns = (now_time - last_imu_stamp_).nanoseconds(); + imu_age_ms = age_ns > 0 ? static_cast(age_ns) / 1.0e6 : 0.0; + } + } + + std::array quat{1.0f, 0.0f, 0.0f, 0.0f}; + const bool mahony_initialized = mahony_initialized_.load(std::memory_order_relaxed); + if (imu_fresh || mahony_initialized) { + if (!mahony_initialized) { + std::array gravity_init = accel; + { + std::scoped_lock lock(imu_mutex_); + if (imu_gravity_sample_count_ >= kImuGravityAlignSamples) { + gravity_init = { + imu_gravity_sum_[0] / static_cast(imu_gravity_sample_count_), + imu_gravity_sum_[1] / static_cast(imu_gravity_sample_count_), + imu_gravity_sum_[2] / static_cast(imu_gravity_sample_count_) + }; + } + } + mahony_filter_->reset_with_accel(gravity_init); + mahony_initialized_.store(true, std::memory_order_relaxed); + } + quat = mahony_filter_->update(accel, gyro, static_cast(dt)); + } + std::array projected_gravity = sim2real_common::get_gravity_orientation(quat); + { + std::scoped_lock lock(imu_mutex_); + projected_gravity_ = projected_gravity; + } + + // Run RuntimeGuard check + if (safety_enabled_ && !safety_triggered_) { + std::vector extra_vals; + extra_vals.reserve(32); + for (std::size_t i = 0; i < 16; ++i) { + extra_vals.push_back(motor_states_[i].position); + extra_vals.push_back(motor_states_[i].velocity); + } + bool estop_active = false; + { + std::scoped_lock lock(target_mutex_); + estop_active = estop_triggered_; + } + + auto guard_decision = runtime_guard_->check(gyro, projected_gravity, imu_age_ms, estop_active, extra_vals); + if (guard_decision.level == sim2real_common::GuardLevel::STOP) { + safety_triggered_ = true; + safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason; + RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str()); + logEvent("ERROR", "safety_triggered", safety_reason_); + logProtectionEvent("runtime_guard_stop", guard_decision.reason, "safety_brake"); + } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str()); + } + } + + // 3. Populate RuntimeState message + sim2real_interfaces::msg::RuntimeState msg; + msg.stamp = now_time; + msg.sequence = state_sequence_++; + msg.source = dry_run_ ? "stub_hw" : "socket_can_hw"; + + fresh_count_ = 0; + holdover_count_ = 0; + stale_max_ = 0; + std::array dry_run_target_snapshot{}; + std::uint32_t dry_run_target_sequence = 0; + if (dry_run_) { + std::scoped_lock lock(target_mutex_); + dry_run_target_snapshot = latest_target_; + dry_run_target_sequence = target_sequence_; + } + + for (std::size_t i = 0; i < 16; ++i) { + // Stale frames detection & holdover count + if (!dry_run_) { + if (motor_states_[i].stale_count > 0) { + holdover_count_++; + stale_max_ = std::max(stale_max_, motor_states_[i].stale_count); + } else { + fresh_count_++; + } + } + + if (dry_run_) { + // Mock motor positions tracking target + msg.joint_pos[i] = dry_run_target_snapshot[i]; + msg.joint_vel[i] = 0.0f; + msg.joint_torque[i] = 0.0f; + msg.update_counts[i] = dry_run_target_sequence; + } else { + msg.joint_pos[i] = motor_states_[i].position; + msg.joint_vel[i] = motor_states_[i].velocity; + msg.joint_torque[i] = motor_states_[i].torque; + msg.update_counts[i] = motor_states_[i].update_count; + } + } + + msg.imu_gyro = gyro; + msg.imu_accel = accel; + msg.quat_wxyz = quat; + msg.projected_gravity = projected_gravity; + msg.imu_age_ms = imu_age_ms; + msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness + + msg.odom_age_ms = 0.0f; + msg.odom_fresh = false; + msg.odom_pos = {0.0f, 0.0f, 0.0f}; + msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f}; + msg.odom_linear_vel = {0.0f, 0.0f, 0.0f}; + msg.odom_angular_vel = {0.0f, 0.0f, 0.0f}; + msg.odom_local_pos = {0.0f, 0.0f, 0.0f}; + msg.odom_local_yaw = 0.0f; + + // Populate odom fields from subscriber data + { + std::scoped_lock lock(odom_mutex_); + if (odom_fresh_) { + double odom_age = (now_time - last_odom_stamp_).seconds() * 1000.0; + msg.odom_age_ms = static_cast(odom_age); + msg.odom_fresh = (odom_age < 200.0); // 200ms threshold + msg.odom_pos = odom_pos_; + msg.odom_quat_wxyz = odom_quat_wxyz_; + msg.odom_linear_vel = odom_linear_vel_; + msg.odom_angular_vel = odom_angular_vel_; + msg.odom_local_pos = odom_pos_; + // Compute yaw from quaternion + float qw = odom_quat_wxyz_[0], qx = odom_quat_wxyz_[1]; + float qy = odom_quat_wxyz_[2], qz = odom_quat_wxyz_[3]; + float siny_c = 2.0f * (qw * qz + qx * qy); + float cosy_c = 1.0f - 2.0f * (qy * qy + qz * qz); + msg.odom_local_yaw = std::atan2(siny_c, cosy_c); + } + } + + msg.fresh_count = dry_run_ ? 16 : fresh_count_; + msg.holdover_count = dry_run_ ? 0 : holdover_count_; + msg.stale_max = dry_run_ ? 0 : stale_max_; + + state_pub_->publish(msg); +} + +void HardwareBridgeNode::onWriteLoop() +{ + const auto now_time = now(); + std::array target{}; + std::array active_default_pose = rough_default_dof_pos_; + std::string target_source; + bool model_switch_active = false; + rclcpp::Time latest_target_stamp{0, 0, RCL_ROS_TIME}; + double age_ms = 0.0; + + { + std::scoped_lock lock(target_mutex_); + target = latest_target_; + target_source = latest_target_source_; + active_default_pose = active_default_dof_pos_; + model_switch_active = model_switch_active_; + latest_target_stamp = latest_target_stamp_; + + if (latest_target_stamp.nanoseconds() > 0) { + const auto age_ns = (now_time - latest_target_stamp).nanoseconds(); + age_ms = age_ns > 0 ? static_cast(age_ns) / 1.0e6 : 0.0; + } + } + + if (target_source == "model_switch_to_model_pose") { + active_default_pose = rough_default_dof_pos_; + } + const bool target_is_model_switch = + target_source.rfind("model_switch_", 0) == 0; + + // Timeout guard: fall back to the active model's reference pose if target is stale + if (latest_target_stamp.nanoseconds() == 0 || age_ms > target_timeout_ms_) { + target = active_default_pose; + target_source = "timeout_hold"; + if (!timeout_hold_logged_) { + timeout_hold_logged_ = true; + logEvent("WARN", "target_timeout_hold", + "runtime target stale, switched to timeout_hold, age_ms=" + std::to_string(age_ms)); + } + } else { + timeout_hold_logged_ = false; + } + + // Run SafetyMonitor check on incoming target commands + std::array gyro{}; + std::array proj_grav{}; + bool estop_active = false; + { + std::scoped_lock lock(imu_mutex_); + gyro = imu_gyro_; + proj_grav = projected_gravity_; + } + { + std::scoped_lock lock(target_mutex_); + estop_active = estop_triggered_; + } + + if (safety_enabled_ && !safety_triggered_) { + auto * active_safety_monitor = + (model_switch_active || target_is_model_switch) && model_switch_safety_monitor_ ? + model_switch_safety_monitor_.get() : + safety_monitor_.get(); + auto safety_decision = active_safety_monitor->check(target, active_default_pose, gyro, proj_grav, estop_active); + if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { + safety_triggered_ = true; + safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; + RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str()); + logEvent("ERROR", "safety_triggered", safety_reason_); + logProtectionEvent("safety_monitor_stop", safety_decision.message, "safety_brake"); + } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { + target = safety_decision.clipped_target; + target_source = "safety_clip"; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped."); + if (!clip_active_logged_) { + clip_active_logged_ = true; + logEvent("WARN", "target_clipped", + "protection=safety_clip, reason=" + safety_decision.message + ", source=" + target_source); + } + } else { + clip_active_logged_ = false; + } + } + + // Override to safety_brake if safety is triggered locally or by estop + if (safety_triggered_) { + target_source = "safety_brake"; + } + + // 1. Joint LPF command filtering (200Hz, dt=0.005s) + std::array legs_in{}; + std::array legs_out{}; + std::array wheels_in{}; + std::array wheels_out{}; + + std::copy(target.begin(), target.begin() + 12, legs_in.begin()); + std::copy(target.begin() + 12, target.end(), wheels_in.begin()); + + lpf_legs_->filter(legs_in.data(), legs_out.data()); + lpf_wheels_->filter(wheels_in.data(), wheels_out.data()); + + std::array filtered_target{}; + std::copy(legs_out.begin(), legs_out.end(), filtered_target.begin()); + std::copy(wheels_out.begin(), wheels_out.end(), filtered_target.begin() + 12); + + // 2. Control execution (MIT mode write over CAN) + if (!dry_run_) { + for (std::size_t i = 0; i < 16; ++i) { + // Coordinate transform: sim_to_real + // real = sign * sim + offset + float sim_val = filtered_target[i]; + float real_val = motors_[i].direction * sim_val + motors_[i].offset; + + int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; + + if (i < 12) { + // Leg joints: MIT position control + // Kp & Kd depend on whether we are holding pose, running policy, or in safety damping mode + double kp_val = sim2real_common::DeploymentContract::kLegKp; + double kd_val = sim2real_common::DeploymentContract::kLegKd; + + if (target_source == "safety_brake" || target_source == "safety_estop") { + kp_val = 0.0; + kd_val = 2.5; // Leg damping Kd + real_val = 0.0; // Set to zero position (sign/offset will be ignored anyway under kp=0) + } else if (target_source == "startup_soft_hold") { + if (startup_soft_hold_start_time_.nanoseconds() == 0) { + startup_soft_hold_start_time_ = now_time; + } + double elapsed = (now_time - startup_soft_hold_start_time_).seconds(); + double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp + kp_val = sim2real_common::DeploymentContract::kLegKp * kp_scale; + kd_val = sim2real_common::DeploymentContract::kLegKd; + } else { + startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + if ( + target_source == "timeout_hold" || + target_source == "boot_hold" || + target_source == "runtime_zero_hold" || + target_source == "runtime_keep_pose" || + target_source == "runtime_keep_return_default" || + target_source == "startup_hold" || + target_source == "model_switch_to_stand" || + target_source == "model_switch_stand_hold" || + target_source == "model_switch_to_model_pose") + { + kp_val = sim2real_common::DeploymentContract::kLegKp; + kd_val = sim2real_common::DeploymentContract::kLegKd; + } + } + updateMotorCommandTracking(i, sim_val, target_source); + writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0); + } else { + // Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0) + double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign + double kd_val = sim2real_common::DeploymentContract::kWheelKd; + if (target_source == "safety_brake" || target_source == "safety_estop") { + vel_real = 0.0; + kd_val = 2.0; // Wheel damping Kd + } + updateMotorCommandTracking(i, sim_val, target_source); + writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0); + } + } + } +} + +bool HardwareBridgeNode::enableMotor(int fd, int motor_id) +{ + std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + return sendCanFrame(fd, ext_id, data, 8); +} + +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); +} + +bool HardwareBridgeNode::readParameter(int fd, int motor_id, std::uint16_t param_id) +{ + std::uint32_t ext_id = (COMM_READ_PARAMETER << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + data[0] = param_id & 0xFF; + data[1] = (param_id >> 8) & 0xFF; + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode) +{ + return writeParameterInt(fd, motor_id, PARAM_MODE, static_cast(mode)); +} + +bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit) +{ + std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + data[0] = param_id & 0xFF; + data[1] = (param_id >> 8) & 0xFF; + std::memcpy(&data[4], &limit, sizeof(float)); + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::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; + const double V_LIMIT = 44.0; + const double T_LIMIT = 17.0; + const double KP_LIMIT = 500.0; + const double KD_LIMIT = 5.0; + + double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos)); + double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel)); + double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val)); + double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val)); + double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque)); + + std::uint16_t pos_u16 = static_cast(((pos_clamped / P_LIMIT) + 1.0) * 32767.0); + std::uint16_t vel_u16 = static_cast(((vel_clamped / V_LIMIT) + 1.0) * 32767.0); + std::uint16_t kp_u16 = static_cast((kp_clamped / KP_LIMIT) * 65535.0); + std::uint16_t kd_u16 = static_cast((kd_clamped / KD_LIMIT) * 65535.0); + std::uint16_t torque_u16 = static_cast(((torque_clamped / T_LIMIT) + 1.0) * 32767.0); + + std::uint8_t data[8]; + pack_u16_be(&data[0], pos_u16); + pack_u16_be(&data[2], vel_u16); + pack_u16_be(&data[4], kp_u16); + pack_u16_be(&data[6], kd_u16); + + std::uint32_t ext_id = (COMM_OPERATION_CONTROL << 24) | (torque_u16 << 8) | motor_id; + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd) +{ + struct sockaddr_can addr; + struct ifreq ifr; + + if ((fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to create SocketCAN socket for %s", ifname.c_str()); + return false; + } + + // Set non-blocking mode + int flags = ::fcntl(fd, F_GETFL, 0); + if (flags < 0 || ::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to set socket to non-blocking for %s", ifname.c_str()); + ::close(fd); + fd = -1; + return false; + } + + std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1); + if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str()); + ::close(fd); + fd = -1; + return false; + } + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (::bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + RCLCPP_ERROR(get_logger(), "Failed to bind SocketCAN socket for %s", ifname.c_str()); + ::close(fd); + fd = -1; + return false; + } + + RCLCPP_INFO(get_logger(), "Successfully bound to SocketCAN interface %s", ifname.c_str()); + return true; +} + +bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc) +{ + if (fd < 0) return false; + struct can_frame frame; + frame.can_id = can_id | CAN_EFF_FLAG; // Extended frame format (29-bit CAN ID) + frame.can_dlc = dlc; + if (data) { + std::memcpy(frame.data, data, dlc); + } else { + std::memset(frame.data, 0, 8); + } + + ssize_t bytes_written = ::write(fd, &frame, sizeof(struct can_frame)); + if (bytes_written != sizeof(struct can_frame)) { + int err = errno; + // Track errors per bus for recovery logic + if (fd == can0_fd_) { + can0_error_count_++; + if (can0_error_count_ >= kCanErrorThreshold) { + RCLCPP_ERROR(get_logger(), "CAN0 write: %d consecutive errors (errno=%d: %s). Attempting reinit.", + can0_error_count_, err, strerror(err)); + if (!reinitCan(can0_name_, can0_fd_, can0_error_count_)) { + RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake."); + safety_triggered_ = true; + safety_reason_ = "CAN0 bus failure - reinit failed"; + logEvent("FATAL", "safety_triggered", safety_reason_); + } + } + } else if (fd == can1_fd_) { + can1_error_count_++; + if (can1_error_count_ >= kCanErrorThreshold) { + RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.", + can1_error_count_, err, strerror(err)); + if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) { + RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake."); + safety_triggered_ = true; + safety_reason_ = "CAN1 bus failure - reinit failed"; + logEvent("FATAL", "safety_triggered", safety_reason_); + } + } + } + return false; + } + // Reset error count on success + if (fd == can0_fd_) can0_error_count_ = 0; + else if (fd == can1_fd_) can1_error_count_ = 0; + return true; +} + +bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us) +{ + if (fd < 0) return false; + auto* frame = static_cast(frame_ptr); + + if (timeout_us > 0) { + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = timeout_us; + fd_set rdfs; + FD_ZERO(&rdfs); + FD_SET(fd, &rdfs); + + int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv); + if (ret < 0) { + int err = errno; + if (err != EINTR) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "CAN select error (fd=%d): %s", fd, strerror(err)); + } + return false; + } + if (ret == 0) { + return false; // timeout, normal + } + } + + ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame)); + if (bytes_read < 0) { + int err = errno; + if (err != EAGAIN && err != EWOULDBLOCK) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "CAN read error (fd=%d): %s", fd, strerror(err)); + } + return false; + } + return (bytes_read == sizeof(struct can_frame)); +} + +bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count) +{ + RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str()); + logEvent("WARN", "can_reinit_attempt", "interface=" + ifname + ", consecutive_errors=" + std::to_string(error_count)); + if (fd >= 0) { + ::close(fd); + fd = -1; + } + bool success = initCan(ifname, fd); + if (success) { + error_count = 0; + const int recovered_bus = (&fd == &can0_fd_) ? 1 : 2; + if (!initializeMotorsOnBus(recovered_bus, "can_reinit")) { + logEvent("ERROR", "motor_reinit_after_can_reinit_failed", + "interface=" + ifname + ", bus=" + std::to_string(recovered_bus)); + } + RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str()); + logEvent("INFO", "can_reinit_success", "interface=" + ifname); + } else { + logEvent("ERROR", "can_reinit_failed", "interface=" + ifname); + } + return success; +} + +} // namespace sim2real_hw + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 6); + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/CMakeLists.txt new file mode 100644 index 0000000..c09fc21 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RuntimeState.msg" + "msg/RuntimeTarget.msg" + DEPENDENCIES builtin_interfaces std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeState.msg b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeState.msg new file mode 100644 index 0000000..03cd276 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeState.msg @@ -0,0 +1,30 @@ +builtin_interfaces/Time stamp +uint32 sequence + +float32[16] joint_pos +float32[16] joint_vel +float32[16] joint_torque + +float32[3] imu_gyro +float32[3] imu_accel +float32[4] quat_wxyz +float32[3] projected_gravity + +float32 imu_age_ms +bool imu_fresh + +float32 odom_age_ms +bool odom_fresh +float32[3] odom_pos +float32[4] odom_quat_wxyz +float32[3] odom_linear_vel +float32[3] odom_angular_vel +float32[3] odom_local_pos +float32 odom_local_yaw + +uint32 fresh_count +uint32 holdover_count +uint32 stale_max +uint32[16] update_counts + +string source diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeTarget.msg b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeTarget.msg new file mode 100644 index 0000000..0fe3831 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/msg/RuntimeTarget.msg @@ -0,0 +1,15 @@ +builtin_interfaces/Time stamp +uint32 sequence + +float32[16] target +float32[16] raw_action +float32[16] scaled_action +float32[3] command +float32[3] raw_command + +bool zero_command +bool runtime_released +float32 release_alpha +float32 target_age_ms + +string target_source diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/package.xml new file mode 100644 index 0000000..58379b2 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_interfaces/package.xml @@ -0,0 +1,18 @@ + + + sim2real_interfaces + 0.1.0 + ROS 2 interfaces for the sim2real wheel-leg runtime. + todo + Proprietary + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/CMakeLists.txt new file mode 100644 index 0000000..a1a51d4 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project(sim2real_nav2) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/a_min/test_route.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/a_min/test_route.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/a_min/test_route.yaml rename to 05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/a_min/test_route.yaml diff --git a/05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/factory_a/goals.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/factory_a/goals.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/factory_a/goals.yaml rename to 05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/factory_a/goals.yaml diff --git a/05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/factory_a/missions.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/factory_a/missions.yaml similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_nav2/config/maps/factory_a/missions.yaml rename to 05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/maps/factory_a/missions.yaml diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/nav2_params.yaml b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/nav2_params.yaml new file mode 100644 index 0000000..52c755b --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/config/nav2_params.yaml @@ -0,0 +1,232 @@ +amcl: + ros__parameters: + use_sim_time: false + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + odom_frame_id: "odom" + laser_likelihood_max_dist: 2.0 + laser_max_range: -1.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + transform_tolerance: 1.0 + update_min_d: 0.25 + update_min_a: 0.2 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: "scan" + +bt_navigator: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_link + odom_frame: odom + default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + plugin_lib_names: + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_recompute_path_to_pose_action_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node + - nav2_recovery_node + - nav2_pipeline_sequence_node + - nav2_round_robin_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + +controller_server: + ros__parameters: + use_sim_time: false + controller_frequency: 10.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.001 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + general_goal_checker: + stateful: true + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + prune_plan: true + prune_distance: 1.0 + debug_trajectory_details: false + trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator" + velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator" + critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"] + + # DWB Velocity parameters matching Units + min_vel_x: 0.0 + max_vel_x: 0.6 + min_vel_y: 0.0 + max_vel_y: 0.0 + max_vel_theta: 2.0 + min_speed_xy: 0.0 + max_speed_xy: 0.6 + min_speed_theta: 0.0 + + # DWB Acceleration parameters matching Units + acc_lim_x: 15.0 + acc_lim_y: 15.0 + acc_lim_theta: 12.0 + decel_lim_x: -15.0 + decel_lim_y: -15.0 + decel_lim_theta: -12.0 + + # Critics tuning + ObstacleFootprint.scale: 0.2 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + +planner_server: + ros__parameters: + expected_planner_frequency: 1.0 + use_sim_time: false + planner_plugins: ["GridTransition"] + GridTransition: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: true + allow_unknown: true + +behavior_server: + ros__parameters: + use_sim_time: false + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::Backup" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false + robot_radius: 0.25 + obstacle_range: 2.5 + raytrace_range: 3.0 + publish_frequency: 1.0 + update_frequency: 1.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 30 + height: 30 + resolution: 0.05 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: pointcloud + pointcloud: + topic: /odin1/cloud_slam + sensor_frame: base_link + data_type: "PointCloud2" + clearing: true + marking: true + max_obstacle_height: 2.0 + min_obstacle_height: 0.05 + obstacle_max_range: 2.5 + obstacle_min_range: 0.1 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.6 + cost_scaling_factor: 4.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false + robot_radius: 0.25 + obstacle_range: 2.5 + raytrace_range: 3.0 + publish_frequency: 5.0 + update_frequency: 5.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 4 + height: 4 + resolution: 0.05 + plugins: ["obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: pointcloud + pointcloud: + topic: /odin1/cloud_slam + sensor_frame: base_link + data_type: "PointCloud2" + clearing: true + marking: true + max_obstacle_height: 2.0 + min_obstacle_height: 0.05 + obstacle_max_range: 2.5 + obstacle_min_range: 0.1 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.4 + cost_scaling_factor: 4.0 diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/launch/nav2.launch.py b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/launch/nav2.launch.py new file mode 100644 index 0000000..1547754 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/launch/nav2.launch.py @@ -0,0 +1,146 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Get package directories + my_share_dir = get_package_share_directory('sim2real_nav2') + + # Declare launch configuration variables + params_file_arg = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(my_share_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes' + ) + + params_file = LaunchConfiguration('params_file') + + # Define Nav2 lifecycle nodes to run + lifecycle_nodes = ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator', + 'global_costmap', 'local_costmap', 'amcl'] + + # Controller server node + controller_server_node = Node( + package='nav2_controller', + executable='controller_server', + name='controller_server', + output='screen', + parameters=[params_file] + ) + + # Planner server node + planner_server_node = Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[params_file] + ) + + # Behavior server node (called recovery_server in Galactic, behavior_server in Humble) + behavior_server_node = Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + parameters=[params_file] + ) + + # BT Navigator node + bt_navigator_node = Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[params_file] + ) + + # Global costmap node + global_costmap_node = Node( + package='nav2_costmap_2d', + executable='nav2_costmap_2d', + name='global_costmap', + output='screen', + parameters=[params_file] + ) + + # Local costmap node + local_costmap_node = Node( + package='nav2_costmap_2d', + executable='nav2_costmap_2d', + name='local_costmap', + output='screen', + parameters=[params_file] + ) + + # AMCL node (Adaptive Monte Carlo Localization), now receives /scan from pointcloud_to_laserscan + amcl_node = Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + parameters=[params_file] + ) + + # PointCloud2 to LaserScan converter (AMCL needs LaserScan, LiDAR publishes PointCloud2) + pointcloud_to_laserscan_node = Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='pointcloud_to_laserscan', + output='screen', + remappings=[ + ('cloud_in', '/odin1/cloud_slam'), + ('scan', '/scan') + ], + parameters=[{ + 'target_frame': 'base_link', + 'transform_tolerance': 0.01, + 'min_height': 0.05, + 'max_height': 2.0, + 'angle_min': -3.14159, + 'angle_max': 3.14159, + 'angle_increment': 0.0087, # ~0.5 degrees + 'scan_time': 0.1, + 'range_min': 0.1, + 'range_max': 10.0, + 'use_inf': True, + 'inf_epsilon': 1.0, + 'concurrency_level': 1 + }] + ) + + # Lifecycle manager node to transition Nav2 nodes to ACTIVE state + lifecycle_manager_node = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{ + 'use_sim_time': False, + 'autostart': True, + 'node_names': lifecycle_nodes + }] + ) + + # Create launch description + ld = LaunchDescription() + + # Set stdout line buffering + ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1')) + + # Add actions + ld.add_action(params_file_arg) + ld.add_action(controller_server_node) + ld.add_action(planner_server_node) + ld.add_action(behavior_server_node) + ld.add_action(bt_navigator_node) + ld.add_action(global_costmap_node) + ld.add_action(local_costmap_node) + ld.add_action(amcl_node) + ld.add_action(pointcloud_to_laserscan_node) + ld.add_action(lifecycle_manager_node) + + return ld diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/package.xml new file mode 100644 index 0000000..58e97c3 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_nav2/package.xml @@ -0,0 +1,18 @@ + + + sim2real_nav2 + 0.0.1 + ROS2 Nav2 configuration package for legged-wheeled quadruped + USER + MIT + + ament_cmake + + nav2_bringup + navigation2 + pointcloud_to_laserscan + + + ament_cmake + + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/CMakeLists.txt b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/CMakeLists.txt new file mode 100644 index 0000000..152afd2 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/CMakeLists.txt @@ -0,0 +1,181 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_runtime) + +find_package(ament_cmake REQUIRED) +find_package(CUDAToolkit QUIET) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sim2real_common REQUIRED) +find_package(sim2real_interfaces REQUIRED) + +# Search for ONNX Runtime headers and library +find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h + PATHS + /usr/include + /usr/include/onnxruntime + /usr/local/include + /usr/local/include/onnxruntime + /opt/onnxruntime/include +) + +find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime + PATHS + /usr/lib + /usr/lib/x86_64-linux-gnu + /usr/lib/aarch64-linux-gnu + /usr/local/lib + /opt/onnxruntime/lib +) + +get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY) + +find_path(TENSORRT_INCLUDE_DIR NvInfer.h + PATHS + /usr/include + /usr/include/aarch64-linux-gnu + /usr/local/include +) + +find_library(TENSORRT_LIBRARY NAMES nvinfer + PATHS + /usr/lib + /usr/lib/aarch64-linux-gnu + /usr/lib/x86_64-linux-gnu + /usr/local/lib +) + +if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY) + message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.") +endif() + +set(SIM2REAL_RUNTIME_HAS_TENSORRT FALSE) +if(TENSORRT_INCLUDE_DIR AND TENSORRT_LIBRARY AND CUDAToolkit_FOUND) + set(SIM2REAL_RUNTIME_HAS_TENSORRT TRUE) + message(STATUS "TensorRT support enabled for sim2real_runtime") +else() + message(STATUS "TensorRT support disabled for sim2real_runtime (missing TensorRT or CUDA toolkit)") +endif() + +add_executable(sim2real_runtime_node + src/policy_runtime_node.cpp +) + +add_executable(odom_relay_node + src/odom_relay_node.cpp +) + +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + add_executable(compare_onnx_trt + src/compare_onnx_trt.cpp + ) +endif() + +target_include_directories(sim2real_runtime_node PRIVATE + include + ${ONNXRUNTIME_INCLUDE_DIR} +) + +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + target_include_directories(sim2real_runtime_node PRIVATE + ${TENSORRT_INCLUDE_DIR} + ) + target_include_directories(compare_onnx_trt PRIVATE + ${TENSORRT_INCLUDE_DIR} + ${ONNXRUNTIME_INCLUDE_DIR} + ) +endif() + +target_include_directories(odom_relay_node PRIVATE include) + +target_link_libraries(sim2real_runtime_node + ${ONNXRUNTIME_LIBRARY} +) + +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + target_link_libraries(sim2real_runtime_node + ${TENSORRT_LIBRARY} + CUDA::cudart + ) + target_link_libraries(compare_onnx_trt + ${ONNXRUNTIME_LIBRARY} + ${TENSORRT_LIBRARY} + CUDA::cudart + ) + target_compile_definitions(sim2real_runtime_node PRIVATE + SIM2REAL_RUNTIME_HAS_TENSORRT=1 + ) + target_compile_definitions(compare_onnx_trt PRIVATE + SIM2REAL_RUNTIME_HAS_TENSORRT=1 + ) +endif() + +set_target_properties(sim2real_runtime_node PROPERTIES + BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" + INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" +) + +target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17) +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + target_compile_features(compare_onnx_trt PRIVATE cxx_std_17) +endif() + +ament_target_dependencies(sim2real_runtime_node + geometry_msgs + nav_msgs + rclcpp + std_msgs + tf2_ros + sim2real_common + sim2real_interfaces +) + +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + ament_target_dependencies(compare_onnx_trt + sim2real_common + ) +endif() + +ament_target_dependencies(odom_relay_node + geometry_msgs + nav_msgs + rclcpp + tf2_ros +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS sim2real_runtime_node odom_relay_node + DESTINATION lib/${PROJECT_NAME} +) + +if(SIM2REAL_RUNTIME_HAS_TENSORRT) + install( + TARGETS compare_onnx_trt + DESTINATION lib/${PROJECT_NAME} + ) +endif() + +install( + PROGRAMS + src/remote_uart_node.py + src/cmd_mux_node.py + src/web_udp_bridge_node.py + src/simple_nav_node.py + src/pcd_nav_click_tool.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + FILES + src/deadzone_velocity_limiter.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/odom_relay_node.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/odom_relay_node.hpp new file mode 100644 index 0000000..8da8166 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/odom_relay_node.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/transform_broadcaster.h" + +namespace sim2real_runtime +{ + +/// Subscribes to odin_ros_driver's odometry (e.g. /odin1/odometry), +/// remaps child_frame_id to "base_link", republishes on /odom, +/// and broadcasts the odom → base_link TF. +class OdomRelayNode : public rclcpp::Node +{ +public: + OdomRelayNode(); + +private: + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; + std::unique_ptr tf_broadcaster_; + + std::string odom_input_topic_; + std::string odom_output_topic_; + std::string base_frame_; + bool publish_tf_; +}; + +} // namespace sim2real_runtime diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp new file mode 100644 index 0000000..4c689cb --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp @@ -0,0 +1,297 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sim2real_interfaces/msg/runtime_state.hpp" +#include "sim2real_interfaces/msg/runtime_target.hpp" +#include "sim2real_common/event_logger.hpp" +#include "sim2real_common/stand_balance_controller.hpp" +#include "sim2real_common/safety_monitor.hpp" +#include "sim2real_common/runtime_guard.hpp" + +// ONNXRuntime C++ API +#include + +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT +#include +#include +#endif + +namespace sim2real_runtime +{ + +class PolicyRuntimeNode : public rclcpp::Node +{ +public: + PolicyRuntimeNode(); + ~PolicyRuntimeNode() override; + +private: + enum class InferenceBackend { + None, + TensorRT, + OnnxRuntime, + }; + + enum class ModelMode { + Rough, + Crawl, + Wall, + }; + + enum class CrawlBackend { + Ik, + Rl, + }; + + enum class ModelSwitchState { + Idle, + ToStand, + StandHold, + ToModelPose, + }; + + enum class StartupState { + BOOT_HOLD, + STARTUP_SOFT_HOLD, + STARTUP_TRANSITION, + STARTUP_HOLD_AFTER, + RUNTIME + }; + + enum class PostureHoldMode { + None, + Keep, + ReturnDefault, + }; + + void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg); + void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg); + void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + void onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg); + void onPostureCmd(const std_msgs::msg::String::SharedPtr msg); + void applyCmdVel(float vx, float vy, float vyaw); + void onPolicyLoop(); + bool initInferenceBackend(); + bool initTensorRt(); + bool initOnnxRuntime(); + void shutdownOnnxRuntime(); + void shutdownTensorRt(); + std::string deriveTensorRtEnginePath(const std::string & onnx_model_path) const; + const std::array & defaultPoseForMode(ModelMode mode) const; + const std::string & modelPathForMode(ModelMode mode) const; + const std::string & modelEnginePathForMode(ModelMode mode) const; + bool switchInferenceModel(ModelMode target_mode); + void publishModelStatus(); + bool modeUsesInference(ModelMode mode) const; + std::array computeHoldTarget( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd); + std::array computeIkCrawlTarget( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd); + float computeCrawlIkCommandScale( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & leg_target) const; + float projectedGravityTiltRad(const std::array & projected_gravity) const; + const char * startupStateName(StartupState state) const; + const char * modelModeName(ModelMode mode) const; + const char * modelSwitchStateName(ModelSwitchState state) const; + const char * postureHoldModeName(PostureHoldMode mode) const; + const char * inferenceBackendName() const; + const char * crawlBackendName() const; + void initializeDebugTrace(); + void appendDebugTrace( + const sim2real_interfaces::msg::RuntimeState & state, + const sim2real_interfaces::msg::RuntimeTarget & target); + + std::array buildObservation( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd, + const std::array & last_actions) const; + + std::array runPolicy(const std::array & obs); + bool isZeroCommand(const std::array & cmd, const std::array & imu_gyro) const; + bool isCommandActive(const std::array & cmd) const; + void startPostureTransition( + PostureHoldMode mode, + const std::array & start_pose, + const std::array & target_pose, + const rclcpp::Time & now_time); + + rclcpp::Publisher::SharedPtr target_pub_; + rclcpp::Publisher::SharedPtr model_status_pub_; + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_stamped_sub_; + rclcpp::Subscription::SharedPtr model_switch_sub_; + rclcpp::Subscription::SharedPtr posture_cmd_sub_; + rclcpp::TimerBase::SharedPtr policy_timer_; + + std::mutex mutex_; + sim2real_interfaces::msg::RuntimeState latest_state_; + bool has_state_{false}; + std::chrono::steady_clock::time_point last_state_recv_time_{}; + std::array cmd_{{0.0f, 0.0f, 0.0f}}; + std::array raw_cmd_{{0.0f, 0.0f, 0.0f}}; + std::array last_actions_{}; + std::uint32_t sequence_{0}; + + // Startup State Machine + StartupState startup_state_{StartupState::BOOT_HOLD}; + std::array start_pose_{}; + std::array startup_delta_{}; + rclcpp::Time state_start_time_{0, 0, RCL_ROS_TIME}; + double transition_time_{4.0}; + double hold_time_{1.0}; + std::unique_ptr stand_balance_; + + ModelMode current_model_mode_{ModelMode::Rough}; + ModelMode requested_model_mode_{ModelMode::Rough}; + ModelMode loaded_model_mode_{ModelMode::Rough}; + CrawlBackend crawl_backend_{CrawlBackend::Ik}; + ModelSwitchState model_switch_state_{ModelSwitchState::Idle}; + bool model_switch_requested_{false}; + bool hold_active_model_pose_when_unreleased_{false}; + std::array rough_default_dof_pos_{}; + std::array crawl_default_dof_pos_{}; + std::array wall_default_dof_pos_{}; + std::array active_default_dof_pos_{}; + std::array safety_reference_dof_pos_{}; + std::array keep_pose_dof_pos_{}; + std::array posture_start_pose_{}; + std::array posture_target_pose_{}; + std::array posture_delta_{}; + PostureHoldMode posture_hold_mode_{PostureHoldMode::None}; + bool posture_transition_active_{false}; + rclcpp::Time posture_transition_start_time_{0, 0, RCL_ROS_TIME}; + double posture_transition_s_{0.8}; + std::array switch_start_pose_{}; + std::array switch_delta_{}; + rclcpp::Time model_switch_state_start_time_{0, 0, RCL_ROS_TIME}; + double model_switch_transition_s_{1.2}; + double model_switch_to_stand_transition_scale_{1.35}; + double model_switch_to_model_transition_scale_{1.55}; + double model_switch_min_transition_s_{0.35}; + double model_switch_stand_hold_s_{0.45}; + double model_switch_stand_max_err_{0.18}; + double model_switch_stand_max_vel_{0.8}; + double active_switch_transition_s_{1.2}; + + std::string rough_model_path_{"policies/model_rough.onnx"}; + std::string rough_model_engine_path_{""}; + std::string crawl_model_path_{"policies/model_crawl.onnx"}; + std::string crawl_model_engine_path_{""}; + std::string wall_model_path_{"policies/model_wall.onnx"}; + std::string wall_model_engine_path_{""}; + float crawl_ik_wheel_linear_gain_{6.25f}; + float crawl_ik_wheel_yaw_gain_{4.0f}; + float crawl_ik_max_wheel_speed_{6.0f}; + float crawl_ik_abduction_clip_{0.45f}; + float crawl_ik_yaw_rate_kp_{0.0f}; + bool crawl_ik_imu_posture_{false}; + float crawl_ik_encoder_posture_kp_{0.0f}; + float crawl_ik_encoder_posture_max_{0.03f}; + bool crawl_ik_encoder_guard_{true}; + float crawl_ik_encoder_guard_start_{0.28f}; + float crawl_ik_encoder_guard_stop_{0.65f}; + bool crawl_ik_imu_guard_{true}; + float crawl_ik_imu_guard_start_rad_{0.20943952f}; + float crawl_ik_imu_guard_stop_rad_{0.48869219f}; + + // ONNX Runtime members + std::string model_path_{"policies/model_rough.onnx"}; + std::string model_engine_path_{""}; + bool prefer_tensorrt_{true}; + bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano + InferenceBackend inference_backend_{InferenceBackend::None}; + std::unique_ptr env_; + std::unique_ptr session_; + std::unique_ptr memory_info_; + + std::vector input_names_str_; + std::vector output_names_str_; + std::vector input_names_char_; + std::vector output_names_char_; + + std::vector input_shape_; + std::vector output_shape_; + +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT + nvinfer1::IRuntime * trt_runtime_{nullptr}; + nvinfer1::ICudaEngine * trt_engine_{nullptr}; + nvinfer1::IExecutionContext * trt_context_{nullptr}; + cudaStream_t trt_stream_{nullptr}; + void * trt_input_buffer_{nullptr}; + void * trt_output_buffer_{nullptr}; + std::string trt_input_name_; + std::string trt_output_name_; +#endif + + // Command filter and release states + std::array filtered_cmd_{{0.0f, 0.0f, 0.0f}}; + float runtime_max_vx_{1.0f}; + float runtime_max_vy_{0.3f}; + float runtime_max_yaw_rate_{1.0f}; + float release_alpha_{0.0f}; + float command_release_s_{0.35f}; + float release_command_hold_s_{0.12f}; + float release_posture_max_err_{0.35f}; + float release_target_blend_s_{0.30f}; + float model_switch_release_scale_{1.3f}; + float clip_obs_{100.0f}; + bool hold_zero_command_pose_{true}; + bool enable_zero_cmd_suppression_{true}; + bool require_active_command_to_release_{true}; + bool zero_cmd_use_yaw_rate_{false}; + bool runtime_released_{false}; + bool slow_release_after_model_switch_{false}; + float release_active_time_{0.0f}; + float zero_cmd_lin_thresh_{0.05f}; + float zero_cmd_yaw_thresh_{0.05f}; + float zero_yaw_rate_thresh_{0.10f}; + + // E-stop and Safety variables + rclcpp::Subscription::SharedPtr estop_sub_; + std::atomic estop_triggered_{false}; + std::atomic safety_enabled_{true}; + std::atomic safety_triggered_{false}; + std::string safety_reason_{""}; + sim2real_common::EventLogger event_logger_; + std::string run_log_dir_; + std::string debug_trace_path_; + bool debug_trace_enabled_{true}; + std::uint32_t debug_trace_decimation_{1}; + std::uint32_t debug_trace_counter_{0}; + std::uint32_t protection_trigger_count_{0}; + std::uint32_t target_clip_count_{0}; + bool clip_active_logged_{false}; + std::unique_ptr safety_monitor_; + std::unique_ptr model_switch_safety_monitor_; + std::unique_ptr runtime_guard_; + + void onEstop(const std_msgs::msg::Bool::SharedPtr msg); + void logEvent( + const std::string & level, + const std::string & event, + const std::string & message); + void logProtectionEvent( + const std::string & trigger, + const std::string & reason, + const std::string & action); + void finalizeRunSummary(); +}; + +} // namespace sim2real_runtime diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/package.xml b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/package.xml new file mode 100644 index 0000000..00ea0e8 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/package.xml @@ -0,0 +1,27 @@ + + + sim2real_runtime + 0.1.0 + Policy runtime node for sim2real_ros2. + todo + Proprietary + + ament_cmake + + geometry_msgs + nav_msgs + rclcpp + std_msgs + tf2_msgs + tf2_ros + sim2real_common + sim2real_interfaces + python3-matplotlib + python3-serial + python3-yaml + rclpy + + + ament_cmake + + diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/cmd_mux_node.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/cmd_mux_node.py new file mode 100644 index 0000000..2a4fd9c --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/cmd_mux_node.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from enum import Enum +from typing import Optional + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Bool, String + +from deadzone_velocity_limiter import limit_deadzone_axis + + +class ControlMode(str, Enum): + DISABLED = "DISABLED" + REMOTE = "REMOTE" + WEB = "WEB" + NAV = "NAV" + KEEP = "KEEP" + + +class CmdMuxNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True) + + self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper() + self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value) + self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value) + self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value) + self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value) + self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value) + self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value) + self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value) + self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value) + self.max_vx_decel = float( + self.declare_parameter("cmd_mux_max_vx_decel", self.max_vx_acc).value + ) + self.max_vy_decel = float( + self.declare_parameter("cmd_mux_max_vy_decel", self.max_vy_acc).value + ) + self.max_yaw_decel = float( + self.declare_parameter("cmd_mux_max_yaw_decel", self.max_yaw_acc).value + ) + self.linear_deadzone_epsilon = float( + self.declare_parameter("cmd_mux_linear_deadzone_epsilon", 0.0).value + ) + self.yaw_deadzone_epsilon = float( + self.declare_parameter("cmd_mux_yaw_deadzone_epsilon", 0.0).value + ) + self.min_effective_vx = float( + self.declare_parameter("cmd_mux_min_effective_vx", 0.0).value + ) + self.min_effective_vy = float( + self.declare_parameter("cmd_mux_min_effective_vy", 0.0).value + ) + self.min_effective_yaw = float( + self.declare_parameter("cmd_mux_min_effective_yaw_rate", 0.0).value + ) + self.deadzone_sources = { + item.strip().lower() + for item in str( + self.declare_parameter("cmd_mux_deadzone_sources", "nav").value + ).split(",") + if item.strip() + } + + self.mode = self.parse_mode(self.default_mode) + self.estop = False + self.remote_enabled = self.mode == ControlMode.REMOTE + self.web_enabled = self.mode == ControlMode.WEB + self.nav_enabled = self.mode == ControlMode.NAV + + self.latest_remote = Twist() + self.latest_web = Twist() + self.latest_nav = Twist() + self.remote_stamp: Optional[rclpy.time.Time] = None + self.web_stamp: Optional[rclpy.time.Time] = None + self.nav_stamp: Optional[rclpy.time.Time] = None + self.last_output = Twist() + self.last_pub_time = self.get_clock().now() + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10) + self.mode_pub = self.create_publisher(String, "control/mode_state", 10) + self.status_pub = self.create_publisher(String, "control/mux_status", 10) + + self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10) + self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10) + self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10) + self.create_subscription(String, "control/mode", self.on_mode, 10) + self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10) + self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10) + self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10) + self.create_subscription(Bool, "/safety/estop", self.on_estop, 10) + + period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02 + self.timer = self.create_timer(period, self.on_timer) + self.get_logger().info(f"Command mux started in mode {self.mode.value}") + + def parse_mode(self, value: str) -> ControlMode: + try: + return ControlMode(value.upper()) + except ValueError: + self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED") + return ControlMode.DISABLED + + def on_remote(self, msg: Twist) -> None: + self.latest_remote = msg + self.remote_stamp = self.get_clock().now() + + def on_web(self, msg: Twist) -> None: + self.latest_web = msg + self.web_stamp = self.get_clock().now() + + def on_nav(self, msg: Twist) -> None: + self.latest_nav = msg + self.nav_stamp = self.get_clock().now() + + def on_mode(self, msg: String) -> None: + new_mode = self.parse_mode(msg.data) + if new_mode != self.mode: + self.mode = new_mode + self.remote_enabled = self.mode == ControlMode.REMOTE + self.web_enabled = self.mode == ControlMode.WEB + self.nav_enabled = self.mode == ControlMode.NAV + self.get_logger().info(f"Control mode changed to {self.mode.value}") + + def on_remote_enabled(self, msg: Bool) -> None: + self.remote_enabled = bool(msg.data) + if self.remote_enabled: + self.mode = ControlMode.REMOTE + + def on_web_enabled(self, msg: Bool) -> None: + self.web_enabled = bool(msg.data) + if self.web_enabled: + self.mode = ControlMode.WEB + + def on_nav_enabled(self, msg: Bool) -> None: + self.nav_enabled = bool(msg.data) + if self.nav_enabled: + self.mode = ControlMode.NAV + + def on_estop(self, msg: Bool) -> None: + self.estop = bool(msg.data) + if self.estop: + self.mode = ControlMode.DISABLED + + def on_timer(self) -> None: + now = self.get_clock().now() + target = Twist() + source = "zero" + + if not self.estop: + if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now): + target = self.latest_remote + source = "remote" + elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now): + target = self.latest_web + source = "web" + elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now): + target = self.latest_nav + source = "nav" + elif self.mode == ControlMode.KEEP: + source = "keep" + + target = self.limit_twist(target) + if self.estop: + target = Twist() + self.last_output = Twist() + self.last_pub_time = now + else: + target = self.accel_limit(target, now, source in self.deadzone_sources) + self.cmd_pub.publish(target) + self.mode_pub.publish(String(data=self.mode.value)) + self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}")) + + def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool: + if stamp is None: + return False + age_ms = (now - stamp).nanoseconds / 1.0e6 + return age_ms <= timeout_ms + + def limit_twist(self, msg: Twist) -> Twist: + out = Twist() + out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx) + out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy) + out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw) + return out + + def accel_limit( + self, + target: Twist, + now: rclpy.time.Time, + apply_deadzone: bool, + ) -> Twist: + dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3) + min_effective_vx = self.min_effective_vx if apply_deadzone else 0.0 + min_effective_vy = self.min_effective_vy if apply_deadzone else 0.0 + min_effective_yaw = self.min_effective_yaw if apply_deadzone else 0.0 + linear_deadzone_epsilon = self.linear_deadzone_epsilon if apply_deadzone else 0.0 + yaw_deadzone_epsilon = self.yaw_deadzone_epsilon if apply_deadzone else 0.0 + out = Twist() + out.linear.x = limit_deadzone_axis( + self.last_output.linear.x, + target.linear.x, + dt, + self.max_vx_acc, + self.max_vx_decel, + min_effective_vx, + linear_deadzone_epsilon, + ) + out.linear.y = limit_deadzone_axis( + self.last_output.linear.y, + target.linear.y, + dt, + self.max_vy_acc, + self.max_vy_decel, + min_effective_vy, + linear_deadzone_epsilon, + ) + out.angular.z = limit_deadzone_axis( + self.last_output.angular.z, + target.angular.z, + dt, + self.max_yaw_acc, + self.max_yaw_decel, + min_effective_yaw, + yaw_deadzone_epsilon, + ) + self.last_output = out + self.last_pub_time = now + return out + + @staticmethod + def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, float(value))) + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = CmdMuxNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/compare_onnx_trt.cpp b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/compare_onnx_trt.cpp similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_runtime/src/compare_onnx_trt.cpp rename to 05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/compare_onnx_trt.cpp diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/deadzone_velocity_limiter.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/deadzone_velocity_limiter.py similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_runtime/src/deadzone_velocity_limiter.py rename to 05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/deadzone_velocity_limiter.py diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/odom_relay_node.cpp b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/odom_relay_node.cpp new file mode 100644 index 0000000..ec733b8 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/odom_relay_node.cpp @@ -0,0 +1,60 @@ +#include "sim2real_runtime/odom_relay_node.hpp" + +namespace sim2real_runtime +{ + +OdomRelayNode::OdomRelayNode() +: Node("odom_relay_node") +{ + odom_input_topic_ = declare_parameter("odom_input_topic", "/odin1/odometry"); + odom_output_topic_ = declare_parameter("odom_output_topic", "/odom"); + base_frame_ = declare_parameter("base_frame", "base_link"); + publish_tf_ = declare_parameter("publish_tf", true); + + odom_sub_ = create_subscription( + odom_input_topic_, 10, + std::bind(&OdomRelayNode::onOdom, this, std::placeholders::_1)); + + odom_pub_ = create_publisher(odom_output_topic_, 10); + + if (publish_tf_) { + tf_broadcaster_ = std::make_unique(*this); + } + + RCLCPP_INFO(get_logger(), + "Odom relay: %s -> %s (base_frame=%s, publish_tf=%s)", + odom_input_topic_.c_str(), odom_output_topic_.c_str(), + base_frame_.c_str(), publish_tf_ ? "true" : "false"); +} + +void OdomRelayNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + // Remap child_frame_id and republish + auto out_msg = *msg; + out_msg.header.frame_id = "odom"; + out_msg.child_frame_id = base_frame_; + odom_pub_->publish(out_msg); + + // Broadcast TF: odom → base_link + if (publish_tf_ && tf_broadcaster_) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = msg->header.stamp; + tf.header.frame_id = "odom"; + tf.child_frame_id = base_frame_; + tf.transform.translation.x = msg->pose.pose.position.x; + tf.transform.translation.y = msg->pose.pose.position.y; + tf.transform.translation.z = msg->pose.pose.position.z; + tf.transform.rotation = msg->pose.pose.orientation; + tf_broadcaster_->sendTransform(tf); + } +} + +} // namespace sim2real_runtime + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/pcd_nav_click_tool.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/pcd_nav_click_tool.py similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_runtime/src/pcd_nav_click_tool.py rename to 05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/pcd_nav_click_tool.py diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/policy_runtime_node.cpp b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/policy_runtime_node.cpp new file mode 100644 index 0000000..97a6d6c --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/policy_runtime_node.cpp @@ -0,0 +1,2013 @@ +#include "sim2real_runtime/policy_runtime_node.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sim2real_common/deployment_contract.hpp" + +using namespace std::chrono_literals; + +namespace sim2real_runtime +{ + +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT +namespace +{ + +class TensorRtLogger final : public nvinfer1::ILogger +{ +public: + explicit TensorRtLogger(rclcpp::Logger logger) + : logger_(std::move(logger)) + { + } + + void log(Severity severity, const char * msg) noexcept override + { + if (msg == nullptr) { + return; + } + + switch (severity) { + case Severity::kINTERNAL_ERROR: + case Severity::kERROR: + RCLCPP_ERROR(logger_, "[TensorRT] %s", msg); + break; + case Severity::kWARNING: + RCLCPP_WARN(logger_, "[TensorRT] %s", msg); + break; + case Severity::kINFO: + RCLCPP_INFO(logger_, "[TensorRT] %s", msg); + break; + default: + RCLCPP_DEBUG(logger_, "[TensorRT] %s", msg); + break; + } + } + +private: + rclcpp::Logger logger_; +}; + +TensorRtLogger & getTensorRtLogger(rclcpp::Logger logger) +{ + static TensorRtLogger trt_logger(logger); + return trt_logger; +} + +template +void destroyTensorRtObject(T *& object) +{ + if (object == nullptr) { + return; + } +#if NV_TENSORRT_MAJOR >= 10 + delete object; +#else + object->destroy(); +#endif + object = nullptr; +} + +} // namespace +#endif + +// Named constants for timing and command filtering +constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz) +constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz) +constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz) + +std::string toLowerCopy(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { + return static_cast(std::tolower(c)); + }); + return value; +} + +double rosTimeToSeconds(const builtin_interfaces::msg::Time & stamp) +{ + return static_cast(stamp.sec) + static_cast(stamp.nanosec) * 1.0e-9; +} + +PolicyRuntimeNode::PolicyRuntimeNode() +: Node("sim2real_runtime_node") +{ + // 1. Declare and get parameters + const std::string event_log_dir = declare_parameter( + "event_log_dir", "logs_v2_web"); + run_log_dir_ = event_log_dir; + event_logger_.configure(event_log_dir, "sim2real_runtime_events"); + model_path_ = declare_parameter("model_path", "policies/model_rough.onnx"); + model_engine_path_ = declare_parameter("model_engine_path", ""); + prefer_tensorrt_ = declare_parameter("prefer_tensorrt", true); + use_cuda_ = declare_parameter("use_cuda", false); // enable CUDA EP on Orin Nano + const std::vector default_rough_pose( + sim2real_common::DeploymentContract::kDefaultDofPos.begin(), + sim2real_common::DeploymentContract::kDefaultDofPos.end()); + const std::vector configured_rough_pose = declare_parameter>( + "rough_default_dof_pos", default_rough_pose); + if (configured_rough_pose.size() == rough_default_dof_pos_.size()) { + for (std::size_t i = 0; i < rough_default_dof_pos_.size(); ++i) { + rough_default_dof_pos_[i] = static_cast(configured_rough_pose[i]); + } + } else { + rough_default_dof_pos_ = sim2real_common::DeploymentContract::kDefaultDofPos; + RCLCPP_WARN( + get_logger(), + "Parameter rough_default_dof_pos has %zu entries, expected 16. Falling back to deployment default pose.", + configured_rough_pose.size()); + } + active_default_dof_pos_ = rough_default_dof_pos_; + safety_reference_dof_pos_ = rough_default_dof_pos_; + const std::vector default_crawl_pose = declare_parameter>( + "crawl_default_dof_pos", + std::vector{ + 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 + }); + if (default_crawl_pose.size() == crawl_default_dof_pos_.size()) { + for (std::size_t i = 0; i < crawl_default_dof_pos_.size(); ++i) { + crawl_default_dof_pos_[i] = static_cast(default_crawl_pose[i]); + } + } else { + crawl_default_dof_pos_ = { + 0.2f, 1.697f, -2.650f, + -0.2f, 1.697f, -2.650f, + 0.2f, 1.697f, -2.650f, + -0.2f, 1.697f, -2.650f, + 0.0f, 0.0f, 0.0f, 0.0f + }; + RCLCPP_WARN( + get_logger(), + "Parameter crawl_default_dof_pos has %zu entries, expected 16. Falling back to training default pose.", + default_crawl_pose.size()); + } + const std::vector default_wall_pose_param( + rough_default_dof_pos_.begin(), rough_default_dof_pos_.end()); + const std::vector default_wall_pose = declare_parameter>( + "wall_default_dof_pos", default_wall_pose_param); + if (default_wall_pose.size() == wall_default_dof_pos_.size()) { + for (std::size_t i = 0; i < wall_default_dof_pos_.size(); ++i) { + wall_default_dof_pos_[i] = static_cast(default_wall_pose[i]); + } + } else { + wall_default_dof_pos_ = rough_default_dof_pos_; + RCLCPP_WARN( + get_logger(), + "Parameter wall_default_dof_pos has %zu entries, expected 16. Falling back to rough default pose.", + default_wall_pose.size()); + } + const double keep_pose_hip_pitch = declare_parameter("keep_pose_hip_pitch", 0.610); + const double keep_pose_knee = declare_parameter("keep_pose_knee", -1.250); + const std::vector default_keep_pose = { + 0.0, keep_pose_hip_pitch, keep_pose_knee, + 0.0, keep_pose_hip_pitch, keep_pose_knee, + 0.0, keep_pose_hip_pitch, keep_pose_knee, + 0.0, keep_pose_hip_pitch, keep_pose_knee, + 0.0, 0.0, 0.0, 0.0 + }; + const std::vector configured_keep_pose = declare_parameter>( + "keep_pose_dof_pos", default_keep_pose); + if (configured_keep_pose.size() == keep_pose_dof_pos_.size()) { + for (std::size_t i = 0; i < keep_pose_dof_pos_.size(); ++i) { + keep_pose_dof_pos_[i] = static_cast(configured_keep_pose[i]); + } + } else { + for (std::size_t i = 0; i < keep_pose_dof_pos_.size(); ++i) { + keep_pose_dof_pos_[i] = static_cast(default_keep_pose[i]); + } + RCLCPP_WARN( + get_logger(), + "Parameter keep_pose_dof_pos has %zu entries, expected 16. Falling back to keep_pose_hip_pitch/keep_pose_knee.", + configured_keep_pose.size()); + } + posture_transition_s_ = declare_parameter("keep_pose_transition_s", 0.8); + rough_model_path_ = model_path_; + rough_model_engine_path_ = declare_parameter("rough_model_engine_path", model_engine_path_); + crawl_model_path_ = declare_parameter("crawl_model_path", "policies/model_crawl.onnx"); + crawl_model_engine_path_ = declare_parameter("crawl_model_engine_path", ""); + wall_model_path_ = declare_parameter("wall_model_path", "policies/model_wall.onnx"); + wall_model_engine_path_ = declare_parameter("wall_model_engine_path", ""); + const std::string crawl_backend = toLowerCopy( + declare_parameter("crawl_backend", "ik")); + crawl_backend_ = crawl_backend == "rl" ? CrawlBackend::Rl : CrawlBackend::Ik; + crawl_ik_wheel_linear_gain_ = static_cast( + declare_parameter("crawl_ik_wheel_linear_gain", 6.25)); + crawl_ik_wheel_yaw_gain_ = static_cast( + declare_parameter("crawl_ik_wheel_yaw_gain", 4.0)); + crawl_ik_max_wheel_speed_ = static_cast( + declare_parameter("crawl_ik_max_wheel_speed", 6.0)); + crawl_ik_abduction_clip_ = static_cast( + declare_parameter("crawl_ik_abduction_clip", 0.45)); + crawl_ik_yaw_rate_kp_ = static_cast( + declare_parameter("crawl_ik_yaw_rate_kp", 1.6)); + crawl_ik_imu_posture_ = declare_parameter("crawl_ik_imu_posture", false); + crawl_ik_encoder_posture_kp_ = static_cast( + declare_parameter("crawl_ik_encoder_posture_kp", 0.0)); + crawl_ik_encoder_posture_max_ = static_cast( + declare_parameter("crawl_ik_encoder_posture_max", 0.03)); + crawl_ik_encoder_guard_ = declare_parameter("crawl_ik_encoder_guard", true); + crawl_ik_encoder_guard_start_ = static_cast( + declare_parameter("crawl_ik_encoder_guard_start", 0.28)); + crawl_ik_encoder_guard_stop_ = static_cast( + declare_parameter("crawl_ik_encoder_guard_stop", 0.65)); + crawl_ik_imu_guard_ = declare_parameter("crawl_ik_imu_guard", true); + const double crawl_ik_imu_guard_start_deg = declare_parameter( + "crawl_ik_imu_guard_start_deg", 12.0); + const double crawl_ik_imu_guard_stop_deg = declare_parameter( + "crawl_ik_imu_guard_stop_deg", 28.0); + constexpr double kPi = 3.14159265358979323846; + crawl_ik_imu_guard_start_rad_ = static_cast( + crawl_ik_imu_guard_start_deg * kPi / 180.0); + crawl_ik_imu_guard_stop_rad_ = static_cast( + crawl_ik_imu_guard_stop_deg * kPi / 180.0); + model_switch_transition_s_ = declare_parameter("model_switch_transition_s", 1.2); + model_switch_to_stand_transition_scale_ = declare_parameter( + "model_switch_to_stand_transition_scale", 1.35); + model_switch_to_model_transition_scale_ = declare_parameter( + "model_switch_to_model_transition_scale", 1.55); + model_switch_min_transition_s_ = declare_parameter("model_switch_min_transition_s", 0.35); + model_switch_stand_hold_s_ = declare_parameter("model_switch_stand_hold_s", 0.45); + model_switch_stand_max_err_ = declare_parameter("model_switch_stand_max_err", 0.18); + model_switch_stand_max_vel_ = declare_parameter("model_switch_stand_max_vel", 0.8); + + // Safety parameters + safety_enabled_ = declare_parameter("safety_enabled", true); + double max_target_offset = declare_parameter("max_target_offset", 0.6); + double model_switch_max_target_offset = declare_parameter( + "model_switch_max_target_offset", std::max(max_target_offset, 1.8)); + double hard_target_offset = declare_parameter("hard_target_offset", 1.2); + double max_ang_vel = declare_parameter("max_ang_vel", 10.0); + double max_tilt_z = declare_parameter("max_tilt_z", -0.3); + int clip_to_brake = declare_parameter("clip_to_brake", 0); + double imu_age_warn_ms = declare_parameter("imu_age_warn_ms", 60.0); + double imu_age_stop_ms = declare_parameter("imu_age_stop_ms", 200.0); + + command_release_s_ = static_cast(declare_parameter("command_release_s", 0.35)); + release_command_hold_s_ = static_cast(declare_parameter("release_command_hold_s", 0.12)); + release_posture_max_err_ = static_cast(declare_parameter("release_posture_max_err", 0.35)); + release_target_blend_s_ = static_cast(declare_parameter("release_target_blend_s", 0.30)); + model_switch_release_scale_ = static_cast( + declare_parameter("model_switch_release_scale", 1.3)); + runtime_max_vx_ = static_cast(declare_parameter("runtime_max_vx", 1.0)); + runtime_max_vy_ = static_cast(declare_parameter("runtime_max_vy", 0.3)); + runtime_max_yaw_rate_ = static_cast( + declare_parameter("runtime_max_yaw_rate", 1.0)); + debug_trace_enabled_ = declare_parameter("debug_trace_enabled", true); + const int debug_trace_decimation = declare_parameter("debug_trace_decimation", 1); + debug_trace_decimation_ = static_cast(std::max(debug_trace_decimation, 1)); + clip_obs_ = static_cast(declare_parameter("clip_obs", 100.0)); + hold_zero_command_pose_ = declare_parameter("hold_zero_command_pose", true); + enable_zero_cmd_suppression_ = declare_parameter("enable_zero_cmd_suppression", true); + require_active_command_to_release_ = declare_parameter("require_active_command_to_release", true); + zero_cmd_use_yaw_rate_ = declare_parameter("zero_cmd_use_yaw_rate", true); + runtime_released_ = !require_active_command_to_release_; + + if (rough_model_engine_path_.empty()) { + rough_model_engine_path_ = deriveTensorRtEnginePath(rough_model_path_); + } + if (model_engine_path_.empty()) { + model_engine_path_ = rough_model_engine_path_; + } + if (crawl_model_engine_path_.empty()) { + crawl_model_engine_path_ = deriveTensorRtEnginePath(crawl_model_path_); + } + if (wall_model_engine_path_.empty()) { + wall_model_engine_path_ = deriveTensorRtEnginePath(wall_model_path_); + } + + RCLCPP_INFO( + get_logger(), + "Policy inference setup: rough_model=%s, rough_engine=%s, crawl_model=%s, crawl_engine=%s, wall_model=%s, wall_engine=%s, crawl_backend=%s, prefer_tensorrt=%s", + rough_model_path_.c_str(), + rough_model_engine_path_.c_str(), + crawl_model_path_.c_str(), + crawl_model_engine_path_.c_str(), + wall_model_path_.c_str(), + wall_model_engine_path_.c_str(), + crawlBackendName(), + prefer_tensorrt_ ? "true" : "false"); + RCLCPP_INFO( + get_logger(), + "Event log file: %s", + event_logger_.componentLogPath().c_str()); + RCLCPP_INFO( + get_logger(), + "Run log directory: %s", + run_log_dir_.c_str()); + logEvent("INFO", "node_start", "Policy runtime node started."); + initializeDebugTrace(); + + // Initialize StandBalanceController + stand_balance_ = std::make_unique(0.02); + const float rough_hip_mean = + (rough_default_dof_pos_[1] + rough_default_dof_pos_[4] + + rough_default_dof_pos_[7] + rough_default_dof_pos_[10]) * 0.25f; + const float rough_knee_mean = + (rough_default_dof_pos_[2] + rough_default_dof_pos_[5] + + rough_default_dof_pos_[8] + rough_default_dof_pos_[11]) * 0.25f; + stand_balance_->setNominalLegPose(rough_hip_mean, rough_knee_mean); + + // Initialize SafetyMonitor and RuntimeGuard + safety_monitor_ = std::make_unique( + static_cast(max_target_offset), + static_cast(max_ang_vel), + static_cast(max_tilt_z), + clip_to_brake, + static_cast(hard_target_offset) + ); + model_switch_safety_monitor_ = std::make_unique( + static_cast(model_switch_max_target_offset), + static_cast(max_ang_vel), + static_cast(max_tilt_z), + clip_to_brake, + static_cast(hard_target_offset) + ); + + runtime_guard_ = std::make_unique( + static_cast(max_ang_vel + 2.0), + static_cast(max_tilt_z), + static_cast(imu_age_warn_ms), + static_cast(imu_age_stop_ms) + ); + + // 2. Initialize inference backend + if (!initInferenceBackend()) { + RCLCPP_FATAL(get_logger(), "Failed to initialize any inference backend."); + throw std::runtime_error("failed to initialize inference backend"); + } + + // 3. Create publishers and subscriptions + target_pub_ = create_publisher("runtime/target", 10); + model_status_pub_ = create_publisher("runtime/model_status", 10); + state_sub_ = create_subscription( + "runtime/state", 10, + std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1)); + cmd_sub_ = create_subscription( + "cmd_vel", 10, + std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1)); + cmd_stamped_sub_ = create_subscription( + "cmd_vel_stamped", 10, + std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1)); + model_switch_sub_ = create_subscription( + "runtime/model_cmd", 10, + std::bind(&PolicyRuntimeNode::onModelSwitchCmd, this, std::placeholders::_1)); + posture_cmd_sub_ = create_subscription( + "runtime/posture_cmd", 10, + std::bind(&PolicyRuntimeNode::onPostureCmd, this, std::placeholders::_1)); + estop_sub_ = create_subscription( + "/safety/estop", 10, + std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1)); + + // 4. Timer at 50Hz (20ms) + policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this)); + + last_actions_.fill(0.0f); + publishModelStatus(); +} + +PolicyRuntimeNode::~PolicyRuntimeNode() +{ + logEvent("INFO", "node_stop", "Policy runtime node stopped."); + finalizeRunSummary(); + shutdownTensorRt(); + shutdownOnnxRuntime(); +} + +void PolicyRuntimeNode::logEvent( + const std::string & level, + const std::string & event, + const std::string & message) +{ + event_logger_.log(level, "sim2real_runtime_node", event, message); +} + +void PolicyRuntimeNode::initializeDebugTrace() +{ + if (!debug_trace_enabled_ || run_log_dir_.empty()) { + return; + } + + std::error_code ec; + std::filesystem::create_directories(run_log_dir_, ec); + debug_trace_path_ = (std::filesystem::path(run_log_dir_) / "runtime_debug_trace.csv").string(); + + std::ofstream stream(debug_trace_path_, std::ios::trunc); + if (!stream.is_open()) { + RCLCPP_WARN(get_logger(), "Failed to open runtime debug trace file: %s", debug_trace_path_.c_str()); + debug_trace_enabled_ = false; + return; + } + + stream + << "stamp_sec,target_seq,state_seq,state_source,startup_state,current_model,requested_model,loaded_model," + << "switch_state,target_source,runtime_released,release_alpha,zero_command,target_age_ms,imu_age_ms,odom_age_ms," + << "cmd_vx,cmd_vy,cmd_wz,raw_cmd_vx,raw_cmd_vy,raw_cmd_wz"; + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + stream << ",joint_pos_" << i; + } + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + stream << ",target_" << i; + } + stream << '\n'; +} + +void PolicyRuntimeNode::appendDebugTrace( + const sim2real_interfaces::msg::RuntimeState & state, + const sim2real_interfaces::msg::RuntimeTarget & target) +{ + if (!debug_trace_enabled_ || debug_trace_path_.empty()) { + return; + } + + if ((debug_trace_counter_++ % debug_trace_decimation_) != 0) { + return; + } + + std::ofstream stream(debug_trace_path_, std::ios::app); + if (!stream.is_open()) { + return; + } + + stream + << std::fixed << std::setprecision(6) + << rosTimeToSeconds(target.stamp) + << ',' << target.sequence + << ',' << state.sequence + << ',' << state.source + << ',' << startupStateName(startup_state_) + << ',' << modelModeName(current_model_mode_) + << ',' << modelModeName(requested_model_mode_) + << ',' << modelModeName(loaded_model_mode_) + << ',' << modelSwitchStateName(model_switch_state_) + << ',' << target.target_source + << ',' << (target.runtime_released ? 1 : 0) + << ',' << target.release_alpha + << ',' << (target.zero_command ? 1 : 0) + << ',' << target.target_age_ms + << ',' << state.imu_age_ms + << ',' << state.odom_age_ms + << ',' << target.command[0] + << ',' << target.command[1] + << ',' << target.command[2] + << ',' << target.raw_command[0] + << ',' << target.raw_command[1] + << ',' << target.raw_command[2]; + + for (float value : state.joint_pos) { + stream << ',' << value; + } + for (float value : target.target) { + stream << ',' << value; + } + stream << '\n'; +} + +void PolicyRuntimeNode::logProtectionEvent( + const std::string & trigger, + const std::string & reason, + const std::string & action) +{ + protection_trigger_count_++; + std::ostringstream oss; + oss << "trigger=" << trigger + << ", protection_action=" << action + << ", reason=" << reason; + logEvent("ERROR", "protection_triggered", oss.str()); +} + +void PolicyRuntimeNode::finalizeRunSummary() +{ + std::ostringstream oss; + oss << "run_dir=" << run_log_dir_ + << ", protection_trigger_count=" << protection_trigger_count_ + << ", target_clip_count=" << target_clip_count_ + << ", final_safety_triggered=" << (safety_triggered_ ? "true" : "false"); + if (!safety_reason_.empty()) { + oss << ", final_safety_reason=" << safety_reason_; + } + event_logger_.logSummary("sim2real_runtime_node", oss.str()); +} + +std::string PolicyRuntimeNode::deriveTensorRtEnginePath(const std::string & onnx_model_path) const +{ + constexpr const char * kSuffix = ".onnx"; + if (onnx_model_path.size() > std::strlen(kSuffix) && + onnx_model_path.compare(onnx_model_path.size() - std::strlen(kSuffix), std::strlen(kSuffix), kSuffix) == 0) { + return onnx_model_path.substr(0, onnx_model_path.size() - std::strlen(kSuffix)) + "_fp16.engine"; + } + return onnx_model_path + ".engine"; +} + +bool PolicyRuntimeNode::initInferenceBackend() +{ + if (prefer_tensorrt_) { + if (initTensorRt()) { + inference_backend_ = InferenceBackend::TensorRT; + return true; + } + RCLCPP_WARN(get_logger(), "TensorRT initialization failed. Falling back to ONNX Runtime."); + } + + if (initOnnxRuntime()) { + inference_backend_ = InferenceBackend::OnnxRuntime; + return true; + } + + inference_backend_ = InferenceBackend::None; + return false; +} + +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT +bool PolicyRuntimeNode::initTensorRt() +{ + shutdownTensorRt(); + inference_backend_ = InferenceBackend::None; + + if (model_engine_path_.empty()) { + RCLCPP_WARN(get_logger(), "TensorRT engine path is empty."); + return false; + } + + std::ifstream engine_file(model_engine_path_, std::ios::binary); + if (!engine_file) { + RCLCPP_WARN(get_logger(), "TensorRT engine file not found: %s", model_engine_path_.c_str()); + return false; + } + + engine_file.seekg(0, std::ios::end); + const std::streamsize engine_size = engine_file.tellg(); + if (engine_size <= 0) { + RCLCPP_WARN(get_logger(), "TensorRT engine file is empty: %s", model_engine_path_.c_str()); + return false; + } + engine_file.seekg(0, std::ios::beg); + + std::vector engine_data(static_cast(engine_size)); + if (!engine_file.read(engine_data.data(), engine_size)) { + RCLCPP_WARN(get_logger(), "Failed to read TensorRT engine file: %s", model_engine_path_.c_str()); + return false; + } + + auto & logger = getTensorRtLogger(get_logger()); + trt_runtime_ = nvinfer1::createInferRuntime(logger); + if (trt_runtime_ == nullptr) { + RCLCPP_WARN(get_logger(), "Failed to create TensorRT runtime."); + return false; + } + + trt_engine_ = trt_runtime_->deserializeCudaEngine(engine_data.data(), engine_data.size()); + if (trt_engine_ == nullptr) { + RCLCPP_WARN(get_logger(), "Failed to deserialize TensorRT engine: %s", model_engine_path_.c_str()); + shutdownTensorRt(); + return false; + } + + trt_context_ = trt_engine_->createExecutionContext(); + if (trt_context_ == nullptr) { + RCLCPP_WARN(get_logger(), "Failed to create TensorRT execution context."); + shutdownTensorRt(); + return false; + } + + if (trt_engine_->getNbIOTensors() != 2) { + RCLCPP_WARN( + get_logger(), + "Unexpected TensorRT IO tensor count: %d (expected 2).", + trt_engine_->getNbIOTensors()); + shutdownTensorRt(); + return false; + } + + for (int i = 0; i < trt_engine_->getNbIOTensors(); ++i) { + const char * tensor_name = trt_engine_->getIOTensorName(i); + if (trt_engine_->getTensorIOMode(tensor_name) == nvinfer1::TensorIOMode::kINPUT) { + trt_input_name_ = tensor_name; + } else { + trt_output_name_ = tensor_name; + } + } + + if (trt_input_name_.empty() || trt_output_name_.empty()) { + RCLCPP_WARN(get_logger(), "Failed to resolve TensorRT input/output tensor names."); + shutdownTensorRt(); + return false; + } + + const auto input_dims = trt_engine_->getTensorShape(trt_input_name_.c_str()); + const auto output_dims = trt_engine_->getTensorShape(trt_output_name_.c_str()); + if (input_dims.nbDims != 2 || output_dims.nbDims != 2) { + RCLCPP_WARN(get_logger(), "Unexpected TensorRT tensor ranks. input=%d output=%d", input_dims.nbDims, output_dims.nbDims); + shutdownTensorRt(); + return false; + } + + if (input_dims.d[1] != 53 || output_dims.d[1] != static_cast(sim2real_common::DeploymentContract::kActionDim)) { + RCLCPP_WARN( + get_logger(), + "TensorRT engine shape mismatch. input second dim=%ld output second dim=%ld", + static_cast(input_dims.d[1]), + static_cast(output_dims.d[1])); + shutdownTensorRt(); + return false; + } + + input_shape_ = {1, 53}; + output_shape_ = {1, static_cast(sim2real_common::DeploymentContract::kActionDim)}; + + if (cudaStreamCreate(&trt_stream_) != cudaSuccess) { + RCLCPP_WARN(get_logger(), "Failed to create CUDA stream for TensorRT."); + shutdownTensorRt(); + return false; + } + + const std::size_t input_bytes = sizeof(float) * 53; + const std::size_t output_bytes = sizeof(float) * sim2real_common::DeploymentContract::kActionDim; + if (cudaMalloc(&trt_input_buffer_, input_bytes) != cudaSuccess || + cudaMalloc(&trt_output_buffer_, output_bytes) != cudaSuccess) { + RCLCPP_WARN(get_logger(), "Failed to allocate TensorRT CUDA buffers."); + shutdownTensorRt(); + return false; + } + + if (!trt_context_->setInputShape(trt_input_name_.c_str(), nvinfer1::Dims2{1, 53})) { + RCLCPP_WARN(get_logger(), "Failed to set TensorRT input shape."); + shutdownTensorRt(); + return false; + } + + if (!trt_context_->setTensorAddress(trt_input_name_.c_str(), trt_input_buffer_) || + !trt_context_->setTensorAddress(trt_output_name_.c_str(), trt_output_buffer_)) { + RCLCPP_WARN(get_logger(), "Failed to bind TensorRT IO buffers."); + shutdownTensorRt(); + return false; + } + + RCLCPP_INFO( + get_logger(), + "TensorRT engine loaded successfully from %s. Input=%s[1x53], Output=%s[1x%ld]", + model_engine_path_.c_str(), + trt_input_name_.c_str(), + trt_output_name_.c_str(), + static_cast(sim2real_common::DeploymentContract::kActionDim)); + return true; +} +#else +bool PolicyRuntimeNode::initTensorRt() +{ + RCLCPP_INFO(get_logger(), "TensorRT support not compiled in; skipping TensorRT initialization."); + return false; +} +#endif + +bool PolicyRuntimeNode::initOnnxRuntime() +{ + shutdownOnnxRuntime(); + + try { + env_ = std::make_unique(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env"); + + const auto available_providers = Ort::GetAvailableProviders(); + std::ostringstream provider_stream; + for (std::size_t i = 0; i < available_providers.size(); ++i) { + if (i > 0) { + provider_stream << ", "; + } + provider_stream << available_providers[i]; + } + RCLCPP_INFO( + get_logger(), + "ONNX Runtime available providers: [%s]", + provider_stream.str().c_str()); + + if (use_cuda_) { + const bool has_cuda_provider = std::find( + available_providers.begin(), + available_providers.end(), + "CUDAExecutionProvider") != available_providers.end(); + if (!has_cuda_provider) { + RCLCPP_WARN( + get_logger(), + "Parameter use_cuda=true, but CUDAExecutionProvider is not available in the current ONNX Runtime build."); + } + } + + Ort::SessionOptions session_options; + session_options.SetIntraOpNumThreads(1); + session_options.SetInterOpNumThreads(1); + session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); + + if (use_cuda_) { + try { + OrtCUDAProviderOptions cuda_opts{}; + cuda_opts.device_id = 0; + session_options.AppendExecutionProvider_CUDA(cuda_opts); + RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)"); + } catch (const std::exception& e) { + RCLCPP_WARN( + get_logger(), + "CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.", + e.what()); + use_cuda_ = false; + } + } + + session_ = std::make_unique(*env_, model_path_.c_str(), session_options); + memory_info_ = std::make_unique(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU)); + + Ort::AllocatorWithDefaultOptions allocator; + input_names_str_.clear(); + output_names_str_.clear(); + input_names_char_.clear(); + output_names_char_.clear(); + + const std::size_t num_inputs = session_->GetInputCount(); + for (std::size_t i = 0; i < num_inputs; ++i) { + auto name = session_->GetInputNameAllocated(i, allocator); + input_names_str_.push_back(std::string(name.get())); + } + for (const auto & name : input_names_str_) { + input_names_char_.push_back(name.c_str()); + } + + const std::size_t num_outputs = session_->GetOutputCount(); + for (std::size_t i = 0; i < num_outputs; ++i) { + auto name = session_->GetOutputNameAllocated(i, allocator); + output_names_str_.push_back(std::string(name.get())); + } + for (const auto & name : output_names_str_) { + output_names_char_.push_back(name.c_str()); + } + + auto input_type_info = session_->GetInputTypeInfo(0); + auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo(); + input_shape_ = input_tensor_info.GetShape(); + if (input_shape_[0] < 0) { + input_shape_[0] = 1; + } + + auto output_type_info = session_->GetOutputTypeInfo(0); + auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo(); + output_shape_ = output_tensor_info.GetShape(); + if (output_shape_[0] < 0) { + output_shape_[0] = 1; + } + + if (output_shape_.size() < 2 || output_shape_[1] != static_cast(sim2real_common::DeploymentContract::kActionDim)) { + throw std::runtime_error("ONNX model output shape mismatch"); + } + + RCLCPP_INFO( + get_logger(), + "ONNX Runtime model loaded successfully from %s. Input shape: [%ld, %ld], Output shape: [%ld, %ld]", + model_path_.c_str(), + input_shape_[0], + input_shape_[1], + output_shape_[0], + output_shape_[1]); + return true; + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "Failed to load ONNX Runtime model: %s", e.what()); + shutdownOnnxRuntime(); + return false; + } +} + +void PolicyRuntimeNode::shutdownOnnxRuntime() +{ + memory_info_.reset(); + session_.reset(); + env_.reset(); + input_names_char_.clear(); + output_names_char_.clear(); + input_names_str_.clear(); + output_names_str_.clear(); +} + +void PolicyRuntimeNode::shutdownTensorRt() +{ +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT + if (trt_input_buffer_ != nullptr) { + cudaFree(trt_input_buffer_); + trt_input_buffer_ = nullptr; + } + if (trt_output_buffer_ != nullptr) { + cudaFree(trt_output_buffer_); + trt_output_buffer_ = nullptr; + } + if (trt_stream_ != nullptr) { + cudaStreamDestroy(trt_stream_); + trt_stream_ = nullptr; + } + if (trt_context_ != nullptr) { + destroyTensorRtObject(trt_context_); + } + if (trt_engine_ != nullptr) { + destroyTensorRtObject(trt_engine_); + } + if (trt_runtime_ != nullptr) { + destroyTensorRtObject(trt_runtime_); + } + trt_input_name_.clear(); + trt_output_name_.clear(); +#endif +} + +void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg) +{ + std::scoped_lock lock(mutex_); + latest_state_ = *msg; + has_state_ = true; + last_state_recv_time_ = std::chrono::steady_clock::now(); +} + +void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw) +{ + std::scoped_lock lock(mutex_); + raw_cmd_[0] = std::clamp(vx, -runtime_max_vx_, runtime_max_vx_); + raw_cmd_[1] = std::clamp(vy, -runtime_max_vy_, runtime_max_vy_); + raw_cmd_[2] = std::clamp(vyaw, -runtime_max_yaw_rate_, runtime_max_yaw_rate_); + cmd_ = raw_cmd_; +} + +void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + applyCmdVel( + static_cast(msg->linear.x), + static_cast(msg->linear.y), + static_cast(msg->angular.z)); +} + +void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg) +{ + applyCmdVel( + static_cast(msg->twist.linear.x), + static_cast(msg->twist.linear.y), + static_cast(msg->twist.angular.z)); +} + +void PolicyRuntimeNode::onModelSwitchCmd(const std_msgs::msg::String::SharedPtr msg) +{ + const std::string command = msg ? msg->data : ""; + const auto to_lower = [](std::string value) { + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char c) { + return static_cast(std::tolower(c)); + }); + return value; + }; + + const std::string normalized = to_lower(command); + ModelMode target_mode = current_model_mode_; + bool recognized = true; + if (normalized == "toggle" || normalized == "switch") { + target_mode = current_model_mode_ == ModelMode::Rough ? ModelMode::Crawl : ModelMode::Rough; + } else if (normalized == "rough") { + target_mode = ModelMode::Rough; + } else if (normalized == "crawl" || normalized == "ik") { + target_mode = ModelMode::Crawl; + } else if (normalized == "wall") { + target_mode = ModelMode::Wall; + } else { + recognized = false; + } + + if (!recognized) { + RCLCPP_WARN(get_logger(), "Ignoring unknown model switch command: %s", command.c_str()); + return; + } + + if (startup_state_ != StartupState::RUNTIME) { + RCLCPP_WARN(get_logger(), "Ignoring model switch command before runtime release: %s", command.c_str()); + return; + } + + if (model_switch_state_ != ModelSwitchState::Idle) { + RCLCPP_WARN( + get_logger(), + "Ignoring model switch command while another switch is active. current_state=%s", + modelSwitchStateName(model_switch_state_)); + return; + } + + if (posture_hold_mode_ != PostureHoldMode::None) { + RCLCPP_WARN( + get_logger(), + "Ignoring model switch command while posture hold is active. posture=%s", + postureHoldModeName(posture_hold_mode_)); + return; + } + if (target_mode == current_model_mode_) { + RCLCPP_INFO(get_logger(), "Model switch requested to current model %s; ignoring.", modelModeName(target_mode)); + return; + } + + { + std::scoped_lock lock(mutex_); + requested_model_mode_ = target_mode; + model_switch_requested_ = true; + } + RCLCPP_INFO( + get_logger(), + "Queued model switch from %s to %s", + modelModeName(current_model_mode_), + modelModeName(target_mode)); + publishModelStatus(); +} + +void PolicyRuntimeNode::startPostureTransition( + PostureHoldMode mode, + const std::array & start_pose, + const std::array & target_pose, + const rclcpp::Time & now_time) +{ + posture_hold_mode_ = mode; + posture_transition_active_ = true; + posture_transition_start_time_ = now_time; + posture_start_pose_ = start_pose; + posture_target_pose_ = target_pose; + posture_start_pose_[12] = posture_start_pose_[13] = posture_start_pose_[14] = posture_start_pose_[15] = 0.0f; + posture_target_pose_[12] = posture_target_pose_[13] = posture_target_pose_[14] = posture_target_pose_[15] = 0.0f; + + for (std::size_t i = 0; i < posture_delta_.size(); ++i) { + float delta = posture_target_pose_[i] - posture_start_pose_[i]; + if (i < sim2real_common::DeploymentContract::kLegJointCount) { + delta = delta - 2.0f * static_cast(M_PI) * std::floor( + (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + } + posture_delta_[i] = delta; + } + + cmd_.fill(0.0f); + raw_cmd_.fill(0.0f); + filtered_cmd_.fill(0.0f); + last_actions_.fill(0.0f); + runtime_released_ = false; + release_alpha_ = 0.0f; + release_active_time_ = 0.0f; + hold_active_model_pose_when_unreleased_ = true; + slow_release_after_model_switch_ = true; + safety_reference_dof_pos_ = mode == PostureHoldMode::Keep ? keep_pose_dof_pos_ : active_default_dof_pos_; +} + +void PolicyRuntimeNode::onPostureCmd(const std_msgs::msg::String::SharedPtr msg) +{ + const std::string command = msg ? msg->data : ""; + const std::string normalized = toLowerCopy(command); + const rclcpp::Time now_time = now(); + + std::scoped_lock lock(mutex_); + if (startup_state_ != StartupState::RUNTIME) { + RCLCPP_WARN(get_logger(), "Ignoring posture command before runtime release: %s", command.c_str()); + return; + } + if (model_switch_state_ != ModelSwitchState::Idle) { + RCLCPP_WARN( + get_logger(), + "Ignoring posture command while model switch is active. current_state=%s", + modelSwitchStateName(model_switch_state_)); + return; + } + std::array start_pose = has_state_ ? latest_state_.joint_pos : active_default_dof_pos_; + start_pose[12] = start_pose[13] = start_pose[14] = start_pose[15] = 0.0f; + + if (normalized == "keep") { + if (posture_hold_mode_ == PostureHoldMode::Keep && !posture_transition_active_) { + return; + } + startPostureTransition(PostureHoldMode::Keep, start_pose, keep_pose_dof_pos_, now_time); + RCLCPP_INFO(get_logger(), "Posture hold requested: keep pose."); + } else if (normalized == "default" || normalized == "release" || normalized == "off") { + if (posture_hold_mode_ == PostureHoldMode::None && !posture_transition_active_) { + return; + } + startPostureTransition(PostureHoldMode::ReturnDefault, start_pose, active_default_dof_pos_, now_time); + RCLCPP_INFO(get_logger(), "Posture hold requested: return to default pose."); + } else if (!normalized.empty()) { + RCLCPP_WARN(get_logger(), "Ignoring unknown posture command: %s", command.c_str()); + } +} +void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg) +{ + std::scoped_lock lock(mutex_); + const bool was_estop = estop_triggered_; + estop_triggered_ = msg->data; + if (estop_triggered_) { + RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!"); + logEvent("WARN", "estop_triggered", "E-stop triggered via /safety/estop."); + } else { + RCLCPP_INFO(get_logger(), "E-stop reset."); + logEvent("INFO", "estop_reset", "E-stop reset via /safety/estop."); + const bool user_estop_latch = + safety_reason_.find("user E-stop") != std::string::npos || + (was_estop && safety_reason_.empty()); + if (safety_triggered_ && user_estop_latch) { + safety_triggered_ = false; + safety_reason_.clear(); + clip_active_logged_ = false; + RCLCPP_INFO(get_logger(), "Cleared user E-stop safety latch."); + logEvent("INFO", "safety_latch_reset", "Cleared user E-stop safety latch."); + } + } +} + +std::array PolicyRuntimeNode::buildObservation( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd, + const std::array & last_actions) const +{ + std::array obs{}; + std::size_t cursor = 0; + + for (int i = 0; i < 3; ++i) { + obs[cursor++] = state.imu_gyro[i] * 0.25f; + } + for (int i = 0; i < 3; ++i) { + obs[cursor++] = state.projected_gravity[i]; + } + for (float v : cmd) { + obs[cursor++] = v; + } + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + obs[cursor++] = state.joint_pos[i] - active_default_dof_pos_[i]; + } + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + obs[cursor++] = state.joint_vel[i] * 0.05f; + } + for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + obs[cursor++] = state.joint_vel[i] * 0.05f; + } + for (float v : last_actions) { + obs[cursor++] = v; + } + + // Clip observations values to ±clip_obs_ + if (clip_obs_ > 0.0f) { + for (float & v : obs) { + v = std::clamp(v, -clip_obs_, clip_obs_); + } + } + + return obs; +} + +std::array PolicyRuntimeNode::runPolicy(const std::array & obs) +{ + std::array action{}; + + switch (inference_backend_) { + case InferenceBackend::TensorRT: +#ifdef SIM2REAL_RUNTIME_HAS_TENSORRT + if (cudaMemcpyAsync( + trt_input_buffer_, + obs.data(), + sizeof(float) * obs.size(), + cudaMemcpyHostToDevice, + trt_stream_) != cudaSuccess) { + RCLCPP_ERROR(get_logger(), "TensorRT H2D copy failed."); + action.fill(0.0f); + break; + } + if (!trt_context_->enqueueV3(trt_stream_)) { + RCLCPP_ERROR(get_logger(), "TensorRT enqueue failed."); + action.fill(0.0f); + break; + } + if (cudaMemcpyAsync( + action.data(), + trt_output_buffer_, + sizeof(float) * action.size(), + cudaMemcpyDeviceToHost, + trt_stream_) != cudaSuccess) { + RCLCPP_ERROR(get_logger(), "TensorRT D2H copy failed."); + action.fill(0.0f); + break; + } + if (cudaStreamSynchronize(trt_stream_) != cudaSuccess) { + RCLCPP_ERROR(get_logger(), "TensorRT stream synchronization failed."); + action.fill(0.0f); + } +#else + action.fill(0.0f); +#endif + break; + case InferenceBackend::OnnxRuntime: + try { + auto input_tensor = Ort::Value::CreateTensor( + *memory_info_, + const_cast(obs.data()), + obs.size(), + input_shape_.data(), + input_shape_.size() + ); + + auto output_tensor = Ort::Value::CreateTensor( + *memory_info_, + action.data(), + action.size(), + output_shape_.data(), + output_shape_.size() + ); + + session_->Run( + Ort::RunOptions{nullptr}, + input_names_char_.data(), + &input_tensor, + 1, + output_names_char_.data(), + &output_tensor, + 1 + ); + } catch (const std::exception& e) { + RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what()); + action.fill(0.0f); + } + break; + default: + RCLCPP_ERROR(get_logger(), "No inference backend available."); + action.fill(0.0f); + break; + } + + for (float& v : action) { + v = std::clamp(v, -10.0f, 10.0f); + } + + return action; +} + +bool PolicyRuntimeNode::isZeroCommand(const std::array & cmd, const std::array & imu_gyro) const +{ + const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]); + const bool cmd_is_zero = planar_cmd < zero_cmd_lin_thresh_ && std::abs(cmd[2]) < zero_cmd_yaw_thresh_; + if (!zero_cmd_use_yaw_rate_) { + return cmd_is_zero; + } + return cmd_is_zero && std::abs(imu_gyro[2]) < zero_yaw_rate_thresh_; +} + +bool PolicyRuntimeNode::isCommandActive(const std::array & cmd) const +{ + const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]); + return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_; +} + +bool PolicyRuntimeNode::modeUsesInference(ModelMode mode) const +{ + return mode == ModelMode::Rough || + mode == ModelMode::Wall || + (mode == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Rl); +} + +const std::array & PolicyRuntimeNode::defaultPoseForMode(ModelMode mode) const +{ + switch (mode) { + case ModelMode::Crawl: + return crawl_default_dof_pos_; + case ModelMode::Wall: + return wall_default_dof_pos_; + case ModelMode::Rough: + default: + return rough_default_dof_pos_; + } +} + +const std::string & PolicyRuntimeNode::modelPathForMode(ModelMode mode) const +{ + switch (mode) { + case ModelMode::Crawl: + return crawl_model_path_; + case ModelMode::Wall: + return wall_model_path_; + case ModelMode::Rough: + default: + return rough_model_path_; + } +} + +const std::string & PolicyRuntimeNode::modelEnginePathForMode(ModelMode mode) const +{ + switch (mode) { + case ModelMode::Crawl: + return crawl_model_engine_path_; + case ModelMode::Wall: + return wall_model_engine_path_; + case ModelMode::Rough: + default: + return rough_model_engine_path_; + } +} + +std::array PolicyRuntimeNode::computeHoldTarget( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd) +{ + if (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) { + auto target = crawl_default_dof_pos_; + if (crawl_ik_imu_posture_) { + const auto balance_target = stand_balance_->computeTarget( + state.projected_gravity, state.imu_gyro, cmd); + for (std::size_t leg_idx = 0; leg_idx < 4; ++leg_idx) { + const std::size_t abduction_index = leg_idx * 3; + target[abduction_index] = std::clamp( + balance_target[abduction_index], -crawl_ik_abduction_clip_, crawl_ik_abduction_clip_); + } + } else { + stand_balance_->computeTarget( + state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); + } + if (crawl_ik_encoder_posture_kp_ > 0.0f) { + for (std::size_t i = 0; i < 12; ++i) { + const float encoder_err = target[i] - state.joint_pos[i]; + const float correction = std::clamp( + crawl_ik_encoder_posture_kp_ * encoder_err, + -crawl_ik_encoder_posture_max_, + crawl_ik_encoder_posture_max_); + target[i] += correction; + } + } + target[12] = 0.0f; + target[13] = 0.0f; + target[14] = 0.0f; + target[15] = 0.0f; + return target; + } + + // Keep the balance controller updated for stability monitoring, while holding + // the active model's default pose for rough/wall runtime zero-command holds. + stand_balance_->computeTarget( + state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); + return active_default_dof_pos_; +} + +std::array PolicyRuntimeNode::computeIkCrawlTarget( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & cmd) +{ + auto target = computeHoldTarget(state, cmd); + const float yaw_rate_cmd = + cmd[2] + crawl_ik_yaw_rate_kp_ * (cmd[2] - state.imu_gyro[2]); + const float command_scale = computeCrawlIkCommandScale(state, target); + + const float left_wheel = std::clamp( + cmd[0] * crawl_ik_wheel_linear_gain_ - yaw_rate_cmd * crawl_ik_wheel_yaw_gain_, + -crawl_ik_max_wheel_speed_, crawl_ik_max_wheel_speed_) * command_scale; + const float right_wheel = std::clamp( + cmd[0] * crawl_ik_wheel_linear_gain_ + yaw_rate_cmd * crawl_ik_wheel_yaw_gain_, + -crawl_ik_max_wheel_speed_, crawl_ik_max_wheel_speed_) * command_scale; + + target[12] = left_wheel; + target[13] = right_wheel; + target[14] = left_wheel; + target[15] = right_wheel; + return target; +} + +float PolicyRuntimeNode::projectedGravityTiltRad( + const std::array & projected_gravity) const +{ + const float lateral = std::hypot(projected_gravity[0], projected_gravity[1]); + const float vertical = std::max(1.0e-6f, std::abs(projected_gravity[2])); + return std::atan2(lateral, vertical); +} + +float PolicyRuntimeNode::computeCrawlIkCommandScale( + const sim2real_interfaces::msg::RuntimeState & state, + const std::array & leg_target) const +{ + float scale = 1.0f; + if (crawl_ik_encoder_guard_) { + float max_leg_err = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + max_leg_err = std::max(max_leg_err, std::abs(state.joint_pos[i] - leg_target[i])); + } + if (max_leg_err >= crawl_ik_encoder_guard_stop_) { + scale = 0.0f; + } else if (max_leg_err > crawl_ik_encoder_guard_start_) { + const float span = std::max( + 1.0e-6f, crawl_ik_encoder_guard_stop_ - crawl_ik_encoder_guard_start_); + scale *= 1.0f - (max_leg_err - crawl_ik_encoder_guard_start_) / span; + } + } + + if (crawl_ik_imu_guard_) { + const float tilt = projectedGravityTiltRad(state.projected_gravity); + if (tilt >= crawl_ik_imu_guard_stop_rad_) { + scale = 0.0f; + } else if (tilt > crawl_ik_imu_guard_start_rad_) { + const float span = std::max( + 1.0e-6f, crawl_ik_imu_guard_stop_rad_ - crawl_ik_imu_guard_start_rad_); + scale *= 1.0f - (tilt - crawl_ik_imu_guard_start_rad_) / span; + } + } + + return std::clamp(scale, 0.0f, 1.0f); +} + +bool PolicyRuntimeNode::switchInferenceModel(ModelMode target_mode) +{ + const ModelMode previous_mode = current_model_mode_; + const ModelMode previous_loaded_mode = loaded_model_mode_; + const std::array previous_default_pose = active_default_dof_pos_; + const std::string previous_model_path = model_path_; + const std::string previous_model_engine_path = model_engine_path_; + const bool target_uses_inference = modeUsesInference(target_mode); + + if (!target_uses_inference) { + current_model_mode_ = target_mode; + active_default_dof_pos_ = defaultPoseForMode(target_mode); + RCLCPP_INFO( + get_logger(), + "Switched active model to %s using backend %s", + modelModeName(current_model_mode_), + crawlBackendName()); + publishModelStatus(); + return true; + } + + if (loaded_model_mode_ == target_mode) { + current_model_mode_ = target_mode; + active_default_dof_pos_ = defaultPoseForMode(target_mode); + RCLCPP_INFO( + get_logger(), + "Switched active model to %s using already loaded backend %s", + modelModeName(current_model_mode_), + inferenceBackendName()); + publishModelStatus(); + return true; + } + + const std::string next_model_path = modelPathForMode(target_mode); + const std::string next_model_engine_path = modelEnginePathForMode(target_mode); + + shutdownTensorRt(); + shutdownOnnxRuntime(); + + model_path_ = next_model_path; + model_engine_path_ = next_model_engine_path; + + if (!initInferenceBackend()) { + RCLCPP_ERROR( + get_logger(), + "Failed to switch inference model to %s (model=%s engine=%s)", + modelModeName(target_mode), + model_path_.c_str(), + model_engine_path_.c_str()); + shutdownTensorRt(); + shutdownOnnxRuntime(); + model_path_ = previous_model_path; + model_engine_path_ = previous_model_engine_path; + current_model_mode_ = previous_mode; + loaded_model_mode_ = previous_loaded_mode; + active_default_dof_pos_ = previous_default_pose; + if (!initInferenceBackend()) { + RCLCPP_FATAL( + get_logger(), + "Failed to restore previous inference model %s after switch failure.", + modelModeName(previous_loaded_mode)); + } else { + RCLCPP_WARN( + get_logger(), + "Restored previous inference model %s after switch failure.", + modelModeName(previous_loaded_mode)); + } + publishModelStatus(); + return false; + } + + loaded_model_mode_ = target_mode; + current_model_mode_ = target_mode; + active_default_dof_pos_ = defaultPoseForMode(target_mode); + RCLCPP_INFO( + get_logger(), + "Switched active model to %s using backend %s", + modelModeName(current_model_mode_), + inferenceBackendName()); + publishModelStatus(); + return true; +} + +const char * PolicyRuntimeNode::modelModeName(ModelMode mode) const +{ + switch (mode) { + case ModelMode::Rough: + return "rough"; + case ModelMode::Crawl: + return crawl_backend_ == CrawlBackend::Ik ? "ik" : "crawl"; + case ModelMode::Wall: + return "wall"; + default: + return "unknown"; + } +} + +const char * PolicyRuntimeNode::startupStateName(StartupState state) const +{ + switch (state) { + case StartupState::BOOT_HOLD: + return "boot_hold"; + case StartupState::STARTUP_SOFT_HOLD: + return "startup_soft_hold"; + case StartupState::STARTUP_TRANSITION: + return "startup_transition"; + case StartupState::STARTUP_HOLD_AFTER: + return "startup_hold_after"; + case StartupState::RUNTIME: + return "runtime"; + default: + return "unknown"; + } +} + +const char * PolicyRuntimeNode::modelSwitchStateName(ModelSwitchState state) const +{ + switch (state) { + case ModelSwitchState::Idle: + return "idle"; + case ModelSwitchState::ToStand: + return "to_stand"; + case ModelSwitchState::StandHold: + return "stand_hold"; + case ModelSwitchState::ToModelPose: + return "to_model_pose"; + default: + return "unknown"; + } +} + +const char * PolicyRuntimeNode::postureHoldModeName(PostureHoldMode mode) const +{ + switch (mode) { + case PostureHoldMode::Keep: + return "keep"; + case PostureHoldMode::ReturnDefault: + return "return_default"; + case PostureHoldMode::None: + default: + return "none"; + } +} +const char * PolicyRuntimeNode::inferenceBackendName() const +{ + switch (inference_backend_) { + case InferenceBackend::TensorRT: + return "tensorrt"; + case InferenceBackend::OnnxRuntime: + return "onnxruntime"; + default: + return "none"; + } +} + +const char * PolicyRuntimeNode::crawlBackendName() const +{ + switch (crawl_backend_) { + case CrawlBackend::Ik: + return "ik"; + case CrawlBackend::Rl: + return "rl"; + default: + return "unknown"; + } +} + +void PolicyRuntimeNode::publishModelStatus() +{ + if (!model_status_pub_) { + return; + } + + const char * active_backend = + (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) ? + crawlBackendName() : inferenceBackendName(); + + std::ostringstream stream; + stream << "{" + << "\"current_model\":\"" << modelModeName(current_model_mode_) << "\"," + << "\"requested_model\":\"" << modelModeName(requested_model_mode_) << "\"," + << "\"switch_state\":\"" << modelSwitchStateName(model_switch_state_) << "\"," + << "\"backend\":\"" << active_backend << "\"," + << "\"crawl_backend\":\"" << crawlBackendName() << "\"," + << "\"inference_backend\":\"" << inferenceBackendName() << "\"," + << "\"switching\":" << (model_switch_state_ != ModelSwitchState::Idle ? "true" : "false") + << "}"; + std_msgs::msg::String msg; + msg.data = stream.str(); + model_status_pub_->publish(msg); +} + +void PolicyRuntimeNode::onPolicyLoop() +{ + sim2real_interfaces::msg::RuntimeState state; + std::array cmd{}; + std::array raw_cmd{}; + std::array last_actions{}; + bool estop_active = false; + bool safety_active = false; + double state_age_ms = 0.0; + bool model_switch_requested = false; + ModelMode requested_model_mode = current_model_mode_; + PostureHoldMode posture_hold_mode = PostureHoldMode::None; + bool posture_transition_active = false; + std::array posture_start_pose{}; + std::array posture_target_pose{}; + std::array posture_delta{}; + rclcpp::Time posture_transition_start_time{0, 0, RCL_ROS_TIME}; + double posture_transition_s = posture_transition_s_; + { + std::scoped_lock lock(mutex_); + if (!has_state_) { + return; + } + state = latest_state_; + cmd = cmd_; + raw_cmd = raw_cmd_; + last_actions = last_actions_; + estop_active = estop_triggered_; + safety_active = safety_triggered_; + model_switch_requested = model_switch_requested_; + requested_model_mode = requested_model_mode_; + posture_hold_mode = posture_hold_mode_; + posture_transition_active = posture_transition_active_; + posture_start_pose = posture_start_pose_; + posture_target_pose = posture_target_pose_; + posture_delta = posture_delta_; + posture_transition_start_time = posture_transition_start_time_; + posture_transition_s = posture_transition_s_; + if (last_state_recv_time_.time_since_epoch().count() != 0) { + state_age_ms = std::chrono::duration( + std::chrono::steady_clock::now() - last_state_recv_time_).count(); + } + } + + // 1) Run RuntimeGuard check + if (safety_enabled_ && !safety_active) { + std::vector extra_vals; + extra_vals.reserve(48); + for (float v : state.joint_pos) extra_vals.push_back(v); + for (float v : state.joint_vel) extra_vals.push_back(v); + for (float v : last_actions) extra_vals.push_back(v); + + const float effective_imu_age_ms = static_cast(std::max( + static_cast(state.imu_age_ms), state_age_ms)); + + auto guard_decision = runtime_guard_->check( + state.imu_gyro, state.projected_gravity, effective_imu_age_ms, estop_active, extra_vals); + if (guard_decision.level == sim2real_common::GuardLevel::STOP) { + { + std::scoped_lock lock(mutex_); + safety_triggered_ = true; + } + safety_active = true; + safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason; + RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str()); + logEvent("ERROR", "safety_triggered", safety_reason_); + logProtectionEvent("runtime_guard_stop", guard_decision.reason, "safety_brake"); + } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str()); + } + } + + if (safety_active) { + sim2real_interfaces::msg::RuntimeTarget target; + target.stamp = now(); + target.sequence = sequence_++; + target.raw_command = raw_cmd; + target.command = cmd; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + target.target = safety_reference_dof_pos_; + target.target_source = "safety_brake"; + target.target_age_ms = 0.0f; + target_pub_->publish(target); + appendDebugTrace(state, target); + return; + } + + sim2real_interfaces::msg::RuntimeTarget target; + target.stamp = now(); + target.sequence = sequence_++; + target.raw_command = raw_cmd; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + target.command = cmd; + + const auto now_time = rclcpp::Time(target.stamp); + + if (startup_state_ == StartupState::BOOT_HOLD) { + // 1. Initial State Read + start_pose_ = state.joint_pos; + start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0 + + // 2. Shortest periodic delta to stand pose + float max_dev = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + float delta = active_default_dof_pos_[i] - start_pose_[i]; + delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + startup_delta_[i] = delta; + max_dev = std::max(max_dev, std::abs(delta)); + } + startup_delta_[12] = startup_delta_[13] = startup_delta_[14] = startup_delta_[15] = 0.0f; + + if (max_dev > 3.0f) { + RCLCPP_WARN(get_logger(), "Measured joint dev too large (%f rad > 3.0 rad). Aborting standup transition.", max_dev); + target.target = start_pose_; + target.target_source = "boot_hold"; + target_pub_->publish(target); + appendDebugTrace(state, target); + return; + } + + // Adapt transition time: min 2s, max 6s, 1.5s per rad + transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0); + startup_state_ = StartupState::STARTUP_SOFT_HOLD; + state_start_time_ = now_time; + RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_); + } + + if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) { + double elapsed = (now_time - state_start_time_).seconds(); + target.target = start_pose_; + target.target_source = "startup_soft_hold"; + + if (elapsed >= 1.0) { // 1s soft hold + startup_state_ = StartupState::STARTUP_TRANSITION; + state_start_time_ = now_time; + RCLCPP_INFO(get_logger(), "Transitioning to stand pose..."); + } + } + else if (startup_state_ == StartupState::STARTUP_TRANSITION) { + double elapsed = (now_time - state_start_time_).seconds(); + double phase = std::min(1.0, elapsed / transition_time_); + + // Cosine blend interpolation + double blend = 0.5 - 0.5 * std::cos(M_PI * phase); + for (std::size_t i = 0; i < 16; ++i) { + target.target[i] = start_pose_[i] + blend * startup_delta_[i]; + } + target.target_source = "startup_hold"; + + if (phase >= 1.0) { + // Settle check + float max_pos_err = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + float delta = active_default_dof_pos_[i] - state.joint_pos[i]; + delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + max_pos_err = std::max(max_pos_err, std::abs(delta)); + } + float max_vel_err = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i])); + } + + if (max_pos_err <= 0.30f && max_vel_err <= 0.6f) { + startup_state_ = StartupState::STARTUP_HOLD_AFTER; + state_start_time_ = now_time; + RCLCPP_INFO(get_logger(), "Pose settled. Holding for 1.0s..."); + } + } + } + else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) { + double elapsed = (now_time - state_start_time_).seconds(); + + // Keep balance stability tracking running, while holding the exact default + // rough pose so startup hold matches the runtime stand posture. + stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd); + target.target = rough_default_dof_pos_; + target.target_source = "startup_hold"; + + if (elapsed >= 1.0 && stand_balance_->isStable()) { + startup_state_ = StartupState::RUNTIME; + RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!"); + } + } + else if (startup_state_ == StartupState::RUNTIME) { + if (model_switch_requested && posture_hold_mode == PostureHoldMode::None && + model_switch_state_ == ModelSwitchState::Idle && requested_model_mode != current_model_mode_) { + switch_start_pose_ = state.joint_pos; + switch_start_pose_[12] = switch_start_pose_[13] = switch_start_pose_[14] = switch_start_pose_[15] = 0.0f; + float max_dev = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + float delta = rough_default_dof_pos_[i] - switch_start_pose_[i]; + delta = delta - 2.0f * static_cast(M_PI) * std::floor((delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + switch_delta_[i] = delta; + max_dev = std::max(max_dev, std::abs(delta)); + } + switch_delta_[12] = switch_delta_[13] = switch_delta_[14] = switch_delta_[15] = 0.0f; + const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); + active_switch_transition_s_ = std::clamp( + max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * + std::max(0.1, model_switch_to_stand_transition_scale_); + model_switch_state_ = ModelSwitchState::ToStand; + model_switch_state_start_time_ = now_time; + safety_reference_dof_pos_ = rough_default_dof_pos_; + runtime_released_ = false; + release_active_time_ = 0.0f; + release_alpha_ = 0.0f; + last_actions.fill(0.0f); + hold_active_model_pose_when_unreleased_ = true; + { + std::scoped_lock lock(mutex_); + model_switch_requested_ = false; + } + RCLCPP_INFO( + get_logger(), + "Starting model switch transition: %s -> stand -> %s", + modelModeName(current_model_mode_), + modelModeName(requested_model_mode)); + publishModelStatus(); + } + + if (model_switch_state_ == ModelSwitchState::ToStand) { + const double elapsed = (now_time - model_switch_state_start_time_).seconds(); + const double phase = std::min(1.0, elapsed / std::max(1.0e-3, active_switch_transition_s_)); + const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); + for (std::size_t i = 0; i < 16; ++i) { + target.target[i] = switch_start_pose_[i] + static_cast(blend) * switch_delta_[i]; + } + target.target_source = "model_switch_to_stand"; + if (phase >= 1.0) { + model_switch_state_ = ModelSwitchState::StandHold; + model_switch_state_start_time_ = now_time; + publishModelStatus(); + } + } else if (model_switch_state_ == ModelSwitchState::StandHold) { + target.target = rough_default_dof_pos_; + target.target_source = "model_switch_stand_hold"; + const double elapsed = (now_time - model_switch_state_start_time_).seconds(); + float max_pos_err = 0.0f; + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + float delta = rough_default_dof_pos_[i] - state.joint_pos[i]; + delta = delta - 2.0f * static_cast(M_PI) * std::floor( + (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + max_pos_err = std::max(max_pos_err, std::abs(delta)); + } + float max_vel_err = 0.0f; + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i])); + } + const bool hold_elapsed = elapsed >= model_switch_stand_hold_s_; + const bool stand_ready = + max_pos_err <= static_cast(model_switch_stand_max_err_) && + max_vel_err <= static_cast(model_switch_stand_max_vel_); + if (hold_elapsed && stand_ready) { + switch_start_pose_ = state.joint_pos; + switch_start_pose_[12] = switch_start_pose_[13] = switch_start_pose_[14] = switch_start_pose_[15] = 0.0f; + if (switchInferenceModel(requested_model_mode)) { + float max_dev = 0.0f; + for (std::size_t i = 0; i < 16; ++i) { + float delta = active_default_dof_pos_[i] - switch_start_pose_[i]; + if (i < sim2real_common::DeploymentContract::kLegJointCount) { + delta = delta - 2.0f * static_cast(M_PI) * std::floor( + (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + max_dev = std::max(max_dev, std::abs(delta)); + } + switch_delta_[i] = delta; + } + const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); + active_switch_transition_s_ = std::clamp( + max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * + std::max(0.1, model_switch_to_model_transition_scale_); + const bool has_pose_delta = std::any_of( + switch_delta_.begin(), switch_delta_.end(), [](float value) { return std::abs(value) > 1.0e-4f; }); + model_switch_state_ = has_pose_delta ? ModelSwitchState::ToModelPose : ModelSwitchState::Idle; + model_switch_state_start_time_ = now_time; + if (!has_pose_delta) { + safety_reference_dof_pos_ = active_default_dof_pos_; + slow_release_after_model_switch_ = true; + } + hold_active_model_pose_when_unreleased_ = true; + publishModelStatus(); + } else { + float max_dev = 0.0f; + for (std::size_t i = 0; i < 16; ++i) { + float delta = active_default_dof_pos_[i] - switch_start_pose_[i]; + if (i < sim2real_common::DeploymentContract::kLegJointCount) { + delta = delta - 2.0f * static_cast(M_PI) * std::floor( + (delta + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + max_dev = std::max(max_dev, std::abs(delta)); + } + switch_delta_[i] = delta; + } + const double min_switch_s = std::max(0.05, model_switch_min_transition_s_); + active_switch_transition_s_ = std::clamp( + max_dev * 1.2, min_switch_s, std::max(min_switch_s, model_switch_transition_s_)) * + std::max(0.1, model_switch_to_model_transition_scale_); + model_switch_state_ = ModelSwitchState::ToModelPose; + model_switch_state_start_time_ = now_time; + hold_active_model_pose_when_unreleased_ = true; + publishModelStatus(); + } + } else if (hold_elapsed) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 1000, + "Model switch stand hold waiting for settle: max_pos_err=%.3f rad, max_vel_err=%.3f rad/s", + max_pos_err, max_vel_err); + } + } else if (model_switch_state_ == ModelSwitchState::ToModelPose) { + const double elapsed = (now_time - model_switch_state_start_time_).seconds(); + const double to_model_transition_s = std::max(1.0e-3, active_switch_transition_s_); + const double phase = std::min(1.0, elapsed / to_model_transition_s); + const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); + for (std::size_t i = 0; i < 16; ++i) { + target.target[i] = switch_start_pose_[i] + static_cast(blend) * switch_delta_[i]; + } + target.target_source = "model_switch_to_model_pose"; + if (phase >= 1.0) { + model_switch_state_ = ModelSwitchState::Idle; + safety_reference_dof_pos_ = active_default_dof_pos_; + hold_active_model_pose_when_unreleased_ = true; + slow_release_after_model_switch_ = true; + publishModelStatus(); + } + } else if (posture_hold_mode != PostureHoldMode::None) { + const double elapsed = posture_transition_active ? + (now_time - posture_transition_start_time).seconds() : posture_transition_s; + const double phase = posture_transition_active ? + std::min(1.0, elapsed / std::max(1.0e-3, posture_transition_s)) : 1.0; + const double blend = 0.5 - 0.5 * std::cos(M_PI * phase); + for (std::size_t i = 0; i < target.target.size(); ++i) { + target.target[i] = posture_start_pose[i] + static_cast(blend) * posture_delta[i]; + } + target.target_source = posture_hold_mode == PostureHoldMode::Keep ? + "runtime_keep_pose" : "runtime_keep_return_default"; + target.runtime_released = false; + target.release_alpha = 0.0f; + target.zero_command = true; + target.command.fill(0.0f); + target.raw_command.fill(0.0f); + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + last_actions.fill(0.0f); + release_alpha_ = 0.0f; + runtime_released_ = false; + release_active_time_ = 0.0f; + + if (phase >= 1.0) { + std::scoped_lock lock(mutex_); + if (posture_hold_mode_ == posture_hold_mode) { + posture_transition_active_ = false; + if (posture_hold_mode == PostureHoldMode::ReturnDefault) { + posture_hold_mode_ = PostureHoldMode::None; + safety_reference_dof_pos_ = active_default_dof_pos_; + } else { + safety_reference_dof_pos_ = keep_pose_dof_pos_; + } + } + } + } else { + // Python template uses the command directly in policy obs/release logic. + // Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here. + filtered_cmd_ = cmd; + + const auto target_hold = computeHoldTarget(state, cmd); + const bool zero_command = isZeroCommand(cmd, state.imu_gyro); + + if (!runtime_released_) { + if (require_active_command_to_release_) { + if (isCommandActive(cmd)) { + release_active_time_ += kPolicyDt; + } else { + release_active_time_ = 0.0f; + } + + const auto & unreleased_hold_target = + hold_active_model_pose_when_unreleased_ ? active_default_dof_pos_ : target_hold; + float max_hold_err = 0.0f; + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - unreleased_hold_target[i])); + } + + const bool active_ready = release_active_time_ >= release_command_hold_s_; + const bool posture_ready = max_hold_err <= release_posture_max_err_; + if (active_ready && posture_ready) { + runtime_released_ = true; + } + } else { + runtime_released_ = true; + } + } + + if (!runtime_released_ || zero_command) { + release_alpha_ = 0.0f; + target.runtime_released = false; + target.release_alpha = 0.0f; + target.zero_command = zero_command; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + last_actions.fill(0.0f); + target.target_source = "runtime_zero_hold"; + target.target = hold_active_model_pose_when_unreleased_ ? active_default_dof_pos_ : target_hold; + if (!runtime_released_) { + target.target_source = "runtime_hold"; + } + } else { + hold_active_model_pose_when_unreleased_ = false; + const float effective_release_scale = slow_release_after_model_switch_ ? + std::max(1.0f, model_switch_release_scale_) : 1.0f; + const float effective_command_release_s = command_release_s_ * effective_release_scale; + const float effective_release_target_blend_s = release_target_blend_s_ * effective_release_scale; + release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(effective_command_release_s, 1.0e-3f)); + target.runtime_released = (release_alpha_ >= 1.0f); + target.release_alpha = release_alpha_; + target.zero_command = false; + target.command = cmd; + + if (current_model_mode_ == ModelMode::Crawl && crawl_backend_ == CrawlBackend::Ik) { + target.target = computeIkCrawlTarget(state, cmd); + target.target_source = "runtime_crawl_ik"; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + last_actions.fill(0.0f); + } else { + auto raw = runPolicy(buildObservation(state, cmd, last_actions)); + for (float & v : raw) { + v *= release_alpha_; + } + target.raw_action = raw; + + const float blend = std::min( + 1.0f, + release_alpha_ * (effective_command_release_s / std::max(effective_release_target_blend_s, kPolicyDt))); + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i]; + const float policy_target = target.scaled_action[i] + active_default_dof_pos_[i]; + target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target; + last_actions[i] = raw[i]; + } + target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy"; + } + if (release_alpha_ >= 1.0f) { + slow_release_after_model_switch_ = false; + } + } + } + } + + // 2) Run SafetyMonitor check on computed target + if (safety_enabled_) { + auto * active_safety_monitor = + (model_switch_state_ != ModelSwitchState::Idle || posture_hold_mode != PostureHoldMode::None) && model_switch_safety_monitor_ ? + model_switch_safety_monitor_.get() : + safety_monitor_.get(); + auto safety_decision = active_safety_monitor->check( + target.target, safety_reference_dof_pos_, state.imu_gyro, state.projected_gravity, estop_active); + if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { + { + std::scoped_lock lock(mutex_); + safety_triggered_ = true; + } + safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; + RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str()); + logEvent("ERROR", "safety_triggered", safety_reason_); + logProtectionEvent("safety_monitor_stop", safety_decision.message, "safety_brake"); + + // Override target to safety_brake damping pose + target.target = safety_reference_dof_pos_; + target.target_source = "safety_brake"; + } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { + target.target = safety_decision.clipped_target; + target.target_source = "safety_clip"; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime."); + target_clip_count_++; + if (!clip_active_logged_) { + clip_active_logged_ = true; + logEvent("WARN", "target_clipped", + "protection=safety_clip, reason=" + safety_decision.message + ", source=" + target.target_source); + } + } else { + clip_active_logged_ = false; + } + } + + target.target_age_ms = 0.0f; + + { + std::scoped_lock lock(mutex_); + last_actions_ = last_actions; + } + + target_pub_->publish(target); + appendDebugTrace(state, target); +} + +} // namespace sim2real_runtime + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/remote_uart_node.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/remote_uart_node.py new file mode 100644 index 0000000..5e38c97 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/remote_uart_node.py @@ -0,0 +1,349 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Optional + +import serial + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Bool, String + +SBUS_FRAME_SIZE = 25 +SBUS_RC_MID = 1024 +SBUS_AXIS_SCALE = 660.0 + +SWITCH_LOW = -1 +SWITCH_MID = 0 +SWITCH_HIGH = 1 + + +@dataclass +class RemoteSwitchState: + ch5: int = SWITCH_MID + ch6: int = SWITCH_MID + ch7: int = SWITCH_MID + ch8: int = SWITCH_MID + ch9: int = SWITCH_MID + ch10: int = SWITCH_MID + + def get(self, channel: int) -> Optional[int]: + return { + 5: self.ch5, + 6: self.ch6, + 7: self.ch7, + 8: self.ch8, + 9: self.ch9, + 10: self.ch10, + }.get(int(channel)) + + +@dataclass +class RemoteControlState: + ch1: int = 0 + ch2: int = 0 + ch3: int = 0 + ch4: int = 0 + switches: RemoteSwitchState = field(default_factory=RemoteSwitchState) + frame_ok: bool = False + + @property + def estop_requested(self) -> bool: + return self.switches.ch7 == SWITCH_HIGH + + +class SbusUartReceiver: + def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int): + self.port = port + self.baudrate = int(baudrate) + self.timeout = float(timeout) + self.axis_deadzone = int(axis_deadzone) + self.serial: Optional[serial.Serial] = None + self.buffer = bytearray() + self.state = RemoteControlState() + + def open(self) -> None: + if self.serial and self.serial.is_open: + return + self.serial = serial.Serial( + port=self.port, + baudrate=self.baudrate, + timeout=self.timeout, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_EVEN, + stopbits=serial.STOPBITS_TWO, + ) + + def close(self) -> None: + if self.serial and self.serial.is_open: + self.serial.close() + + def poll(self) -> RemoteControlState: + if not self.serial or not self.serial.is_open: + raise RuntimeError("remote uart is not open") + + waiting = self.serial.in_waiting + if waiting: + self.buffer.extend(self.serial.read(waiting)) + + while len(self.buffer) >= SBUS_FRAME_SIZE: + start_idx = self.buffer.find(0x0F) + if start_idx < 0: + self.buffer.clear() + break + if start_idx > 0: + del self.buffer[:start_idx] + if len(self.buffer) < SBUS_FRAME_SIZE: + break + frame = bytes(self.buffer[:SBUS_FRAME_SIZE]) + del self.buffer[:SBUS_FRAME_SIZE] + parsed = self._parse_frame(frame) + if parsed is not None: + self.state = parsed + return self.state + + def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]: + if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F: + return None + + channels = [0] * 16 + channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF + channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF + channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF + channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF + channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF + channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF + channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF + channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF + channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF + channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF + + if channels[0] < 100: + return None + + state = RemoteControlState( + ch1=self._normalize_axis(channels[0]), + ch2=self._normalize_axis(channels[1]), + ch3=self._normalize_axis(channels[3]), + ch4=self._normalize_axis(channels[2]), + switches=RemoteSwitchState( + ch5=self._decode_switch(channels[4]), + ch6=self._decode_switch(channels[5]), + ch7=self._decode_switch(channels[6]), + ch8=self._decode_switch(channels[7]), + ch9=self._decode_switch(channels[8]), + ch10=self._decode_switch(channels[9]), + ), + frame_ok=True, + ) + if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)): + return None + return state + + def _normalize_axis(self, value: int) -> int: + mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0)) + return 0 if abs(mapped) <= self.axis_deadzone else mapped + + @staticmethod + def _decode_switch(value: int) -> int: + if value < 500: + return SWITCH_LOW + if value > 1500: + return SWITCH_HIGH + return SWITCH_MID + + +class RemoteUartNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True) + + self.enabled = bool(self.declare_parameter("remote_enabled", True).value) + self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value) + self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value) + self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value) + self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value) + self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value) + self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0) + self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value) + self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value) + self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value) + self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value) + self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value) + self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value) + self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value) + self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).strip().upper() + self.model_switch_enabled = bool(self.declare_parameter("remote_model_switch_enabled", True).value) + self.model_switch_channel = int(self.declare_parameter("remote_model_switch_channel", 10).value) + self.model_switch_debounce_frames = max(int(self.declare_parameter("remote_model_switch_debounce_frames", 3).value), 1) + self.model_switch_rough_level = self.parse_switch_level( + str(self.declare_parameter("remote_model_switch_rough_level", "low").value) + ) + legacy_ik_level = str(self.declare_parameter("remote_model_switch_crawl_level", "").value).strip() + ik_level_default = legacy_ik_level if legacy_ik_level else "high" + self.model_switch_ik_level = self.parse_switch_level( + str(self.declare_parameter("remote_model_switch_ik_level", ik_level_default).value) + ) + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10) + self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) + self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10) + self.receiver: Optional[SbusUartReceiver] = None + self.estop_published = False + self.open_error_logged = False + self.remote_mode_active = self.default_mode == "REMOTE" + self.model_switch_candidate: Optional[int] = None + self.model_switch_candidate_count = 0 + self.model_switch_stable: Optional[int] = None + + self.create_subscription(String, "control/mode_state", self.on_mode_state, 10) + + if self.enabled: + self.receiver = SbusUartReceiver( + port=self.port, + baudrate=self.baudrate, + timeout=self.timeout, + axis_deadzone=self.axis_deadzone, + ) + try: + self.receiver.open() + self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud") + except Exception as exc: + self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}") + self.open_error_logged = True + else: + self.get_logger().warn("Remote UART node is disabled by parameter") + + period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02 + self.timer = self.create_timer(period, self.on_timer) + + def destroy_node(self) -> bool: + if self.receiver is not None: + self.receiver.close() + return super().destroy_node() + + def on_timer(self) -> None: + if not self.enabled or self.receiver is None: + return + + try: + if not self.receiver.serial or not self.receiver.serial.is_open: + self.receiver.open() + state = self.receiver.poll() + except Exception as exc: + if not self.open_error_logged: + self.get_logger().error(f"Remote UART poll failed: {exc}") + self.open_error_logged = True + return + + self.open_error_logged = False + + if state.estop_requested: + if not self.estop_published or not self.estop_latch: + self.estop_pub.publish(Bool(data=True)) + self.get_logger().warn("Remote E-stop requested by CH7 high") + self.estop_published = True + self.publish_zero_cmd() + return + + if not self.estop_latch and self.estop_published: + self.estop_pub.publish(Bool(data=False)) + self.estop_published = False + + self.handle_model_switch(state) + + active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4)) + if active or self.publish_inactive_zero: + cmd = Twist() + cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx) + cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy) + cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw) + self.cmd_pub.publish(cmd) + + def publish_zero_cmd(self) -> None: + self.cmd_pub.publish(Twist()) + + def on_mode_state(self, msg: String) -> None: + mode = str(msg.data).strip().upper() + remote_mode_active = mode == "REMOTE" + if remote_mode_active == self.remote_mode_active: + return + + self.remote_mode_active = remote_mode_active + self.reset_model_switch_tracking() + + def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float: + if abs(raw_value) <= self.active_threshold: + return 0.0 + scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale)) + if invert: + scaled = -scaled + return float(scaled * limit) + + @staticmethod + def parse_switch_level(value: str) -> int: + normalized = value.strip().lower() + if normalized == "low": + return SWITCH_LOW + if normalized == "high": + return SWITCH_HIGH + return SWITCH_MID + + def handle_model_switch(self, state: RemoteControlState) -> None: + if not self.model_switch_enabled or not self.remote_mode_active: + return + + switch_level = state.switches.get(self.model_switch_channel) + if switch_level is None: + return + + if switch_level == self.model_switch_candidate: + self.model_switch_candidate_count += 1 + else: + self.model_switch_candidate = switch_level + self.model_switch_candidate_count = 1 + + if self.model_switch_candidate_count < self.model_switch_debounce_frames: + return + + if switch_level == self.model_switch_stable: + return + + self.model_switch_stable = switch_level + + if switch_level == self.model_switch_rough_level: + self.model_cmd_pub.publish(String(data="rough")) + self.get_logger().info( + f"Remote model switch: CH{self.model_switch_channel} -> rough" + ) + elif switch_level == self.model_switch_ik_level: + self.model_cmd_pub.publish(String(data="ik")) + self.get_logger().info( + f"Remote model switch: CH{self.model_switch_channel} -> ik" + ) + + def reset_model_switch_tracking(self) -> None: + self.model_switch_candidate = None + self.model_switch_candidate_count = 0 + self.model_switch_stable = None + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = RemoteUartNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/simple_nav_node.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/simple_nav_node.py similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_runtime/src/simple_nav_node.py rename to 05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/simple_nav_node.py diff --git a/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/web_udp_bridge_node.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/web_udp_bridge_node.py new file mode 100644 index 0000000..f6ae53b --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/src/web_udp_bridge_node.py @@ -0,0 +1,1742 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import json +import math +import socket +import threading +from datetime import datetime +from http import HTTPStatus +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from typing import Any, Optional +from urllib.parse import urlparse + +import rclpy +import yaml +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_msgs.msg import TFMessage +from tf2_ros import Buffer, TransformBroadcaster, TransformException, TransformListener + + +class WebUdpBridgeNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True) + + self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value) + self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value) + self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value) + self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value) + self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value) + self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value) + self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value) + self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value) + self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value) + self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value) + + self.http_host = str(self.declare_parameter("web_http_host", "0.0.0.0").value) + self.http_port = int(self.declare_parameter("web_http_port", 18080).value) + self.web_static_dir = str(self.declare_parameter("web_static_dir", "").value) + raw_localization_mode = str(self.declare_parameter("localization_mode", "odom").value) + self.localization_mode = self.normalize_localization_mode(raw_localization_mode) + self.odom_fallback_allowed = self.localization_mode == "odom" + 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) + self.nav_route_file = str(self.declare_parameter("nav_route_file", "").value) + self.nav_route_task_file = str(self.declare_parameter("nav_route_task_file", "").value) + self.pcd_nav_file = str(self.declare_parameter("pcd_nav_file", "").value) + self.pcd_floor_z_min = float(self.declare_parameter("pcd_floor_z_min", -1.6).value) + self.pcd_floor_z_max = float(self.declare_parameter("pcd_floor_z_max", 0.4).value) + self.pcd_sample_step = max(1, int(self.declare_parameter("pcd_sample_step", 25).value)) + self.route_align_enabled = bool( + self.declare_parameter("nav_route_auto_align_enabled", True).value + ) + self.route_rotation_offset_deg = float( + self.declare_parameter("nav_route_rotation_offset_deg", 0.0).value + ) + self.route_align_max_angle_deg = abs( + float(self.declare_parameter("nav_route_align_max_angle_deg", 6.0).value) + ) + self.route_align_angle_step_deg = max( + 0.1, + float(self.declare_parameter("nav_route_align_angle_step_deg", 0.5).value), + ) + self.route_align_search_radius = max( + 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 + ) + default_odom_trace_dir = Path(__file__).resolve().parents[3] / "map" / "load" + self.odom_trace_export_dir = str( + self.declare_parameter("odom_trace_export_dir", str(default_odom_trace_dir)).value + ) + self.odom_trace_sample_hz = max( + 0.2, + float(self.declare_parameter("odom_trace_sample_hz", 5.0).value), + ) + self.odom_trace_min_distance = max( + 0.0, + float(self.declare_parameter("odom_trace_min_distance", 0.03).value), + ) + self.odom_trace_max_points = max( + 2, + int(self.declare_parameter("odom_trace_max_points", 20000).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.odom_trace_active = False + self.odom_trace_points: list[dict[str, Any]] = [] + self.odom_trace_started_at = 0.0 + self.odom_trace_mission_name = "" + self.odom_trace_anchor: dict[str, Any] = {} + self.odom_trace_last_pose: Optional[dict[str, Any]] = None + self.odom_trace_export_path = "" + 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) + self.sock.bind((self.listen_host, self.listen_port)) + + self.client_addr: Optional[tuple[str, int]] = None + if self.remote_host: + self.client_addr = (self.remote_host, self.remote_port) + + self.latest_target: Optional[RuntimeTarget] = None + self.latest_state: Optional[RuntimeState] = None + self.latest_model_status: dict[str, Any] = { + "current_model": "rough", + "requested_model": "rough", + "switch_state": "idle", + "backend": "unknown", + "switching": False, + } + self.latest_cmd = Twist() + self.latest_mode = "UNKNOWN" + self.latest_mux_status = "" + self.latest_nav_status = "" + self.latest_nav_path: dict[str, Any] = { + "goal_name": "", + "stage": "idle", + "path_index": 0, + "points": [], + } + self.estop = False + self.web_enabled = False + self.last_cmd_time = self.get_clock().now() + self.timeout_estop_sent = False + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10) + self.nav_cmd_pub = self.create_publisher(String, "/simple_nav/cmd", 10) + self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) + self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10) + self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10) + self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10) + self.mode_pub = self.create_publisher(String, "control/mode", 10) + self.model_cmd_pub = self.create_publisher(String, "runtime/model_cmd", 10) + self.posture_cmd_pub = self.create_publisher(String, "runtime/posture_cmd", 10) + + self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10) + self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10) + self.create_subscription(String, "runtime/model_status", self.on_model_status, 10) + self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10) + self.create_subscription(Bool, "/safety/estop", self.on_estop, 10) + self.create_subscription(String, "control/mode_state", self.on_mode_state, 10) + self.create_subscription(String, "control/mux_status", self.on_mux_status, 10) + self.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]] = [] + self.mission_specs: list[dict[str, Any]] = [] + self.default_mission_name: Optional[str] = None + self.task_specs: list[dict[str, Any]] = [] + self.route_alignment_info: dict[str, Any] = {} + self.route_avoid_regions: list[dict[str, Any]] = [] + self.reload_nav_task_config() + + self.http_server: Optional[ThreadingHTTPServer] = None + self.http_thread: Optional[threading.Thread] = None + self.static_dir = ( + Path(self.web_static_dir) + if self.web_static_dir + else Path(__file__).resolve().parents[3] / "tools" / "win_web_debug" / "static" + ) + self.start_http_server() + + self.rx_timer = self.create_timer(0.01, self.on_rx_timer) + self.state_timer = self.create_timer( + 1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, + self.on_state_timer, + ) + self.guard_timer = self.create_timer(0.05, self.on_guard_timer) + self.odom_fallback_timer = self.create_timer(0.05, self.on_odom_fallback_timer) + self.odom_trace_timer = self.create_timer( + 1.0 / self.odom_trace_sample_hz, + self.on_odom_trace_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}") + self.get_logger().info( + f"Localization mode: {self.localization_mode} " + f"(odom_fallback_allowed={self.odom_fallback_allowed})" + ) + + def load_filtered_pcd(self, path: Path) -> list[list[float]]: + if not path.exists(): + self.get_logger().warn(f"PCD file not found: {path}") + return [] + + points: list[list[float]] = [] + data_started = False + with path.open("r", encoding="utf-8") as f: + for line in f: + stripped = line.strip() + if not stripped: + continue + if data_started: + parts = stripped.split() + if len(parts) < 3: + continue + try: + x = float(parts[0]) + y = float(parts[1]) + z = float(parts[2]) + except ValueError: + continue + if self.pcd_floor_z_min <= z <= self.pcd_floor_z_max: + points.append([round(x, 3), round(y, 3)]) + elif stripped.upper().startswith("DATA"): + data_started = True + return points[:: self.pcd_sample_step] + + def _load_yaml(self, path_value: str) -> dict[str, Any]: + if not path_value: + return {} + path = Path(path_value).expanduser() + if not path.exists(): + self.get_logger().warn(f"Navigation file not found: {path}") + return {} + try: + with path.open("r", encoding="utf-8") as f: + data = yaml.safe_load(f) or {} + return data if isinstance(data, dict) else {} + except Exception as exc: + self.get_logger().warn(f"Failed to load navigation file {path}: {exc}") + return {} + + @staticmethod + def _normalize_policy(value: Any) -> Optional[str]: + text = str(value).strip().lower() + if not text: + return None + if text == "ik": + return "crawl" + if text in {"rough", "crawl", "wall"}: + return text + return None + + @staticmethod + def _normalize_task(value: Any) -> Optional[str]: + text = str(value).strip().lower() + return text or None + + @staticmethod + def normalize_localization_mode(value: Any) -> str: + text = str(value or "").strip().lower() + if text in {"relocal", "reloc", "relocalization", "localization"}: + return "relocal" + return "odom" + + @staticmethod + def _get_value(data: dict[str, Any], *keys: str) -> Any: + for key in keys: + if key in data and data.get(key) is not None: + return data.get(key) + return None + + @classmethod + def _get_float(cls, data: dict[str, Any], *keys: str) -> Optional[float]: + value = cls._get_value(data, *keys) + if value is None: + return None + try: + return float(value) + except (TypeError, ValueError): + return None + + @classmethod + def _get_bool(cls, data: dict[str, Any], *keys: str) -> Optional[bool]: + value = cls._get_value(data, *keys) + if value is None: + return None + if isinstance(value, bool): + return value + text = str(value).strip().lower() + if text in {"1", "true", "yes", "on"}: + return True + if text in {"0", "false", "no", "off"}: + return False + return None + + @classmethod + def _get_int(cls, data: dict[str, Any], *keys: str) -> Optional[int]: + value = cls._get_value(data, *keys) + if value is None: + return None + try: + return int(value) + except (TypeError, ValueError): + return None + + def load_nav_task_config( + self, + ) -> tuple[ + list[dict[str, Any]], + list[dict[str, Any]], + Optional[str], + dict[str, Any], + list[dict[str, Any]], + list[dict[str, Any]], + ]: + goals_by_name = self._load_goals(self.nav_goals_file) + missions_by_name = self._load_missions(self.nav_missions_file) + route_goals, route_missions, route_default, route_alignment, route_regions = self._load_route( + self.get_route_source_file() + ) + goals_by_name.update(route_goals) + missions_by_name.update(route_missions) + goal_specs = list(goals_by_name.values()) + mission_specs = [ + {"name": mission_name, "goals": list(goal_names)} + for mission_name, goal_names in missions_by_name.items() + ] + default_mission_name = route_default or (mission_specs[0]["name"] if mission_specs else None) + task_specs = self.build_task_specs(goal_specs, mission_specs, default_mission_name) + return goal_specs, mission_specs, default_mission_name, route_alignment, route_regions, task_specs + + def reload_nav_task_config(self) -> None: + ( + self.goal_specs, + self.mission_specs, + self.default_mission_name, + self.route_alignment_info, + self.route_avoid_regions, + self.task_specs, + ) = self.load_nav_task_config() + + def build_task_specs( + self, + goal_specs: list[dict[str, Any]], + mission_specs: list[dict[str, Any]], + default_mission_name: Optional[str], + ) -> list[dict[str, Any]]: + if not default_mission_name: + return [] + goal_lookup = {str(goal.get("name", "")): goal for goal in goal_specs if goal.get("name")} + default_mission = next( + (mission for mission in mission_specs if mission.get("name") == default_mission_name), + None, + ) + if not default_mission: + return [] + + tasks: list[dict[str, Any]] = [] + task_by_name: dict[str, dict[str, Any]] = {} + for index, goal_name in enumerate(default_mission.get("goals", []), start=1): + goal = goal_lookup.get(str(goal_name)) + if not goal: + continue + task_name = self._normalize_task(goal.get("task")) + if not task_name: + continue + spec = task_by_name.get(task_name) + if spec is None: + spec = { + "name": task_name, + "start_index": index, + "end_index": index, + "count": 0, + "first_goal": str(goal_name), + "last_goal": str(goal_name), + "first_id": goal.get("id"), + "last_id": goal.get("id"), + "goals": [], + "policies": [], + } + task_by_name[task_name] = spec + tasks.append(spec) + spec["end_index"] = index + spec["count"] = int(spec["count"]) + 1 + spec["last_goal"] = str(goal_name) + spec["last_id"] = goal.get("id") + spec["goals"].append(str(goal_name)) + policy = goal.get("policy") + if policy and policy not in spec["policies"]: + spec["policies"].append(policy) + return tasks + + def get_route_source_file(self) -> str: + candidate = self.nav_route_task_file.strip() if self.nav_route_task_file else "" + if candidate: + return candidate + return self.nav_route_file + + def _load_goals(self, path_value: str) -> dict[str, dict[str, Any]]: + data = self._load_yaml(path_value) + raw_goals = data.get("goals", {}) + parsed: dict[str, dict[str, Any]] = {} + if not isinstance(raw_goals, dict): + return parsed + for name, spec in raw_goals.items(): + if not isinstance(spec, dict): + continue + pos = spec.get("position", [0.0, 0.0, 0.0]) + if not isinstance(pos, list) or len(pos) < 2: + continue + try: + yaw_tolerance_deg = self._get_float(spec, "yaw_tolerance_deg", "yawToleranceDeg") + yaw_deg = self._get_float(spec, "yaw_deg", "yawDeg") + require_yaw = self._get_bool(spec, "require_yaw", "requireYaw") + parsed[str(name)] = { + "name": str(name), + "id": self._get_int(spec, "id", "waypoint_id", "waypointId"), + "task": self._normalize_task(spec.get("task")), + "segment": self._normalize_task(spec.get("segment")), + "x": float(pos[0]), + "y": float(pos[1]), + "yaw_deg": yaw_deg, + "yaw_tolerance_deg": yaw_tolerance_deg, + "tolerance": float(spec.get("tolerance", 0.20)), + "policy": self._normalize_policy(spec.get("policy")), + "speed": float(spec["speed"]) if spec.get("speed") is not None else None, + "require_yaw": bool(yaw_deg is not None if require_yaw is None else require_yaw) and yaw_deg is not None, + "pre_dock_distance": self._get_float(spec, "pre_dock_distance", "preDockDistance"), + "pre_dock_tolerance": self._get_float(spec, "pre_dock_tolerance", "preDockTolerance"), + } + except (TypeError, ValueError): + continue + return parsed + + def _load_missions(self, path_value: str) -> dict[str, list[str]]: + data = self._load_yaml(path_value) + raw_missions = data.get("missions", {}) + parsed: dict[str, list[str]] = {} + if not isinstance(raw_missions, dict): + return parsed + for name, spec in raw_missions.items(): + if not isinstance(spec, dict): + continue + goals = spec.get("goals", []) + if isinstance(goals, list) and goals and all(isinstance(item, str) for item in goals): + parsed[str(name)] = [str(item) for item in goals] + return parsed + + def _load_route( + self, path_value: str + ) -> tuple[dict[str, dict[str, Any]], dict[str, list[str]], Optional[str], dict[str, Any], list[dict[str, Any]]]: + data = self._load_yaml(path_value) + if not data: + return {}, {}, None, {}, [] + + route_name = str(data.get("name", "")).strip() or Path(path_value).stem + route_default_yaw_tolerance_deg = self._get_float( + data, + "yaw_tolerance_deg_default", + "yawToleranceDegDefault", + ) + route_default_require_yaw = self._get_bool( + data, + "require_yaw_default", + "requireYawDefault", + ) + route_default_pre_dock_distance = self._get_float( + data, + "pre_dock_distance_default", + "preDockDistanceDefault", + ) + route_default_pre_dock_tolerance = self._get_float( + data, + "pre_dock_tolerance_default", + "preDockToleranceDefault", + ) + raw_segments = data.get("segments", []) + if not isinstance(raw_segments, list) or not raw_segments: + top_level_waypoints = data.get("waypoints", []) + if isinstance(top_level_waypoints, list) and top_level_waypoints: + raw_segments = [{ + "name": "segment_1", + "obstacle": str(data.get("obstacle", "flat") or "flat"), + "waypoints": top_level_waypoints, + }] + else: + return {}, {}, None, {}, [] + + raw_waypoints: list[dict[str, Any]] = [] + for segment_index, segment in enumerate(raw_segments, start=1): + if not isinstance(segment, dict): + continue + segment_name = str(segment.get("name", f"segment_{segment_index}")).strip() or f"segment_{segment_index}" + waypoints = segment.get("waypoints", []) + if not isinstance(waypoints, list): + continue + for waypoint_index, waypoint in enumerate(waypoints, start=1): + if not isinstance(waypoint, dict): + continue + try: + waypoint_yaw_deg = self._get_float(waypoint, "yaw_deg", "yawDeg") + waypoint_yaw_tolerance_deg = self._get_float( + waypoint, + "yaw_tolerance_deg", + "yawToleranceDeg", + ) + waypoint_require_yaw = self._get_bool( + waypoint, + "require_yaw", + "requireYaw", + ) + if waypoint_require_yaw is None: + waypoint_require_yaw = bool(route_default_require_yaw) if route_default_require_yaw is not None else False + if waypoint_yaw_tolerance_deg is None: + waypoint_yaw_tolerance_deg = route_default_yaw_tolerance_deg + waypoint_pre_dock_distance = self._get_float( + waypoint, + "pre_dock_distance", + "preDockDistance", + ) + if waypoint_pre_dock_distance is None: + waypoint_pre_dock_distance = route_default_pre_dock_distance + waypoint_pre_dock_tolerance = self._get_float( + waypoint, + "pre_dock_tolerance", + "preDockTolerance", + ) + if waypoint_pre_dock_tolerance is None: + waypoint_pre_dock_tolerance = route_default_pre_dock_tolerance + raw_waypoints.append( + { + "segment": segment_name, + "id": int(waypoint.get("id", waypoint_index)), + "task": self._normalize_task(waypoint.get("task", segment.get("obstacle", ""))), + "x": float(waypoint["x"]), + "y": float(waypoint["y"]), + "yaw_deg": waypoint_yaw_deg, + "yaw_tolerance_deg": waypoint_yaw_tolerance_deg, + "tolerance": float(waypoint.get("tolerance", 0.20)), + "policy": self._normalize_policy(waypoint.get("policy")), + "speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None, + "require_yaw": bool(waypoint_require_yaw) and waypoint_yaw_deg is not None, + "pre_dock_distance": waypoint_pre_dock_distance, + "pre_dock_tolerance": waypoint_pre_dock_tolerance, + } + ) + except (KeyError, TypeError, ValueError): + continue + + if not raw_waypoints: + return {}, {}, None, {}, [] + + aligned_waypoints, alignment_info = self._align_route_waypoints(raw_waypoints) + applied_angle_rad = math.radians(float(alignment_info.get("applied_deg", 0.0))) if alignment_info else 0.0 + aligned_regions = self._rotate_avoid_regions( + self._load_avoid_regions(data), + float(raw_waypoints[0]["x"]), + float(raw_waypoints[0]["y"]), + applied_angle_rad, + ) + goals: dict[str, dict[str, Any]] = {} + mission_goal_names: list[str] = [] + for index, waypoint in enumerate(aligned_waypoints, start=1): + goal_name = f"{route_name}_p{index:02d}" + goals[goal_name] = { + "name": goal_name, + "id": int(waypoint["id"]) if waypoint.get("id") is not None else None, + "task": self._normalize_task(waypoint.get("task")), + "segment": str(waypoint.get("segment", "") or "") or None, + "x": round(float(waypoint["x"]), 3), + "y": round(float(waypoint["y"]), 3), + "yaw_deg": round(float(waypoint["yaw_deg"]), 3) if waypoint.get("yaw_deg") is not None else None, + "yaw_tolerance_deg": round(float(waypoint["yaw_tolerance_deg"]), 3) + if waypoint.get("yaw_tolerance_deg") is not None else None, + "tolerance": round(float(waypoint.get("tolerance", 0.20)), 3), + "policy": self._normalize_policy(waypoint.get("policy")), + "speed": float(waypoint["speed"]) if waypoint.get("speed") is not None else None, + "require_yaw": bool(waypoint.get("require_yaw", False)), + "pre_dock_distance": round(float(waypoint["pre_dock_distance"]), 3) + if waypoint.get("pre_dock_distance") is not None else None, + "pre_dock_tolerance": round(float(waypoint["pre_dock_tolerance"]), 3) + if waypoint.get("pre_dock_tolerance") is not None else None, + } + mission_goal_names.append(goal_name) + + return goals, {route_name: mission_goal_names}, route_name, alignment_info, aligned_regions + + def _load_avoid_regions(self, data: dict[str, Any]) -> list[dict[str, Any]]: + rows = data.get("regions") + if not isinstance(rows, list) or not rows: + rows = data.get("avoid_regions", []) + if not isinstance(rows, list): + return [] + + regions: list[dict[str, Any]] = [] + for index, row in enumerate(rows, start=1): + if not isinstance(row, dict): + continue + kind = str(row.get("kind", "avoid")).strip().lower() + if kind not in {"avoid", "no_go", "no-go", "nogo", "forbidden", "blocked"}: + continue + polygon_rows = row.get("polygon", []) + if not isinstance(polygon_rows, list): + continue + polygon: list[dict[str, float]] = [] + for point in polygon_rows: + try: + if isinstance(point, dict): + polygon.append({"x": float(point["x"]), "y": float(point["y"])}) + elif isinstance(point, (list, tuple)) and len(point) >= 2: + polygon.append({"x": float(point[0]), "y": float(point[1])}) + except (KeyError, TypeError, ValueError): + continue + if len(polygon) >= 3: + regions.append( + { + "name": str(row.get("name", f"avoid_{index}")), + "kind": kind, + "polygon": polygon, + } + ) + return regions + + def _rotate_avoid_regions( + self, + regions: list[dict[str, Any]], + anchor_x: float, + anchor_y: float, + angle_rad: float, + ) -> list[dict[str, Any]]: + if not regions or abs(angle_rad) <= 1.0e-12: + return regions + rotated: list[dict[str, Any]] = [] + for region in regions: + polygon = [] + for point in region.get("polygon", []): + x, y = self._rotate_xy(float(point["x"]), float(point["y"]), anchor_x, anchor_y, angle_rad) + polygon.append({"x": x, "y": y}) + rotated.append({**region, "polygon": polygon}) + return rotated + + def _align_route_waypoints( + self, waypoints: list[dict[str, Any]] + ) -> tuple[list[dict[str, Any]], dict[str, Any]]: + if not waypoints: + return [], {} + + if not self.map_points: + info = { + "enabled": False, + "reason": "pcd unavailable", + "manual_offset_deg": round(self.route_rotation_offset_deg, 3), + "auto_offset_deg": 0.0, + "applied_deg": round(self.route_rotation_offset_deg, 3), + "hits": 0, + "total": len(waypoints), + } + return self._rotate_waypoints(waypoints, math.radians(self.route_rotation_offset_deg)), info + + manual_offset_deg = self.route_rotation_offset_deg + if not self.route_align_enabled or len(waypoints) < 2: + info = { + "enabled": False, + "reason": "auto align disabled", + "manual_offset_deg": round(manual_offset_deg, 3), + "auto_offset_deg": 0.0, + "applied_deg": round(manual_offset_deg, 3), + "hits": 0, + "total": len(waypoints), + } + return self._rotate_waypoints(waypoints, math.radians(manual_offset_deg)), info + + anchor_x = float(waypoints[0]["x"]) + anchor_y = float(waypoints[0]["y"]) + search_radius_sq = self.route_align_search_radius * self.route_align_search_radius + best_hits = -1 + best_score = float("inf") + best_angle_deg = manual_offset_deg + manual_hits = -1 + manual_score = float("inf") + steps = max( + 1, + int(round((self.route_align_max_angle_deg * 2.0) / self.route_align_angle_step_deg)), + ) + + for step_index in range(steps + 1): + auto_delta_deg = -self.route_align_max_angle_deg + step_index * self.route_align_angle_step_deg + angle_deg = manual_offset_deg + auto_delta_deg + angle_rad = math.radians(angle_deg) + hits = 0 + score = 0.0 + for waypoint in waypoints: + tx, ty = self._rotate_xy( + float(waypoint["x"]), + float(waypoint["y"]), + anchor_x, + anchor_y, + angle_rad, + ) + nearest_sq = search_radius_sq + for px, py in self.map_points: + dx = px - tx + dy = py - ty + dist_sq = dx * dx + dy * dy + if dist_sq < nearest_sq: + nearest_sq = dist_sq + if nearest_sq < search_radius_sq: + hits += 1 + score += nearest_sq + if abs(angle_deg - manual_offset_deg) <= 1.0e-9: + manual_hits = hits + manual_score = score + better_hits = hits > best_hits + better_score = hits == best_hits and ( + score < best_score - 1.0e-9 + or ( + abs(score - best_score) <= 1.0e-9 + and abs(auto_delta_deg) < abs(best_angle_deg - manual_offset_deg) + ) + ) + if better_hits or better_score: + best_hits = hits + best_score = score + best_angle_deg = angle_deg + + hits_improved = best_hits > manual_hits + score_improvement = ( + (manual_score - best_score) / max(manual_score, 1.0e-9) + if manual_score < float("inf") + else 0.0 + ) + if not hits_improved and score_improvement < 0.05: + applied_angle_deg = manual_offset_deg + rotated = self._rotate_waypoints(waypoints, math.radians(applied_angle_deg)) + info = { + "enabled": True, + "reason": "ambiguous-auto-align", + "manual_offset_deg": round(manual_offset_deg, 3), + "auto_offset_deg": 0.0, + "applied_deg": round(applied_angle_deg, 3), + "score": round(manual_score, 6) if manual_score < float("inf") else 0.0, + "hits": int(manual_hits if manual_hits >= 0 else 0), + "total": len(waypoints), + } + return rotated, info + + rotated = self._rotate_waypoints(waypoints, math.radians(best_angle_deg)) + info = { + "enabled": True, + "manual_offset_deg": round(manual_offset_deg, 3), + "auto_offset_deg": round(best_angle_deg - manual_offset_deg, 3), + "applied_deg": round(best_angle_deg, 3), + "score": round(best_score, 6), + "hits": int(best_hits), + "total": len(waypoints), + } + return rotated, info + + def _rotate_waypoints(self, waypoints: list[dict[str, Any]], angle_rad: float) -> list[dict[str, Any]]: + if not waypoints: + return [] + anchor_x = float(waypoints[0]["x"]) + anchor_y = float(waypoints[0]["y"]) + rotated: list[dict[str, Any]] = [] + for waypoint in waypoints: + x, y = self._rotate_xy( + float(waypoint["x"]), + float(waypoint["y"]), + anchor_x, + anchor_y, + angle_rad, + ) + updated = dict(waypoint) + updated["x"] = x + updated["y"] = y + if updated.get("yaw_deg") is not None: + updated["yaw_deg"] = round( + math.degrees( + self.normalize_angle(math.radians(float(updated["yaw_deg"])) + angle_rad) + ), + 3, + ) + rotated.append(updated) + return rotated + + @staticmethod + def _rotate_xy(x: float, y: float, anchor_x: float, anchor_y: float, angle_rad: float) -> tuple[float, float]: + dx = x - anchor_x + dy = y - anchor_y + cos_a = math.cos(angle_rad) + sin_a = math.sin(angle_rad) + return ( + anchor_x + dx * cos_a - dy * sin_a, + anchor_y + dx * sin_a + dy * cos_a, + ) + + def lookup_pose(self) -> Optional[dict[str, float]]: + try: + transform = self.tf_buffer.lookup_transform( + self.nav_map_frame, + self.nav_base_frame, + rclpy.time.Time(), + ) + except TransformException: + return None + t = transform.transform.translation + q = transform.transform.rotation + yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w) + return {"x": round(float(t.x), 3), "y": round(float(t.y), 3), "yaw": round(float(yaw), 6)} + + def start_http_server(self) -> None: + node = self + static_dir = self.static_dir + + class Handler(BaseHTTPRequestHandler): + def do_GET(self): + request_path = urlparse(self.path).path + if request_path in ["/", "/index.html"]: + self.serve_file(static_dir / "index.html", "text/html; charset=utf-8") + elif request_path == "/app.js": + self.serve_file(static_dir / "app.js", "application/javascript; charset=utf-8") + elif request_path == "/style.css": + self.serve_file(static_dir / "style.css", "text/css; charset=utf-8") + elif request_path == "/api/state": + self.send_json(node.build_state_packet()) + elif request_path == "/api/map": + self.send_json(node.build_map_packet()) + else: + self.send_error(HTTPStatus.NOT_FOUND) + + def do_POST(self): + if self.path != "/api/control": + self.send_error(HTTPStatus.NOT_FOUND) + return + try: + length = int(self.headers.get("Content-Length", "0")) + raw = self.rfile.read(length) + payload = json.loads(raw.decode("utf-8")) + node.handle_http_control(payload) + self.send_json({"ok": True}) + except Exception as exc: + self.send_json( + {"ok": False, "error": str(exc)}, + status=HTTPStatus.BAD_REQUEST, + ) + + def serve_file(self, path: Path, content_type: str): + if not path.exists(): + self.send_error(HTTPStatus.NOT_FOUND) + return + data = path.read_bytes() + self.send_response(HTTPStatus.OK) + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def send_json(self, payload: dict[str, Any], status: HTTPStatus = HTTPStatus.OK): + data = json.dumps(payload, separators=(",", ":")).encode("utf-8") + self.send_response(status) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def log_message(self, format: str, *args): + return + + self.http_server = ThreadingHTTPServer((self.http_host, self.http_port), Handler) + self.http_thread = threading.Thread(target=self.http_server.serve_forever, daemon=True) + self.http_thread.start() + + def build_odom_trace_packet(self) -> dict[str, Any]: + points = [[self._f(point["x"]), self._f(point["y"])] for point in self.odom_trace_points] + max_display_points = 240 + if len(points) > max_display_points: + stride = max(1, math.ceil(len(points) / max_display_points)) + points = points[::stride] + return { + "active": self.odom_trace_active, + "count": len(self.odom_trace_points), + "display_count": len(points), + "points": points, + "mission": self.odom_trace_mission_name, + "export_path": self.odom_trace_export_path, + "sample_hz": self.odom_trace_sample_hz, + "min_distance": self.odom_trace_min_distance, + } + def build_map_packet(self) -> dict[str, Any]: + 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, + "mission_specs": self.mission_specs, + "default_mission_name": self.default_mission_name, + "default_mission_goals": default_goals, + "task_specs": self.task_specs, + "route_alignment": self.route_alignment_info, + "avoid_regions": self.route_avoid_regions, + "localization_mode": self.localization_mode, + "odom_fallback_allowed": self.odom_fallback_allowed, + "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), + "odom_trace": self.build_odom_trace_packet(), + } + + def handle_http_control(self, payload: dict[str, Any]) -> None: + msg_type = str(payload.get("type", "")).lower() + if msg_type in { + "cmd_vel", + "zero", + "estop", + "mode", + "web_enable", + "remote_enable", + "nav_enable", + "ping", + "nav_cmd", + "nav_stop_keep", + "task_resume", + "task_only", + "task_skip", + "go_to", + "go_rel", + "odom_task", + "odom_stop", + "estop_reset", + "model_toggle", + "model_cmd", + }: + self.handle_packet(payload) + return + raise ValueError(f"unknown control type: {msg_type}") + + def on_target(self, msg: RuntimeTarget) -> None: + self.latest_target = msg + + def on_state(self, msg: RuntimeState) -> None: + self.latest_state = msg + + def on_model_status(self, msg: String) -> None: + try: + data = json.loads(msg.data) if msg.data else {} + if isinstance(data, dict): + self.latest_model_status = { + "current_model": str(data.get("current_model", "rough")), + "requested_model": str( + data.get("requested_model", data.get("current_model", "rough")) + ), + "switch_state": str(data.get("switch_state", "idle")), + "backend": str(data.get("backend", "unknown")), + "switching": bool(data.get("switching", False)), + } + except Exception as exc: + self.get_logger().warn(f"Failed to parse model status: {exc}") + + def on_cmd_vel(self, msg: Twist) -> None: + self.latest_cmd = msg + + def on_estop(self, msg: Bool) -> None: + was_estop = self.estop + self.estop = bool(msg.data) + if self.estop and not was_estop: + self.stop_navigation_for_keep("estop requested", enter_keep=False) + + def on_mode_state(self, msg: String) -> None: + self.latest_mode = msg.data + + def on_mux_status(self, msg: String) -> None: + self.latest_mux_status = msg.data + + def on_nav_status(self, msg: String) -> None: + self.latest_nav_status = msg.data + if msg.data.startswith("reloaded goals="): + self.reload_nav_task_config() + + 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), + "received_at": round(self.now_sec(), 3), + } + 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: + data = json.loads(msg.data) if msg.data else {} + except Exception: + return + if not isinstance(data, dict): + return + raw_points = data.get("points", []) + points: list[list[float]] = [] + if isinstance(raw_points, list): + for item in raw_points: + if ( + isinstance(item, (list, tuple)) + and len(item) >= 2 + ): + try: + points.append([round(float(item[0]), 3), round(float(item[1]), 3)]) + except (TypeError, ValueError): + continue + try: + path_index = max(0, int(data.get("path_index", 0) or 0)) + except (TypeError, ValueError): + path_index = 0 + self.latest_nav_path = { + "goal_name": str(data.get("goal_name", "")), + "stage": str(data.get("stage", "idle")), + "path_index": path_index, + "points": points, + } + + def on_rx_timer(self) -> None: + if not self.enabled: + return + while True: + try: + data, addr = self.sock.recvfrom(self.max_packet_bytes) + except BlockingIOError: + break + except OSError as exc: + self.get_logger().warn(f"UDP receive failed: {exc}") + break + self.client_addr = addr + try: + payload = json.loads(data.decode("utf-8")) + self.handle_packet(payload) + except Exception as exc: + self.send_packet({"type": "error", "message": str(exc)}) + + def handle_packet(self, payload: dict[str, Any]) -> None: + msg_type = str(payload.get("type", "")).lower() + if msg_type == "cmd_vel": + cmd = self.parse_twist(payload) + self.set_control_mode("WEB") + self.cmd_pub.publish(cmd) + self.last_cmd_time = self.get_clock().now() + self.timeout_estop_sent = False + elif msg_type == "zero": + self.cmd_pub.publish(Twist()) + self.last_cmd_time = self.get_clock().now() + elif msg_type == "estop": + self.handle_estop_request(bool(payload.get("data", True))) + elif msg_type == "mode": + mode = str(payload.get("mode", "DISABLED")).upper() + self.set_control_mode(mode) + elif msg_type == "web_enable": + self.web_enabled = bool(payload.get("data", False)) + self.web_enabled_pub.publish(Bool(data=self.web_enabled)) + if self.web_enabled: + self.set_control_mode("WEB") + elif msg_type == "remote_enable": + enabled = bool(payload.get("data", False)) + self.remote_enabled_pub.publish(Bool(data=enabled)) + if enabled: + self.set_control_mode("REMOTE") + elif msg_type == "nav_enable": + enabled = bool(payload.get("data", False)) + self.nav_enabled_pub.publish(Bool(data=enabled)) + if enabled: + self.set_control_mode("NAV") + elif msg_type == "nav_cmd": + command = str(payload.get("command", "")).strip() + if command: + self.set_control_mode("NAV") + self.nav_cmd_pub.publish(String(data=command)) + elif msg_type == "nav_stop_keep": + self.stop_navigation_for_keep("manual stop") + elif msg_type == "task_resume": + task_name = str(payload.get("task", "")).strip() + if task_name: + self.ensure_navigation_pose(f"task resume {task_name}") + self.set_control_mode("NAV") + self.nav_cmd_pub.publish(String(data="reload")) + self.nav_cmd_pub.publish(String(data=f"run_from_task {task_name}")) + elif msg_type == "task_only": + task_name = str(payload.get("task", "")).strip() + if task_name: + self.ensure_navigation_pose(f"task only {task_name}") + self.set_control_mode("NAV") + self.nav_cmd_pub.publish(String(data="reload")) + self.nav_cmd_pub.publish(String(data=f"run_only_task {task_name}")) + elif msg_type == "task_skip": + task_name = str(payload.get("task", "")).strip() + self.ensure_navigation_pose(f"task skip {task_name or 'current'}") + self.set_control_mode("NAV") + command = f"skip_task {task_name}" if task_name else "skip_task" + self.nav_cmd_pub.publish(String(data="reload")) + self.nav_cmd_pub.publish(String(data=command)) + elif msg_type == "go_to": + x = float(payload.get("x", 0.0)) + y = float(payload.get("y", 0.0)) + self.set_control_mode("NAV") + self.nav_cmd_pub.publish(String(data=f"go {x:.3f} {y:.3f}")) + elif msg_type == "go_rel": + dx = float(payload.get("dx", 0.0)) + 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 == "estop_reset": + self.handle_estop_request(False) + elif msg_type == "model_toggle": + self.model_cmd_pub.publish(String(data="toggle")) + elif msg_type == "model_cmd": + command = str(payload.get("command", "")).strip() + if command: + self.model_cmd_pub.publish(String(data=command)) + elif msg_type == "ping": + self.send_packet({"type": "pong", "stamp": self.now_sec()}) + elif msg_type == "map_request": + packet = self.build_map_packet() + packet["type"] = "map" + self.send_packet(packet) + else: + self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"}) + + def set_control_mode(self, mode: str) -> None: + mode = str(mode).upper() + self.mode_pub.publish(String(data=mode)) + self.web_enabled = mode == "WEB" + self.web_enabled_pub.publish(Bool(data=mode == "WEB")) + self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE")) + self.nav_enabled_pub.publish(Bool(data=mode == "NAV")) + if mode == "KEEP": + self.posture_cmd_pub.publish(String(data="keep")) + elif mode in {"REMOTE", "NAV", "WEB"}: + self.posture_cmd_pub.publish(String(data="default")) + + def stop_navigation_for_keep(self, reason: str, enter_keep: bool = True) -> None: + self.nav_cmd_pub.publish(String(data="stop")) + if enter_keep: + self.set_control_mode("KEEP") + + def handle_estop_request(self, active: bool) -> None: + if active: + self.stop_navigation_for_keep("estop requested", enter_keep=False) + self.estop_pub.publish(Bool(data=True)) + else: + self.estop_pub.publish(Bool(data=False)) + self.stop_navigation_for_keep("estop reset") + + 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 get_current_task_name(self) -> str: + status = self.latest_nav_status or "" + marker = "task=" + marker_index = status.find(marker) + if marker_index >= 0: + tail = status[marker_index + len(marker):].strip() + task = tail.split()[0].strip() if tail else "" + if task: + return task + active_goal_name = str(self.latest_nav_path.get("goal_name", "") or "") + if active_goal_name: + for goal in self.goal_specs: + if goal.get("name") == active_goal_name: + return str(goal.get("task") or "") + return "" + + def ensure_navigation_pose(self, reason: str) -> None: + if self.lookup_pose() is not None: + return + if self.odom_fallback_active: + return + if not self.odom_fallback_allowed: + raise ValueError( + f"{reason} waiting for relocalization TF {self.nav_map_frame}->{self.nav_base_frame}; " + "move the robot until reloc shows on/tf ok" + ) + + self.reload_nav_task_config() + default_goals = self.get_default_mission_goals() + if not self.default_mission_name or not default_goals: + raise ValueError(f"{reason} failed: 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")), + ) + if not self.odom_trace_active: + self.start_odom_trace(self.default_mission_name or "odom_task", anchor_goal) + self.get_logger().info(f"{reason}: started odom fallback anchor before navigation command") + + def start_odom_fallback_task(self) -> None: + if not self.odom_fallback_allowed: + raise ValueError("odom task unavailable in relocalization mode; wait for reloc on/tf ok then use resume") + self.reload_nav_task_config() + self.ensure_navigation_pose("odom task") + if not self.default_mission_name: + raise ValueError("odom task unavailable: no default mission loaded") + 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 start_odom_trace(self, mission_name: str, anchor_goal: dict[str, Any]) -> None: + self.odom_trace_active = True + self.odom_trace_points = [] + self.odom_trace_started_at = self.now_sec() + self.odom_trace_mission_name = mission_name + self.odom_trace_last_pose = None + self.odom_trace_export_path = "" + self.odom_trace_anchor = { + "name": str(anchor_goal.get("name", "route_p01")), + "x": self._f(anchor_goal.get("x", 0.0)), + "y": self._f(anchor_goal.get("y", 0.0)), + "yaw_deg": self._f(anchor_goal.get("yaw_deg", anchor_goal.get("yawDeg", 0.0))), + } + self.record_odom_trace_sample(force=True) + self.get_logger().info( + f"Odom trace recording started: mission={self.odom_trace_mission_name}, " + f"export_dir={self.odom_trace_export_dir}" + ) + + def on_odom_trace_timer(self) -> None: + if self.odom_trace_active: + self.record_odom_trace_sample(force=False) + + def record_odom_trace_sample(self, force: bool = False) -> None: + if not self.odom_trace_active and not force: + return + if len(self.odom_trace_points) >= self.odom_trace_max_points: + return + pose = self.lookup_pose() + if pose is None: + return + now_sec = self.now_sec() + x = float(pose.get("x", 0.0)) + y = float(pose.get("y", 0.0)) + yaw = float(pose.get("yaw", 0.0)) + if self.odom_trace_last_pose is not None and not force: + dx = x - float(self.odom_trace_last_pose.get("x", 0.0)) + dy = y - float(self.odom_trace_last_pose.get("y", 0.0)) + if math.hypot(dx, dy) < self.odom_trace_min_distance: + return + sample = { + "t": round(now_sec - self.odom_trace_started_at, 3), + "x": round(x, 4), + "y": round(y, 4), + "yaw": round(yaw, 6), + "yawDeg": round(math.degrees(yaw), 3), + } + self.odom_trace_points.append(sample) + self.odom_trace_last_pose = sample + + def finish_odom_trace(self, reason: str) -> Optional[Path]: + if not self.odom_trace_active and not self.odom_trace_points: + return None + if self.odom_trace_active: + self.record_odom_trace_sample(force=True) + self.odom_trace_active = False + if not self.odom_trace_points: + return None + + try: + export_dir = Path(self.odom_trace_export_dir).expanduser() + export_dir.mkdir(parents=True, exist_ok=True) + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + safe_mission = "".join( + ch if ch.isalnum() or ch in {"_", "-"} else "_" + for ch in (self.odom_trace_mission_name or "odom") + ).strip("_") or "odom" + export_path = export_dir / f"odom_trace_{safe_mission}_{stamp}.json" + waypoints = [] + for index, point in enumerate(self.odom_trace_points, start=1): + waypoints.append({ + "id": index, + "x": point["x"], + "y": point["y"], + "world_x": point["x"], + "world_y": point["y"], + "yawDeg": point["yawDeg"], + "speed": 0.0, + "policy": "odom_trace", + "tolerance": 0.0, + }) + payload = { + "name": f"odom_trace_{safe_mission}_{stamp}", + "type": "odom_trace", + "created_at": datetime.now().isoformat(timespec="seconds"), + "frame": self.nav_map_frame, + "base_frame": self.nav_base_frame, + "odom_frame": self.nav_odom_frame, + "mission": self.odom_trace_mission_name, + "reason": reason, + "source_route_file": self.get_route_source_file(), + "anchor": dict(self.odom_trace_anchor), + "origin": {"mode": "world", "x": 0.0, "y": 0.0, "yaw_deg": 0.0}, + "sample_hz": self.odom_trace_sample_hz, + "min_distance": self.odom_trace_min_distance, + "samples": self.odom_trace_points, + "waypoints": waypoints, + "segments": [{"name": "odom_trace", "obstacle": "odom_trace", "waypoints": waypoints}], + } + export_path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8") + except Exception as exc: + self.get_logger().error(f"Odom trace export failed: {exc}") + self.odom_trace_export_path = "" + return None + self.odom_trace_export_path = str(export_path) + self.get_logger().info( + f"Odom trace exported: {export_path} points={len(self.odom_trace_points)} reason={reason}" + ) + return export_path + + def validate_odom_fallback_preconditions(self) -> None: + if not self.odom_fallback_allowed: + raise ValueError("odom fallback disabled in relocalization mode") + 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 + if was_active: + self.finish_odom_trace(reason) + 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 {} + angular = payload.get("angular", {}) or {} + cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx) + cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy) + cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw) + return cmd + + def on_guard_timer(self) -> None: + age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6 + if age_ms > self.cmd_timeout_ms: + self.cmd_pub.publish(Twist()) + if self.estop_on_timeout and not self.timeout_estop_sent: + self.estop_pub.publish(Bool(data=True)) + self.timeout_estop_sent = True + + def on_state_timer(self) -> None: + if not self.enabled: + return + self.send_packet(self.build_state_packet()) + + def build_state_packet(self) -> dict[str, Any]: + target = self.latest_target + state = self.latest_state + packet: dict[str, Any] = { + "type": "state", + "stamp": self.now_sec(), + "mode": self.latest_mode, + "mux_status": self.latest_mux_status, + "nav_status": self.latest_nav_status, + "estop": self.estop, + "web_enabled": self.web_enabled, + "connected": True, + "local_receive_time": self.now_sec(), + "cmd_vel": self.twist_to_dict(self.latest_cmd), + "runtime": {}, + "robot": {}, + "nav": { + "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), + "current_task": self.get_current_task_name(), + "task_specs": self.task_specs, + "localization_mode": self.localization_mode, + "odom_fallback_allowed": self.odom_fallback_allowed, + "odom_trace": self.build_odom_trace_packet(), + "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, + }, + "relocalization": self.build_relocalization_packet(), + }, + "model": dict(self.latest_model_status), + } + if target is not None: + packet["runtime"] = { + "target_source": str(target.target_source), + "zero_command": bool(target.zero_command), + "runtime_released": bool(target.runtime_released), + "release_alpha": self._f(target.release_alpha), + "command": [self._f(v) for v in target.command], + "raw_command": [self._f(v) for v in target.raw_command], + } + if state is not None: + packet["robot"] = { + "joint_pos": [self._f(v) for v in state.joint_pos], + "joint_vel": [self._f(v) for v in state.joint_vel], + "joint_torque": [self._f(v) for v in state.joint_torque], + "imu_gyro": [self._f(v) for v in state.imu_gyro], + "imu_accel": [self._f(v) for v in state.imu_accel], + "projected_gravity": [self._f(v) for v in state.projected_gravity], + "quat_wxyz": [self._f(v) for v in state.quat_wxyz], + "imu_age_ms": self._f(state.imu_age_ms), + "imu_fresh": bool(state.imu_fresh), + "odom_age_ms": self._f(state.odom_age_ms), + "odom_fresh": bool(state.odom_fresh), + "odom_local_pos": [self._f(v) for v in state.odom_local_pos], + "odom_local_yaw": self._f(state.odom_local_yaw), + "fresh_count": int(state.fresh_count), + "holdover_count": int(state.holdover_count), + "stale_max": int(state.stale_max), + "update_counts": [int(v) for v in state.update_counts], + } + return packet + + def build_relocalization_packet(self) -> dict[str, Any]: + external_tf = dict(self.last_external_map_odom_tf or {}) + received_at = self._f(external_tf.get("received_at")) if external_tf else 0.0 + age_s = self.now_sec() - received_at if received_at > 0.0 else 0.0 + return { + "localization_mode": self.localization_mode, + "odom_fallback_allowed": self.odom_fallback_allowed, + "external_map_odom_tf": external_tf, + "external_tf_seen": bool(external_tf), + "external_tf_age_s": self._f(age_s), + "handoff_pending": self.odom_fallback_handoff_pending, + "handoff": dict(self.odom_fallback_handoff_info), + } + + def send_packet(self, payload: dict[str, Any]) -> None: + if self.client_addr is None: + return + try: + data = json.dumps(payload, separators=(",", ":")).encode("utf-8") + self.sock.sendto(data, self.client_addr) + except OSError as exc: + self.get_logger().warn(f"UDP send failed: {exc}") + + @staticmethod + def _f(v: Any) -> float: + try: + f = float(v) + if f != f: + return 0.0 + return round(f, 6) + except (TypeError, ValueError): + return 0.0 + + def now_sec(self) -> float: + return self.get_clock().now().nanoseconds / 1.0e9 + + @staticmethod + def 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 { + "linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z}, + "angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z}, + } + + @staticmethod + def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float: + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + return math.atan2(siny_cosp, cosy_cosp) + + @staticmethod + def normalize_angle(angle: float) -> float: + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + @staticmethod + def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = WebUdpBridgeNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + if node.http_server is not None: + node.http_server.shutdown() + node.http_server.server_close() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/test/test_deadzone_velocity_limiter.py b/05_software/real/sim2real_ros2_v3/src/sim2real_runtime/test/test_deadzone_velocity_limiter.py similarity index 100% rename from 05_software/real/sim2real_ros2/src/sim2real_runtime/test/test_deadzone_velocity_limiter.py rename to 05_software/real/sim2real_ros2_v3/src/sim2real_runtime/test/test_deadzone_velocity_limiter.py diff --git a/05_software/real/sim2real_ros2_v3/start_sim2real.sh b/05_software/real/sim2real_ros2_v3/start_sim2real.sh new file mode 100644 index 0000000..3b90321 --- /dev/null +++ b/05_software/real/sim2real_ros2_v3/start_sim2real.sh @@ -0,0 +1,75 @@ +#!/bin/bash + +# Exit immediately if a command exits with a non-zero status +set -e + +# Define color codes for pretty output +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +RED='\033[0;31m' +NC='\033[0m' # No Color + +echo -e "${YELLOW}====================================================${NC}" +echo -e "${GREEN} Starting Sim2Real Locomotion ROS2 Stack ${NC}" +echo -e "${YELLOW}====================================================${NC}" + +# 1. Source ROS2 Humble environment +if [ -f "/opt/ros/humble/setup.bash" ]; then + echo -e "[System] Sourcing ROS2 Humble..." + source /opt/ros/humble/setup.bash +else + echo -e "${RED}[Error] ROS2 Humble not found. Please install ROS2 Humble first.${NC}" + exit 1 +fi + +# 2. Check if local workspace is compiled and source it +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +cd "$SCRIPT_DIR" + +if [ -f "install/setup.bash" ]; then + echo -e "[Workspace] Sourcing local workspace..." + source install/setup.bash +elif [ -f "../../install/setup.bash" ]; then + echo -e "[Workspace] Sourcing parent install/setup.bash..." + source ../../install/setup.bash +else + echo -e "${YELLOW}[Warning] install/setup.bash not found. Attempting to build the workspace first...${NC}" + if command -v colcon &> /dev/null; then + echo -e "[Build] Running colcon build..." + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release + source install/setup.bash + else + echo -e "${RED}[Error] 'colcon' tool not found. Please compile the workspace manually before running.${NC}" + exit 1 + fi +fi + +# 3. Check for SocketCAN interfaces (in non-dry-run mode) +# Reading dry_run parameter from yaml config +if [ -f "src/sim2real_bringup/config/runtime.yaml" ]; then + # Use sed for portability (busybox-compatible, avoids GNU grep -oP dependency) + DRY_RUN=$(sed -n 's/^[[:space:]]*dry_run:[[:space:]]*//p' src/sim2real_bringup/config/runtime.yaml | head -n 1 || echo "true") + # Trim trailing whitespace/newlines + DRY_RUN=$(echo "$DRY_RUN" | tr -d '[:space:]') +else + DRY_RUN="true" +fi + +if [ "$DRY_RUN" = "false" ]; then + echo -e "[Network] Checking CAN interfaces..." + if ip link show can0 &> /dev/null && ip link show can1 &> /dev/null; then + echo -e "[Network] can0 and can1 interfaces detected." + else + echo -e "${YELLOW}[Warning] CAN interfaces (can0/can1) not fully active.${NC}" + echo -e "To configure CAN interfaces, run:" + echo -e " sudo ip link set can0 up type can bitrate 1000000" + echo -e " sudo ip link set can1 up type can bitrate 1000000" + fi +else + echo -e "${YELLOW}[Dry-Run] Running in Dry-Run mode. SocketCAN will not be accessed.${NC}" +fi + +# 4. Run the ROS2 Launch file +echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}" +ros2 launch sim2real_bringup sim2real_system.launch.py "$@" + diff --git a/05_software/train/rc_mjlab/README.md b/05_software/train/rc_mjlab/README.md index bcc6a8b..dae24c0 100644 --- a/05_software/train/rc_mjlab/README.md +++ b/05_software/train/rc_mjlab/README.md @@ -1,234 +1,153 @@ # rc_mjlab -基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。 +基于 [mjlab](https://github.com/mujocolab/mjlab) 的 16DOF 串联轮足机器人强化学习、MuJoCo 验证与策略部署工程。 -> 当前目录对应 `v0.8.1`：保留比赛训练架构、后期 MuJoCo/Sim2Sim 和比赛最终 Rough 策略，并补充导航地图、打点工具、路线迭代与抽样 PCD。`model_rough.pt` 仍作为早期参考权重保留。 +## 版本定位 ---- +当前目录是多个里程碑累积后的工作树，不应整体写成“对应 `v0.8.1`”： -## 🛠️ 项目简介 +| Tag | 本目录中的主要变化 | +| --- | --- | +| `v0.4.0` | 第一份新 MJCF 与新 mjlab 训练基线（`uni_mjlab(1)`） | +| `v0.5.0` | 随机化增强训练版本（`uni_mjlab_new`） | +| `v0.6.0` | `best` 比赛训练架构：分轴速度奖励、自适应指令课程和障碍释放课程 | +| `v0.7.0` | 补充后期 `mujoco_sim` 姿态、IK、动力学和 MPC 工具 | +| `v0.8.0` | 补充后期 Sim2Sim、路线检查和比赛 Rough 策略 `model_6800.onnx` | +| `v0.8.1` | 补充导航打点、路线迭代与抽样 PCD 工具 | -本项目针对一台 **4 腿 × 3 关节 + 4 驱动轮（轮腿混合）** 的移动机器人，在 MuJoCo 物理引擎中利用 PPO 算法进行多任务运动控制策略训练。 +后续 ROS 2 真机版本没有把本目录重新定义为新的训练版本。要查看某个阶段的真实代码，请切换对应 Tag；当前训练主体以 `v0.6.0` 的 `best` 架构为基础，工具链累计到 `v0.8.1`。 -系统设计特点包括： -1. **高保真动力学步进**：物理仿真计算步长设为 **`2ms` (0.002s)**，为碰撞、地面力学传递提供极高的解算频宽与稳定性。 -2. **50Hz 控制决策循环**：通过在环境中设置 `decimation = 10`，策略决策周期为 `20ms` ($0.002\text{s} \times 10 = 0.02\text{s}$)，即控制决策频率为 **`50Hz`**，完全对齐真机控制周期。 -3. **混合滤波执行器**： - - 腿部 12 个位置控制关节采用位置 PD 伺服（$K_p=40, K_d=1$），并叠加截止频率为 **`5Hz`** 的低通滤波器进行动作平滑，减小高频机械抖动。 - - 轮部 4 个速度驱动关节采用阻尼速度伺服（$K_d=0.5$），叠加截止频率为 **`15Hz`** 的低通速度滤波器，保证转速响应的灵敏度。 -4. **大规模并行加速**：利用 GPU 并行（通过 Warp 和 MuJoCo GPU 物理管线），支持最多 $4096$ 环境同时训练，并包含对动作变化率、关节加速度的惩罚项以平抑噪声。 +## 目录说明 ---- - -## 📦 项目结构 - -``` +```text rc_mjlab/ -├── src/robot/ # RL 训练任务包（主体代码） -│ ├── __init__.py # 任务注册（Robot-Flat-v0 / Robot-Rough-v0 / Robot-Crawl-v0） -│ ├── robot_cfg.py # 机器人物理参数（PD 增益、执行器上限、碰撞属性） -│ ├── config/ -│ │ ├── env_cfgs.py # 三套环境完整配置（观测、奖励、事件、地形、终止条件） -│ │ └── rl_cfg.py # PPO 超参数（网络结构、学习率、折扣因子等） -│ ├── mdp/ -│ │ ├── rewards.py # 自定义奖励函数（速度追踪、姿态约束、接触、越障反射惩罚等） -│ │ ├── curriculums.py # 地形关卡课程（严格速度约束版）+ 自适应速度范围 -│ │ ├── lowpass_actions.py # 低通滤波动作包装（腿 5 Hz / 轮 15 Hz IIR 滤波） -│ │ ├── disturbances.py # 持续外力扰动（一阶低通滤波平滑随机外力/扭矩） -│ │ ├── mode_command.py # 离散步态模式命令（保留扩展用） -│ │ └── only_positive_rewards.py # HIMLoco 风格：每步总奖励截断为 ≥ 0，防止消极逃避 -│ └── terrains/ -│ └── competition_terrains.py # 竞赛自定义地形（高墙障碍、低杆障碍） -├── sim2sim/ # Sim2Sim 物理部署与高精度交互回放工具 -│ ├── nav_sim2sim.py # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪 -│ ├── nav_route_sim2sim_check.py # ONNX 策略批量路线检查 -│ ├── ik_slalom_sim2sim.py # 纯 IK、路径跟踪与绕桩验证 -│ ├── ik_compensation_sweep.py # IK 补偿参数扫描 -│ ├── export_onnx.py # PT actor 导出与 ONNX 一致性检查 -│ ├── sim2sim.py # 简易版键盘调试工具 -│ ├── interface/ -│ │ └── mujoco_io.py # MuJoCo 输入输出与传感器、低通滤波器接口 -│ ├── tools/ -│ │ └── math_utils.py # 姿态重力等数学转换 -│ ├── policy/ # 保存的 pt 策略权重 -│ └── terrain/ -│ └── scene_terrain.xml # 完整越障比赛场地的物理 XML 定义 -├── mjcf/ -│ ├── wheelleg.xml # 机器人 MuJoCo 模型（含网格引用） -│ ├── scene.xml # mjlab 场景入口文件 -│ └── meshes/ # STL/OBJ 碰撞与外观网格 -├── mujoco_sim/ # 姿态、IK、动力学和 MPC 独立工具 -├── tools/nav_tools/ # 地图/PCD/航点编辑、路线检查与比赛路线数据 -├── model_rough.pt # 早期 Rough 参考 checkpoint -├── model_6800.onnx # 比赛最终 Rough 策略 -├── pyproject.toml # 项目依赖（uv 管理，含清华镜像源加速） -└── uv.lock # 精确依赖锁定文件 +├─ src/robot/ # Robot-Flat/Rough/Crawl 任务、PPO 配置和本地 RSL-RL/HIM 代码 +├─ mjcf/ # 机器人、场景与网格资源 +├─ mjlab/ # 固定基准并带本地补丁的 mjlab 源码 +├─ mujoco_sim/ # 姿态、IK、动力学、MPC 与 GUI 工具 +├─ sim2sim/ # ONNX/PT 回放、比赛场景、IK 与路线检查 +├─ tools/nav_tools/ # PCD、地图、航点和路线编辑工具 +├─ model_rough.pt # 早期 Rough 参考 checkpoint +├─ model_6800.onnx # 比赛最终部署使用的 Rough 策略 +├─ pyproject.toml # Python 包和依赖声明 +├─ uv.lock # 历史环境的精确锁文件 +└─ DEPENDENCIES.md # 上游基准、本地补丁和可选依赖说明 ``` ---- +`model_6800.onnx` 是最终部署工件，不等于训练代码版本号。训练过程中存在基模、继续训练和 checkpoint 筛选，仅凭该 ONNX 不能恢复完整训练日志。 -## 🚀 常用命令 +## 已注册任务 -### 1. 训练与回放 +| Task ID | 用途 | 默认训练时长 | +| --- | --- | --- | +| `Robot-Flat-v0` | 平地基础运动 | 20 s/episode | +| `Robot-Rough-v0` | 粗糙地形、台阶、随机网格、高墙和坡面 | 20 s/episode | +| `Robot-Crawl-v0` | 低杆、低姿态和匍匐任务 | 30 s/episode | + +任务入口由 [`src/robot/__init__.py`](src/robot/__init__.py) 注册；环境真值见 [`src/robot/config/env_cfgs.py`](src/robot/config/env_cfgs.py)，PPO 真值见 [`src/robot/config/rl_cfg.py`](src/robot/config/rl_cfg.py)。 + +## 当前控制与模型参数 + +以下参数来自当前工作树源码，不代表所有历史 Tag： + +| 项目 | 当前值 | +| --- | --- | +| MuJoCo 物理步长 | `0.005 s`（200 Hz） | +| 控制降采样 | `decimation = 4` | +| 策略周期 | `0.020 s`（50 Hz） | +| 默认并行环境 | 2048 | +| 腿部执行器 | 位置控制，`Kp=50.0`、`Kd=1.5`、力矩上限 `17 Nm` | +| 轮部执行器 | 速度控制，`Kd=1.0`、力矩上限 `17 Nm` | +| 关节速度参考常量 | `13 rad/s`；当前 Builtin actuator 构造未显式传入该常量 | +| 默认站姿 | hip pitch `0.550`、knee `-1.125`、机身高度 `0.42 m` | +| 外展关节动作缩放 | `0.125 rad` | +| 其余腿关节动作缩放 | `0.25 rad` | +| 轮速动作缩放 | `5.0 rad/s` | +| 动作延迟 | 每个环境随机 `0～2` 个控制步 | +| 低通截止频率 | 腿 `5 Hz`、轮 `15 Hz` | + +README 原先写的 `0.002 s × decimation 10`、4096 环境、`Kp=40/Kd=1` 和轮部 `Kd=0.5` 均不对应当前代码，已删除。 + +## 观测与动作契约 + +Actor 单步观测为 53 维： + +| 观测项 | 维度 | +| --- | ---: | +| 基座角速度 | 3 | +| 投影重力 | 3 | +| 速度/航向指令 | 3 | +| 12 个腿关节相对位置 | 12 | +| 12 个腿关节速度 | 12 | +| 4 个轮关节速度 | 4 | +| 上一步 16 维动作 | 16 | + +动作共 16 维：12 个腿关节位置目标和 4 个轮关节速度目标。Critic 在 Actor 观测之外增加基座线速度、轮地接触和高度扫描等特权信息。 + +## Rough 当前配置摘要 + +`Robot-Rough-v0` 当前混合八类地形： + +| 地形 | 比例 | 当前范围摘要 | +| --- | ---: | --- | +| 平地 | 15% | 8 m × 8 m | +| 正向台阶 | 5% | 阶高 `0～0.20 m` | +| 反向台阶 | 35% | 阶高 `0～0.20 m` | +| 随机网格 | 27% | 高度 `0～0.20 m` | +| 随机粗糙面 | 1% | 起伏 `0～0.06 m` | +| Perlin 噪声 | 1% | 起伏 `0～0.06 m` | +| 自定义高墙 | 15% | 高度 `0.10～0.35 m` | +| 金字塔坡面 | 1% | 坡度 `0.052～0.325` | + +课程学习从平地、粗糙面、Perlin、坡面和正向台阶开始，随后按训练步数释放随机网格、反向台阶和高墙。同时对 X、Y、Yaw 三个指令轴分别做自适应范围调整。 + +当前 Rough 奖励使用分轴 `vx/vy/yaw` 跟踪，并启用每步总奖励不低于 0 的截断，而不是旧 README 中的 `track_lin_vel=4.5`、`track_ang_vel=2.0`。它还包含动作变化率、扭矩/功率、腿轮加速度、关节限位、镜像姿态、静止姿态、接触力和非期望碰撞等约束；精确权重以 `rough_env_cfg()` 为准。 + +## 当前域随机化边界 + +基础配置实际启用的主要随机项包括： + +- 基座质心三轴偏移：各 `[-0.05, 0.05] m`； +- 碰撞几何摩擦：`[0.3, 1.0]`； +- 执行器刚度、阻尼缩放：各 `[0.9, 1.1]`，log-uniform； +- 基座附加质量：`[-1.0, 3.0] kg`； +- Rough 间歇推扰：每 `5～10 s` 设置一次 X/Y `[-0.5, 0.5] m/s` 速度扰动； +- 关节动作延迟：`0～2` 个策略步。 + +旧 README 中列出的 encoder bias、持续外力、关节摩擦和力矩上限随机化并非当前 Rough 配置的完整真实状态，因此不再作为“当前已启用项”陈述。历史随机化差异见 [`../../../01_doc/training_evolution.md`](../../../01_doc/training_evolution.md)。 + +## 环境安装与基本命令 + +在本目录执行： ```bash -# 运行平地基础训练 (Robot-Flat-v0) +uv sync + uv run train Robot-Flat-v0 - -# 运行多障碍复杂地形训练 (Robot-Rough-v0)，可从 Flat 的Checkpoint热启动 -uv run train Robot-Rough-v0 --agent.resume True --agent.experiment-name robot_flat - -# 运行爬坡与匍匐限高任务 (Robot-Crawl-v0) +uv run train Robot-Rough-v0 uv run train Robot-Crawl-v0 -# 使用默认 20 个并行环境回放最新 checkpoint 效果 uv run play Robot-Rough-v0 ``` -### 2. 交互式 Sim2Sim 自动导航仪表盘 +GPU、CUDA、MuJoCo development wheel 和驱动要求见 [`DEPENDENCIES.md`](DEPENDENCIES.md)。恢复训练时需要明确 checkpoint/run 来源，不建议仅凭 README 猜测跨实验热启动参数。 -我们提供了一个强大的 GUI 交互和全自动障碍赛追踪平台，位于 `sim2sim` 目录下： +## Sim2Sim 与导航工具 ```bash -# 启动 2D 交互导航平台 -cd sim2sim -uv run python nav_sim2sim.py +uv run --with-requirements sim2sim/requirements.txt python sim2sim/nav_sim2sim.py + +uv run --with-requirements tools/nav_tools/requirements.txt python tools/nav_tools/nav_map_viewer.py ``` -后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。 +- 后期 Sim2Sim 入口和策略边界：[`sim2sim/README.md`](sim2sim/README.md) +- 导航打点与路线数据：[`tools/nav_tools/README.md`](tools/nav_tools/README.md) +- 训练版本演进：[`../../../01_doc/training_evolution.md`](../../../01_doc/training_evolution.md) +- 全项目版本历史：[`../../../01_doc/version_history.md`](../../../01_doc/version_history.md) -导航打点工具、路线快照和抽样点云说明见 [`tools/nav_tools/README.md`](tools/nav_tools/README.md)。 +## 复现边界 ---- - -## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py) - -该平台包含一个 **Pygame 2D HUD 监控面板** 和一个 **实时 MuJoCo 3D 渲染器**，支持对仿真参数和任务执行的精细控制。 - -### 1. 按钮面板分区与布局 - -面板在垂直方向进行了高紧凑性排版，避免控件重叠，并在底端留有安全间距： -* **【预设任务列表】**（按物理穿越顺序排列）： - - **S形绕杆 (Slalom)**：绕过红蓝两色障碍杆路径。 - - **限高下蹲 (Crawl)**：降低机身高度穿过低杆障碍。 - - **砂砾碎石 (Gravel)**：平稳低速通过多颗粒非结构碎石坑。 - - **高墙越障 (Wall)**：高速度冲向障碍高墙，利用前轮攀爬反射爬越。 - - **台阶攀爬 (Stairs)**：攀越分段式台阶。 - - **斜坡木桥 (Bridge)**：穿过A坡并稳健从B坡落地。 - - **障碍赛大满贯 (Grand)**：**科技紫**圆角高亮按钮。点击后，机器人将以**顺时针**方向，自动、连贯且闭环地一次性穿越上述全部 6 个核心比赛障碍，并在木桥落地后，通过安全通道直角返航至起终点。 -* **【系统与视图控制】**： - - **清除与停止 (Stop)**：一键紧急停止并重置当前目标航点。 - - **视角居中 (Center)**：一键锁定相机随机器人机身移动。 - - **物理流速三联排 (倍速- / 标准 / 倍速+)**：在不破坏物理计算数值稳定性的前提下，实现对仿真总体时间的平滑加速与慢放（支持 `0.2x` ~ `5.0x`，可随时点击“标准”一键归位 `1.0x`）。 -* **【目标微调与命令终端】**： - - 拥有高精度航点微调发令键。 - - 底部命令行支持输入 `speed <倍率>` 更改仿真速度，或者输入 `grand` 直接开启大满贯。 - ---- - -## 📊 机器人系统规格参数 - -### 1. 机器人本体参数 - -| 参数项 | 基准数值 | 说明 | -|---|---|---| -| **物理步长 ($dt_{physics}$)** | `0.002s` (2ms) | 底层 MuJoCo 求解器步长，物理精度极高 | -| **控制决策频率 ($Freq_{ctrl}$)** | `50Hz` (20ms) | $decimation = 10$，环境每 10 个子步进行一次交互决策 | -| **单轮仿真时长** | `30.0s` | 最大决策步数上限为 $30.0 / 0.02 = 1500$ 步 | -| **腿部控制** | 位置 PD 伺服 | 目标关节角限幅 ±0.25 rad，叠加 **5Hz** 低通滤波器 | -| **轮部控制** | 阻尼速度伺服 | 目标速度限幅 ±10.0 rad/s，叠加 **15Hz** 低通滤波器 | -| **结构形式** | 4腿 × 3关节 + 4轮 | 腿：hip abduction, hip pitch, knee；轮半径 0.1m，左右轮距 0.32m | -| **关节扭矩上限** | 17.0 Nm | 关节最大输出力矩（训练时含 80%~100% 随机缩放） | -| **最大关节角速度** | 13.0 rad/s | 关节最大运动速度限制 | - -### 2. 状态观测空间 (Actor Obs, 53维) - -网络输入包含 $6$ 步历史数据，并在训练时注入均匀高斯噪声以提升泛化能力： - -| 观测项目 | 维度 | 缩放比例 | 噪声范围 | -|---|---|---|---| -| 基座角速度 (ang_vel) | 3 | 0.25 | $[-0.2, 0.2]$ rad/s | -| 投影重力向量 (projected_gravity) | 3 | 1.0 | $[-0.05, 0.05]$ | -| 指令速度 (vx, vy, wz/heading) | 3 | 1.0 | — | -| 腿部关节相对角度 (joint_pos_rel) | 12 | 1.0 | $[-0.01, 0.01]$ rad | -| 腿部关节角速度 (joint_vel) | 12 | 0.05 | $[-1.5, 1.5]$ rad/s | -| 轮子角速度 (wheel_vel) | 4 | 0.05 | $[-1.0, 1.0]$ rad/s | -| 上一步动作缓存 (last_actions) | 16 | 1.0 | — | - -> **Critic 附加观测**：包含高精度基座物理线速度、轮地实际接触状态、以及 $1.6\text{m} \times 1.0\text{m}$ 分辨率为 $0.08\text{m}$ 的高度雷达扫描网格，提供大范围越障感知。 - ---- - -## ⚖️ 奖惩体系设计 (Robot-Rough-v0) - -复杂地形任务采用 **“仅正奖励截断”** 机制（即每步累加的总奖励若小于0则强制截断为0），防止机器人在困难关卡早期选择倒下自杀来规避负惩罚。 - -### 1. 运动追踪与状态惩罚 - -| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 | -|---|---|---| -| **track_lin_vel** | `+4.5` | L1 范数水平线速度跟踪奖励，平缓高速漂移 | -| **track_ang_vel** | `+2.0` | 偏航角速度指数跟踪奖励 | -| **stand_still** | `-2.0` | 当速度指令为 0 时，严厉惩罚关节多余晃动，保持稳立 | -| **joint_pos_penalty** | `-0.8` | 当速度指令为 0 时，惩罚关节角度偏离初始对齐姿态，维持高刚度 | -| **roll_penalty** | `-1.0` | 机身横滚角 (Roll) 倾斜惩罚，抑制左右倾倒抖动 | -| **pitch_penalty** | `-1.5` | 俯仰角 (Pitch) 死区惩罚，限制仰角不超过 29 度，抑制越障瞬间前轮翘头和后翻 | -| **base_height_l2** | `-0.5` | 机身高度偏离 0.36m 惩罚（基于高度扫描均值，允许自适应高低） | - -### 2. 能量正则与平滑惩罚 (平抑高频抖动) - -| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 | -|---|---|---| -| **action_rate_curriculum** | `-0.005` | 动作变化率 L2 惩罚，迫使连续两个决策步的输出动作变化平滑 | -| **joint_torques** | `-1.0e-4` | 关节输出扭矩 L2 正则，降低电机总发热和冲击性载荷 | -| **leg_joint_acc_l2** | `-2.5e-7` | 限制腿部 12 关节**角加速度**，直接抑制关节高频电磁和机械震荡 | -| **wheel_joint_acc_l2** | `-2.5e-9` | 限制 4 个驱动轮的**角加速度**，平缓轮速切换，降低打滑振荡 | -| **joint_pos_limits** | `-0.2` | 极度接近关节极限限位阻挡时的硬惩罚 | - -### 3. 接触反射与安全约束 - -| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 | -|---|---|---| -| **feet_contact_without_cmd** | `+0.1` | 当速度指令为 0 时，鼓励四轮保持稳定接地的正向收益 | -| **body_collision** | `-1.0` | 腿部连杆（大腿、小腿）触地碰撞惩罚，迫使抬腿跨越障碍 | -| **base_collision** | `-5.0` | 机身/底盘硬撞障碍物时的严厉惩罚，逼迫机器人学会抬起前轮支撑攀爬 | -| **is_terminated** | `0.0` | 关闭越障任务的提早终止，允许机器人跌倒后自行挣扎起立，提高生存极限 | - ---- - -## 🌀 域随机化 (Domain Randomization) - -为了使训练的控制策略具有卓越的零样本真机部署能力，在环境重置及仿真运行中注入了高强度的域随机化参数： - -| 随机化项目 | 扰动操作 | 随机范围 | -|---|---|---| -| **机身质心偏移 (base_com)** | 加法 | X, Y, Z 三轴分别随机偏置 `[-0.05, 0.05]` 米 | -| **角度传感器零偏 (encoder_bias)** | 加法 | 关节传感器绝对偏置 `[-0.015, 0.015]` rad (约 $\pm 0.85^{\circ}$) | -| **几何表面摩擦力 (body_friction)** | 绝对值 | 地面及机器人碰撞几何体摩擦力在 `[0.3, 1.2]` 均匀随机 | -| **关节摩擦阻尼 (joint_friction)** | 乘法 | 所有旋转轴关节运动阻尼摩擦在原值的 `[0.7, 1.3]` 倍间随机 | -| **关节传动刚度 (actuator_stiffness)** | 乘法 | Kp 刚度系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 | -| **关节传动阻尼 (actuator_damping)** | 乘法 | Kd 阻尼系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 | -| **力矩输出上限 (actuator_effort_limit)**| 乘法 | 最大输出扭矩极限随机在原值的 `[0.8, 1.0]` 倍均匀缩放 | -| **负载质量 (payload_mass)** | 加法 | 在机身处添加载荷质量，扰动范围在 `[-1.0, 3.0]` kg | -| **瞬时侧向推撞 (push_robot)** | 脉冲 | 每隔 `[5.0, 10.0]` 秒，瞬间施加 X/Y 轴 `[-0.5, 0.5]` m/s 冲击速度 | -| **一阶低通持续风阻 (continuous_disturbance)** | 连续 | 机身持续叠加随机外力（±15N）与力矩（±10Nm），低通周期 0.5s | - ---- - -## 🏆 多地形关卡难度控制 (Robot-Rough-v0) - -共有 8 种子地形按照比例混合，通过自适应升级距离控制关卡难度的推进： - -| 地形名称 | 混合比例 (Proportion) | 最大配置难度 | -|---|---|---| -| **平地 (flat)** | `5%` | 作为初始安定性恢复区域 | -| **金字塔台阶 (pyramid_stairs)** | `25%` | 最大阶梯高度上限 `0.30` 米，级宽 0.30m | -| **倒金字塔台阶 (pyramid_stairs_inv)** | `10%` | 最大倒台阶高度上限 `0.30` 米，级宽 0.30m | -| **随机高度网格 (random_grid)** | `10%` | 最大网格方块起伏上限 `0.30` 米 | -| **随机粗糙地形 (random_rough)** | `5%` | 地表最大颗粒随机噪声起伏 `0.06` 米 | -| **柏林噪声地形 (perlin_noise)** | `5%` | 大范围高平缓起伏最大高度 `0.06` 米 | -| **越障高墙地形 (rc_wall)** | `25%` | 自定义跳跃垂直高墙，最大墙高上限 `0.45` 米 | -| **平台斜坡地形 (sloped_terrain)** | `15%` | 最大坡度限制 `0.325` (约 $18.5^{\circ}$) | - -> **地形升级规则**：当机器人朝指令方向行进距离超过当前地块的一半（4米），且实际行进距离大于速度指令对应期望距离的 45% 时，该环境关卡等级 +1。 -> **地形降级规则**：当指令速度大于 0.1m/s 但实际行进距离小于期望距离的 25%，或者实际移动不足 2.0米时，环境难度等级 -1。 +- `uv.lock` 保存依赖解析结果，但仍需要匹配的 NVIDIA 驱动和 CUDA 环境。 +- TensorRT engine 属于真机部署环境，本目录以训练代码、PT/ONNX 和仿真验证为主。 +- 比赛最终真机工程位于 [`../../real/sim2real_ros2_v3`](../../real/sim2real_ros2_v3)。 +- 参数若与本文冲突，以当前 Tag 中的配置源码为准；不同 Tag 之间不要直接混用奖励权重、站姿和模型。