diff --git a/01_doc/version_history.md b/01_doc/version_history.md index dc703eb..af4c837 100644 --- a/01_doc/version_history.md +++ b/01_doc/version_history.md @@ -15,6 +15,7 @@ | `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 | | `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 | | `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 | +| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 | > 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。 @@ -25,6 +26,14 @@ - 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。 - 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。 +## `v0.10.0` 的 ROS 2/C++ Sim2Real 初版 + +- 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。 +- 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。 +- 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。 +- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。 +- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。 + ## `v0.4.0` 的模型变化 - 机械 CAD 不变。 diff --git a/05_software/README.md b/05_software/README.md index bdeb792..9669d1b 100644 --- a/05_software/README.md +++ b/05_software/README.md @@ -9,7 +9,8 @@ └─ real/ ├─ ik_real/ # IK 轨迹与早期真机控制 ├─ sim2real/ # 第一代 Python 策略真机部署 - └─ sim2real_v2/ # Python Sim2Real v2 + ├─ sim2real_v2/ # Python Sim2Real v2 + └─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版 ``` ## 数据流 @@ -25,11 +26,13 @@ MJCF + mjlab task +----> Sim2Sim 策略验证 | +----> Python Sim2Real / v2 ----> 电机 / IMU + | + +----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航 IK real --------------------------------> 电机 ``` -`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应重排主线的 `v0.9.0`。 +`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ Sim2Real 初版对应 `v0.10.0`。 详细说明见： diff --git a/05_software/real/README.md b/05_software/real/README.md index b606c77..a5448bd 100644 --- a/05_software/real/README.md +++ b/05_software/real/README.md @@ -26,6 +26,12 @@ Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新 部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。 +## `sim2real_ros2` + +ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。 + +原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。 + ## 实机记录 [![第一代 Sim2Real 真机验证](../../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 new file mode 100644 index 0000000..8ba3d84 --- /dev/null +++ b/05_software/real/sim2real_ros2/.gitignore @@ -0,0 +1,6 @@ +build/ +install/ +log/ +.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 new file mode 100644 index 0000000..17b4c70 --- /dev/null +++ b/05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md @@ -0,0 +1,178 @@ +# ROS2 C++ Sim2Real 运动控制栈 - 部署指南 + +本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。 + +--- + +## 1. 前提条件与环境 + +### 硬件 +* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。 +* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。 +* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。 + +### 主机依赖 +* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。 +* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。 +* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。 +* **库与 ROS2 包**： + * `libyaml-cpp-dev` + * `libeigen3-dev` + * `libusb-1.0-0-dev`（Odin USB 传感器通信） + * `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理） + * `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器） + * `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用） + * `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理） + * `can-utils`（SocketCAN 验证工具） + +--- + +## 2. 本地编译与部署 + +按以下步骤在主机系统上编译运行整个栈： + +### 步骤 1：安装系统依赖 +```bash +sudo apt-get update +sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \ + libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \ + ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \ + ros-humble-cv-bridge ros-humble-pcl-conversions +``` + +### 步骤 2：下载 ONNXRuntime C++ SDK +策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录： + +```bash +# 创建目录 +sudo mkdir -p /opt/onnxruntime +cd /opt + +# 针对 Jetson Orin (ARM64 / aarch64)： +sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz +sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime + +# 或标准桌面仿真 (x86_64 / amd64)： +# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz +# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime +``` + +导出 CMake 辅助变量： +```bash +export ONNXRUNTIME_DIR=/opt/onnxruntime +``` + +### 步骤 3：构建工作区 +进入包含 `src/` 的本包根目录，运行 `colcon`： +```bash +colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +### 步骤 4：配置 SocketCAN 接口 +启动前，以 1 Mbps 波特率激活 CAN 接口： +```bash +sudo ip link set can0 up type can bitrate 1000000 +sudo ip link set can1 up type can bitrate 1000000 +``` +使用 `ifconfig` 或 `ip link` 验证接口已启动。 + +### 步骤 5：启动节点 +使启动脚本可执行并运行： +```bash +chmod +x start_sim2real.sh +./start_sim2real.sh +``` + +--- + +## 3. Docker 部署（推荐） + +强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。 + +### 步骤 1：构建镜像 +确保在 `sim2real_ros2` 目录中（包含 `Dockerfile`）： +```bash +# 使用标准 docker build： +docker build -t sim2real_ros2:latest . + +# 或使用 Docker Compose： +docker compose build +``` + +### 步骤 2：运行容器 +对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度： + +```bash +# 选项 A：手动运行 +docker run -it \ + --network host \ + --privileged \ + --cap-add=sys_nice \ + --volume=/dev:/dev \ + --shm-size=2g \ + --name sim2real_ros2_run \ + sim2real_ros2:latest + +# 选项 B：通过 Docker Compose 运行（最简单） +docker compose up -d +``` + +--- + +## 4. 系统拓扑与话题 + +控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互： + +* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。 +* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。 +* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。 +* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。 +* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。 +* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。 + +### TF 树 +``` +odom ──→ base_link （由 odom_relay_node 广播） +map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式） +``` + +--- + +## 5. 集成 ROS 2 导航与传感器驱动 + +### USB 设备权限（Odin 传感器） +要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则： + +```bash +# 1. 添加 udev 规则 +echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules + +# 2. 重新加载 udev 规则并重新插拔传感器 +sudo udevadm control --reload +sudo udevadm trigger +``` + +### 集成启动参数 +统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈： + +* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。 +* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。 + +#### 1. 完整真实硬件闭环（默认） +启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true +``` + +#### 2. Dry-Run / 仿真航点测试 +在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true +``` + +#### 3. 仅运动控制（无导航） +禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）： +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false +``` + diff --git a/05_software/real/sim2real_ros2/Dockerfile b/05_software/real/sim2real_ros2/Dockerfile new file mode 100644 index 0000000..d7b8907 --- /dev/null +++ b/05_software/real/sim2real_ros2/Dockerfile @@ -0,0 +1,78 @@ +# 使用 ROS2 官方 Humble 基础镜像 +FROM ros:humble-ros-base-jammy + +ENV DEBIAN_FRONTEND=noninteractive + +# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库 +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + can-utils \ + libyaml-cpp-dev \ + libeigen3-dev \ + libusb-1.0-0-dev \ + libpcl-dev \ + libopencv-dev \ + ros-humble-navigation2 \ + ros-humble-nav2-bringup \ + ros-humble-pointcloud-to-laserscan \ + ros-humble-cv-bridge \ + ros-humble-pcl-conversions \ + wget \ + tar \ + python3-pip \ + && rm -rf /var/lib/apt/lists/* + +# ============================================================================ +# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速 +# ============================================================================ +# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP） +# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用） +WORKDIR /opt +RUN ARCH=$(uname -m) && \ + if [ "$ARCH" = "aarch64" ]; then \ + echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \ + pip3 install --no-cache-dir onnxruntime-gpu && \ + SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \ + mkdir -p onnxruntime/include onnxruntime/lib && \ + cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \ + cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \ + echo "[ONNX] CUDA ONNX Runtime installed."; \ + else \ + echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \ + wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \ + tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \ + mv onnxruntime-linux-x64-1.16.3 onnxruntime && \ + rm onnxruntime-linux-x64-1.16.3.tgz; \ + fi + +ENV ONNXRUNTIME_DIR=/opt/onnxruntime + +# 创建工作空间，将所有 C++ 源码包拷入 +WORKDIR /sim2real_ws/src +COPY src/sim2real_bringup sim2real_bringup +COPY src/sim2real_common sim2real_common +COPY src/sim2real_hw sim2real_hw +COPY src/sim2real_interfaces sim2real_interfaces +COPY src/sim2real_runtime sim2real_runtime +COPY src/odin_ros_driver odin_ros_driver +COPY src/sim2real_nav2 sim2real_nav2 + +# 拷贝策略文件与运行脚本 +WORKDIR /sim2real_ws +COPY policies policies +COPY start_sim2real.sh start_sim2real.sh +RUN chmod +x start_sim2real.sh + +# 编译 ROS2 工作空间 +SHELL ["/bin/bash", "-c"] +RUN source /opt/ros/humble/setup.bash && \ + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release + +# 拷贝 Docker 入口脚本并设置 +COPY docker_entrypoint.sh /docker_entrypoint.sh +RUN chmod +x /docker_entrypoint.sh + +ENTRYPOINT ["/docker_entrypoint.sh"] +CMD ["./start_sim2real.sh"] diff --git a/05_software/real/sim2real_ros2/README.md b/05_software/real/sim2real_ros2/README.md new file mode 100644 index 0000000..25d4398 --- /dev/null +++ b/05_software/real/sim2real_ros2/README.md @@ -0,0 +1,74 @@ +# ROS 2/C++ Sim2Real 初版 + +本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。 + +本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++： + +- `53D` 策略观测契约不变 +- `16D` 动作契约不变 +- `50Hz` 策略循环与训练对齐 +- `200Hz` 电机循环为专用 C++ 热路径 +- ROS 2 作为导航、TF、诊断和启动管理的系统集成层 + +## 工作区布局 + +- `src/sim2real_interfaces` + 硬件桥接与策略运行时共享的 ROS 2 消息定义。 +- `src/sim2real_common` + 共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。 +- `src/sim2real_hw` + 面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。 +- `src/sim2real_runtime` + 策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。 + 同时包含 `odom_relay_node`（里程计中继与 TF 广播）。 +- `src/sim2real_nav2` + ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。 +- `src/sim2real_bringup` + 统一启动文件与运行时参数配置。 +- `src/odin_ros_driver` + Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。 +- `docs` + 架构说明与迁移计划。 + +## 目标架构 + +```text +Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw + | | | + v v v + RuntimeState RuntimeTarget 电机 CAN 指令 + | | + +-------> 诊断 / 遥测 + +Nav2 / cmd_vel ------------------------------> sim2real_runtime + (经 odom_relay_node 提供 odom→base_link TF) +``` + +## 当前状态 + +已完成 Phase 0-5 的全部迁移： + +1. ✅ 冻结部署契约（deployment_contract.hpp） +2. ✅ ROS 2 包结构搭建 +3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环） +4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环） +5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF） + +## 契约来源 + +迁移过程中以下文件被视为真值源： + +- `sim2real/deployment_manifest.yaml` +- `sim2real/interface/motor_mapping.py` +- `sim2real/interface/real_io.py` +- `sim2real/policy/policy_runner.py` +- `sim2real/web/session.py` + +## 注意事项 + +- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。 +- Windows 仅作为编辑环境使用。 +- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改， + 除非训练与部署同步更新。 +- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。 +- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。 diff --git a/05_software/real/sim2real_ros2/docker-compose.yml b/05_software/real/sim2real_ros2/docker-compose.yml new file mode 100644 index 0000000..1d2e414 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/docker_entrypoint.sh b/05_software/real/sim2real_ros2/docker_entrypoint.sh new file mode 100644 index 0000000..c4e1054 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/docs/ARCHITECTURE.md b/05_software/real/sim2real_ros2/docs/ARCHITECTURE.md new file mode 100644 index 0000000..f3b1d00 --- /dev/null +++ b/05_software/real/sim2real_ros2/docs/ARCHITECTURE.md @@ -0,0 +1,102 @@ +# sim2real_ros2 架构说明 + +## 设计目标 + +- 保留已验证的 RL 部署契约不变 +- 将低延迟循环从 Python 迁移至 C++ +- 暴露标准 ROS 2 接口用于导航和系统集成 +- 保持安全边界独立于策略正确性 + +## 各包职责 + +### `sim2real_interfaces`（接口消息） + +定义最小化的运行时消息： + +- `RuntimeState` + 硬件桥接发布的归一化运行时状态快照 +- `RuntimeTarget` + 策略运行时发送至硬件桥接的最新策略目标 + +### `sim2real_common`（共享常量） + +存储编译期常量和部署契约辅助： + +- 观测维度和字段布局 +- 动作维度和轮子索引 +- 关节顺序和默认站姿 +- 动作缩放因子和默认循环频率 +- Mahony 姿态滤波器 +- 站立平衡控制器 +- 安全监控器（SafetyMonitor / RuntimeGuard） + +### `sim2real_hw`（硬件桥接） + +拥有硬件侧执行循环和安全边界： + +- RobStride CAN 收发 +- IMU 与 Odin 状态采集 +- 电机丢帧检测与保活逻辑（holdover） +- 看门狗与阻尼刹车 +- 发布 `RuntimeState` +- 订阅 `RuntimeTarget` +- 订阅 `/odom` 里程计数据 + +目标热路径： + +- 以 `200Hz` 频率读取状态 +- 应用最新安全目标 +- 超时或安全违规时立即停机 + +### `sim2real_runtime`（策略运行时） + +拥有策略侧执行： + +- 订阅 `RuntimeState` +- 按当前部署契约精确构建 `53D` 观测 +- 以 `50Hz` 运行 ONNXRuntime 推理 +- 对 raw_action 做 `[-10, 10]` 安全裁剪 +- 发布 `RuntimeTarget` +- 仲裁命令来源：estop > safety_hold > startup > navigation > web + +同时包含： +- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF + +### `sim2real_nav2`（导航配置） + +拥有： + +- Nav2 参数文件（planner、controller、costmap、AMCL、behavior） +- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan） + +### `sim2real_bringup`（启动管理） + +拥有： + +- 参数文件 +- 启动组合 +- 运行时模式选择 +- 集成 odin_ros_driver、sim2real_nav2 的条件启动 + +## 迁移规则 + +1. 优化之前先冻结当前契约 +2. 先迁移传输和循环结构，再调整控制算法 +3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用 +4. 按段测量延迟： + - 观测延迟 + - 策略推理延迟 + - 目标传输延迟 + - 执行器响应延迟 + +## 首个里程碑 + +首个里程碑不是"机器人在 ROS 2 下行走"，而是： + +1. `sim2real_hw` 发布稳定的 `RuntimeState` +2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测 +3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget` +4. `sim2real_hw` 消费最新目标并执行超时刹车 +5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约 + +> ✅ 以上里程碑已全部完成。 diff --git a/05_software/real/sim2real_ros2/docs/MIGRATION_PLAN.md b/05_software/real/sim2real_ros2/docs/MIGRATION_PLAN.md new file mode 100644 index 0000000..d39e093 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/docs/REMOTE_CONTROL_USAGE.md b/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md new file mode 100644 index 0000000..2c56a6b --- /dev/null +++ b/05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md @@ -0,0 +1,495 @@ +# sim2real_ros2 遥控器调用说明 + +本文档说明如何在 `sim2real_ros2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。 + +## 1. 当前接入关系 + +遥控器节点位于： + +```text +src/sim2real_runtime/src/remote_uart_node.py +``` + +该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题： + +| 输入 | 输出 | 作用 | +|---|---|---| +| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 | +| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 | + +策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。 + +## 2. 通道映射 + +通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。 + +| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 | +|---|---|---|---:| +| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` | +| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` | +| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` | +| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - | + +默认方向反转配置： + +| 参数 | 默认值 | 含义 | +|---|---:|---| +| `remote_invert_vx` | `true` | 反转前后方向 | +| `remote_invert_vy` | `false` | 不反转横移方向 | +| `remote_invert_yaw` | `true` | 反转转向方向 | + +## 3. 参数位置 + +遥控器参数在： + +```text +src/sim2real_bringup/config/runtime.yaml +``` + +当前默认参数： + +```yaml +remote_enabled: true +remote_port: "/dev/ttyACM0" +remote_baudrate: 100000 +remote_timeout: 0.02 +remote_axis_deadzone: 50 +remote_active_threshold: 50 +remote_axis_full_scale: 660.0 +remote_max_vx: 0.8 +remote_max_vy: 0.3 +remote_max_yaw_rate: 0.5 +remote_invert_vx: true +remote_invert_vy: false +remote_invert_yaw: true +remote_publish_inactive_zero: true +remote_estop_latch: true +remote_poll_hz: 50.0 +``` + +如果遥控器串口不是 `/dev/ttyACM0`，需要修改： + +```yaml +remote_port: "/dev/ttyUSB0" +``` + +或改成实际设备路径。 + +## 4. 启动前检查 + +### 4.1 确认串口存在 + +```bash +ls /dev/ttyACM* /dev/ttyUSB* +``` + +如果使用默认配置，应能看到： + +```bash +/dev/ttyACM0 +``` + +### 4.2 确认串口权限 + +如果节点提示串口权限不足，可以临时执行： + +```bash +sudo chmod 666 /dev/ttyACM0 +``` + +更推荐的长期方式是把当前用户加入 `dialout` 组： + +```bash +sudo usermod -aG dialout $USER +``` + +然后重新登录。 + +### 4.3 确认 Python serial 依赖 + +节点依赖 `pyserial`。如果系统没有安装： + +```bash +sudo apt update +sudo apt install -y python3-serial +``` + +## 5. 构建 + +如果刚修改过代码或参数，建议重新构建相关包： + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install +``` + +构建完成后 source 环境： + +```bash +source install/setup.bash +``` + +确认可执行节点存在： + +```bash +ros2 pkg executables sim2real_runtime +``` + +应包含： + +```text +sim2real_runtime remote_uart_node.py +``` + +## 6. 推荐启动方式 + +### 6.1 启动完整系统，不启动 Nav2 + +这是你当前常用方式： + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。 + +等价完整写法： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true +``` + +### 6.2 不启动遥控器 + +如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false +``` + +## 7. 单独启动遥控器节点 + +如果系统已经在运行，只想单独测试遥控器节点： + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml +``` + +如果要临时指定串口： + +```bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args \ + --params-file src/sim2real_bringup/config/runtime.yaml \ + -p remote_port:=/dev/ttyUSB0 +``` + +## 8. 执行后会产生什么效果 + +启动以下命令后： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +系统会产生以下效果。 + +### 8.1 启动硬件桥接节点 + +节点： + +```text +/sim2real_hw_node +``` + +效果： + +1. 打开 `can0` 和 `can1`。 +2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。 +3. 设置电机 MIT 模式。 +4. 设置电机速度限制和力矩限制。 +5. 以 `200Hz` 运行硬件读写循环。 +6. 发布 `/runtime/state`。 +7. 订阅 `/runtime/target` 执行策略目标。 + +### 8.2 启动策略运行节点 + +节点： + +```text +/sim2real_runtime_node +``` + +效果： + +1. 加载 ONNX 策略模型。 +2. 订阅 `/runtime/state`。 +3. 订阅 `/cmd_vel`。 +4. 订阅 `/safety/estop`。 +5. 执行启动站立流程： + - `boot_hold` + - `startup_soft_hold` + - `startup_hold` + - `runtime_zero_hold` + - `runtime_policy` +6. 以 `50Hz` 发布 `/runtime/target`。 + +### 8.3 启动遥控器节点 + +节点： + +```text +/sim2real_remote_uart_node +``` + +效果： + +1. 打开默认串口 `/dev/ttyACM0`。 +2. 以 `50Hz` 轮询 SBUS 数据。 +3. 遥控器摇杆居中时持续发布零速度： + +```text +/cmd_vel: + linear.x = 0.0 + linear.y = 0.0 + angular.z = 0.0 +``` + +4. 推动遥控器时发布非零速度，例如： + +```text +/cmd_vel: + linear.x = vx + linear.y = vy + angular.z = yaw +``` + +5. 当 CH7 打到高位时发布： + +```text +/safety/estop: true +``` + +由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。 + +### 8.4 机器人行为效果 + +正常启动后，机器人不会立即按策略行走，而是按阶段执行： + +1. 电机使能。 +2. 读取当前关节位置。 +3. 软保持当前姿态。 +4. 平滑过渡到默认站立姿态。 +5. 稳定后进入 runtime。 +6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。 +7. 遥控器有输入时进入策略控制，即 `runtime_policy`。 + +也就是说： + +| 遥控器状态 | 机器人效果 | +|---|---| +| 摇杆居中 | 站立保持，不主动行走 | +| CH2 前后推动 | 前进/后退 | +| CH4 左右推动 | 横向移动 | +| CH1 左右推动 | 原地转向 | +| CH7 高位 | 软件急停，进入安全刹车 | + +## 9. 如何确认遥控器已经生效 + +### 9.1 查看节点是否存在 + +```bash +ros2 node list +``` + +应看到： + +```text +/sim2real_remote_uart_node +/sim2real_runtime_node +/sim2real_hw_node +``` + +### 9.2 查看 `/cmd_vel` + +```bash +ros2 topic echo /cmd_vel +``` + +摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。 + +### 9.3 查看 `/safety/estop` + +```bash +ros2 topic echo /safety/estop +``` + +CH7 高位时应看到： + +```yaml +data: true +``` + +### 9.4 查看策略目标阶段 + +```bash +ros2 topic echo /runtime/target --field target_source +``` + +常见输出含义： + +| `target_source` | 含义 | +|---|---| +| `boot_hold` | 刚启动，保持初始姿态 | +| `startup_soft_hold` | 启动软保持 | +| `startup_hold` | 正在站立或站立后保持 | +| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 | +| `runtime_policy` | 遥控器有输入，策略已经介入 | +| `safety_brake` | 安全刹车 | +| `timeout_hold` | 目标超时，硬件保持默认姿态 | + +### 9.5 查看完整目标状态 + +```bash +ros2 topic echo --once /runtime/target +``` + +重点关注字段： + +```yaml +target_source: +zero_command: +runtime_released: +release_alpha: +command: +raw_command: +``` + +如果遥控器摇杆有输入，通常会看到： + +```yaml +target_source: runtime_policy +zero_command: false +runtime_released: true +release_alpha: 1.0 +``` + +## 10. 常见问题 + +### 10.1 启动后提示无法打开串口 + +可能原因： + +1. 串口路径不对。 +2. 权限不足。 +3. 设备没有插好。 +4. 设备被其他程序占用。 + +检查： + +```bash +ls /dev/ttyACM* /dev/ttyUSB* +``` + +修改 `runtime.yaml`： + +```yaml +remote_port: "/dev/ttyUSB0" +``` + +### 10.2 `/cmd_vel` 没有变化 + +检查： + +```bash +ros2 node list +ros2 topic echo /cmd_vel +``` + +如果节点存在但无变化，可能是： + +1. 遥控器没有输出 SBUS。 +2. 串口波特率不对。 +3. SBUS 接线错误。 +4. 遥控器通道未校准。 +5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。 + +### 10.3 摇杆方向反了 + +修改： + +```yaml +remote_invert_vx: true +remote_invert_vy: false +remote_invert_yaw: true +``` + +例如前后方向反了，就切换： + +```yaml +remote_invert_vx: false +``` + +### 10.4 急停后不恢复 + +当前配置： + +```yaml +remote_estop_latch: true +``` + +这表示急停锁存。触发后建议： + +1. 先确认机器人物理安全。 +2. 停止 launch。 +3. 将 CH7 打回安全位置。 +4. 重新启动系统。 + +如果需要非锁存模式，可以改为： + +```yaml +remote_estop_latch: false +``` + +但实机调试时更建议使用锁存模式。 + +## 11. 快速验证命令清单 + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +source install/setup.bash + +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +另开终端： + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +source install/setup.bash + +ros2 node list +ros2 topic echo /cmd_vel +ros2 topic echo /runtime/target --field target_source +``` + +如果只测遥控器，不启动电机系统： + +```bash +ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml +``` + +另开终端： + +```bash +ros2 topic echo /cmd_vel +ros2 topic echo /safety/estop +``` diff --git a/05_software/real/sim2real_ros2/docs/WEB_DEBUG_USAGE.md b/05_software/real/sim2real_ros2/docs/WEB_DEBUG_USAGE.md new file mode 100644 index 0000000..c1b1c54 --- /dev/null +++ b/05_software/real/sim2real_ros2/docs/WEB_DEBUG_USAGE.md @@ -0,0 +1,246 @@ +# sim2real_ros2 Web UDP 调试说明 + +本文档说明本次新增的最小 Web 调试链路。 + +## 1. 架构 + +```text +Windows 本地浏览器/HTTP 服务 + | + | UDP JSON + v +Nano: sim2real_web_udp_bridge_node.py + | + | ROS 2 topics + v +sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node +``` + +Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。 + +## 2. 新增 ROS2 节点 + +### `remote_uart_node.py` + +遥控器节点现在发布： + +```text +/cmd_vel_remote +``` + +不再直接发布 `/cmd_vel`。 + +通道触发阈值改为： + +```yaml +remote_axis_deadzone: 40 +remote_active_threshold: 40 +``` + +只有通道归一化值绝对值大于 `40` 才认为是有效输入。 + +### `cmd_mux_node.py` + +输入： + +```text +/cmd_vel_remote +/cmd_vel_web +/cmd_vel_nav +/control/mode +/remote/enabled +/web/enabled +/nav/enabled +/safety/estop +``` + +输出： + +```text +/cmd_vel +/control/mode_state +/control/mux_status +``` + +控制模式： + +```text +DISABLED +REMOTE +WEB +NAV +``` + +急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。 + +### `web_udp_bridge_node.py` + +Nano 端 UDP 监听： + +```text +0.0.0.0:15000 +``` + +发布： + +```text +/cmd_vel_web +/safety/estop +/control/mode +/web/enabled +/remote/enabled +/nav/enabled +``` + +订阅并回传状态： + +```text +/runtime/state +/runtime/target +/cmd_vel +/safety/estop +/control/mode_state +/control/mux_status +``` + +## 3. Nano 启动 + +```bash +cd /path/to/sim2real_ros2 +source /opt/ros/humble/setup.bash +source install/setup.bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false +``` + +默认会启动： + +```text +sim2real_remote_uart_node +sim2real_cmd_mux_node +sim2real_web_udp_bridge_node +``` + +如果不想启动 Web UDP bridge： + +```bash +ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false +``` + +## 4. Windows 本地 Web 启动 + +把目录复制到 Windows 或通过共享目录访问： + +```text +tools/win_web_debug +``` + +在 Windows 上安装 Python 3 后运行： + +```bash +python server.py --nano-host --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/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 new file mode 100644 index 0000000..b1ee747 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/odin_ros_driver/README.md @@ -0,0 +1,5 @@ +# Odin 驱动依赖占位 + +`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。 + +因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。 diff --git a/05_software/real/sim2real_ros2/src/sim2real_bringup/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_bringup/CMakeLists.txt new file mode 100644 index 0000000..260fc4a --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_bringup/config/deployment_contract.yaml b/05_software/real/sim2real_ros2/src/sim2real_bringup/config/deployment_contract.yaml new file mode 100644 index 0000000..45a3290 --- /dev/null +++ b/05_software/real/sim2real_ros2/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.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0] + +motor_mapping: + can_id_map: + fl_hip_abduction: [1, 1] + fl_hip_pitch: [1, 2] + fl_knee: [1, 3] + fl_wheel: [1, 4] + fr_hip_abduction: [1, 5] + fr_hip_pitch: [1, 6] + fr_knee: [1, 7] + fr_wheel: [1, 8] + rl_hip_abduction: [2, 1] + rl_hip_pitch: [2, 2] + rl_knee: [2, 3] + rl_wheel: [2, 4] + rr_hip_abduction: [2, 5] + rr_hip_pitch: [2, 6] + rr_knee: [2, 7] + rr_wheel: [2, 8] + direction_map: + fl_hip_abduction: -1 + fl_hip_pitch: -1 + fl_knee: -1 + fl_wheel: -1 + fr_hip_abduction: -1 + fr_hip_pitch: 1 + fr_knee: 1 + fr_wheel: 1 + rl_hip_abduction: 1 + rl_hip_pitch: -1 + rl_knee: -1 + rl_wheel: -1 + rr_hip_abduction: 1 + rr_hip_pitch: 1 + rr_knee: 1 + rr_wheel: 1 + zero_offset_map: + fl_hip_abduction: 0.003 + fl_hip_pitch: 0.030 + fl_knee: 0.028 + fl_wheel: 0.0 + fr_hip_abduction: 0.004 + fr_hip_pitch: 0.038 + fr_knee: 0.011 + fr_wheel: 0.0 + rl_hip_abduction: 0.019 + rl_hip_pitch: -0.034 + rl_knee: 0.025 + rl_wheel: 0.0 + rr_hip_abduction: -0.001 + rr_hip_pitch: 0.039 + rr_knee: 0.018 + rr_wheel: 0.0 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 new file mode 100644 index 0000000..69cd43a --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_bringup/config/runtime.yaml @@ -0,0 +1,80 @@ +/**: + ros__parameters: + policy_hz: 50.0 + motor_hz: 200.0 + status_hz: 10.0 + target_timeout_ms: 150.0 + model_path: policies/model_rough.onnx + use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速） + contract_file: deployment_contract.yaml + dry_run: false + can0_name: "can0" + can1_name: "can1" + imu_topic: "/odin1/imu" + odom_topic: "/odom" + + # Remote UART / SBUS parameters, aligned with the Python deployment + remote_enabled: true + remote_port: "/dev/ttyACM0" + remote_baudrate: 100000 + remote_timeout: 0.02 + remote_axis_deadzone: 40 + remote_active_threshold: 40 + remote_axis_full_scale: 660.0 + remote_max_vx: 0.8 + remote_max_vy: 0.3 + remote_max_yaw_rate: 0.5 + remote_invert_vx: true + remote_invert_vy: false + remote_invert_yaw: true + remote_publish_inactive_zero: true + remote_estop_latch: true + remote_poll_hz: 50.0 + + # Command mux parameters + cmd_mux_default_mode: "REMOTE" + cmd_mux_output_hz: 50.0 + cmd_mux_remote_timeout_ms: 250.0 + cmd_mux_web_timeout_ms: 300.0 + cmd_mux_nav_timeout_ms: 500.0 + cmd_mux_max_vx: 0.8 + cmd_mux_max_vy: 0.3 + cmd_mux_max_yaw_rate: 0.5 + cmd_mux_max_vx_acc: 1.0 + cmd_mux_max_vy_acc: 1.0 + cmd_mux_max_yaw_acc: 1.5 + + # Windows/Nano Web UDP bridge parameters + web_bridge_enabled: true + web_udp_listen_host: "0.0.0.0" + web_udp_listen_port: 15000 + web_udp_remote_host: "" + web_udp_remote_port: 15001 + web_udp_state_hz: 20.0 + web_udp_cmd_timeout_ms: 300.0 + web_udp_max_packet_bytes: 8192 + web_udp_max_vx: 0.8 + web_udp_max_vy: 0.3 + web_udp_max_yaw_rate: 0.5 + web_udp_estop_on_timeout: false + + # Safety parameters + safety_enabled: true + max_target_offset: 0.6 + hard_target_offset: 2.0 + max_ang_vel: 10.0 + max_tilt_z: -0.3 + clip_to_brake: 0 + imu_age_warn_ms: 60.0 + imu_age_stop_ms: 200.0 + + # Policy alignment with the Python deployment + command_release_s: 0.35 + release_command_hold_s: 0.12 + release_posture_max_err: 0.35 + release_target_blend_s: 0.30 + clip_obs: 100.0 + hold_zero_command_pose: true + enable_zero_cmd_suppression: true + require_active_command_to_release: true + zero_cmd_use_yaw_rate: true 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 new file mode 100644 index 0000000..e691bd4 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_bringup/launch/sim2real_system.launch.py @@ -0,0 +1,129 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + runtime_params = ParameterFile( + PathJoinSubstitution([ + FindPackageShare("sim2real_bringup"), + "config", + "runtime.yaml", + ]), + allow_substs=True, + ) + + # Declare launch configurations + launch_driver_arg = DeclareLaunchArgument( + 'launch_driver', + default_value='true', + description='Whether to launch the odin_ros_driver sensor node' + ) + + launch_nav2_arg = DeclareLaunchArgument( + 'launch_nav2', + default_value='true', + description='Whether to launch the Nav2 navigation stack' + ) + + launch_remote_arg = DeclareLaunchArgument( + 'launch_remote', + default_value='true', + description='Whether to launch the SBUS UART remote control node' + ) + + launch_web_bridge_arg = DeclareLaunchArgument( + 'launch_web_bridge', + default_value='true', + description='Whether to launch the Windows/Nano UDP web debug bridge' + ) + + # Include odin_ros_driver launch + driver_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('odin_ros_driver'), + 'launch', + 'odin1_ros2.launch.py' + ]) + ), + launch_arguments={'launch_rviz': 'false'}.items(), + condition=IfCondition(LaunchConfiguration('launch_driver')) + ) + + # Include sim2real_nav2 launch + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('sim2real_nav2'), + 'launch', + 'nav2.launch.py' + ]) + ), + condition=IfCondition(LaunchConfiguration('launch_nav2')) + ) + + return LaunchDescription([ + launch_driver_arg, + launch_nav2_arg, + launch_remote_arg, + launch_web_bridge_arg, + Node( + package="sim2real_hw", + executable="sim2real_hw_node", + name="sim2real_hw_node", + output="screen", + parameters=[runtime_params], + ), + Node( + package="sim2real_runtime", + executable="sim2real_runtime_node", + name="sim2real_runtime_node", + output="screen", + parameters=[runtime_params], + ), + Node( + package="sim2real_runtime", + executable="cmd_mux_node.py", + name="sim2real_cmd_mux_node", + output="screen", + parameters=[runtime_params], + ), + Node( + package="sim2real_runtime", + executable="web_udp_bridge_node.py", + name="sim2real_web_udp_bridge_node", + output="screen", + parameters=[runtime_params], + condition=IfCondition(LaunchConfiguration('launch_web_bridge')), + ), + Node( + package="sim2real_runtime", + executable="remote_uart_node.py", + name="sim2real_remote_uart_node", + output="screen", + parameters=[runtime_params], + condition=IfCondition(LaunchConfiguration('launch_remote')), + ), + Node( + package="sim2real_runtime", + executable="odom_relay_node", + name="odom_relay_node", + output="screen", + parameters=[{ + "odom_input_topic": "/odin1/odometry", + "odom_output_topic": "/odom", + "base_frame": "base_link", + "publish_tf": True, + }], + condition=IfCondition(LaunchConfiguration('launch_driver')), + ), + driver_launch, + nav2_launch, + ]) + diff --git a/05_software/real/sim2real_ros2/src/sim2real_bringup/package.xml b/05_software/real/sim2real_ros2/src/sim2real_bringup/package.xml new file mode 100644 index 0000000..990dda7 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_common/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_common/CMakeLists.txt new file mode 100644 index 0000000..79683b9 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_common/include/sim2real_common/deployment_contract.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/deployment_contract.hpp new file mode 100644 index 0000000..35105ce --- /dev/null +++ b/05_software/real/sim2real_ros2/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 = 80.0f; + static constexpr float kLegHoldKd = 4.0f; + 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.9f, -1.8f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.9f, -1.8f, + 0.0f, 0.0f, 0.0f, 0.0f + }; +}; + +static constexpr std::array 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/low_pass_filter.hpp b/05_software/real/sim2real_ros2/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/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/src/sim2real_common/include/sim2real_common/mahony_filter.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/mahony_filter.hpp new file mode 100644 index 0000000..8d9b964 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_common/include/sim2real_common/runtime_guard.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/runtime_guard.hpp new file mode 100644 index 0000000..5abe453 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_common/include/sim2real_common/safety_monitor.hpp b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/safety_monitor.hpp new file mode 100644 index 0000000..628f87f --- /dev/null +++ b/05_software/real/sim2real_ros2/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/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 new file mode 100644 index 0000000..e158815 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_common/include/sim2real_common/stand_balance_controller.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace sim2real_common +{ + +class StandBalanceController +{ +public: + StandBalanceController(double control_dt = 0.02) + : control_dt_(control_dt) + { + profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f}; + profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f}; + profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f}; + reset(); + } + + void reset() + { + stable_time_ = 0.0f; + } + + std::array computeTarget( + const std::array& projected_gravity, + const std::array& imu_gyro, + const std::array& cmd) + { + float hip_base = 0.9f; + float knee_base = -1.8f; + estimateBaseLegPose(hip_base, knee_base); + + float roll = 0.0f; + float pitch = 0.0f; + estimateRollPitch(projected_gravity, roll, pitch); + + float roll_rate = imu_gyro[0]; + float pitch_rate = imu_gyro[1]; + + float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate; + + float lateral_lean = lateral_lean_gain_ * cmd[1]; + + std::array 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))); + } + + 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 height_{0.33f}; + float kp_roll_{0.85f}; + float kd_roll_rate_{0.03f}; + float lateral_lean_gain_{0.0f}; + float hip_abduction_clip_{0.45f}; + std::array 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}; + + std::vector profile_h_; + std::vector profile_hip_; + std::vector profile_knee_; + + float stable_time_{0.0f}; +}; + +} // namespace sim2real_common diff --git a/05_software/real/sim2real_ros2/src/sim2real_common/package.xml b/05_software/real/sim2real_ros2/src/sim2real_common/package.xml new file mode 100644 index 0000000..da27c1e --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_hw/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_hw/CMakeLists.txt new file mode 100644 index 0000000..d1b38b5 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/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 new file mode 100644 index 0000000..598a499 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_hw/include/sim2real_hw/hardware_bridge_node.hpp @@ -0,0 +1,156 @@ +#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 "sim2real_interfaces/msg/runtime_state.hpp" +#include "sim2real_interfaces/msg/runtime_target.hpp" +#include "sim2real_common/low_pass_filter.hpp" +#include "sim2real_common/mahony_filter.hpp" +#include "sim2real_common/safety_monitor.hpp" +#include "sim2real_common/runtime_guard.hpp" + +namespace sim2real_hw +{ + +struct MotorConfig +{ + int bus; // 1 or 2 + int id; // motor CAN id + float direction; + float offset; +}; + +struct MotorStateInternal +{ + float position{0.0f}; + float velocity{0.0f}; + float torque{0.0f}; + float temperature{0.0f}; + std::uint32_t update_count{0}; + std::uint32_t stale_count{0}; + // Hold-over state + float last_valid_pos{0.0f}; + float last_valid_vel{0.0f}; + float last_valid_torque{0.0f}; + std::uint32_t prev_update_count{0}; + bool has_valid_data{false}; +}; + +class HardwareBridgeNode : public rclcpp::Node +{ +public: + HardwareBridgeNode(); + ~HardwareBridgeNode(); + +private: + void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg); + void onReadLoop(); + void onWriteLoop(); + void onImu(const sensor_msgs::msg::Imu::SharedPtr msg); + void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); + + bool initCan(const std::string& ifname, int& fd); + bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc); + bool readCanFrame(int fd, void* frame, int timeout_us); + + bool enableMotor(int fd, int motor_id); + bool disableMotor(int fd, int motor_id); + bool setModeRaw(int fd, int motor_id, std::int8_t mode); + bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit); + bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque); + + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr target_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_; + + 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::uint32_t target_sequence_{0}; + std::uint32_t state_sequence_{0}; + double target_timeout_ms_{150.0}; + + // SocketCAN file descriptors + int can0_fd_{-1}; + int can1_fd_{-1}; + std::string can0_name_{"can0"}; + std::string can1_name_{"can1"}; + + // CAN error recovery + static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit + int can0_error_count_{0}; + int can1_error_count_{0}; + bool reinitCan(const std::string& ifname, int& fd, int& error_count); + + // Hold-over constants + static constexpr std::uint32_t kHoldoverThreshold = 2; + + // Motor configurations and states + std::array 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 runtime_guard_; + bool mahony_initialized_{false}; + rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME}; + + // Telemetry + std::uint32_t fresh_count_{0}; + std::uint32_t holdover_count_{0}; + std::uint32_t stale_max_{0}; + std::uint32_t holdover_events_total_{0}; + + bool dry_run_{false}; + std::atomic estop_triggered_{false}; + std::atomic safety_enabled_{true}; + std::atomic safety_triggered_{false}; + std::string safety_reason_{""}; + + void onEstop(const std_msgs::msg::Bool::SharedPtr msg); +}; + +} // namespace sim2real_hw diff --git a/05_software/real/sim2real_ros2/src/sim2real_hw/package.xml b/05_software/real/sim2real_ros2/src/sim2real_hw/package.xml new file mode 100644 index 0000000..acbe32f --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_hw/src/hardware_bridge_node.cpp b/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp new file mode 100644 index 0000000..09131aa --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_hw/src/hardware_bridge_node.cpp @@ -0,0 +1,852 @@ +#include "sim2real_hw/hardware_bridge_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sim2real_common/deployment_contract.hpp" + +using namespace std::chrono_literals; + +namespace sim2real_hw +{ + +// Protocol constants +const std::uint32_t COMM_ENABLE = 3; +const std::uint32_t COMM_DISABLE = 4; +const std::uint32_t COMM_WRITE_PARAMETER = 18; +const std::uint32_t COMM_OPERATION_CONTROL = 1; +const std::uint32_t COMM_SET_ZERO_POSITION = 6; +const std::uint16_t PARAM_MODE = 0x7005; +const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017; +const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B; +const std::uint8_t HOST_ID = 0xFD; + +inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val) +{ + buf[0] = (val >> 8) & 0xFF; + buf[1] = val & 0xFF; +} + +inline float nearest_periodic(float val, float ref) +{ + float diff = val - ref; + float wrapped = diff - 2.0f * static_cast(M_PI) * std::floor((diff + static_cast(M_PI)) / (2.0f * static_cast(M_PI))); + return ref + wrapped; +} + +HardwareBridgeNode::HardwareBridgeNode() +: Node("sim2real_hw_node") +{ + // 1. Declare and get parameters + 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 + + // Safety parameters + safety_enabled_ = declare_parameter("safety_enabled", true); + double max_target_offset = declare_parameter("max_target_offset", 0.6); + 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); + + RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false"); + if (safety_enabled_) { + RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel); + } else { + RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!"); + } + + // 2. Set up logical motors mapping matching contract + // Mapping index in array matches joint ordering in kJointLabels + for (std::size_t i = 0; i < 16; ++i) { + motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i]; + motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i]; + motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i]; + motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i]; + } + + // 3. Initialize filters & safety monitors + lpf_legs_ = std::make_unique(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) + ); + + 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_ = sim2real_common::DeploymentContract::kDefaultDofPos; + latest_raw_action_.fill(0.0f); + + // 6. Set up ROS publishers & subscriptions + state_pub_ = create_publisher("runtime/state", 10); + target_sub_ = create_subscription( + "runtime/target", 10, + 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)); + estop_sub_ = create_subscription( + "/safety/estop", 10, + 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)); + + // 7. Enable motors on total startup + if (!dry_run_) { + RCLCPP_INFO(get_logger(), "Enabling RobStride motors..."); + for (std::size_t i = 0; i < 16; ++i) { + int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; + enableMotor(fd, motors_[i].id); + setModeRaw(fd, motors_[i].id, 0); // MIT Mode + writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f); + writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f); + } + } + + // 8. Timers at 200Hz (5ms) + read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this)); + write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this)); +} + +HardwareBridgeNode::~HardwareBridgeNode() +{ + if (!dry_run_) { + RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown..."); + for (std::size_t i = 0; i < 16; ++i) { + int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_; + disableMotor(fd, motors_[i].id); + } + if (can0_fd_ >= 0) { + if (::close(can0_fd_) < 0) { + RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno)); + } + } + if (can1_fd_ >= 0) { + if (::close(can1_fd_) < 0) { + RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno)); + } + } + } +} + +void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg) +{ + std::scoped_lock lock(target_mutex_); + latest_target_ = msg->target; + latest_raw_action_ = msg->raw_action; + latest_target_source_ = msg->target_source; + latest_target_stamp_ = rclcpp::Time(msg->stamp); + target_sequence_ = msg->sequence; +} + +void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg) +{ + std::scoped_lock 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_ && 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_); + estop_triggered_ = msg->data; + if (estop_triggered_) { + RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!"); + } else { + RCLCPP_INFO(get_logger(), "Physical E-stop reset."); + } +} + +void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::scoped_lock 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++; + } + + 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) { + if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) { + motor_states_[i].position = motor_states_[i].last_valid_pos; + motor_states_[i].velocity = motor_states_[i].last_valid_vel; + motor_states_[i].torque = motor_states_[i].last_valid_torque; + holdover_events_total_++; + } + } + } + + // 2. Fetch IMU data & update MahonyFilter + auto now_time = now(); + double dt = 0.005; + if (last_read_time_.nanoseconds() > 0) { + dt = (now_time - last_read_time_).seconds(); + if (dt <= 0.0 || dt > 0.5) { + dt = 0.005; + } + } + last_read_time_ = now_time; + + std::array 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}; + 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_ = true; + } + 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()); + } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str()); + } + } + + // 3. Populate RuntimeState message + sim2real_interfaces::msg::RuntimeState msg; + msg.stamp = now_time; + msg.sequence = state_sequence_++; + msg.source = dry_run_ ? "stub_hw" : "socket_can_hw"; + + fresh_count_ = 0; + holdover_count_ = 0; + stale_max_ = 0; + + for (std::size_t i = 0; i < 16; ++i) { + // Stale frames detection & holdover count + if (!dry_run_) { + if (motor_states_[i].stale_count > 0) { + holdover_count_++; + stale_max_ = std::max(stale_max_, motor_states_[i].stale_count); + } else { + fresh_count_++; + } + } + + if (dry_run_) { + // Mock motor positions tracking target + msg.joint_pos[i] = latest_target_[i]; + msg.joint_vel[i] = 0.0f; + msg.joint_torque[i] = 0.0f; + msg.update_counts[i] = target_sequence_; + } else { + msg.joint_pos[i] = motor_states_[i].position; + msg.joint_vel[i] = motor_states_[i].velocity; + msg.joint_torque[i] = motor_states_[i].torque; + msg.update_counts[i] = motor_states_[i].update_count; + } + } + + msg.imu_gyro = gyro; + msg.imu_accel = accel; + msg.quat_wxyz = quat; + msg.projected_gravity = projected_gravity; + msg.imu_age_ms = imu_age_ms; + msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness + + msg.odom_age_ms = 0.0f; + msg.odom_fresh = false; + msg.odom_pos = {0.0f, 0.0f, 0.0f}; + msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f}; + msg.odom_linear_vel = {0.0f, 0.0f, 0.0f}; + msg.odom_angular_vel = {0.0f, 0.0f, 0.0f}; + msg.odom_local_pos = {0.0f, 0.0f, 0.0f}; + msg.odom_local_yaw = 0.0f; + + // Populate odom fields from subscriber data + { + std::scoped_lock 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::string target_source; + double age_ms = 0.0; + + { + std::scoped_lock lock(target_mutex_); + target = latest_target_; + target_source = latest_target_source_; + + if (latest_target_stamp_.nanoseconds() > 0) { + const auto age_ns = (now_time - latest_target_stamp_).nanoseconds(); + age_ms = age_ns > 0 ? static_cast(age_ns) / 1.0e6 : 0.0; + } + } + + // Timeout guard: default stand pose if target is stale + if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) { + target = sim2real_common::DeploymentContract::kDefaultDofPos; + target_source = "timeout_hold"; + } + + // Run SafetyMonitor check on incoming target commands + std::array 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 safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active); + if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { + safety_triggered_ = true; + safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; + RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str()); + } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { + target = safety_decision.clipped_target; + target_source = "safety_clip"; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped."); + } + } + + // Override to safety_brake if safety is triggered locally or by estop + if (safety_triggered_) { + target_source = "safety_brake"; + } + + // 1. Joint LPF command filtering (200Hz, dt=0.005s) + std::array 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::kLegHoldKp * kp_scale; + kd_val = sim2real_common::DeploymentContract::kLegHoldKd; + } else { + startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") { + kp_val = sim2real_common::DeploymentContract::kLegHoldKp; + kd_val = sim2real_common::DeploymentContract::kLegHoldKd; + } + } + writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0); + } else { + // Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0) + double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign + double kd_val = sim2real_common::DeploymentContract::kWheelKd; + if (target_source == "safety_brake" || target_source == "safety_estop") { + vel_real = 0.0; + kd_val = 2.0; // Wheel damping Kd + } + writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0); + } + } + } +} + +bool HardwareBridgeNode::enableMotor(int fd, int motor_id) +{ + std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id; + return sendCanFrame(fd, ext_id, nullptr, 0); +} + +bool HardwareBridgeNode::disableMotor(int fd, int motor_id) +{ + std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode) +{ + std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + data[0] = PARAM_MODE & 0xFF; + data[1] = (PARAM_MODE >> 8) & 0xFF; + data[4] = static_cast(mode); + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit) +{ + std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id; + std::uint8_t data[8] = {0}; + data[0] = param_id & 0xFF; + data[1] = (param_id >> 8) & 0xFF; + std::memcpy(&data[4], &limit, sizeof(float)); + return sendCanFrame(fd, ext_id, data, 8); +} + +bool HardwareBridgeNode::writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp_val, double kd_val, double torque) +{ + const double P_LIMIT = 4.0 * M_PI; + const double V_LIMIT = 44.0; + const double T_LIMIT = 17.0; + const double KP_LIMIT = 500.0; + const double KD_LIMIT = 5.0; + + double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos)); + double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel)); + double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val)); + double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val)); + double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque)); + + std::uint16_t pos_u16 = static_cast(((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"; + } + } + } else if (fd == can1_fd_) { + can1_error_count_++; + if (can1_error_count_ >= kCanErrorThreshold) { + RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.", + can1_error_count_, err, strerror(err)); + if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) { + RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake."); + safety_triggered_ = true; + safety_reason_ = "CAN1 bus failure - reinit failed"; + } + } + } + return false; + } + // Reset error count on success + if (fd == can0_fd_) can0_error_count_ = 0; + else if (fd == can1_fd_) can1_error_count_ = 0; + return true; +} + +bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us) +{ + if (fd < 0) return false; + auto* frame = static_cast(frame_ptr); + + if (timeout_us > 0) { + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = timeout_us; + fd_set rdfs; + FD_ZERO(&rdfs); + FD_SET(fd, &rdfs); + + int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv); + if (ret < 0) { + int err = errno; + if (err != EINTR) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "CAN select error (fd=%d): %s", fd, strerror(err)); + } + return false; + } + if (ret == 0) { + return false; // timeout, normal + } + } + + ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame)); + if (bytes_read < 0) { + int err = errno; + if (err != EAGAIN && err != EWOULDBLOCK) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, + "CAN read error (fd=%d): %s", fd, strerror(err)); + } + return false; + } + return (bytes_read == sizeof(struct can_frame)); +} + +bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count) +{ + RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str()); + if (fd >= 0) { + ::close(fd); + fd = -1; + } + bool success = initCan(ifname, fd); + if (success) { + error_count = 0; + RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str()); + } + return success; +} + +} // namespace sim2real_hw + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/05_software/real/sim2real_ros2/src/sim2real_interfaces/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_interfaces/CMakeLists.txt new file mode 100644 index 0000000..c09fc21 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_interfaces/msg/RuntimeState.msg b/05_software/real/sim2real_ros2/src/sim2real_interfaces/msg/RuntimeState.msg new file mode 100644 index 0000000..03cd276 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_interfaces/msg/RuntimeTarget.msg b/05_software/real/sim2real_ros2/src/sim2real_interfaces/msg/RuntimeTarget.msg new file mode 100644 index 0000000..0fe3831 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_interfaces/package.xml b/05_software/real/sim2real_ros2/src/sim2real_interfaces/package.xml new file mode 100644 index 0000000..58379b2 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_nav2/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_nav2/CMakeLists.txt new file mode 100644 index 0000000..a1a51d4 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/nav2_params.yaml b/05_software/real/sim2real_ros2/src/sim2real_nav2/config/nav2_params.yaml new file mode 100644 index 0000000..f847f1e --- /dev/null +++ b/05_software/real/sim2real_ros2/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: false + allow_unknown: true + +behavior_server: + ros__parameters: + use_sim_time: false + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::Backup" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false + robot_radius: 0.25 + obstacle_range: 2.5 + raytrace_range: 3.0 + publish_frequency: 1.0 + update_frequency: 1.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 30 + height: 30 + resolution: 0.05 + track_unknown_space: true + plugins: ["obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: pointcloud + pointcloud: + topic: /odin1/cloud_slam + sensor_frame: base_link + data_type: "PointCloud2" + clearing: true + marking: true + max_obstacle_height: 2.0 + min_obstacle_height: 0.05 + obstacle_max_range: 2.5 + obstacle_min_range: 0.1 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.6 + cost_scaling_factor: 4.0 + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false + robot_radius: 0.25 + obstacle_range: 2.5 + raytrace_range: 3.0 + publish_frequency: 5.0 + update_frequency: 5.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 4 + height: 4 + resolution: 0.05 + plugins: ["obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: true + observation_sources: pointcloud + pointcloud: + topic: /odin1/cloud_slam + sensor_frame: base_link + data_type: "PointCloud2" + clearing: true + marking: true + max_obstacle_height: 2.0 + min_obstacle_height: 0.05 + obstacle_max_range: 2.5 + obstacle_min_range: 0.1 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + enabled: true + inflation_radius: 0.4 + cost_scaling_factor: 4.0 diff --git a/05_software/real/sim2real_ros2/src/sim2real_nav2/launch/nav2.launch.py b/05_software/real/sim2real_ros2/src/sim2real_nav2/launch/nav2.launch.py new file mode 100644 index 0000000..1547754 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_nav2/package.xml b/05_software/real/sim2real_ros2/src/sim2real_nav2/package.xml new file mode 100644 index 0000000..58e97c3 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/src/sim2real_runtime/CMakeLists.txt b/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt new file mode 100644 index 0000000..58decba --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.8) +project(sim2real_runtime) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(sim2real_common REQUIRED) +find_package(sim2real_interfaces REQUIRED) + +# Search for ONNX Runtime headers and library +find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h + PATHS + /usr/include + /usr/include/onnxruntime + /usr/local/include + /usr/local/include/onnxruntime + /opt/onnxruntime/include +) + +find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime + PATHS + /usr/lib + /usr/lib/x86_64-linux-gnu + /usr/lib/aarch64-linux-gnu + /usr/local/lib + /opt/onnxruntime/lib +) + +get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY) + +if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY) + message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.") +endif() + +add_executable(sim2real_runtime_node + src/policy_runtime_node.cpp +) + +add_executable(odom_relay_node + src/odom_relay_node.cpp +) + +target_include_directories(sim2real_runtime_node PRIVATE + include + ${ONNXRUNTIME_INCLUDE_DIR} +) + +target_include_directories(odom_relay_node PRIVATE include) + +target_link_libraries(sim2real_runtime_node + ${ONNXRUNTIME_LIBRARY} +) + +set_target_properties(sim2real_runtime_node PROPERTIES + BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" + INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}" +) + +target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17) + +ament_target_dependencies(sim2real_runtime_node + geometry_msgs + nav_msgs + rclcpp + std_msgs + tf2_ros + sim2real_common + sim2real_interfaces +) + +ament_target_dependencies(odom_relay_node + geometry_msgs + nav_msgs + rclcpp + tf2_ros +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS sim2real_runtime_node odom_relay_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + PROGRAMS + src/remote_uart_node.py + src/cmd_mux_node.py + src/web_udp_bridge_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/odom_relay_node.hpp b/05_software/real/sim2real_ros2/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/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/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 new file mode 100644 index 0000000..ee9496b --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/include/sim2real_runtime/policy_runtime_node.hpp @@ -0,0 +1,124 @@ +#pragma once + +#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 "rclcpp/rclcpp.hpp" +#include "sim2real_interfaces/msg/runtime_state.hpp" +#include "sim2real_interfaces/msg/runtime_target.hpp" +#include "sim2real_common/stand_balance_controller.hpp" +#include "sim2real_common/safety_monitor.hpp" +#include "sim2real_common/runtime_guard.hpp" + +// ONNXRuntime C++ API +#include + +namespace sim2real_runtime +{ + +class PolicyRuntimeNode : public rclcpp::Node +{ +public: + PolicyRuntimeNode(); + +private: + void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg); + void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg); + void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg); + void applyCmdVel(float vx, float vy, float vyaw); + void onPolicyLoop(); + + std::array 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; + + rclcpp::Publisher::SharedPtr target_pub_; + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_stamped_sub_; + rclcpp::TimerBase::SharedPtr policy_timer_; + + std::mutex mutex_; + sim2real_interfaces::msg::RuntimeState latest_state_; + bool has_state_{false}; + std::chrono::steady_clock::time_point last_state_recv_time_{}; + std::array 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 + 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_{}; + 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_; + + // ONNX Runtime members + std::string model_path_{"policies/model_rough.onnx"}; + bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano + std::unique_ptr 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_; + + // Command filter and release states + std::array filtered_cmd_{{0.0f, 0.0f, 0.0f}}; + float release_alpha_{0.0f}; + float command_release_s_{0.35f}; + float release_command_hold_s_{0.12f}; + float release_posture_max_err_{0.35f}; + float release_target_blend_s_{0.30f}; + float clip_obs_{100.0f}; + bool hold_zero_command_pose_{true}; + bool enable_zero_cmd_suppression_{true}; + bool require_active_command_to_release_{true}; + bool zero_cmd_use_yaw_rate_{false}; + bool runtime_released_{false}; + float release_active_time_{0.0f}; + float zero_cmd_lin_thresh_{0.05f}; + float zero_cmd_yaw_thresh_{0.05f}; + float zero_yaw_rate_thresh_{0.10f}; + + // E-stop and Safety variables + rclcpp::Subscription::SharedPtr estop_sub_; + std::atomic estop_triggered_{false}; + std::atomic safety_enabled_{true}; + std::atomic safety_triggered_{false}; + std::string safety_reason_{""}; + std::unique_ptr safety_monitor_; + std::unique_ptr runtime_guard_; + + void onEstop(const std_msgs::msg::Bool::SharedPtr msg); +}; + +} // 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 new file mode 100644 index 0000000..a9aefb2 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/package.xml @@ -0,0 +1,24 @@ + + + sim2real_runtime + 0.1.0 + Policy runtime node for sim2real_ros2. + todo + Proprietary + + ament_cmake + + geometry_msgs + nav_msgs + rclcpp + std_msgs + tf2_ros + sim2real_common + sim2real_interfaces + python3-serial + rclpy + + + ament_cmake + + 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 new file mode 100644 index 0000000..cccf5fe --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/cmd_mux_node.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from enum import Enum +from typing import Optional + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Bool, String + + +class ControlMode(str, Enum): + DISABLED = "DISABLED" + REMOTE = "REMOTE" + WEB = "WEB" + NAV = "NAV" + + +class CmdMuxNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True) + + self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper() + self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value) + self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value) + self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value) + self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value) + self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value) + self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value) + self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value) + self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value) + + self.mode = self.parse_mode(self.default_mode) + self.estop = False + self.remote_enabled = self.mode == ControlMode.REMOTE + self.web_enabled = self.mode == ControlMode.WEB + self.nav_enabled = self.mode == ControlMode.NAV + + self.latest_remote = Twist() + self.latest_web = Twist() + self.latest_nav = Twist() + self.remote_stamp: Optional[rclpy.time.Time] = None + self.web_stamp: Optional[rclpy.time.Time] = None + self.nav_stamp: Optional[rclpy.time.Time] = None + self.last_output = Twist() + self.last_pub_time = self.get_clock().now() + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10) + self.mode_pub = self.create_publisher(String, "control/mode_state", 10) + self.status_pub = self.create_publisher(String, "control/mux_status", 10) + + self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10) + self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10) + self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10) + self.create_subscription(String, "control/mode", self.on_mode, 10) + self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10) + self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10) + self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10) + self.create_subscription(Bool, "/safety/estop", self.on_estop, 10) + + period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02 + self.timer = self.create_timer(period, self.on_timer) + self.get_logger().info(f"Command mux started in mode {self.mode.value}") + + def parse_mode(self, value: str) -> ControlMode: + try: + return ControlMode(value.upper()) + except ValueError: + self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED") + return ControlMode.DISABLED + + def on_remote(self, msg: Twist) -> None: + self.latest_remote = msg + self.remote_stamp = self.get_clock().now() + + def on_web(self, msg: Twist) -> None: + self.latest_web = msg + self.web_stamp = self.get_clock().now() + + def on_nav(self, msg: Twist) -> None: + self.latest_nav = msg + self.nav_stamp = self.get_clock().now() + + def on_mode(self, msg: String) -> None: + new_mode = self.parse_mode(msg.data) + if new_mode != self.mode: + self.mode = new_mode + self.remote_enabled = self.mode == ControlMode.REMOTE + self.web_enabled = self.mode == ControlMode.WEB + self.nav_enabled = self.mode == ControlMode.NAV + self.get_logger().info(f"Control mode changed to {self.mode.value}") + + def on_remote_enabled(self, msg: Bool) -> None: + self.remote_enabled = bool(msg.data) + if self.remote_enabled: + self.mode = ControlMode.REMOTE + + def on_web_enabled(self, msg: Bool) -> None: + self.web_enabled = bool(msg.data) + if self.web_enabled: + self.mode = ControlMode.WEB + + def on_nav_enabled(self, msg: Bool) -> None: + self.nav_enabled = bool(msg.data) + if self.nav_enabled: + self.mode = ControlMode.NAV + + def on_estop(self, msg: Bool) -> None: + self.estop = bool(msg.data) + if self.estop: + self.mode = ControlMode.DISABLED + + def on_timer(self) -> None: + now = self.get_clock().now() + target = Twist() + source = "zero" + + if not self.estop: + if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now): + target = self.latest_remote + source = "remote" + elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now): + target = self.latest_web + source = "web" + elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now): + target = self.latest_nav + source = "nav" + + target = self.limit_twist(target) + target = self.accel_limit(target, now) + self.cmd_pub.publish(target) + self.mode_pub.publish(String(data=self.mode.value)) + self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}")) + + def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool: + if stamp is None: + return False + age_ms = (now - stamp).nanoseconds / 1.0e6 + return age_ms <= timeout_ms + + def limit_twist(self, msg: Twist) -> Twist: + out = Twist() + out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx) + out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy) + out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw) + return out + + def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist: + dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3) + out = Twist() + out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt) + out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt) + out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt) + self.last_output = out + self.last_pub_time = now + return out + + @staticmethod + def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, float(value))) + + @staticmethod + def step(current: float, target: float, max_delta: float) -> float: + delta = target - current + if delta > max_delta: + return current + max_delta + if delta < -max_delta: + return current - max_delta + return target + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = CmdMuxNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/src/sim2real_runtime/src/odom_relay_node.cpp b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/odom_relay_node.cpp new file mode 100644 index 0000000..ec733b8 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/policy_runtime_node.cpp b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp new file mode 100644 index 0000000..8fa328d --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/policy_runtime_node.cpp @@ -0,0 +1,583 @@ +#include "sim2real_runtime/policy_runtime_node.hpp" + +#include +#include +#include + +#include "sim2real_common/deployment_contract.hpp" + +using namespace std::chrono_literals; + +namespace sim2real_runtime +{ + +// Named constants for timing and command filtering +constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz) +constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz) +constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz) + +PolicyRuntimeNode::PolicyRuntimeNode() +: Node("sim2real_runtime_node") +{ + // 1. Declare and get parameters + model_path_ = declare_parameter("model_path", "policies/model_rough.onnx"); + use_cuda_ = declare_parameter("use_cuda", false); // enable CUDA EP on Orin Nano + + // Safety parameters + safety_enabled_ = declare_parameter("safety_enabled", true); + double max_target_offset = declare_parameter("max_target_offset", 0.6); + 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)); + 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_; + + RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str()); + + // Initialize StandBalanceController + stand_balance_ = std::make_unique(0.02); + + // 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) + ); + + 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 Ort C++ environment + try { + env_ = std::make_unique(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env"); + + Ort::SessionOptions session_options; + // single-thread ORIN optimization to prevent thread scheduling jitter + session_options.SetIntraOpNumThreads(1); + session_options.SetInterOpNumThreads(1); + session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); + + // CUDA Execution Provider (Orin Nano GPU acceleration) + if (use_cuda_) { + try { + OrtCUDAProviderOptions cuda_opts{}; + cuda_opts.device_id = 0; + // enable_cuda_graph: false for single-inference RL policy (avoids overhead) + session_options.AppendExecutionProvider_CUDA(cuda_opts); + RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)"); + } catch (const std::exception& e) { + RCLCPP_WARN(get_logger(), + "CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.", + e.what()); + use_cuda_ = false; + } + } + + session_ = std::make_unique(*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; + + std::size_t num_inputs = session_->GetInputCount(); + for (std::size_t i = 0; i < num_inputs; ++i) { + auto name = session_->GetInputNameAllocated(i, allocator); + input_names_str_.push_back(std::string(name.get())); + } + for (const auto& name : input_names_str_) { + input_names_char_.push_back(name.c_str()); + } + + std::size_t num_outputs = session_->GetOutputCount(); + for (std::size_t i = 0; i < num_outputs; ++i) { + auto name = session_->GetOutputNameAllocated(i, allocator); + output_names_str_.push_back(std::string(name.get())); + } + for (const auto& name : output_names_str_) { + output_names_char_.push_back(name.c_str()); + } + + auto input_type_info = session_->GetInputTypeInfo(0); + auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo(); + input_shape_ = input_tensor_info.GetShape(); + if (input_shape_[0] < 0) { + input_shape_[0] = 1; + } + + auto output_type_info = session_->GetOutputTypeInfo(0); + auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo(); + output_shape_ = output_tensor_info.GetShape(); + if (output_shape_[0] < 0) { + output_shape_[0] = 1; + } + + // Validate output shape matches expected action dimension + if (output_shape_.size() < 2 || output_shape_[1] != static_cast(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(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]", + input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]); + } catch (const std::exception& e) { + RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what()); + throw; + } + + // 3. Create publishers and subscriptions + target_pub_ = create_publisher("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)); + + // 4. Timer at 50Hz (20ms) + policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this)); + + last_actions_.fill(0.0f); +} + +void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg) +{ + std::scoped_lock lock(mutex_); + latest_state_ = *msg; + has_state_ = true; + last_state_recv_time_ = std::chrono::steady_clock::now(); +} + +void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw) +{ + // Velocity saturation limits (consistent with training domain) + constexpr float kMaxLinVelX = 0.8f; // m/s + constexpr float kMaxLinVelY = 0.3f; // m/s + constexpr float kMaxAngVelZ = 0.5f; // rad/s + + std::scoped_lock lock(mutex_); + raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX); + raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY); + raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ); + cmd_ = raw_cmd_; +} + +void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + applyCmdVel( + static_cast(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::onEstop(const std_msgs::msg::Bool::SharedPtr msg) +{ + std::scoped_lock lock(mutex_); + estop_triggered_ = msg->data; + if (estop_triggered_) { + RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!"); + } else { + RCLCPP_INFO(get_logger(), "E-stop reset."); + } +} + +std::array 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] - sim2real_common::DeploymentContract::kDefaultDofPos[i]; + } + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + obs[cursor++] = state.joint_vel[i] * 0.05f; + } + for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + obs[cursor++] = state.joint_vel[i] * 0.05f; + } + for (float v : last_actions) { + obs[cursor++] = v; + } + + // Clip observations values to ±clip_obs_ + if (clip_obs_ > 0.0f) { + for (float & v : obs) { + v = std::clamp(v, -clip_obs_, clip_obs_); + } + } + + return obs; +} + +std::array 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() + ); + + 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); + } + + 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_; +} + +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; + { + 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_; + 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()); + } else if (guard_decision.level == sim2real_common::GuardLevel::WARN) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str()); + } + } + + if (safety_active) { + sim2real_interfaces::msg::RuntimeTarget target; + target.stamp = now(); + target.sequence = sequence_++; + target.raw_command = raw_cmd; + target.command = cmd; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + target.target = sim2real_common::DeploymentContract::kDefaultDofPos; + target.target_source = "safety_brake"; + target.target_age_ms = 0.0f; + target_pub_->publish(target); + return; + } + + sim2real_interfaces::msg::RuntimeTarget target; + target.stamp = now(); + target.sequence = sequence_++; + target.raw_command = raw_cmd; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + target.command = cmd; + + const auto now_time = rclcpp::Time(target.stamp); + + if (startup_state_ == StartupState::BOOT_HOLD) { + // 1. Initial State Read + start_pose_ = state.joint_pos; + start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0 + + // 2. Shortest periodic delta to stand pose + float max_dev = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i]; + delta = delta - 2.0f * static_cast(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); + return; + } + + // Adapt transition time: min 2s, max 6s, 1.5s per rad + transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0); + startup_state_ = StartupState::STARTUP_SOFT_HOLD; + state_start_time_ = now_time; + RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_); + } + + if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) { + double elapsed = (now_time - state_start_time_).seconds(); + target.target = start_pose_; + target.target_source = "startup_soft_hold"; + + if (elapsed >= 1.0) { // 1s soft hold + startup_state_ = StartupState::STARTUP_TRANSITION; + state_start_time_ = now_time; + RCLCPP_INFO(get_logger(), "Transitioning to stand pose..."); + } + } + else if (startup_state_ == StartupState::STARTUP_TRANSITION) { + double elapsed = (now_time - state_start_time_).seconds(); + double phase = std::min(1.0, elapsed / transition_time_); + + // Cosine blend interpolation + double blend = 0.5 - 0.5 * std::cos(M_PI * phase); + for (std::size_t i = 0; i < 16; ++i) { + target.target[i] = start_pose_[i] + blend * startup_delta_[i]; + } + target.target_source = "startup_hold"; + + if (phase >= 1.0) { + // Settle check + float max_pos_err = 0.0f; + for (std::size_t i = 0; i < 12; ++i) { + float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i]; + delta = delta - 2.0f * static_cast(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(); + + // Run stand balance controller during holding phase + target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd); + target.target_source = "startup_hold"; + + if (elapsed >= 1.0 && stand_balance_->isStable()) { + startup_state_ = StartupState::RUNTIME; + RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!"); + } + } + else if (startup_state_ == StartupState::RUNTIME) { + // Python template uses the command directly in policy obs/release logic. + // Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here. + filtered_cmd_ = cmd; + + const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array{0.0f, 0.0f, 0.0f}); + const bool zero_command = isZeroCommand(cmd, state.imu_gyro); + + if (!runtime_released_) { + if (require_active_command_to_release_) { + if (isCommandActive(cmd)) { + release_active_time_ += kPolicyDt; + } else { + release_active_time_ = 0.0f; + } + + float max_hold_err = 0.0f; + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) { + max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i])); + } + + const bool active_ready = release_active_time_ >= release_command_hold_s_; + const bool posture_ready = max_hold_err <= release_posture_max_err_; + if (active_ready && posture_ready) { + runtime_released_ = true; + } + } else { + runtime_released_ = true; + } + } + + if (!runtime_released_ || zero_command) { + release_alpha_ = 0.0f; + target.runtime_released = false; + target.release_alpha = 0.0f; + target.zero_command = zero_command; + target.raw_action.fill(0.0f); + target.scaled_action.fill(0.0f); + last_actions.fill(0.0f); + target.target_source = "runtime_zero_hold"; + target.target = target_hold; + if (!runtime_released_) { + target.target_source = "runtime_hold"; + } + } else { + release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f)); + target.runtime_released = (release_alpha_ >= 1.0f); + target.release_alpha = release_alpha_; + target.zero_command = false; + target.command = cmd; + + auto raw = runPolicy(buildObservation(state, cmd, last_actions)); + for (float & v : raw) { + v *= release_alpha_; + } + target.raw_action = raw; + + const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt))); + for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) { + target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i]; + const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i]; + target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target; + last_actions[i] = raw[i]; + } + target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy"; + } + } + + // 2) Run SafetyMonitor check on computed target + if (safety_enabled_) { + auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active); + if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) { + { + std::scoped_lock lock(mutex_); + safety_triggered_ = true; + } + safety_reason_ = "Safety Monitor Stop: " + safety_decision.message; + RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str()); + + // Override target to safety_brake damping pose + target.target = sim2real_common::DeploymentContract::kDefaultDofPos; + target.target_source = "safety_brake"; + } else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) { + target.target = safety_decision.clipped_target; + target.target_source = "safety_clip"; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime."); + } + } + + target.target_age_ms = 0.0f; + + { + std::scoped_lock lock(mutex_); + last_actions_ = last_actions; + } + + target_pub_->publish(target); +} + +} // namespace sim2real_runtime + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} 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 new file mode 100644 index 0000000..22f4c86 --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/remote_uart_node.py @@ -0,0 +1,250 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Optional + +import serial + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import Bool + +SBUS_FRAME_SIZE = 25 +SBUS_RC_MID = 1024 +SBUS_AXIS_SCALE = 660.0 + +SWITCH_LOW = -1 +SWITCH_MID = 0 +SWITCH_HIGH = 1 + + +@dataclass +class RemoteSwitchState: + ch7: int = SWITCH_MID + + +@dataclass +class RemoteControlState: + ch1: int = 0 + ch2: int = 0 + ch3: int = 0 + ch4: int = 0 + switches: RemoteSwitchState = field(default_factory=RemoteSwitchState) + frame_ok: bool = False + + @property + def estop_requested(self) -> bool: + return self.switches.ch7 == SWITCH_HIGH + + +class SbusUartReceiver: + def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int): + self.port = port + self.baudrate = int(baudrate) + self.timeout = float(timeout) + self.axis_deadzone = int(axis_deadzone) + self.serial: Optional[serial.Serial] = None + self.buffer = bytearray() + self.state = RemoteControlState() + + def open(self) -> None: + if self.serial and self.serial.is_open: + return + self.serial = serial.Serial( + port=self.port, + baudrate=self.baudrate, + timeout=self.timeout, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_EVEN, + stopbits=serial.STOPBITS_TWO, + ) + + def close(self) -> None: + if self.serial and self.serial.is_open: + self.serial.close() + + def poll(self) -> RemoteControlState: + if not self.serial or not self.serial.is_open: + raise RuntimeError("remote uart is not open") + + waiting = self.serial.in_waiting + if waiting: + self.buffer.extend(self.serial.read(waiting)) + + while len(self.buffer) >= SBUS_FRAME_SIZE: + start_idx = self.buffer.find(0x0F) + if start_idx < 0: + self.buffer.clear() + break + if start_idx > 0: + del self.buffer[:start_idx] + if len(self.buffer) < SBUS_FRAME_SIZE: + break + frame = bytes(self.buffer[:SBUS_FRAME_SIZE]) + del self.buffer[:SBUS_FRAME_SIZE] + parsed = self._parse_frame(frame) + if parsed is not None: + self.state = parsed + return self.state + + def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]: + if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F: + return None + + channels = [0] * 16 + channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF + channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF + channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF + channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF + channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF + channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF + channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF + channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF + channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF + channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF + + if channels[0] < 100: + return None + + state = RemoteControlState( + ch1=self._normalize_axis(channels[0]), + ch2=self._normalize_axis(channels[1]), + ch3=self._normalize_axis(channels[3]), + ch4=self._normalize_axis(channels[2]), + switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])), + frame_ok=True, + ) + if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)): + return None + return state + + def _normalize_axis(self, value: int) -> int: + mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0)) + return 0 if abs(mapped) <= self.axis_deadzone else mapped + + @staticmethod + def _decode_switch(value: int) -> int: + if value < 500: + return SWITCH_LOW + if value > 1500: + return SWITCH_HIGH + return SWITCH_MID + + +class RemoteUartNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True) + + self.enabled = bool(self.declare_parameter("remote_enabled", True).value) + self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value) + self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value) + self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value) + self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value) + self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value) + self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0) + self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value) + self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value) + self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value) + self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value) + self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value) + self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value) + self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value) + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10) + self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) + self.receiver: Optional[SbusUartReceiver] = None + self.estop_published = False + self.open_error_logged = False + + if self.enabled: + self.receiver = SbusUartReceiver( + port=self.port, + baudrate=self.baudrate, + timeout=self.timeout, + axis_deadzone=self.axis_deadzone, + ) + try: + self.receiver.open() + self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud") + except Exception as exc: + self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}") + self.open_error_logged = True + else: + self.get_logger().warn("Remote UART node is disabled by parameter") + + period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02 + self.timer = self.create_timer(period, self.on_timer) + + def destroy_node(self) -> bool: + if self.receiver is not None: + self.receiver.close() + return super().destroy_node() + + def on_timer(self) -> None: + if not self.enabled or self.receiver is None: + return + + try: + if not self.receiver.serial or not self.receiver.serial.is_open: + self.receiver.open() + state = self.receiver.poll() + except Exception as exc: + if not self.open_error_logged: + self.get_logger().error(f"Remote UART poll failed: {exc}") + self.open_error_logged = True + return + + self.open_error_logged = False + + if state.estop_requested: + if not self.estop_published or not self.estop_latch: + self.estop_pub.publish(Bool(data=True)) + self.get_logger().warn("Remote E-stop requested by CH7 high") + self.estop_published = True + self.publish_zero_cmd() + return + + if not self.estop_latch and self.estop_published: + self.estop_pub.publish(Bool(data=False)) + self.estop_published = False + + active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4)) + if active or self.publish_inactive_zero: + cmd = Twist() + cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx) + cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy) + cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw) + self.cmd_pub.publish(cmd) + + def publish_zero_cmd(self) -> None: + self.cmd_pub.publish(Twist()) + + def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float: + if abs(raw_value) <= self.active_threshold: + return 0.0 + scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale)) + if invert: + scaled = -scaled + return float(scaled * limit) + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = RemoteUartNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 new file mode 100644 index 0000000..5ac698c --- /dev/null +++ b/05_software/real/sim2real_ros2/src/sim2real_runtime/src/web_udp_bridge_node.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import json +import socket +from typing import Any, Optional + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from sim2real_interfaces.msg import RuntimeState, RuntimeTarget +from std_msgs.msg import Bool, String + + +class WebUdpBridgeNode(Node): + def __init__(self) -> None: + super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True) + + self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value) + self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value) + self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value) + self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value) + self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value) + self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value) + self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value) + self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value) + self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value) + self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value) + self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value) + self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value) + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.sock.setblocking(False) + self.sock.bind((self.listen_host, self.listen_port)) + + self.client_addr: Optional[tuple[str, int]] = None + if self.remote_host: + self.client_addr = (self.remote_host, self.remote_port) + + self.latest_target: Optional[RuntimeTarget] = None + self.latest_state: Optional[RuntimeState] = None + self.latest_cmd = Twist() + self.latest_mode = "UNKNOWN" + self.latest_mux_status = "" + self.estop = False + self.web_enabled = False + self.last_cmd_time = self.get_clock().now() + self.timeout_estop_sent = False + + self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10) + self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10) + self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10) + self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10) + self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10) + self.mode_pub = self.create_publisher(String, "control/mode", 10) + + self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10) + self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10) + self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10) + self.create_subscription(Bool, "/safety/estop", self.on_estop, 10) + self.create_subscription(String, "control/mode_state", self.on_mode_state, 10) + self.create_subscription(String, "control/mux_status", self.on_mux_status, 10) + + self.rx_timer = self.create_timer(0.01, self.on_rx_timer) + self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer) + self.guard_timer = self.create_timer(0.05, self.on_guard_timer) + + self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}") + + def on_target(self, msg: RuntimeTarget) -> None: + self.latest_target = msg + + def on_state(self, msg: RuntimeState) -> None: + self.latest_state = msg + + def on_cmd_vel(self, msg: Twist) -> None: + self.latest_cmd = msg + + def on_estop(self, msg: Bool) -> None: + self.estop = bool(msg.data) + + def on_mode_state(self, msg: String) -> None: + self.latest_mode = msg.data + + def on_mux_status(self, msg: String) -> None: + self.latest_mux_status = msg.data + + def on_rx_timer(self) -> None: + if not self.enabled: + return + while True: + try: + data, addr = self.sock.recvfrom(self.max_packet_bytes) + except BlockingIOError: + break + except OSError as exc: + self.get_logger().warn(f"UDP receive failed: {exc}") + break + self.client_addr = addr + try: + payload = json.loads(data.decode("utf-8")) + self.handle_packet(payload) + except Exception as exc: + self.send_packet({"type": "error", "message": str(exc)}) + + def handle_packet(self, payload: dict[str, Any]) -> None: + msg_type = str(payload.get("type", "")).lower() + if msg_type == "cmd_vel": + cmd = self.parse_twist(payload) + self.cmd_pub.publish(cmd) + self.last_cmd_time = self.get_clock().now() + self.timeout_estop_sent = False + elif msg_type == "zero": + self.cmd_pub.publish(Twist()) + self.last_cmd_time = self.get_clock().now() + elif msg_type == "estop": + self.estop_pub.publish(Bool(data=bool(payload.get("data", True)))) + elif msg_type == "mode": + mode = str(payload.get("mode", "DISABLED")).upper() + self.mode_pub.publish(String(data=mode)) + self.web_enabled = mode == "WEB" + self.web_enabled_pub.publish(Bool(data=mode == "WEB")) + self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE")) + self.nav_enabled_pub.publish(Bool(data=mode == "NAV")) + elif msg_type == "web_enable": + self.web_enabled = bool(payload.get("data", False)) + self.web_enabled_pub.publish(Bool(data=self.web_enabled)) + if self.web_enabled: + self.mode_pub.publish(String(data="WEB")) + elif msg_type == "remote_enable": + enabled = bool(payload.get("data", False)) + self.remote_enabled_pub.publish(Bool(data=enabled)) + if enabled: + self.mode_pub.publish(String(data="REMOTE")) + elif msg_type == "nav_enable": + enabled = bool(payload.get("data", False)) + self.nav_enabled_pub.publish(Bool(data=enabled)) + if enabled: + self.mode_pub.publish(String(data="NAV")) + elif msg_type == "ping": + self.send_packet({"type": "pong", "stamp": self.now_sec()}) + else: + self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"}) + + def parse_twist(self, payload: dict[str, Any]) -> Twist: + cmd = Twist() + linear = payload.get("linear", {}) or {} + angular = payload.get("angular", {}) or {} + cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx) + cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy) + cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw) + return cmd + + def on_guard_timer(self) -> None: + age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6 + if age_ms > self.cmd_timeout_ms: + self.cmd_pub.publish(Twist()) + if self.estop_on_timeout and not self.timeout_estop_sent: + self.estop_pub.publish(Bool(data=True)) + self.timeout_estop_sent = True + + def on_state_timer(self) -> None: + if not self.enabled: + return + self.send_packet(self.build_state_packet()) + + def build_state_packet(self) -> dict[str, Any]: + target = self.latest_target + state = self.latest_state + packet: dict[str, Any] = { + "type": "state", + "stamp": self.now_sec(), + "mode": self.latest_mode, + "mux_status": self.latest_mux_status, + "estop": self.estop, + "web_enabled": self.web_enabled, + "cmd_vel": self.twist_to_dict(self.latest_cmd), + "runtime": {}, + "robot": {}, + } + if target is not None: + packet["runtime"] = { + "target_source": str(target.target_source), + "zero_command": bool(target.zero_command), + "runtime_released": bool(target.runtime_released), + "release_alpha": self._f(target.release_alpha), + "command": [self._f(v) for v in target.command], + "raw_command": [self._f(v) for v in target.raw_command], + } + if state is not None: + packet["robot"] = { + "joint_pos": [self._f(v) for v in state.joint_pos], + "joint_vel": [self._f(v) for v in state.joint_vel], + "joint_torque": [self._f(v) for v in state.joint_torque], + "imu_gyro": [self._f(v) for v in state.imu_gyro], + "imu_accel": [self._f(v) for v in state.imu_accel], + "projected_gravity": [self._f(v) for v in state.projected_gravity], + "quat_wxyz": [self._f(v) for v in state.quat_wxyz], + "imu_age_ms": self._f(state.imu_age_ms), + "imu_fresh": bool(state.imu_fresh), + "odom_age_ms": self._f(state.odom_age_ms), + "odom_fresh": bool(state.odom_fresh), + "odom_local_pos": [self._f(v) for v in state.odom_local_pos], + "odom_local_yaw": self._f(state.odom_local_yaw), + "fresh_count": int(state.fresh_count), + "holdover_count": int(state.holdover_count), + "stale_max": int(state.stale_max), + "update_counts": [int(v) for v in state.update_counts], + } + return packet + + def send_packet(self, payload: dict[str, Any]) -> None: + if self.client_addr is None: + return + try: + data = json.dumps(payload, separators=(",", ":")).encode("utf-8") + self.sock.sendto(data, self.client_addr) + except OSError as exc: + self.get_logger().warn(f"UDP send failed: {exc}") + + @staticmethod + def _f(v: Any) -> float: + try: + f = float(v) + if f != f: + return 0.0 + return round(f, 6) + except (TypeError, ValueError): + return 0.0 + + def now_sec(self) -> float: + return self.get_clock().now().nanoseconds / 1.0e9 + + @staticmethod + def twist_to_dict(msg: Twist) -> dict[str, Any]: + return { + "linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z}, + "angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z}, + } + + @staticmethod + def clamp(value: float, low: float, high: float) -> float: + return max(low, min(high, value)) + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = WebUdpBridgeNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_ros2/start_sim2real.sh b/05_software/real/sim2real_ros2/start_sim2real.sh new file mode 100644 index 0000000..3b90321 --- /dev/null +++ b/05_software/real/sim2real_ros2/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/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/README.md b/README.md index 8816f83..e4022f8 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ RC_WheelLeg/ - [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具 - [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略 - [x] 整理导航地图、打点工具、路线迭代和抽样 PCD +- [x] 整理 Python Sim2Real v2 与 ROS 2/C++ 初版 - [ ] 核对比赛机械与仿真模型参数 - [ ] 整理 URDF/MJCF 机器人描述 - [ ] 整理后续统一训练、ROS 2 和比赛版本