[real] 整理 ROS 2/C++ Sim2Real 初版
This commit is contained in:
@@ -15,6 +15,7 @@
|
|||||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
| `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`。
|
> 原先临时归档为 `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 工具和安全保护链路。
|
- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
|
||||||
- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
|
- 排除运行日志、测试日志、临时 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` 的模型变化
|
## `v0.4.0` 的模型变化
|
||||||
|
|
||||||
- 机械 CAD 不变。
|
- 机械 CAD 不变。
|
||||||
|
|||||||
@@ -9,7 +9,8 @@
|
|||||||
└─ real/
|
└─ real/
|
||||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
├─ 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 策略验证
|
+----> Sim2Sim 策略验证
|
||||||
|
|
|
|
||||||
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
||||||
|
|
|
||||||
|
+----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
|
||||||
|
|
||||||
IK real --------------------------------> 电机
|
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`。
|
||||||
|
|
||||||
详细说明见：
|
详细说明见：
|
||||||
|
|
||||||
|
|||||||
@@ -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_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)。
|
||||||
|
|
||||||
## 实机记录
|
## 实机记录
|
||||||
|
|
||||||
[](../../06_assets/videos/early_sim2real.mp4)
|
[](../../06_assets/videos/early_sim2real.mp4)
|
||||||
|
|||||||
@@ -0,0 +1,6 @@
|
|||||||
|
build/
|
||||||
|
install/
|
||||||
|
log/
|
||||||
|
.colcon/
|
||||||
|
.vscode/
|
||||||
|
compile_commands.json
|
||||||
@@ -0,0 +1,178 @@
|
|||||||
|
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||||
|
|
||||||
|
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 1. 前提条件与环境
|
||||||
|
|
||||||
|
### 硬件
|
||||||
|
* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。
|
||||||
|
* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。
|
||||||
|
* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。
|
||||||
|
|
||||||
|
### 主机依赖
|
||||||
|
* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。
|
||||||
|
* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。
|
||||||
|
* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。
|
||||||
|
* **库与 ROS2 包**：
|
||||||
|
* `libyaml-cpp-dev`
|
||||||
|
* `libeigen3-dev`
|
||||||
|
* `libusb-1.0-0-dev`（Odin USB 传感器通信）
|
||||||
|
* `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理）
|
||||||
|
* `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器）
|
||||||
|
* `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用）
|
||||||
|
* `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理）
|
||||||
|
* `can-utils`（SocketCAN 验证工具）
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 本地编译与部署
|
||||||
|
|
||||||
|
按以下步骤在主机系统上编译运行整个栈：
|
||||||
|
|
||||||
|
### 步骤 1：安装系统依赖
|
||||||
|
```bash
|
||||||
|
sudo apt-get update
|
||||||
|
sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \
|
||||||
|
libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \
|
||||||
|
ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \
|
||||||
|
ros-humble-cv-bridge ros-humble-pcl-conversions
|
||||||
|
```
|
||||||
|
|
||||||
|
### 步骤 2：下载 ONNXRuntime C++ SDK
|
||||||
|
策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 创建目录
|
||||||
|
sudo mkdir -p /opt/onnxruntime
|
||||||
|
cd /opt
|
||||||
|
|
||||||
|
# 针对 Jetson Orin (ARM64 / aarch64)：
|
||||||
|
sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz
|
||||||
|
sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||||
|
|
||||||
|
# 或标准桌面仿真 (x86_64 / amd64)：
|
||||||
|
# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz
|
||||||
|
# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||||
|
```
|
||||||
|
|
||||||
|
导出 CMake 辅助变量：
|
||||||
|
```bash
|
||||||
|
export ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||||
|
```
|
||||||
|
|
||||||
|
### 步骤 3：构建工作区
|
||||||
|
进入包含 `src/` 的本包根目录，运行 `colcon`：
|
||||||
|
```bash
|
||||||
|
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||||
|
```
|
||||||
|
|
||||||
|
### 步骤 4：配置 SocketCAN 接口
|
||||||
|
启动前，以 1 Mbps 波特率激活 CAN 接口：
|
||||||
|
```bash
|
||||||
|
sudo ip link set can0 up type can bitrate 1000000
|
||||||
|
sudo ip link set can1 up type can bitrate 1000000
|
||||||
|
```
|
||||||
|
使用 `ifconfig` 或 `ip link` 验证接口已启动。
|
||||||
|
|
||||||
|
### 步骤 5：启动节点
|
||||||
|
使启动脚本可执行并运行：
|
||||||
|
```bash
|
||||||
|
chmod +x start_sim2real.sh
|
||||||
|
./start_sim2real.sh
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. Docker 部署（推荐）
|
||||||
|
|
||||||
|
强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。
|
||||||
|
|
||||||
|
### 步骤 1：构建镜像
|
||||||
|
确保在 `sim2real_ros2` 目录中（包含 `Dockerfile`）：
|
||||||
|
```bash
|
||||||
|
# 使用标准 docker build：
|
||||||
|
docker build -t sim2real_ros2:latest .
|
||||||
|
|
||||||
|
# 或使用 Docker Compose：
|
||||||
|
docker compose build
|
||||||
|
```
|
||||||
|
|
||||||
|
### 步骤 2：运行容器
|
||||||
|
对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 选项 A：手动运行
|
||||||
|
docker run -it \
|
||||||
|
--network host \
|
||||||
|
--privileged \
|
||||||
|
--cap-add=sys_nice \
|
||||||
|
--volume=/dev:/dev \
|
||||||
|
--shm-size=2g \
|
||||||
|
--name sim2real_ros2_run \
|
||||||
|
sim2real_ros2:latest
|
||||||
|
|
||||||
|
# 选项 B：通过 Docker Compose 运行（最简单）
|
||||||
|
docker compose up -d
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 系统拓扑与话题
|
||||||
|
|
||||||
|
控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互：
|
||||||
|
|
||||||
|
* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。
|
||||||
|
* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。
|
||||||
|
* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。
|
||||||
|
* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。
|
||||||
|
* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。
|
||||||
|
* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。
|
||||||
|
|
||||||
|
### TF 树
|
||||||
|
```
|
||||||
|
odom ──→ base_link （由 odom_relay_node 广播）
|
||||||
|
map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式）
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 集成 ROS 2 导航与传感器驱动
|
||||||
|
|
||||||
|
### USB 设备权限（Odin 传感器）
|
||||||
|
要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 1. 添加 udev 规则
|
||||||
|
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules
|
||||||
|
|
||||||
|
# 2. 重新加载 udev 规则并重新插拔传感器
|
||||||
|
sudo udevadm control --reload
|
||||||
|
sudo udevadm trigger
|
||||||
|
```
|
||||||
|
|
||||||
|
### 集成启动参数
|
||||||
|
统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
|
||||||
|
|
||||||
|
* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
|
||||||
|
* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
|
||||||
|
|
||||||
|
#### 1. 完整真实硬件闭环（默认）
|
||||||
|
启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
#### 2. Dry-Run / 仿真航点测试
|
||||||
|
在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）：
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
#### 3. 仅运动控制（无导航）
|
||||||
|
禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）：
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
|
||||||
|
```
|
||||||
|
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
# 使用 ROS2 官方 Humble 基础镜像
|
||||||
|
FROM ros:humble-ros-base-jammy
|
||||||
|
|
||||||
|
ENV DEBIAN_FRONTEND=noninteractive
|
||||||
|
|
||||||
|
# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
|
||||||
|
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||||
|
build-essential \
|
||||||
|
cmake \
|
||||||
|
git \
|
||||||
|
can-utils \
|
||||||
|
libyaml-cpp-dev \
|
||||||
|
libeigen3-dev \
|
||||||
|
libusb-1.0-0-dev \
|
||||||
|
libpcl-dev \
|
||||||
|
libopencv-dev \
|
||||||
|
ros-humble-navigation2 \
|
||||||
|
ros-humble-nav2-bringup \
|
||||||
|
ros-humble-pointcloud-to-laserscan \
|
||||||
|
ros-humble-cv-bridge \
|
||||||
|
ros-humble-pcl-conversions \
|
||||||
|
wget \
|
||||||
|
tar \
|
||||||
|
python3-pip \
|
||||||
|
&& rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
|
# ============================================================================
|
||||||
|
# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
|
||||||
|
# ============================================================================
|
||||||
|
# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
|
||||||
|
# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
|
||||||
|
WORKDIR /opt
|
||||||
|
RUN ARCH=$(uname -m) && \
|
||||||
|
if [ "$ARCH" = "aarch64" ]; then \
|
||||||
|
echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
|
||||||
|
pip3 install --no-cache-dir onnxruntime-gpu && \
|
||||||
|
SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
|
||||||
|
mkdir -p onnxruntime/include onnxruntime/lib && \
|
||||||
|
cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
|
||||||
|
cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
|
||||||
|
echo "[ONNX] CUDA ONNX Runtime installed."; \
|
||||||
|
else \
|
||||||
|
echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
|
||||||
|
wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
|
||||||
|
tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
|
||||||
|
mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
|
||||||
|
rm onnxruntime-linux-x64-1.16.3.tgz; \
|
||||||
|
fi
|
||||||
|
|
||||||
|
ENV ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||||
|
|
||||||
|
# 创建工作空间，将所有 C++ 源码包拷入
|
||||||
|
WORKDIR /sim2real_ws/src
|
||||||
|
COPY src/sim2real_bringup sim2real_bringup
|
||||||
|
COPY src/sim2real_common sim2real_common
|
||||||
|
COPY src/sim2real_hw sim2real_hw
|
||||||
|
COPY src/sim2real_interfaces sim2real_interfaces
|
||||||
|
COPY src/sim2real_runtime sim2real_runtime
|
||||||
|
COPY src/odin_ros_driver odin_ros_driver
|
||||||
|
COPY src/sim2real_nav2 sim2real_nav2
|
||||||
|
|
||||||
|
# 拷贝策略文件与运行脚本
|
||||||
|
WORKDIR /sim2real_ws
|
||||||
|
COPY policies policies
|
||||||
|
COPY start_sim2real.sh start_sim2real.sh
|
||||||
|
RUN chmod +x start_sim2real.sh
|
||||||
|
|
||||||
|
# 编译 ROS2 工作空间
|
||||||
|
SHELL ["/bin/bash", "-c"]
|
||||||
|
RUN source /opt/ros/humble/setup.bash && \
|
||||||
|
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||||
|
|
||||||
|
# 拷贝 Docker 入口脚本并设置
|
||||||
|
COPY docker_entrypoint.sh /docker_entrypoint.sh
|
||||||
|
RUN chmod +x /docker_entrypoint.sh
|
||||||
|
|
||||||
|
ENTRYPOINT ["/docker_entrypoint.sh"]
|
||||||
|
CMD ["./start_sim2real.sh"]
|
||||||
@@ -0,0 +1,74 @@
|
|||||||
|
# ROS 2/C++ Sim2Real 初版
|
||||||
|
|
||||||
|
本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
|
||||||
|
|
||||||
|
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||||
|
|
||||||
|
- `53D` 策略观测契约不变
|
||||||
|
- `16D` 动作契约不变
|
||||||
|
- `50Hz` 策略循环与训练对齐
|
||||||
|
- `200Hz` 电机循环为专用 C++ 热路径
|
||||||
|
- ROS 2 作为导航、TF、诊断和启动管理的系统集成层
|
||||||
|
|
||||||
|
## 工作区布局
|
||||||
|
|
||||||
|
- `src/sim2real_interfaces`
|
||||||
|
硬件桥接与策略运行时共享的 ROS 2 消息定义。
|
||||||
|
- `src/sim2real_common`
|
||||||
|
共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。
|
||||||
|
- `src/sim2real_hw`
|
||||||
|
面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。
|
||||||
|
- `src/sim2real_runtime`
|
||||||
|
策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。
|
||||||
|
同时包含 `odom_relay_node`（里程计中继与 TF 广播）。
|
||||||
|
- `src/sim2real_nav2`
|
||||||
|
ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。
|
||||||
|
- `src/sim2real_bringup`
|
||||||
|
统一启动文件与运行时参数配置。
|
||||||
|
- `src/odin_ros_driver`
|
||||||
|
Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。
|
||||||
|
- `docs`
|
||||||
|
架构说明与迁移计划。
|
||||||
|
|
||||||
|
## 目标架构
|
||||||
|
|
||||||
|
```text
|
||||||
|
Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw
|
||||||
|
| | |
|
||||||
|
v v v
|
||||||
|
RuntimeState RuntimeTarget 电机 CAN 指令
|
||||||
|
| |
|
||||||
|
+-------> 诊断 / 遥测
|
||||||
|
|
||||||
|
Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||||
|
(经 odom_relay_node 提供 odom→base_link TF)
|
||||||
|
```
|
||||||
|
|
||||||
|
## 当前状态
|
||||||
|
|
||||||
|
已完成 Phase 0-5 的全部迁移：
|
||||||
|
|
||||||
|
1. ✅ 冻结部署契约（deployment_contract.hpp）
|
||||||
|
2. ✅ ROS 2 包结构搭建
|
||||||
|
3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
|
||||||
|
4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
|
||||||
|
5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
|
||||||
|
|
||||||
|
## 契约来源
|
||||||
|
|
||||||
|
迁移过程中以下文件被视为真值源：
|
||||||
|
|
||||||
|
- `sim2real/deployment_manifest.yaml`
|
||||||
|
- `sim2real/interface/motor_mapping.py`
|
||||||
|
- `sim2real/interface/real_io.py`
|
||||||
|
- `sim2real/policy/policy_runner.py`
|
||||||
|
- `sim2real/web/session.py`
|
||||||
|
|
||||||
|
## 注意事项
|
||||||
|
|
||||||
|
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
|
||||||
|
- Windows 仅作为编辑环境使用。
|
||||||
|
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||||
|
除非训练与部署同步更新。
|
||||||
|
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
|
||||||
|
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
|
||||||
@@ -0,0 +1,22 @@
|
|||||||
|
version: '3.8'
|
||||||
|
|
||||||
|
services:
|
||||||
|
sim2real_ros2:
|
||||||
|
build:
|
||||||
|
context: .
|
||||||
|
dockerfile: Dockerfile
|
||||||
|
container_name: sim2real_ros2_node
|
||||||
|
runtime: nvidia
|
||||||
|
network_mode: host
|
||||||
|
privileged: true
|
||||||
|
stdin_open: true
|
||||||
|
tty: true
|
||||||
|
environment:
|
||||||
|
- NVIDIA_VISIBLE_DEVICES=all
|
||||||
|
- NVIDIA_DRIVER_CAPABILITIES=compute,utility
|
||||||
|
cap_add:
|
||||||
|
- SYS_NICE
|
||||||
|
shm_size: '2gb'
|
||||||
|
volumes:
|
||||||
|
- /dev:/dev
|
||||||
|
restart: unless-stopped
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
set -e
|
||||||
|
|
||||||
|
# Source ROS2 Humble environment
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
|
||||||
|
# Source workspace install setup if compiled
|
||||||
|
if [ -f "/sim2real_ws/install/setup.bash" ]; then
|
||||||
|
source /sim2real_ws/install/setup.bash
|
||||||
|
fi
|
||||||
|
|
||||||
|
exec "$@"
|
||||||
@@ -0,0 +1,102 @@
|
|||||||
|
# sim2real_ros2 架构说明
|
||||||
|
|
||||||
|
## 设计目标
|
||||||
|
|
||||||
|
- 保留已验证的 RL 部署契约不变
|
||||||
|
- 将低延迟循环从 Python 迁移至 C++
|
||||||
|
- 暴露标准 ROS 2 接口用于导航和系统集成
|
||||||
|
- 保持安全边界独立于策略正确性
|
||||||
|
|
||||||
|
## 各包职责
|
||||||
|
|
||||||
|
### `sim2real_interfaces`（接口消息）
|
||||||
|
|
||||||
|
定义最小化的运行时消息：
|
||||||
|
|
||||||
|
- `RuntimeState`
|
||||||
|
硬件桥接发布的归一化运行时状态快照
|
||||||
|
- `RuntimeTarget`
|
||||||
|
策略运行时发送至硬件桥接的最新策略目标
|
||||||
|
|
||||||
|
### `sim2real_common`（共享常量）
|
||||||
|
|
||||||
|
存储编译期常量和部署契约辅助：
|
||||||
|
|
||||||
|
- 观测维度和字段布局
|
||||||
|
- 动作维度和轮子索引
|
||||||
|
- 关节顺序和默认站姿
|
||||||
|
- 动作缩放因子和默认循环频率
|
||||||
|
- Mahony 姿态滤波器
|
||||||
|
- 站立平衡控制器
|
||||||
|
- 安全监控器（SafetyMonitor / RuntimeGuard）
|
||||||
|
|
||||||
|
### `sim2real_hw`（硬件桥接）
|
||||||
|
|
||||||
|
拥有硬件侧执行循环和安全边界：
|
||||||
|
|
||||||
|
- RobStride CAN 收发
|
||||||
|
- IMU 与 Odin 状态采集
|
||||||
|
- 电机丢帧检测与保活逻辑（holdover）
|
||||||
|
- 看门狗与阻尼刹车
|
||||||
|
- 发布 `RuntimeState`
|
||||||
|
- 订阅 `RuntimeTarget`
|
||||||
|
- 订阅 `/odom` 里程计数据
|
||||||
|
|
||||||
|
目标热路径：
|
||||||
|
|
||||||
|
- 以 `200Hz` 频率读取状态
|
||||||
|
- 应用最新安全目标
|
||||||
|
- 超时或安全违规时立即停机
|
||||||
|
|
||||||
|
### `sim2real_runtime`（策略运行时）
|
||||||
|
|
||||||
|
拥有策略侧执行：
|
||||||
|
|
||||||
|
- 订阅 `RuntimeState`
|
||||||
|
- 按当前部署契约精确构建 `53D` 观测
|
||||||
|
- 以 `50Hz` 运行 ONNXRuntime 推理
|
||||||
|
- 对 raw_action 做 `[-10, 10]` 安全裁剪
|
||||||
|
- 发布 `RuntimeTarget`
|
||||||
|
- 仲裁命令来源：estop > safety_hold > startup > navigation > web
|
||||||
|
|
||||||
|
同时包含：
|
||||||
|
- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
|
||||||
|
|
||||||
|
### `sim2real_nav2`（导航配置）
|
||||||
|
|
||||||
|
拥有：
|
||||||
|
|
||||||
|
- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
|
||||||
|
- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
|
||||||
|
|
||||||
|
### `sim2real_bringup`（启动管理）
|
||||||
|
|
||||||
|
拥有：
|
||||||
|
|
||||||
|
- 参数文件
|
||||||
|
- 启动组合
|
||||||
|
- 运行时模式选择
|
||||||
|
- 集成 odin_ros_driver、sim2real_nav2 的条件启动
|
||||||
|
|
||||||
|
## 迁移规则
|
||||||
|
|
||||||
|
1. 优化之前先冻结当前契约
|
||||||
|
2. 先迁移传输和循环结构，再调整控制算法
|
||||||
|
3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
|
||||||
|
4. 按段测量延迟：
|
||||||
|
- 观测延迟
|
||||||
|
- 策略推理延迟
|
||||||
|
- 目标传输延迟
|
||||||
|
- 执行器响应延迟
|
||||||
|
|
||||||
|
## 首个里程碑
|
||||||
|
|
||||||
|
首个里程碑不是"机器人在 ROS 2 下行走"，而是：
|
||||||
|
|
||||||
|
1. `sim2real_hw` 发布稳定的 `RuntimeState`
|
||||||
|
2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
|
||||||
|
3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
|
||||||
|
4. `sim2real_hw` 消费最新目标并执行超时刹车
|
||||||
|
5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
|
||||||
|
|
||||||
|
> ✅ 以上里程碑已全部完成。
|
||||||
@@ -0,0 +1,80 @@
|
|||||||
|
# 迁移计划
|
||||||
|
|
||||||
|
## Phase 1: 硬件核心迁移 ✅ 已完成
|
||||||
|
|
||||||
|
将当前高频热路径从 Python 迁出。
|
||||||
|
|
||||||
|
吸收的源文件：
|
||||||
|
|
||||||
|
- `sim2real/interface/motor_driver.py`
|
||||||
|
- `sim2real/interface/motor_mapping.py`
|
||||||
|
- `sim2real/interface/imu_client.py`
|
||||||
|
- `sim2real/safety/runtime_guard.py`
|
||||||
|
- `sim2real/web/session.py`
|
||||||
|
|
||||||
|
交付物：
|
||||||
|
|
||||||
|
- C++ SocketCAN 电机总线封装
|
||||||
|
- C++ 状态缓存
|
||||||
|
- target 超时保活（timeout hold）
|
||||||
|
- 阻尼刹车 / 急停通路
|
||||||
|
- 发布 `RuntimeState`
|
||||||
|
|
||||||
|
## Phase 2: 策略运行时迁移 ✅ 已完成
|
||||||
|
|
||||||
|
吸收的源文件：
|
||||||
|
|
||||||
|
- `sim2real/policy/policy_runner.py`
|
||||||
|
- `sim2real/interface/real_io.py`
|
||||||
|
- `sim2real/web/session.py`
|
||||||
|
|
||||||
|
交付物：
|
||||||
|
|
||||||
|
- 精确的 `53D` 观测构造器
|
||||||
|
- ONNXRuntime C++ 推理封装
|
||||||
|
- `50Hz` 策略定时器
|
||||||
|
- 命令平滑与来源仲裁
|
||||||
|
- raw_action `[-10, 10]` 安全裁剪
|
||||||
|
- 发布 `RuntimeTarget`
|
||||||
|
|
||||||
|
## Phase 3: ROS 2 系统集成 ✅ 已完成
|
||||||
|
|
||||||
|
参考的源项目：
|
||||||
|
|
||||||
|
- `00_ reference/odin_ros_driver`
|
||||||
|
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||||
|
- `00_ reference/rl_sar`
|
||||||
|
|
||||||
|
交付物：
|
||||||
|
|
||||||
|
- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
|
||||||
|
- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
|
||||||
|
- 诊断话题
|
||||||
|
- rosbag/foxglove 可观测性
|
||||||
|
|
||||||
|
## Phase 4: 导航集成 ✅ 已完成
|
||||||
|
|
||||||
|
目标：
|
||||||
|
|
||||||
|
- 导航通过 ROS 2 发送身体速度指令
|
||||||
|
- RL 运行时保持为 locomotion 控制器
|
||||||
|
- 看门狗和安全边界始终在导航之下
|
||||||
|
|
||||||
|
规则：
|
||||||
|
|
||||||
|
- 导航绝不直接写电机指令
|
||||||
|
- 策略契约在重新训练前保持不变
|
||||||
|
- 任何新增历史项或里程计项必须版本化
|
||||||
|
|
||||||
|
## 当前状态
|
||||||
|
|
||||||
|
所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
|
||||||
|
|
||||||
|
| 组件 | 节点 | 说明 |
|
||||||
|
|------|------|------|
|
||||||
|
| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
|
||||||
|
| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
|
||||||
|
| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
|
||||||
|
| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
|
||||||
|
| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
|
||||||
|
| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||||
@@ -0,0 +1,495 @@
|
|||||||
|
# sim2real_ros2 遥控器调用说明
|
||||||
|
|
||||||
|
本文档说明如何在 `sim2real_ros2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
|
||||||
|
|
||||||
|
## 1. 当前接入关系
|
||||||
|
|
||||||
|
遥控器节点位于：
|
||||||
|
|
||||||
|
```text
|
||||||
|
src/sim2real_runtime/src/remote_uart_node.py
|
||||||
|
```
|
||||||
|
|
||||||
|
该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
|
||||||
|
|
||||||
|
| 输入 | 输出 | 作用 |
|
||||||
|
|---|---|---|
|
||||||
|
| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
|
||||||
|
| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
|
||||||
|
|
||||||
|
策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
|
||||||
|
|
||||||
|
## 2. 通道映射
|
||||||
|
|
||||||
|
通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
|
||||||
|
|
||||||
|
| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
|
||||||
|
|---|---|---|---:|
|
||||||
|
| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
|
||||||
|
| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
|
||||||
|
| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
|
||||||
|
| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
|
||||||
|
|
||||||
|
默认方向反转配置：
|
||||||
|
|
||||||
|
| 参数 | 默认值 | 含义 |
|
||||||
|
|---|---:|---|
|
||||||
|
| `remote_invert_vx` | `true` | 反转前后方向 |
|
||||||
|
| `remote_invert_vy` | `false` | 不反转横移方向 |
|
||||||
|
| `remote_invert_yaw` | `true` | 反转转向方向 |
|
||||||
|
|
||||||
|
## 3. 参数位置
|
||||||
|
|
||||||
|
遥控器参数在：
|
||||||
|
|
||||||
|
```text
|
||||||
|
src/sim2real_bringup/config/runtime.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
当前默认参数：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_enabled: true
|
||||||
|
remote_port: "/dev/ttyACM0"
|
||||||
|
remote_baudrate: 100000
|
||||||
|
remote_timeout: 0.02
|
||||||
|
remote_axis_deadzone: 50
|
||||||
|
remote_active_threshold: 50
|
||||||
|
remote_axis_full_scale: 660.0
|
||||||
|
remote_max_vx: 0.8
|
||||||
|
remote_max_vy: 0.3
|
||||||
|
remote_max_yaw_rate: 0.5
|
||||||
|
remote_invert_vx: true
|
||||||
|
remote_invert_vy: false
|
||||||
|
remote_invert_yaw: true
|
||||||
|
remote_publish_inactive_zero: true
|
||||||
|
remote_estop_latch: true
|
||||||
|
remote_poll_hz: 50.0
|
||||||
|
```
|
||||||
|
|
||||||
|
如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_port: "/dev/ttyUSB0"
|
||||||
|
```
|
||||||
|
|
||||||
|
或改成实际设备路径。
|
||||||
|
|
||||||
|
## 4. 启动前检查
|
||||||
|
|
||||||
|
### 4.1 确认串口存在
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ls /dev/ttyACM* /dev/ttyUSB*
|
||||||
|
```
|
||||||
|
|
||||||
|
如果使用默认配置，应能看到：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
/dev/ttyACM0
|
||||||
|
```
|
||||||
|
|
||||||
|
### 4.2 确认串口权限
|
||||||
|
|
||||||
|
如果节点提示串口权限不足，可以临时执行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo chmod 666 /dev/ttyACM0
|
||||||
|
```
|
||||||
|
|
||||||
|
更推荐的长期方式是把当前用户加入 `dialout` 组：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo usermod -aG dialout $USER
|
||||||
|
```
|
||||||
|
|
||||||
|
然后重新登录。
|
||||||
|
|
||||||
|
### 4.3 确认 Python serial 依赖
|
||||||
|
|
||||||
|
节点依赖 `pyserial`。如果系统没有安装：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo apt update
|
||||||
|
sudo apt install -y python3-serial
|
||||||
|
```
|
||||||
|
|
||||||
|
## 5. 构建
|
||||||
|
|
||||||
|
如果刚修改过代码或参数，建议重新构建相关包：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
|
||||||
|
```
|
||||||
|
|
||||||
|
构建完成后 source 环境：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
source install/setup.bash
|
||||||
|
```
|
||||||
|
|
||||||
|
确认可执行节点存在：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 pkg executables sim2real_runtime
|
||||||
|
```
|
||||||
|
|
||||||
|
应包含：
|
||||||
|
|
||||||
|
```text
|
||||||
|
sim2real_runtime remote_uart_node.py
|
||||||
|
```
|
||||||
|
|
||||||
|
## 6. 推荐启动方式
|
||||||
|
|
||||||
|
### 6.1 启动完整系统，不启动 Nav2
|
||||||
|
|
||||||
|
这是你当前常用方式：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
|
||||||
|
|
||||||
|
等价完整写法：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
### 6.2 不启动遥控器
|
||||||
|
|
||||||
|
如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
## 7. 单独启动遥控器节点
|
||||||
|
|
||||||
|
如果系统已经在运行，只想单独测试遥控器节点：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
如果要临时指定串口：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run sim2real_runtime remote_uart_node.py --ros-args \
|
||||||
|
--params-file src/sim2real_bringup/config/runtime.yaml \
|
||||||
|
-p remote_port:=/dev/ttyUSB0
|
||||||
|
```
|
||||||
|
|
||||||
|
## 8. 执行后会产生什么效果
|
||||||
|
|
||||||
|
启动以下命令后：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
系统会产生以下效果。
|
||||||
|
|
||||||
|
### 8.1 启动硬件桥接节点
|
||||||
|
|
||||||
|
节点：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/sim2real_hw_node
|
||||||
|
```
|
||||||
|
|
||||||
|
效果：
|
||||||
|
|
||||||
|
1. 打开 `can0` 和 `can1`。
|
||||||
|
2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
|
||||||
|
3. 设置电机 MIT 模式。
|
||||||
|
4. 设置电机速度限制和力矩限制。
|
||||||
|
5. 以 `200Hz` 运行硬件读写循环。
|
||||||
|
6. 发布 `/runtime/state`。
|
||||||
|
7. 订阅 `/runtime/target` 执行策略目标。
|
||||||
|
|
||||||
|
### 8.2 启动策略运行节点
|
||||||
|
|
||||||
|
节点：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/sim2real_runtime_node
|
||||||
|
```
|
||||||
|
|
||||||
|
效果：
|
||||||
|
|
||||||
|
1. 加载 ONNX 策略模型。
|
||||||
|
2. 订阅 `/runtime/state`。
|
||||||
|
3. 订阅 `/cmd_vel`。
|
||||||
|
4. 订阅 `/safety/estop`。
|
||||||
|
5. 执行启动站立流程：
|
||||||
|
- `boot_hold`
|
||||||
|
- `startup_soft_hold`
|
||||||
|
- `startup_hold`
|
||||||
|
- `runtime_zero_hold`
|
||||||
|
- `runtime_policy`
|
||||||
|
6. 以 `50Hz` 发布 `/runtime/target`。
|
||||||
|
|
||||||
|
### 8.3 启动遥控器节点
|
||||||
|
|
||||||
|
节点：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/sim2real_remote_uart_node
|
||||||
|
```
|
||||||
|
|
||||||
|
效果：
|
||||||
|
|
||||||
|
1. 打开默认串口 `/dev/ttyACM0`。
|
||||||
|
2. 以 `50Hz` 轮询 SBUS 数据。
|
||||||
|
3. 遥控器摇杆居中时持续发布零速度：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel:
|
||||||
|
linear.x = 0.0
|
||||||
|
linear.y = 0.0
|
||||||
|
angular.z = 0.0
|
||||||
|
```
|
||||||
|
|
||||||
|
4. 推动遥控器时发布非零速度，例如：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel:
|
||||||
|
linear.x = vx
|
||||||
|
linear.y = vy
|
||||||
|
angular.z = yaw
|
||||||
|
```
|
||||||
|
|
||||||
|
5. 当 CH7 打到高位时发布：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/safety/estop: true
|
||||||
|
```
|
||||||
|
|
||||||
|
由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||||
|
|
||||||
|
### 8.4 机器人行为效果
|
||||||
|
|
||||||
|
正常启动后，机器人不会立即按策略行走，而是按阶段执行：
|
||||||
|
|
||||||
|
1. 电机使能。
|
||||||
|
2. 读取当前关节位置。
|
||||||
|
3. 软保持当前姿态。
|
||||||
|
4. 平滑过渡到默认站立姿态。
|
||||||
|
5. 稳定后进入 runtime。
|
||||||
|
6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
|
||||||
|
7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
|
||||||
|
|
||||||
|
也就是说：
|
||||||
|
|
||||||
|
| 遥控器状态 | 机器人效果 |
|
||||||
|
|---|---|
|
||||||
|
| 摇杆居中 | 站立保持，不主动行走 |
|
||||||
|
| CH2 前后推动 | 前进/后退 |
|
||||||
|
| CH4 左右推动 | 横向移动 |
|
||||||
|
| CH1 左右推动 | 原地转向 |
|
||||||
|
| CH7 高位 | 软件急停，进入安全刹车 |
|
||||||
|
|
||||||
|
## 9. 如何确认遥控器已经生效
|
||||||
|
|
||||||
|
### 9.1 查看节点是否存在
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 node list
|
||||||
|
```
|
||||||
|
|
||||||
|
应看到：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/sim2real_remote_uart_node
|
||||||
|
/sim2real_runtime_node
|
||||||
|
/sim2real_hw_node
|
||||||
|
```
|
||||||
|
|
||||||
|
### 9.2 查看 `/cmd_vel`
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /cmd_vel
|
||||||
|
```
|
||||||
|
|
||||||
|
摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
|
||||||
|
|
||||||
|
### 9.3 查看 `/safety/estop`
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /safety/estop
|
||||||
|
```
|
||||||
|
|
||||||
|
CH7 高位时应看到：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
data: true
|
||||||
|
```
|
||||||
|
|
||||||
|
### 9.4 查看策略目标阶段
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /runtime/target --field target_source
|
||||||
|
```
|
||||||
|
|
||||||
|
常见输出含义：
|
||||||
|
|
||||||
|
| `target_source` | 含义 |
|
||||||
|
|---|---|
|
||||||
|
| `boot_hold` | 刚启动，保持初始姿态 |
|
||||||
|
| `startup_soft_hold` | 启动软保持 |
|
||||||
|
| `startup_hold` | 正在站立或站立后保持 |
|
||||||
|
| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
|
||||||
|
| `runtime_policy` | 遥控器有输入，策略已经介入 |
|
||||||
|
| `safety_brake` | 安全刹车 |
|
||||||
|
| `timeout_hold` | 目标超时，硬件保持默认姿态 |
|
||||||
|
|
||||||
|
### 9.5 查看完整目标状态
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo --once /runtime/target
|
||||||
|
```
|
||||||
|
|
||||||
|
重点关注字段：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
target_source:
|
||||||
|
zero_command:
|
||||||
|
runtime_released:
|
||||||
|
release_alpha:
|
||||||
|
command:
|
||||||
|
raw_command:
|
||||||
|
```
|
||||||
|
|
||||||
|
如果遥控器摇杆有输入，通常会看到：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
target_source: runtime_policy
|
||||||
|
zero_command: false
|
||||||
|
runtime_released: true
|
||||||
|
release_alpha: 1.0
|
||||||
|
```
|
||||||
|
|
||||||
|
## 10. 常见问题
|
||||||
|
|
||||||
|
### 10.1 启动后提示无法打开串口
|
||||||
|
|
||||||
|
可能原因：
|
||||||
|
|
||||||
|
1. 串口路径不对。
|
||||||
|
2. 权限不足。
|
||||||
|
3. 设备没有插好。
|
||||||
|
4. 设备被其他程序占用。
|
||||||
|
|
||||||
|
检查：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ls /dev/ttyACM* /dev/ttyUSB*
|
||||||
|
```
|
||||||
|
|
||||||
|
修改 `runtime.yaml`：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_port: "/dev/ttyUSB0"
|
||||||
|
```
|
||||||
|
|
||||||
|
### 10.2 `/cmd_vel` 没有变化
|
||||||
|
|
||||||
|
检查：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 node list
|
||||||
|
ros2 topic echo /cmd_vel
|
||||||
|
```
|
||||||
|
|
||||||
|
如果节点存在但无变化，可能是：
|
||||||
|
|
||||||
|
1. 遥控器没有输出 SBUS。
|
||||||
|
2. 串口波特率不对。
|
||||||
|
3. SBUS 接线错误。
|
||||||
|
4. 遥控器通道未校准。
|
||||||
|
5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
|
||||||
|
|
||||||
|
### 10.3 摇杆方向反了
|
||||||
|
|
||||||
|
修改：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_invert_vx: true
|
||||||
|
remote_invert_vy: false
|
||||||
|
remote_invert_yaw: true
|
||||||
|
```
|
||||||
|
|
||||||
|
例如前后方向反了，就切换：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_invert_vx: false
|
||||||
|
```
|
||||||
|
|
||||||
|
### 10.4 急停后不恢复
|
||||||
|
|
||||||
|
当前配置：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_estop_latch: true
|
||||||
|
```
|
||||||
|
|
||||||
|
这表示急停锁存。触发后建议：
|
||||||
|
|
||||||
|
1. 先确认机器人物理安全。
|
||||||
|
2. 停止 launch。
|
||||||
|
3. 将 CH7 打回安全位置。
|
||||||
|
4. 重新启动系统。
|
||||||
|
|
||||||
|
如果需要非锁存模式，可以改为：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_estop_latch: false
|
||||||
|
```
|
||||||
|
|
||||||
|
但实机调试时更建议使用锁存模式。
|
||||||
|
|
||||||
|
## 11. 快速验证命令清单
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
另开终端：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
|
||||||
|
ros2 node list
|
||||||
|
ros2 topic echo /cmd_vel
|
||||||
|
ros2 topic echo /runtime/target --field target_source
|
||||||
|
```
|
||||||
|
|
||||||
|
如果只测遥控器，不启动电机系统：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
另开终端：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /cmd_vel
|
||||||
|
ros2 topic echo /safety/estop
|
||||||
|
```
|
||||||
@@ -0,0 +1,246 @@
|
|||||||
|
# sim2real_ros2 Web UDP 调试说明
|
||||||
|
|
||||||
|
本文档说明本次新增的最小 Web 调试链路。
|
||||||
|
|
||||||
|
## 1. 架构
|
||||||
|
|
||||||
|
```text
|
||||||
|
Windows 本地浏览器/HTTP 服务
|
||||||
|
|
|
||||||
|
| UDP JSON
|
||||||
|
v
|
||||||
|
Nano: sim2real_web_udp_bridge_node.py
|
||||||
|
|
|
||||||
|
| ROS 2 topics
|
||||||
|
v
|
||||||
|
sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
|
||||||
|
```
|
||||||
|
|
||||||
|
Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
|
||||||
|
|
||||||
|
## 2. 新增 ROS2 节点
|
||||||
|
|
||||||
|
### `remote_uart_node.py`
|
||||||
|
|
||||||
|
遥控器节点现在发布：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel_remote
|
||||||
|
```
|
||||||
|
|
||||||
|
不再直接发布 `/cmd_vel`。
|
||||||
|
|
||||||
|
通道触发阈值改为：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
remote_axis_deadzone: 40
|
||||||
|
remote_active_threshold: 40
|
||||||
|
```
|
||||||
|
|
||||||
|
只有通道归一化值绝对值大于 `40` 才认为是有效输入。
|
||||||
|
|
||||||
|
### `cmd_mux_node.py`
|
||||||
|
|
||||||
|
输入：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel_remote
|
||||||
|
/cmd_vel_web
|
||||||
|
/cmd_vel_nav
|
||||||
|
/control/mode
|
||||||
|
/remote/enabled
|
||||||
|
/web/enabled
|
||||||
|
/nav/enabled
|
||||||
|
/safety/estop
|
||||||
|
```
|
||||||
|
|
||||||
|
输出：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel
|
||||||
|
/control/mode_state
|
||||||
|
/control/mux_status
|
||||||
|
```
|
||||||
|
|
||||||
|
控制模式：
|
||||||
|
|
||||||
|
```text
|
||||||
|
DISABLED
|
||||||
|
REMOTE
|
||||||
|
WEB
|
||||||
|
NAV
|
||||||
|
```
|
||||||
|
|
||||||
|
急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
|
||||||
|
|
||||||
|
### `web_udp_bridge_node.py`
|
||||||
|
|
||||||
|
Nano 端 UDP 监听：
|
||||||
|
|
||||||
|
```text
|
||||||
|
0.0.0.0:15000
|
||||||
|
```
|
||||||
|
|
||||||
|
发布：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/cmd_vel_web
|
||||||
|
/safety/estop
|
||||||
|
/control/mode
|
||||||
|
/web/enabled
|
||||||
|
/remote/enabled
|
||||||
|
/nav/enabled
|
||||||
|
```
|
||||||
|
|
||||||
|
订阅并回传状态：
|
||||||
|
|
||||||
|
```text
|
||||||
|
/runtime/state
|
||||||
|
/runtime/target
|
||||||
|
/cmd_vel
|
||||||
|
/safety/estop
|
||||||
|
/control/mode_state
|
||||||
|
/control/mux_status
|
||||||
|
```
|
||||||
|
|
||||||
|
## 3. Nano 启动
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd /path/to/sim2real_ros2
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
默认会启动：
|
||||||
|
|
||||||
|
```text
|
||||||
|
sim2real_remote_uart_node
|
||||||
|
sim2real_cmd_mux_node
|
||||||
|
sim2real_web_udp_bridge_node
|
||||||
|
```
|
||||||
|
|
||||||
|
如果不想启动 Web UDP bridge：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
|
||||||
|
```
|
||||||
|
|
||||||
|
## 4. Windows 本地 Web 启动
|
||||||
|
|
||||||
|
把目录复制到 Windows 或通过共享目录访问：
|
||||||
|
|
||||||
|
```text
|
||||||
|
tools/win_web_debug
|
||||||
|
```
|
||||||
|
|
||||||
|
在 Windows 上安装 Python 3 后运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
|
||||||
|
```
|
||||||
|
|
||||||
|
浏览器打开：
|
||||||
|
|
||||||
|
```text
|
||||||
|
http://127.0.0.1:8088
|
||||||
|
```
|
||||||
|
|
||||||
|
## 5. UDP 命令格式
|
||||||
|
|
||||||
|
### 切换模式
|
||||||
|
|
||||||
|
```json
|
||||||
|
{"type":"mode","mode":"REMOTE"}
|
||||||
|
```
|
||||||
|
|
||||||
|
```json
|
||||||
|
{"type":"mode","mode":"WEB"}
|
||||||
|
```
|
||||||
|
|
||||||
|
```json
|
||||||
|
{"type":"mode","mode":"DISABLED"}
|
||||||
|
```
|
||||||
|
|
||||||
|
### Web 速度控制
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"type": "cmd_vel",
|
||||||
|
"linear": {"x": 0.2, "y": 0.0, "z": 0.0},
|
||||||
|
"angular": {"x": 0.0, "y": 0.0, "z": 0.1}
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
Nano 端会再次限幅：
|
||||||
|
|
||||||
|
```text
|
||||||
|
vx <= ±0.8 m/s
|
||||||
|
vy <= ±0.3 m/s
|
||||||
|
yaw <= ±0.5 rad/s
|
||||||
|
```
|
||||||
|
|
||||||
|
### 零速度
|
||||||
|
|
||||||
|
```json
|
||||||
|
{"type":"zero"}
|
||||||
|
```
|
||||||
|
|
||||||
|
### 软急停
|
||||||
|
|
||||||
|
```json
|
||||||
|
{"type":"estop","data":true}
|
||||||
|
```
|
||||||
|
|
||||||
|
## 6. 安全保护
|
||||||
|
|
||||||
|
当前最小版本已经包含：
|
||||||
|
|
||||||
|
1. 遥控器误触发阈值：`40`。
|
||||||
|
2. 遥控器/Web/Nav 互斥控制模式。
|
||||||
|
3. `cmd_mux` 二次限幅。
|
||||||
|
4. `cmd_mux` 加速度限制。
|
||||||
|
5. Web UDP 超时自动发布零速度。
|
||||||
|
6. 急停优先级最高。
|
||||||
|
7. Web 页面切换到 `WEB` 模式需要确认。
|
||||||
|
8. Web 松开虚拟摇杆会自动发送零速度。
|
||||||
|
|
||||||
|
建议实机调试流程：
|
||||||
|
|
||||||
|
1. 先点击 `DISABLED`。
|
||||||
|
2. 确认 `/cmd_vel` 为零。
|
||||||
|
3. 如果使用遥控器，点击 `REMOTE`。
|
||||||
|
4. 如果使用 Web，点击 `WEB` 并确认周围安全。
|
||||||
|
5. 一旦异常，立即点击 `软急停`。
|
||||||
|
|
||||||
|
## 7. 验证命令
|
||||||
|
|
||||||
|
查看最终输出速度：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /cmd_vel
|
||||||
|
```
|
||||||
|
|
||||||
|
查看遥控器输入：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /cmd_vel_remote
|
||||||
|
```
|
||||||
|
|
||||||
|
查看 Web 输入：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /cmd_vel_web
|
||||||
|
```
|
||||||
|
|
||||||
|
查看当前仲裁模式：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /control/mode_state
|
||||||
|
```
|
||||||
|
|
||||||
|
查看策略状态：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /runtime/target --field target_source
|
||||||
|
```
|
||||||
@@ -0,0 +1,20 @@
|
|||||||
|
Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150,
|
||||||
|
0.002390, -0.000340, -1.000000, -0.011850,
|
||||||
|
0.999960, -0.009160, 0.002390, 0.005360,
|
||||||
|
0.000000, 0.000000, 0.000000, 1.000000]
|
||||||
|
cam_0:
|
||||||
|
image_width: 1600
|
||||||
|
image_height: 1296
|
||||||
|
k2: 0.000656
|
||||||
|
k3: -0.028961
|
||||||
|
k4: 0.045390
|
||||||
|
k5: -0.064513
|
||||||
|
k6: 0.038735
|
||||||
|
k7: -0.009903
|
||||||
|
p1: 0.000000
|
||||||
|
p2: 0.000000
|
||||||
|
A11: 736.894262
|
||||||
|
A12: -0.161150
|
||||||
|
A22: 736.611354
|
||||||
|
u0: 806.125535
|
||||||
|
v0: 639.650710
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,5 @@
|
|||||||
|
# Odin 驱动依赖占位
|
||||||
|
|
||||||
|
`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
|
||||||
|
|
||||||
|
因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
|
||||||
@@ -0,0 +1,11 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(sim2real_bringup)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY launch config
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
@@ -0,0 +1,103 @@
|
|||||||
|
# =============================================================================
|
||||||
|
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||||
|
# =============================================================================
|
||||||
|
#
|
||||||
|
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||||
|
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||||
|
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||||
|
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||||
|
#
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
model:
|
||||||
|
path: "policies/model_rough.onnx"
|
||||||
|
source_pt: "policies/model_rough.pt"
|
||||||
|
backend: "onnxruntime"
|
||||||
|
obs_dim: 53
|
||||||
|
action_dim: 16
|
||||||
|
clip_obs: 100.0
|
||||||
|
|
||||||
|
observation:
|
||||||
|
terms:
|
||||||
|
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||||
|
- {name: projected_gravity, dim: 3}
|
||||||
|
- {name: command, dim: 3}
|
||||||
|
- {name: joint_pos_rel, dim: 12}
|
||||||
|
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||||
|
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||||
|
- {name: last_actions, dim: 16}
|
||||||
|
|
||||||
|
action:
|
||||||
|
joint_order:
|
||||||
|
- fl_hip_abduction
|
||||||
|
- fl_hip_pitch
|
||||||
|
- fl_knee
|
||||||
|
- fr_hip_abduction
|
||||||
|
- fr_hip_pitch
|
||||||
|
- fr_knee
|
||||||
|
- rl_hip_abduction
|
||||||
|
- rl_hip_pitch
|
||||||
|
- rl_knee
|
||||||
|
- rr_hip_abduction
|
||||||
|
- rr_hip_pitch
|
||||||
|
- rr_knee
|
||||||
|
- fl_wheel
|
||||||
|
- fr_wheel
|
||||||
|
- rl_wheel
|
||||||
|
- rr_wheel
|
||||||
|
wheel_indices: [12, 13, 14, 15]
|
||||||
|
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||||
|
default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
|
||||||
|
motor_mapping:
|
||||||
|
can_id_map:
|
||||||
|
fl_hip_abduction: [1, 1]
|
||||||
|
fl_hip_pitch: [1, 2]
|
||||||
|
fl_knee: [1, 3]
|
||||||
|
fl_wheel: [1, 4]
|
||||||
|
fr_hip_abduction: [1, 5]
|
||||||
|
fr_hip_pitch: [1, 6]
|
||||||
|
fr_knee: [1, 7]
|
||||||
|
fr_wheel: [1, 8]
|
||||||
|
rl_hip_abduction: [2, 1]
|
||||||
|
rl_hip_pitch: [2, 2]
|
||||||
|
rl_knee: [2, 3]
|
||||||
|
rl_wheel: [2, 4]
|
||||||
|
rr_hip_abduction: [2, 5]
|
||||||
|
rr_hip_pitch: [2, 6]
|
||||||
|
rr_knee: [2, 7]
|
||||||
|
rr_wheel: [2, 8]
|
||||||
|
direction_map:
|
||||||
|
fl_hip_abduction: -1
|
||||||
|
fl_hip_pitch: -1
|
||||||
|
fl_knee: -1
|
||||||
|
fl_wheel: -1
|
||||||
|
fr_hip_abduction: -1
|
||||||
|
fr_hip_pitch: 1
|
||||||
|
fr_knee: 1
|
||||||
|
fr_wheel: 1
|
||||||
|
rl_hip_abduction: 1
|
||||||
|
rl_hip_pitch: -1
|
||||||
|
rl_knee: -1
|
||||||
|
rl_wheel: -1
|
||||||
|
rr_hip_abduction: 1
|
||||||
|
rr_hip_pitch: 1
|
||||||
|
rr_knee: 1
|
||||||
|
rr_wheel: 1
|
||||||
|
zero_offset_map:
|
||||||
|
fl_hip_abduction: 0.003
|
||||||
|
fl_hip_pitch: 0.030
|
||||||
|
fl_knee: 0.028
|
||||||
|
fl_wheel: 0.0
|
||||||
|
fr_hip_abduction: 0.004
|
||||||
|
fr_hip_pitch: 0.038
|
||||||
|
fr_knee: 0.011
|
||||||
|
fr_wheel: 0.0
|
||||||
|
rl_hip_abduction: 0.019
|
||||||
|
rl_hip_pitch: -0.034
|
||||||
|
rl_knee: 0.025
|
||||||
|
rl_wheel: 0.0
|
||||||
|
rr_hip_abduction: -0.001
|
||||||
|
rr_hip_pitch: 0.039
|
||||||
|
rr_knee: 0.018
|
||||||
|
rr_wheel: 0.0
|
||||||
@@ -0,0 +1,80 @@
|
|||||||
|
/**:
|
||||||
|
ros__parameters:
|
||||||
|
policy_hz: 50.0
|
||||||
|
motor_hz: 200.0
|
||||||
|
status_hz: 10.0
|
||||||
|
target_timeout_ms: 150.0
|
||||||
|
model_path: policies/model_rough.onnx
|
||||||
|
use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
|
||||||
|
contract_file: deployment_contract.yaml
|
||||||
|
dry_run: false
|
||||||
|
can0_name: "can0"
|
||||||
|
can1_name: "can1"
|
||||||
|
imu_topic: "/odin1/imu"
|
||||||
|
odom_topic: "/odom"
|
||||||
|
|
||||||
|
# Remote UART / SBUS parameters, aligned with the Python deployment
|
||||||
|
remote_enabled: true
|
||||||
|
remote_port: "/dev/ttyACM0"
|
||||||
|
remote_baudrate: 100000
|
||||||
|
remote_timeout: 0.02
|
||||||
|
remote_axis_deadzone: 40
|
||||||
|
remote_active_threshold: 40
|
||||||
|
remote_axis_full_scale: 660.0
|
||||||
|
remote_max_vx: 0.8
|
||||||
|
remote_max_vy: 0.3
|
||||||
|
remote_max_yaw_rate: 0.5
|
||||||
|
remote_invert_vx: true
|
||||||
|
remote_invert_vy: false
|
||||||
|
remote_invert_yaw: true
|
||||||
|
remote_publish_inactive_zero: true
|
||||||
|
remote_estop_latch: true
|
||||||
|
remote_poll_hz: 50.0
|
||||||
|
|
||||||
|
# Command mux parameters
|
||||||
|
cmd_mux_default_mode: "REMOTE"
|
||||||
|
cmd_mux_output_hz: 50.0
|
||||||
|
cmd_mux_remote_timeout_ms: 250.0
|
||||||
|
cmd_mux_web_timeout_ms: 300.0
|
||||||
|
cmd_mux_nav_timeout_ms: 500.0
|
||||||
|
cmd_mux_max_vx: 0.8
|
||||||
|
cmd_mux_max_vy: 0.3
|
||||||
|
cmd_mux_max_yaw_rate: 0.5
|
||||||
|
cmd_mux_max_vx_acc: 1.0
|
||||||
|
cmd_mux_max_vy_acc: 1.0
|
||||||
|
cmd_mux_max_yaw_acc: 1.5
|
||||||
|
|
||||||
|
# Windows/Nano Web UDP bridge parameters
|
||||||
|
web_bridge_enabled: true
|
||||||
|
web_udp_listen_host: "0.0.0.0"
|
||||||
|
web_udp_listen_port: 15000
|
||||||
|
web_udp_remote_host: ""
|
||||||
|
web_udp_remote_port: 15001
|
||||||
|
web_udp_state_hz: 20.0
|
||||||
|
web_udp_cmd_timeout_ms: 300.0
|
||||||
|
web_udp_max_packet_bytes: 8192
|
||||||
|
web_udp_max_vx: 0.8
|
||||||
|
web_udp_max_vy: 0.3
|
||||||
|
web_udp_max_yaw_rate: 0.5
|
||||||
|
web_udp_estop_on_timeout: false
|
||||||
|
|
||||||
|
# Safety parameters
|
||||||
|
safety_enabled: true
|
||||||
|
max_target_offset: 0.6
|
||||||
|
hard_target_offset: 2.0
|
||||||
|
max_ang_vel: 10.0
|
||||||
|
max_tilt_z: -0.3
|
||||||
|
clip_to_brake: 0
|
||||||
|
imu_age_warn_ms: 60.0
|
||||||
|
imu_age_stop_ms: 200.0
|
||||||
|
|
||||||
|
# Policy alignment with the Python deployment
|
||||||
|
command_release_s: 0.35
|
||||||
|
release_command_hold_s: 0.12
|
||||||
|
release_posture_max_err: 0.35
|
||||||
|
release_target_blend_s: 0.30
|
||||||
|
clip_obs: 100.0
|
||||||
|
hold_zero_command_pose: true
|
||||||
|
enable_zero_cmd_suppression: true
|
||||||
|
require_active_command_to_release: true
|
||||||
|
zero_cmd_use_yaw_rate: true
|
||||||
@@ -0,0 +1,129 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||||
|
from launch.conditions import IfCondition
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.parameter_descriptions import ParameterFile
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
runtime_params = ParameterFile(
|
||||||
|
PathJoinSubstitution([
|
||||||
|
FindPackageShare("sim2real_bringup"),
|
||||||
|
"config",
|
||||||
|
"runtime.yaml",
|
||||||
|
]),
|
||||||
|
allow_substs=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
# Declare launch configurations
|
||||||
|
launch_driver_arg = DeclareLaunchArgument(
|
||||||
|
'launch_driver',
|
||||||
|
default_value='true',
|
||||||
|
description='Whether to launch the odin_ros_driver sensor node'
|
||||||
|
)
|
||||||
|
|
||||||
|
launch_nav2_arg = DeclareLaunchArgument(
|
||||||
|
'launch_nav2',
|
||||||
|
default_value='true',
|
||||||
|
description='Whether to launch the Nav2 navigation stack'
|
||||||
|
)
|
||||||
|
|
||||||
|
launch_remote_arg = DeclareLaunchArgument(
|
||||||
|
'launch_remote',
|
||||||
|
default_value='true',
|
||||||
|
description='Whether to launch the SBUS UART remote control node'
|
||||||
|
)
|
||||||
|
|
||||||
|
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||||
|
'launch_web_bridge',
|
||||||
|
default_value='true',
|
||||||
|
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||||
|
)
|
||||||
|
|
||||||
|
# Include odin_ros_driver launch
|
||||||
|
driver_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
PathJoinSubstitution([
|
||||||
|
FindPackageShare('odin_ros_driver'),
|
||||||
|
'launch',
|
||||||
|
'odin1_ros2.launch.py'
|
||||||
|
])
|
||||||
|
),
|
||||||
|
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||||
|
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||||
|
)
|
||||||
|
|
||||||
|
# Include sim2real_nav2 launch
|
||||||
|
nav2_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
PathJoinSubstitution([
|
||||||
|
FindPackageShare('sim2real_nav2'),
|
||||||
|
'launch',
|
||||||
|
'nav2.launch.py'
|
||||||
|
])
|
||||||
|
),
|
||||||
|
condition=IfCondition(LaunchConfiguration('launch_nav2'))
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
launch_driver_arg,
|
||||||
|
launch_nav2_arg,
|
||||||
|
launch_remote_arg,
|
||||||
|
launch_web_bridge_arg,
|
||||||
|
Node(
|
||||||
|
package="sim2real_hw",
|
||||||
|
executable="sim2real_hw_node",
|
||||||
|
name="sim2real_hw_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[runtime_params],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="sim2real_runtime",
|
||||||
|
executable="sim2real_runtime_node",
|
||||||
|
name="sim2real_runtime_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[runtime_params],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="sim2real_runtime",
|
||||||
|
executable="cmd_mux_node.py",
|
||||||
|
name="sim2real_cmd_mux_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[runtime_params],
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="sim2real_runtime",
|
||||||
|
executable="web_udp_bridge_node.py",
|
||||||
|
name="sim2real_web_udp_bridge_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[runtime_params],
|
||||||
|
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="sim2real_runtime",
|
||||||
|
executable="remote_uart_node.py",
|
||||||
|
name="sim2real_remote_uart_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[runtime_params],
|
||||||
|
condition=IfCondition(LaunchConfiguration('launch_remote')),
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="sim2real_runtime",
|
||||||
|
executable="odom_relay_node",
|
||||||
|
name="odom_relay_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[{
|
||||||
|
"odom_input_topic": "/odin1/odometry",
|
||||||
|
"odom_output_topic": "/odom",
|
||||||
|
"base_frame": "base_link",
|
||||||
|
"publish_tf": True,
|
||||||
|
}],
|
||||||
|
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||||
|
),
|
||||||
|
driver_launch,
|
||||||
|
nav2_launch,
|
||||||
|
])
|
||||||
|
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_bringup</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Launch and configuration package for sim2real_ros2.</description>
|
||||||
|
<maintainer email="todo@example.com">todo</maintainer>
|
||||||
|
<license>Proprietary</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>launch</exec_depend>
|
||||||
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
<exec_depend>sim2real_common</exec_depend>
|
||||||
|
<exec_depend>sim2real_hw</exec_depend>
|
||||||
|
<exec_depend>sim2real_interfaces</exec_depend>
|
||||||
|
<exec_depend>sim2real_runtime</exec_depend>
|
||||||
|
<exec_depend>sim2real_nav2</exec_depend>
|
||||||
|
<exec_depend>odin_ros_driver</exec_depend>
|
||||||
|
<exec_depend>tf2_ros</exec_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(sim2real_common)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
add_library(${PROJECT_NAME} INTERFACE)
|
||||||
|
target_include_directories(${PROJECT_NAME} INTERFACE
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY include/
|
||||||
|
DESTINATION include
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
TARGETS ${PROJECT_NAME}
|
||||||
|
EXPORT export_${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||||
|
ament_package()
|
||||||
+94
@@ -0,0 +1,94 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <cstddef>
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
struct DeploymentContract
|
||||||
|
{
|
||||||
|
static constexpr std::size_t kObsDim = 53;
|
||||||
|
static constexpr std::size_t kActionDim = 16;
|
||||||
|
static constexpr std::size_t kLegJointCount = 12;
|
||||||
|
static constexpr std::size_t kWheelCount = 4;
|
||||||
|
static constexpr double kPolicyHz = 50.0;
|
||||||
|
static constexpr double kMotorHz = 200.0;
|
||||||
|
static constexpr double kStatusHz = 10.0;
|
||||||
|
|
||||||
|
static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
|
||||||
|
static constexpr float kLegKp = 50.0f;
|
||||||
|
static constexpr float kLegKd = 1.5f;
|
||||||
|
static constexpr float kLegHoldKp = 80.0f;
|
||||||
|
static constexpr float kLegHoldKd = 4.0f;
|
||||||
|
static constexpr float kWheelKd = 1.0f;
|
||||||
|
|
||||||
|
static constexpr std::array<int, 16> kCanBusMap = {
|
||||||
|
1, 1, 1, // fl legs
|
||||||
|
1, 1, 1, // fr legs
|
||||||
|
2, 2, 2, // rl legs
|
||||||
|
2, 2, 2, // rr legs
|
||||||
|
1, 1, 2, 2 // wheels: fl, fr, rl, rr
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<int, 16> kCanIdMap = {
|
||||||
|
1, 2, 3, // fl legs
|
||||||
|
5, 6, 7, // fr legs
|
||||||
|
1, 2, 3, // rl legs
|
||||||
|
5, 6, 7, // rr legs
|
||||||
|
4, 8, 4, 8 // wheels: fl, fr, rl, rr
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<float, 16> kDirectionMap = {
|
||||||
|
-1.0f, -1.0f, -1.0f, // fl
|
||||||
|
-1.0f, 1.0f, 1.0f, // fr
|
||||||
|
1.0f, -1.0f, -1.0f, // rl
|
||||||
|
1.0f, 1.0f, 1.0f, // rr
|
||||||
|
-1.0f, 1.0f, -1.0f, 1.0f // wheels
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<float, 16> kZeroOffsetMap = {
|
||||||
|
0.003f, 0.030f, 0.028f, // fl
|
||||||
|
0.004f, 0.038f, 0.011f, // fr
|
||||||
|
0.019f, -0.034f, 0.025f, // rl
|
||||||
|
-0.001f, 0.039f, 0.018f, // rr
|
||||||
|
0.000f, 0.000f, 0.000f, 0.000f // wheels
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<float, 16> kActionScale = {
|
||||||
|
0.125f, 0.25f, 0.25f,
|
||||||
|
0.125f, 0.25f, 0.25f,
|
||||||
|
0.125f, 0.25f, 0.25f,
|
||||||
|
0.125f, 0.25f, 0.25f,
|
||||||
|
5.0f, 5.0f, 5.0f, 5.0f
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||||
|
0.0f, 0.9f, -1.8f,
|
||||||
|
0.0f, 0.9f, -1.8f,
|
||||||
|
0.0f, 0.9f, -1.8f,
|
||||||
|
0.0f, 0.9f, -1.8f,
|
||||||
|
0.0f, 0.0f, 0.0f, 0.0f
|
||||||
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
static constexpr std::array<const char *, 16> kJointLabels = {
|
||||||
|
"fl_hip_abduction",
|
||||||
|
"fl_hip_pitch",
|
||||||
|
"fl_knee",
|
||||||
|
"fr_hip_abduction",
|
||||||
|
"fr_hip_pitch",
|
||||||
|
"fr_knee",
|
||||||
|
"rl_hip_abduction",
|
||||||
|
"rl_hip_pitch",
|
||||||
|
"rl_knee",
|
||||||
|
"rr_hip_abduction",
|
||||||
|
"rr_hip_pitch",
|
||||||
|
"rr_knee",
|
||||||
|
"fl_wheel",
|
||||||
|
"fr_wheel",
|
||||||
|
"rl_wheel",
|
||||||
|
"rr_wheel"
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
+55
@@ -0,0 +1,55 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#ifndef M_PI
|
||||||
|
#define M_PI 3.14159265358979323846
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
class LowPassFilter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LowPassFilter(double cutoff_freq, double dt, std::size_t dim)
|
||||||
|
: dim_(dim), initialized_(false)
|
||||||
|
{
|
||||||
|
alpha_ = static_cast<float>(1.0 - std::exp(-2.0 * M_PI * cutoff_freq * dt));
|
||||||
|
y_prev_.resize(dim, 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
void filter(const float* x, float* y)
|
||||||
|
{
|
||||||
|
if (!initialized_) {
|
||||||
|
for (std::size_t i = 0; i < dim_; ++i) {
|
||||||
|
y_prev_[i] = x[i];
|
||||||
|
}
|
||||||
|
initialized_ = true;
|
||||||
|
}
|
||||||
|
for (std::size_t i = 0; i < dim_; ++i) {
|
||||||
|
y[i] = alpha_ * x[i] + (1.0f - alpha_) * y_prev_[i];
|
||||||
|
y_prev_[i] = y[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void filter(const std::vector<float>& x, std::vector<float>& y)
|
||||||
|
{
|
||||||
|
filter(x.data(), y.data());
|
||||||
|
}
|
||||||
|
|
||||||
|
void reset()
|
||||||
|
{
|
||||||
|
initialized_ = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
float alpha_;
|
||||||
|
std::size_t dim_;
|
||||||
|
bool initialized_;
|
||||||
|
std::vector<float> y_prev_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
+152
@@ -0,0 +1,152 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
// Helper to calculate gravity orientation from quaternion [w, x, y, z]
|
||||||
|
inline std::array<float, 3> get_gravity_orientation(const std::array<float, 4>& quat_wxyz)
|
||||||
|
{
|
||||||
|
float qw = quat_wxyz[0];
|
||||||
|
float qx = quat_wxyz[1];
|
||||||
|
float qy = quat_wxyz[2];
|
||||||
|
float qz = quat_wxyz[3];
|
||||||
|
|
||||||
|
float gx = 2.0f * (-qz * qx + qw * qy);
|
||||||
|
float gy = -2.0f * (qz * qy + qw * qx);
|
||||||
|
float gz = 1.0f - 2.0f * (qw * qw + qz * qz);
|
||||||
|
return {gx, gy, gz};
|
||||||
|
}
|
||||||
|
|
||||||
|
// Helper to create quaternion from acceleration vector
|
||||||
|
inline std::array<float, 4> quat_from_accel(const std::array<float, 3>& accel)
|
||||||
|
{
|
||||||
|
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||||
|
if (norm_a < 1e-9f) {
|
||||||
|
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
|
||||||
|
float ax = accel[0] / norm_a;
|
||||||
|
float ay = accel[1] / norm_a;
|
||||||
|
float az = accel[2] / norm_a;
|
||||||
|
|
||||||
|
// Ref gravity vector is [0.0, 0.0, 1.0]
|
||||||
|
float cross_x = -ay;
|
||||||
|
float cross_y = ax;
|
||||||
|
float cross_z = 0.0f;
|
||||||
|
float dot = az;
|
||||||
|
|
||||||
|
if (dot < -0.999999f) {
|
||||||
|
return {0.0f, 1.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
|
||||||
|
float s = std::sqrt((1.0f + dot) * 2.0f);
|
||||||
|
std::array<float, 4> q = {
|
||||||
|
s * 0.5f,
|
||||||
|
cross_x / s,
|
||||||
|
cross_y / s,
|
||||||
|
cross_z / s
|
||||||
|
};
|
||||||
|
|
||||||
|
float norm_q = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||||
|
if (norm_q < 1e-9f) {
|
||||||
|
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
q[0] /= norm_q;
|
||||||
|
q[1] /= norm_q;
|
||||||
|
q[2] /= norm_q;
|
||||||
|
q[3] /= norm_q;
|
||||||
|
|
||||||
|
return q;
|
||||||
|
}
|
||||||
|
|
||||||
|
class MahonyFilter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
MahonyFilter(float kp = 2.0f, float ki = 0.0f)
|
||||||
|
: kp_(kp), ki_(ki)
|
||||||
|
{
|
||||||
|
q_ = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
|
||||||
|
void reset_with_accel(const std::array<float, 3>& accel)
|
||||||
|
{
|
||||||
|
q_ = quat_from_accel(accel);
|
||||||
|
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<float, 4> update(const std::array<float, 3>& accel, const std::array<float, 3>& gyro, float dt)
|
||||||
|
{
|
||||||
|
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||||
|
std::array<float, 3> gyro_corr = gyro;
|
||||||
|
|
||||||
|
if (norm_a > 1e-6f) {
|
||||||
|
float ax = accel[0] / norm_a;
|
||||||
|
float ay = accel[1] / norm_a;
|
||||||
|
float az = accel[2] / norm_a;
|
||||||
|
|
||||||
|
float qw = q_[0];
|
||||||
|
float qx = q_[1];
|
||||||
|
float qy = q_[2];
|
||||||
|
float qz = q_[3];
|
||||||
|
|
||||||
|
float vx = 2.0f * (qx * qz - qw * qy);
|
||||||
|
float vy = 2.0f * (qw * qx + qy * qz);
|
||||||
|
float vz = qw * qw - qx * qx - qy * qy + qz * qz;
|
||||||
|
|
||||||
|
// Error = cross(a, v)
|
||||||
|
float ex = ay * vz - az * vy;
|
||||||
|
float ey = az * vx - ax * vz;
|
||||||
|
float ez = ax * vy - ay * vx;
|
||||||
|
|
||||||
|
if (ki_ > 0.0f) {
|
||||||
|
e_int_[0] += ex * dt;
|
||||||
|
e_int_[1] += ey * dt;
|
||||||
|
e_int_[2] += ez * dt;
|
||||||
|
} else {
|
||||||
|
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||||
|
}
|
||||||
|
|
||||||
|
gyro_corr[0] += kp_ * ex + ki_ * e_int_[0];
|
||||||
|
gyro_corr[1] += kp_ * ey + ki_ * e_int_[1];
|
||||||
|
gyro_corr[2] += kp_ * ez + ki_ * e_int_[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
float qw = q_[0];
|
||||||
|
float qx = q_[1];
|
||||||
|
float qy = q_[2];
|
||||||
|
float qz = q_[3];
|
||||||
|
|
||||||
|
float q_dot_w = 0.5f * (-qx * gyro_corr[0] - qy * gyro_corr[1] - qz * gyro_corr[2]);
|
||||||
|
float q_dot_x = 0.5f * ( qw * gyro_corr[0] + qy * gyro_corr[2] - qz * gyro_corr[1]);
|
||||||
|
float q_dot_y = 0.5f * ( qw * gyro_corr[1] - qx * gyro_corr[2] + qz * gyro_corr[0]);
|
||||||
|
float q_dot_z = 0.5f * ( qw * gyro_corr[2] + qx * gyro_corr[1] - qy * gyro_corr[0]);
|
||||||
|
|
||||||
|
q_[0] += q_dot_w * dt;
|
||||||
|
q_[1] += q_dot_x * dt;
|
||||||
|
q_[2] += q_dot_y * dt;
|
||||||
|
q_[3] += q_dot_z * dt;
|
||||||
|
|
||||||
|
float norm_q = std::sqrt(q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3]) + 1e-9f;
|
||||||
|
q_[0] /= norm_q;
|
||||||
|
q_[1] /= norm_q;
|
||||||
|
q_[2] /= norm_q;
|
||||||
|
q_[3] /= norm_q;
|
||||||
|
|
||||||
|
return q_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::array<float, 4>& get_q() const { return q_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
float kp_;
|
||||||
|
float ki_;
|
||||||
|
std::array<float, 4> q_;
|
||||||
|
std::array<float, 3> e_int_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
+114
@@ -0,0 +1,114 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
enum class GuardLevel : int {
|
||||||
|
OK = 0,
|
||||||
|
WARN = 1,
|
||||||
|
STOP = 2
|
||||||
|
};
|
||||||
|
|
||||||
|
struct GuardDecision {
|
||||||
|
GuardLevel level{GuardLevel::OK};
|
||||||
|
std::string reason;
|
||||||
|
};
|
||||||
|
|
||||||
|
class RuntimeGuard {
|
||||||
|
public:
|
||||||
|
RuntimeGuard(
|
||||||
|
float max_ang_vel = 12.0f,
|
||||||
|
float max_tilt_z = -0.30f,
|
||||||
|
float imu_age_warn_ms = 60.0f,
|
||||||
|
float imu_age_stop_ms = 200.0f)
|
||||||
|
: max_ang_vel_(max_ang_vel),
|
||||||
|
max_tilt_z_(max_tilt_z),
|
||||||
|
imu_age_warn_ms_(imu_age_warn_ms),
|
||||||
|
imu_age_stop_ms_(imu_age_stop_ms)
|
||||||
|
{}
|
||||||
|
|
||||||
|
GuardDecision check(
|
||||||
|
const std::array<float, 3>& imu_gyro,
|
||||||
|
const std::array<float, 3>& projected_gravity,
|
||||||
|
float imu_age_ms,
|
||||||
|
bool estop_triggered,
|
||||||
|
const std::vector<float>& extra_vals = {})
|
||||||
|
{
|
||||||
|
GuardDecision decision;
|
||||||
|
|
||||||
|
// 1) user E-stop
|
||||||
|
if (estop_triggered) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "user E-stop";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2) NaN/Inf check
|
||||||
|
for (float v : imu_gyro) {
|
||||||
|
if (std::isnan(v) || std::isinf(v)) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "NaN/Inf detected in imu_gyro";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (float v : projected_gravity) {
|
||||||
|
if (std::isnan(v) || std::isinf(v)) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "NaN/Inf detected in projected_gravity";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (float v : extra_vals) {
|
||||||
|
if (std::isnan(v) || std::isinf(v)) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "NaN/Inf detected in checked values";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3) IMU stale
|
||||||
|
if (imu_age_ms > imu_age_stop_ms_) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "IMU stale " + std::to_string(imu_age_ms) + "ms";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
bool warned_imu = (imu_age_ms > imu_age_warn_ms_);
|
||||||
|
|
||||||
|
// 4) Tilt check
|
||||||
|
if (projected_gravity[2] > max_tilt_z_) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "tilt: g_z=" + std::to_string(projected_gravity[2]);
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 5) Angular velocity check
|
||||||
|
float ang_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||||
|
if (ang_norm > max_ang_vel_) {
|
||||||
|
decision.level = GuardLevel::STOP;
|
||||||
|
decision.reason = "ang_vel overflow: |w|=" + std::to_string(ang_norm);
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (warned_imu) {
|
||||||
|
decision.level = GuardLevel::WARN;
|
||||||
|
decision.reason = "IMU age " + std::to_string(imu_age_ms) + "ms";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
decision.level = GuardLevel::OK;
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
float max_ang_vel_;
|
||||||
|
float max_tilt_z_;
|
||||||
|
float imu_age_warn_ms_;
|
||||||
|
float imu_age_stop_ms_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
+119
@@ -0,0 +1,119 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <string>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
enum class SafetyLevel : int {
|
||||||
|
NORMAL = 0,
|
||||||
|
CLIP = 1,
|
||||||
|
BRAKE = 2,
|
||||||
|
ESTOP = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
struct SafetyDecision {
|
||||||
|
SafetyLevel level{SafetyLevel::NORMAL};
|
||||||
|
std::string message;
|
||||||
|
std::array<float, 16> clipped_target{};
|
||||||
|
};
|
||||||
|
|
||||||
|
class SafetyMonitor {
|
||||||
|
public:
|
||||||
|
SafetyMonitor(
|
||||||
|
float max_target_offset = 0.6f,
|
||||||
|
float max_ang_vel = 10.0f,
|
||||||
|
float max_tilt_z = -0.3f,
|
||||||
|
int clip_to_brake = 0,
|
||||||
|
float hard_target_offset = 1.2f)
|
||||||
|
: max_target_offset_(max_target_offset),
|
||||||
|
max_ang_vel_(max_ang_vel),
|
||||||
|
max_tilt_z_(max_tilt_z),
|
||||||
|
clip_to_brake_(clip_to_brake),
|
||||||
|
hard_target_offset_(hard_target_offset),
|
||||||
|
consecutive_clips_(0)
|
||||||
|
{}
|
||||||
|
|
||||||
|
SafetyDecision check(
|
||||||
|
const std::array<float, 16>& target_pose,
|
||||||
|
const std::array<float, 16>& default_pose,
|
||||||
|
const std::array<float, 3>& imu_gyro,
|
||||||
|
const std::array<float, 3>& projected_gravity,
|
||||||
|
bool estop_triggered)
|
||||||
|
{
|
||||||
|
SafetyDecision decision;
|
||||||
|
decision.clipped_target = target_pose;
|
||||||
|
|
||||||
|
if (estop_triggered) {
|
||||||
|
decision.level = SafetyLevel::ESTOP;
|
||||||
|
decision.message = "user E-stop";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Tilt check (g_z should be ~ -1.0, if it is > max_tilt_z e.g. -0.3, it is tilted)
|
||||||
|
if (projected_gravity[2] > max_tilt_z_) {
|
||||||
|
decision.level = SafetyLevel::BRAKE;
|
||||||
|
decision.message = "tilt detected: g_z=" + std::to_string(projected_gravity[2]);
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Angular velocity norm check
|
||||||
|
float ang_vel_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||||
|
if (ang_vel_norm > max_ang_vel_) {
|
||||||
|
decision.level = SafetyLevel::BRAKE;
|
||||||
|
decision.message = "angular velocity overflow: |w|=" + std::to_string(ang_vel_norm);
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Offset check
|
||||||
|
bool needs_clip = false;
|
||||||
|
float max_offset = 0.0f;
|
||||||
|
for (std::size_t i = 0; i < 12; ++i) { // check leg joint offsets from default pose
|
||||||
|
float offset = target_pose[i] - default_pose[i];
|
||||||
|
max_offset = std::max(max_offset, std::abs(offset));
|
||||||
|
if (std::abs(offset) > max_target_offset_) {
|
||||||
|
needs_clip = true;
|
||||||
|
float clipped_val = std::clamp(offset, -max_target_offset_, max_target_offset_);
|
||||||
|
decision.clipped_target[i] = default_pose[i] + clipped_val;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (needs_clip) {
|
||||||
|
consecutive_clips_++;
|
||||||
|
if (hard_target_offset_ > 0.0f && max_offset > hard_target_offset_) {
|
||||||
|
decision.level = SafetyLevel::BRAKE;
|
||||||
|
decision.message = "target leg offset exceeds hard limit: " + std::to_string(max_offset);
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
if (clip_to_brake_ > 0 && consecutive_clips_ >= clip_to_brake_) {
|
||||||
|
decision.level = SafetyLevel::BRAKE;
|
||||||
|
decision.message = "clipped " + std::to_string(consecutive_clips_) + " frames in a row";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
decision.level = SafetyLevel::CLIP;
|
||||||
|
decision.message = "target leg offset out of range";
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
consecutive_clips_ = 0;
|
||||||
|
decision.level = SafetyLevel::NORMAL;
|
||||||
|
return decision;
|
||||||
|
}
|
||||||
|
|
||||||
|
void reset() {
|
||||||
|
consecutive_clips_ = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
float max_target_offset_;
|
||||||
|
float max_ang_vel_;
|
||||||
|
float max_tilt_z_;
|
||||||
|
int clip_to_brake_;
|
||||||
|
float hard_target_offset_;
|
||||||
|
int consecutive_clips_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
+127
@@ -0,0 +1,127 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <vector>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#ifndef M_PI
|
||||||
|
#define M_PI 3.14159265358979323846
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace sim2real_common
|
||||||
|
{
|
||||||
|
|
||||||
|
class StandBalanceController
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
StandBalanceController(double control_dt = 0.02)
|
||||||
|
: control_dt_(control_dt)
|
||||||
|
{
|
||||||
|
profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f};
|
||||||
|
profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f};
|
||||||
|
profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f};
|
||||||
|
reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
void reset()
|
||||||
|
{
|
||||||
|
stable_time_ = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<float, 16> computeTarget(
|
||||||
|
const std::array<float, 3>& projected_gravity,
|
||||||
|
const std::array<float, 3>& imu_gyro,
|
||||||
|
const std::array<float, 3>& cmd)
|
||||||
|
{
|
||||||
|
float hip_base = 0.9f;
|
||||||
|
float knee_base = -1.8f;
|
||||||
|
estimateBaseLegPose(hip_base, knee_base);
|
||||||
|
|
||||||
|
float roll = 0.0f;
|
||||||
|
float pitch = 0.0f;
|
||||||
|
estimateRollPitch(projected_gravity, roll, pitch);
|
||||||
|
|
||||||
|
float roll_rate = imu_gyro[0];
|
||||||
|
float pitch_rate = imu_gyro[1];
|
||||||
|
|
||||||
|
float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate;
|
||||||
|
|
||||||
|
float lateral_lean = lateral_lean_gain_ * cmd[1];
|
||||||
|
|
||||||
|
std::array<float, 16> target{};
|
||||||
|
for (int leg_idx = 0; leg_idx < 4; ++leg_idx) {
|
||||||
|
float side = (leg_idx == 0 || leg_idx == 2) ? 1.0f : -1.0f;
|
||||||
|
|
||||||
|
target[leg_idx * 3 + 0] = std::clamp(side * roll_corr + lateral_lean, -hip_abduction_clip_, hip_abduction_clip_);
|
||||||
|
target[leg_idx * 3 + 1] = std::clamp(hip_base, hip_pitch_clip_[0], hip_pitch_clip_[1]);
|
||||||
|
target[leg_idx * 3 + 2] = std::clamp(knee_base, knee_clip_[0], knee_clip_[1]);
|
||||||
|
}
|
||||||
|
// wheels 0
|
||||||
|
target[12] = target[13] = target[14] = target[15] = 0.0f;
|
||||||
|
|
||||||
|
bool stable = (std::abs(roll * 180.0f / static_cast<float>(M_PI)) <= stable_roll_deg_) &&
|
||||||
|
(std::abs(pitch * 180.0f / static_cast<float>(M_PI)) <= stable_pitch_deg_) &&
|
||||||
|
(std::max(std::abs(roll_rate * 180.0f / static_cast<float>(M_PI)), std::abs(pitch_rate * 180.0f / static_cast<float>(M_PI))) <= stable_gyro_deg_s_);
|
||||||
|
|
||||||
|
stable_time_ = stable ? (stable_time_ + static_cast<float>(control_dt_)) : 0.0f;
|
||||||
|
|
||||||
|
return target;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isStable() const
|
||||||
|
{
|
||||||
|
return stable_time_ >= enter_hold_s_;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
void estimateRollPitch(const std::array<float, 3>& projected_gravity, float& roll, float& pitch)
|
||||||
|
{
|
||||||
|
float gx = projected_gravity[0];
|
||||||
|
float gy = projected_gravity[1];
|
||||||
|
float gz = projected_gravity[2];
|
||||||
|
roll = std::atan2(-gy, std::max(1e-6f, -gz));
|
||||||
|
pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz)));
|
||||||
|
}
|
||||||
|
|
||||||
|
void estimateBaseLegPose(float& hip, float& knee)
|
||||||
|
{
|
||||||
|
float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back());
|
||||||
|
hip = interpolate(h_clamp, profile_h_, profile_hip_);
|
||||||
|
knee = interpolate(h_clamp, profile_h_, profile_knee_);
|
||||||
|
}
|
||||||
|
|
||||||
|
float interpolate(float x, const std::vector<float>& xp, const std::vector<float>& fp)
|
||||||
|
{
|
||||||
|
if (x <= xp.front()) return fp.front();
|
||||||
|
if (x >= xp.back()) return fp.back();
|
||||||
|
for (std::size_t i = 0; i < xp.size() - 1; ++i) {
|
||||||
|
if (x >= xp[i] && x <= xp[i+1]) {
|
||||||
|
float f = (x - xp[i]) / (xp[i+1] - xp[i]);
|
||||||
|
return fp[i] + f * (fp[i+1] - fp[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return fp.back();
|
||||||
|
}
|
||||||
|
|
||||||
|
double control_dt_;
|
||||||
|
float height_{0.33f};
|
||||||
|
float kp_roll_{0.85f};
|
||||||
|
float kd_roll_rate_{0.03f};
|
||||||
|
float lateral_lean_gain_{0.0f};
|
||||||
|
float hip_abduction_clip_{0.45f};
|
||||||
|
std::array<float, 2> hip_pitch_clip_{-1.0f, 2.5f};
|
||||||
|
std::array<float, 2> knee_clip_{-2.6f, -0.3f};
|
||||||
|
float stable_roll_deg_{6.0f};
|
||||||
|
float stable_pitch_deg_{8.0f};
|
||||||
|
float stable_gyro_deg_s_{45.0f};
|
||||||
|
float enter_hold_s_{1.0f};
|
||||||
|
|
||||||
|
std::vector<float> profile_h_;
|
||||||
|
std::vector<float> profile_hip_;
|
||||||
|
std::vector<float> profile_knee_;
|
||||||
|
|
||||||
|
float stable_time_{0.0f};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_common
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_common</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Shared constants and deployment contract helpers for sim2real_ros2.</description>
|
||||||
|
<maintainer email="todo@example.com">todo</maintainer>
|
||||||
|
<license>Proprietary</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,37 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(sim2real_hw)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(sensor_msgs REQUIRED)
|
||||||
|
find_package(std_msgs REQUIRED)
|
||||||
|
find_package(nav_msgs REQUIRED)
|
||||||
|
find_package(sim2real_common REQUIRED)
|
||||||
|
find_package(sim2real_interfaces REQUIRED)
|
||||||
|
|
||||||
|
add_executable(sim2real_hw_node
|
||||||
|
src/hardware_bridge_node.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(sim2real_hw_node PRIVATE include)
|
||||||
|
target_compile_features(sim2real_hw_node PRIVATE cxx_std_17)
|
||||||
|
ament_target_dependencies(sim2real_hw_node
|
||||||
|
rclcpp
|
||||||
|
sensor_msgs
|
||||||
|
std_msgs
|
||||||
|
nav_msgs
|
||||||
|
sim2real_common
|
||||||
|
sim2real_interfaces
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY include/
|
||||||
|
DESTINATION include
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
TARGETS sim2real_hw_node
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
+156
@@ -0,0 +1,156 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <mutex>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "sensor_msgs/msg/imu.hpp"
|
||||||
|
#include "nav_msgs/msg/odometry.hpp"
|
||||||
|
#include "std_msgs/msg/bool.hpp"
|
||||||
|
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||||
|
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||||
|
#include "sim2real_common/low_pass_filter.hpp"
|
||||||
|
#include "sim2real_common/mahony_filter.hpp"
|
||||||
|
#include "sim2real_common/safety_monitor.hpp"
|
||||||
|
#include "sim2real_common/runtime_guard.hpp"
|
||||||
|
|
||||||
|
namespace sim2real_hw
|
||||||
|
{
|
||||||
|
|
||||||
|
struct MotorConfig
|
||||||
|
{
|
||||||
|
int bus; // 1 or 2
|
||||||
|
int id; // motor CAN id
|
||||||
|
float direction;
|
||||||
|
float offset;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct MotorStateInternal
|
||||||
|
{
|
||||||
|
float position{0.0f};
|
||||||
|
float velocity{0.0f};
|
||||||
|
float torque{0.0f};
|
||||||
|
float temperature{0.0f};
|
||||||
|
std::uint32_t update_count{0};
|
||||||
|
std::uint32_t stale_count{0};
|
||||||
|
// Hold-over state
|
||||||
|
float last_valid_pos{0.0f};
|
||||||
|
float last_valid_vel{0.0f};
|
||||||
|
float last_valid_torque{0.0f};
|
||||||
|
std::uint32_t prev_update_count{0};
|
||||||
|
bool has_valid_data{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
class HardwareBridgeNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
HardwareBridgeNode();
|
||||||
|
~HardwareBridgeNode();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||||
|
void onReadLoop();
|
||||||
|
void onWriteLoop();
|
||||||
|
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||||
|
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||||
|
|
||||||
|
bool initCan(const std::string& ifname, int& fd);
|
||||||
|
bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc);
|
||||||
|
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||||
|
|
||||||
|
bool enableMotor(int fd, int motor_id);
|
||||||
|
bool disableMotor(int fd, int motor_id);
|
||||||
|
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||||
|
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||||
|
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||||
|
|
||||||
|
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||||
|
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||||
|
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||||
|
|
||||||
|
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||||
|
rclcpp::TimerBase::SharedPtr write_timer_;
|
||||||
|
|
||||||
|
std::mutex target_mutex_;
|
||||||
|
std::array<float, 16> latest_target_{};
|
||||||
|
std::array<float, 16> latest_raw_action_{};
|
||||||
|
std::string latest_target_source_{"boot_hold"};
|
||||||
|
rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME};
|
||||||
|
std::uint32_t target_sequence_{0};
|
||||||
|
std::uint32_t state_sequence_{0};
|
||||||
|
double target_timeout_ms_{150.0};
|
||||||
|
|
||||||
|
// SocketCAN file descriptors
|
||||||
|
int can0_fd_{-1};
|
||||||
|
int can1_fd_{-1};
|
||||||
|
std::string can0_name_{"can0"};
|
||||||
|
std::string can1_name_{"can1"};
|
||||||
|
|
||||||
|
// CAN error recovery
|
||||||
|
static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit
|
||||||
|
int can0_error_count_{0};
|
||||||
|
int can1_error_count_{0};
|
||||||
|
bool reinitCan(const std::string& ifname, int& fd, int& error_count);
|
||||||
|
|
||||||
|
// Hold-over constants
|
||||||
|
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||||
|
|
||||||
|
// Motor configurations and states
|
||||||
|
std::array<MotorConfig, 16> motors_;
|
||||||
|
std::array<MotorStateInternal, 16> motor_states_;
|
||||||
|
|
||||||
|
// IMU state
|
||||||
|
std::mutex imu_mutex_;
|
||||||
|
std::array<float, 3> imu_gyro_{};
|
||||||
|
std::array<float, 3> imu_accel_{};
|
||||||
|
std::array<float, 3> projected_gravity_{0.0f, 0.0f, -1.0f};
|
||||||
|
bool imu_fresh_{false};
|
||||||
|
rclcpp::Time last_imu_stamp_{0, 0, RCL_ROS_TIME};
|
||||||
|
std::chrono::steady_clock::time_point last_imu_recv_time_{};
|
||||||
|
bool has_received_imu_{false};
|
||||||
|
std::array<float, 3> imu_gravity_sum_{0.0f, 0.0f, 0.0f};
|
||||||
|
std::uint32_t imu_gravity_sample_count_{0};
|
||||||
|
static constexpr std::uint32_t kImuGravityAlignSamples = 50;
|
||||||
|
|
||||||
|
// Odom state
|
||||||
|
std::mutex odom_mutex_;
|
||||||
|
rclcpp::Time last_odom_stamp_{0, 0, RCL_ROS_TIME};
|
||||||
|
std::array<float, 3> odom_pos_{};
|
||||||
|
std::array<float, 4> odom_quat_wxyz_{1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
std::array<float, 3> odom_linear_vel_{};
|
||||||
|
std::array<float, 3> odom_angular_vel_{};
|
||||||
|
bool odom_fresh_{false};
|
||||||
|
|
||||||
|
// Filters and Estimators
|
||||||
|
std::unique_ptr<sim2real_common::LowPassFilter> lpf_legs_;
|
||||||
|
std::unique_ptr<sim2real_common::LowPassFilter> lpf_wheels_;
|
||||||
|
std::unique_ptr<sim2real_common::MahonyFilter> mahony_filter_;
|
||||||
|
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||||
|
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||||
|
bool mahony_initialized_{false};
|
||||||
|
rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME};
|
||||||
|
rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME};
|
||||||
|
|
||||||
|
// Telemetry
|
||||||
|
std::uint32_t fresh_count_{0};
|
||||||
|
std::uint32_t holdover_count_{0};
|
||||||
|
std::uint32_t stale_max_{0};
|
||||||
|
std::uint32_t holdover_events_total_{0};
|
||||||
|
|
||||||
|
bool dry_run_{false};
|
||||||
|
std::atomic<bool> estop_triggered_{false};
|
||||||
|
std::atomic<bool> safety_enabled_{true};
|
||||||
|
std::atomic<bool> safety_triggered_{false};
|
||||||
|
std::string safety_reason_{""};
|
||||||
|
|
||||||
|
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_hw
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_hw</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Hardware bridge and safety boundary for sim2real_ros2.</description>
|
||||||
|
<maintainer email="todo@example.com">todo</maintainer>
|
||||||
|
<license>Proprietary</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<depend>nav_msgs</depend>
|
||||||
|
<depend>sim2real_common</depend>
|
||||||
|
<depend>sim2real_interfaces</depend>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,852 @@
|
|||||||
|
#include "sim2real_hw/hardware_bridge_node.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <cstring>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <linux/can.h>
|
||||||
|
#include <linux/can/raw.h>
|
||||||
|
#include <net/if.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <errno.h>
|
||||||
|
|
||||||
|
#include "sim2real_common/deployment_contract.hpp"
|
||||||
|
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
|
||||||
|
namespace sim2real_hw
|
||||||
|
{
|
||||||
|
|
||||||
|
// Protocol constants
|
||||||
|
const std::uint32_t COMM_ENABLE = 3;
|
||||||
|
const std::uint32_t COMM_DISABLE = 4;
|
||||||
|
const std::uint32_t COMM_WRITE_PARAMETER = 18;
|
||||||
|
const std::uint32_t COMM_OPERATION_CONTROL = 1;
|
||||||
|
const std::uint32_t COMM_SET_ZERO_POSITION = 6;
|
||||||
|
const std::uint16_t PARAM_MODE = 0x7005;
|
||||||
|
const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017;
|
||||||
|
const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B;
|
||||||
|
const std::uint8_t HOST_ID = 0xFD;
|
||||||
|
|
||||||
|
inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val)
|
||||||
|
{
|
||||||
|
buf[0] = (val >> 8) & 0xFF;
|
||||||
|
buf[1] = val & 0xFF;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float nearest_periodic(float val, float ref)
|
||||||
|
{
|
||||||
|
float diff = val - ref;
|
||||||
|
float wrapped = diff - 2.0f * static_cast<float>(M_PI) * std::floor((diff + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||||
|
return ref + wrapped;
|
||||||
|
}
|
||||||
|
|
||||||
|
HardwareBridgeNode::HardwareBridgeNode()
|
||||||
|
: Node("sim2real_hw_node")
|
||||||
|
{
|
||||||
|
// 1. Declare and get parameters
|
||||||
|
target_timeout_ms_ = declare_parameter<double>("target_timeout_ms", 150.0);
|
||||||
|
can0_name_ = declare_parameter<std::string>("can0_name", "can0");
|
||||||
|
can1_name_ = declare_parameter<std::string>("can1_name", "can1");
|
||||||
|
dry_run_ = declare_parameter<bool>("dry_run", true); // Default to dry-run for safety
|
||||||
|
|
||||||
|
// Safety parameters
|
||||||
|
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||||
|
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||||
|
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||||
|
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||||
|
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||||
|
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||||
|
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||||
|
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false");
|
||||||
|
if (safety_enabled_) {
|
||||||
|
RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel);
|
||||||
|
} else {
|
||||||
|
RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2. Set up logical motors mapping matching contract
|
||||||
|
// Mapping index in array matches joint ordering in kJointLabels
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i];
|
||||||
|
motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i];
|
||||||
|
motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i];
|
||||||
|
motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3. Initialize filters & safety monitors
|
||||||
|
lpf_legs_ = std::make_unique<sim2real_common::LowPassFilter>(5.0, 0.005, 12);
|
||||||
|
lpf_wheels_ = std::make_unique<sim2real_common::LowPassFilter>(15.0, 0.005, 4);
|
||||||
|
mahony_filter_ = std::make_unique<sim2real_common::MahonyFilter>(2.0f, 0.0f);
|
||||||
|
|
||||||
|
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||||
|
static_cast<float>(max_target_offset),
|
||||||
|
static_cast<float>(max_ang_vel),
|
||||||
|
static_cast<float>(max_tilt_z),
|
||||||
|
clip_to_brake,
|
||||||
|
static_cast<float>(hard_target_offset)
|
||||||
|
);
|
||||||
|
|
||||||
|
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||||
|
static_cast<float>(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop
|
||||||
|
static_cast<float>(max_tilt_z),
|
||||||
|
static_cast<float>(imu_age_warn_ms),
|
||||||
|
static_cast<float>(imu_age_stop_ms)
|
||||||
|
);
|
||||||
|
|
||||||
|
// 4. Initialize CAN sockets if not in dry-run
|
||||||
|
if (!dry_run_) {
|
||||||
|
if (!initCan(can0_name_, can0_fd_) || !initCan(can1_name_, can1_fd_)) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "CAN initialization failed! Falling back to dry-run.");
|
||||||
|
dry_run_ = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 5. Initialize motor target states
|
||||||
|
latest_target_ = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||||
|
latest_raw_action_.fill(0.0f);
|
||||||
|
|
||||||
|
// 6. Set up ROS publishers & subscriptions
|
||||||
|
state_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeState>("runtime/state", 10);
|
||||||
|
target_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeTarget>(
|
||||||
|
"runtime/target", 10,
|
||||||
|
std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1));
|
||||||
|
std::string imu_topic = declare_parameter<std::string>("imu_topic", "/odin1/imu");
|
||||||
|
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
|
||||||
|
imu_topic, 10,
|
||||||
|
std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1));
|
||||||
|
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||||
|
"/safety/estop", 10,
|
||||||
|
std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
// Odom subscription
|
||||||
|
std::string odom_topic = declare_parameter<std::string>("odom_topic", "/odom");
|
||||||
|
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||||
|
odom_topic, 10,
|
||||||
|
std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
// 7. Enable motors on total startup
|
||||||
|
if (!dry_run_) {
|
||||||
|
RCLCPP_INFO(get_logger(), "Enabling RobStride motors...");
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||||
|
enableMotor(fd, motors_[i].id);
|
||||||
|
setModeRaw(fd, motors_[i].id, 0); // MIT Mode
|
||||||
|
writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f);
|
||||||
|
writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 8. Timers at 200Hz (5ms)
|
||||||
|
read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this));
|
||||||
|
write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this));
|
||||||
|
}
|
||||||
|
|
||||||
|
HardwareBridgeNode::~HardwareBridgeNode()
|
||||||
|
{
|
||||||
|
if (!dry_run_) {
|
||||||
|
RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown...");
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||||
|
disableMotor(fd, motors_[i].id);
|
||||||
|
}
|
||||||
|
if (can0_fd_ >= 0) {
|
||||||
|
if (::close(can0_fd_) < 0) {
|
||||||
|
RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (can1_fd_ >= 0) {
|
||||||
|
if (::close(can1_fd_) < 0) {
|
||||||
|
RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||||
|
latest_target_ = msg->target;
|
||||||
|
latest_raw_action_ = msg->raw_action;
|
||||||
|
latest_target_source_ = msg->target_source;
|
||||||
|
latest_target_stamp_ = rclcpp::Time(msg->stamp);
|
||||||
|
target_sequence_ = msg->sequence;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||||
|
|
||||||
|
const float gyro_x = static_cast<float>(msg->angular_velocity.x);
|
||||||
|
const float gyro_y = static_cast<float>(msg->angular_velocity.y);
|
||||||
|
const float gyro_z = static_cast<float>(msg->angular_velocity.z);
|
||||||
|
|
||||||
|
const float accel_x = static_cast<float>(msg->linear_acceleration.x);
|
||||||
|
const float accel_y = static_cast<float>(msg->linear_acceleration.y);
|
||||||
|
const float accel_z = static_cast<float>(msg->linear_acceleration.z);
|
||||||
|
|
||||||
|
imu_gyro_ = {gyro_x, gyro_y, gyro_z};
|
||||||
|
imu_accel_ = {accel_x, accel_y, accel_z};
|
||||||
|
if (!mahony_initialized_ && imu_gravity_sample_count_ < kImuGravityAlignSamples) {
|
||||||
|
imu_gravity_sum_[0] += accel_x;
|
||||||
|
imu_gravity_sum_[1] += accel_y;
|
||||||
|
imu_gravity_sum_[2] += accel_z;
|
||||||
|
imu_gravity_sample_count_++;
|
||||||
|
}
|
||||||
|
// Track both ROS header time and local receive time. The local steady clock
|
||||||
|
// is used for stale detection so scheduler jitter or device timestamp quirks
|
||||||
|
// don't falsely trip the runtime guard.
|
||||||
|
last_imu_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||||
|
last_imu_recv_time_ = std::chrono::steady_clock::now();
|
||||||
|
has_received_imu_ = true;
|
||||||
|
imu_fresh_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||||
|
estop_triggered_ = msg->data;
|
||||||
|
if (estop_triggered_) {
|
||||||
|
RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!");
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(get_logger(), "Physical E-stop reset.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||||
|
last_odom_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||||
|
|
||||||
|
odom_pos_[0] = static_cast<float>(msg->pose.pose.position.x);
|
||||||
|
odom_pos_[1] = static_cast<float>(msg->pose.pose.position.y);
|
||||||
|
odom_pos_[2] = static_cast<float>(msg->pose.pose.position.z);
|
||||||
|
|
||||||
|
odom_quat_wxyz_[0] = static_cast<float>(msg->pose.pose.orientation.w);
|
||||||
|
odom_quat_wxyz_[1] = static_cast<float>(msg->pose.pose.orientation.x);
|
||||||
|
odom_quat_wxyz_[2] = static_cast<float>(msg->pose.pose.orientation.y);
|
||||||
|
odom_quat_wxyz_[3] = static_cast<float>(msg->pose.pose.orientation.z);
|
||||||
|
|
||||||
|
odom_linear_vel_[0] = static_cast<float>(msg->twist.twist.linear.x);
|
||||||
|
odom_linear_vel_[1] = static_cast<float>(msg->twist.twist.linear.y);
|
||||||
|
odom_linear_vel_[2] = static_cast<float>(msg->twist.twist.linear.z);
|
||||||
|
|
||||||
|
odom_angular_vel_[0] = static_cast<float>(msg->twist.twist.angular.x);
|
||||||
|
odom_angular_vel_[1] = static_cast<float>(msg->twist.twist.angular.y);
|
||||||
|
odom_angular_vel_[2] = static_cast<float>(msg->twist.twist.angular.z);
|
||||||
|
|
||||||
|
odom_fresh_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onReadLoop()
|
||||||
|
{
|
||||||
|
// 1. Process CAN messages (only if CAN is open)
|
||||||
|
if (!dry_run_) {
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
motor_states_[i].stale_count++;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct can_frame frame;
|
||||||
|
// Process can0 (bus 1)
|
||||||
|
while (readCanFrame(can0_fd_, &frame, 50)) {
|
||||||
|
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||||
|
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||||
|
if (comm_type == 2) { // Status Frame
|
||||||
|
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||||
|
int motor_id = extra_data & 0xFF;
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
if (motors_[i].bus == 1 && motors_[i].id == motor_id) {
|
||||||
|
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||||
|
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||||
|
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||||
|
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||||
|
|
||||||
|
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||||
|
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||||
|
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||||
|
|
||||||
|
// Apply motor mapping: real_to_sim
|
||||||
|
// real = sign * sim + offset -> sim = (real - offset) / sign
|
||||||
|
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||||
|
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||||
|
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||||
|
|
||||||
|
if (i < 12) {
|
||||||
|
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
motor_states_[i].position = pos_sim;
|
||||||
|
motor_states_[i].velocity = vel_sim;
|
||||||
|
motor_states_[i].torque = torque_sim;
|
||||||
|
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||||
|
motor_states_[i].update_count++;
|
||||||
|
motor_states_[i].stale_count = 0;
|
||||||
|
// Update hold-over valid data
|
||||||
|
motor_states_[i].last_valid_pos = pos_sim;
|
||||||
|
motor_states_[i].last_valid_vel = vel_sim;
|
||||||
|
motor_states_[i].last_valid_torque = torque_sim;
|
||||||
|
motor_states_[i].has_valid_data = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Process can1 (bus 2)
|
||||||
|
while (readCanFrame(can1_fd_, &frame, 50)) {
|
||||||
|
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||||
|
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||||
|
if (comm_type == 2) {
|
||||||
|
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||||
|
int motor_id = extra_data & 0xFF;
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
if (motors_[i].bus == 2 && motors_[i].id == motor_id) {
|
||||||
|
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||||
|
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||||
|
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||||
|
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||||
|
|
||||||
|
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||||
|
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||||
|
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||||
|
|
||||||
|
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||||
|
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||||
|
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||||
|
|
||||||
|
if (i < 12) {
|
||||||
|
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
motor_states_[i].position = pos_sim;
|
||||||
|
motor_states_[i].velocity = vel_sim;
|
||||||
|
motor_states_[i].torque = torque_sim;
|
||||||
|
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||||
|
motor_states_[i].update_count++;
|
||||||
|
motor_states_[i].stale_count = 0;
|
||||||
|
// Update hold-over valid data
|
||||||
|
motor_states_[i].last_valid_pos = pos_sim;
|
||||||
|
motor_states_[i].last_valid_vel = vel_sim;
|
||||||
|
motor_states_[i].last_valid_torque = torque_sim;
|
||||||
|
motor_states_[i].has_valid_data = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Hold-over: apply last valid data for stale motors
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) {
|
||||||
|
motor_states_[i].position = motor_states_[i].last_valid_pos;
|
||||||
|
motor_states_[i].velocity = motor_states_[i].last_valid_vel;
|
||||||
|
motor_states_[i].torque = motor_states_[i].last_valid_torque;
|
||||||
|
holdover_events_total_++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2. Fetch IMU data & update MahonyFilter
|
||||||
|
auto now_time = now();
|
||||||
|
double dt = 0.005;
|
||||||
|
if (last_read_time_.nanoseconds() > 0) {
|
||||||
|
dt = (now_time - last_read_time_).seconds();
|
||||||
|
if (dt <= 0.0 || dt > 0.5) {
|
||||||
|
dt = 0.005;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
last_read_time_ = now_time;
|
||||||
|
|
||||||
|
std::array<float, 3> gyro{};
|
||||||
|
std::array<float, 3> accel{0.0f, 0.0f, 9.81f};
|
||||||
|
bool imu_fresh = false;
|
||||||
|
double imu_age_ms = 0.0;
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||||
|
gyro = imu_gyro_;
|
||||||
|
accel = imu_accel_;
|
||||||
|
imu_fresh = imu_fresh_;
|
||||||
|
imu_fresh_ = false;
|
||||||
|
|
||||||
|
if (has_received_imu_) {
|
||||||
|
const auto age = std::chrono::steady_clock::now() - last_imu_recv_time_;
|
||||||
|
imu_age_ms = std::chrono::duration<double, std::milli>(age).count();
|
||||||
|
} else if (last_imu_stamp_.nanoseconds() > 0) {
|
||||||
|
const auto age_ns = (now_time - last_imu_stamp_).nanoseconds();
|
||||||
|
imu_age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<float, 4> quat{1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
if (imu_fresh || mahony_initialized_) {
|
||||||
|
if (!mahony_initialized_) {
|
||||||
|
std::array<float, 3> gravity_init = accel;
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||||
|
if (imu_gravity_sample_count_ >= kImuGravityAlignSamples) {
|
||||||
|
gravity_init = {
|
||||||
|
imu_gravity_sum_[0] / static_cast<float>(imu_gravity_sample_count_),
|
||||||
|
imu_gravity_sum_[1] / static_cast<float>(imu_gravity_sample_count_),
|
||||||
|
imu_gravity_sum_[2] / static_cast<float>(imu_gravity_sample_count_)
|
||||||
|
};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
mahony_filter_->reset_with_accel(gravity_init);
|
||||||
|
mahony_initialized_ = true;
|
||||||
|
}
|
||||||
|
quat = mahony_filter_->update(accel, gyro, static_cast<float>(dt));
|
||||||
|
}
|
||||||
|
std::array<float, 3> projected_gravity = sim2real_common::get_gravity_orientation(quat);
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||||
|
projected_gravity_ = projected_gravity;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Run RuntimeGuard check
|
||||||
|
if (safety_enabled_ && !safety_triggered_) {
|
||||||
|
std::vector<float> extra_vals;
|
||||||
|
extra_vals.reserve(32);
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
extra_vals.push_back(motor_states_[i].position);
|
||||||
|
extra_vals.push_back(motor_states_[i].velocity);
|
||||||
|
}
|
||||||
|
bool estop_active = false;
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||||
|
estop_active = estop_triggered_;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto guard_decision = runtime_guard_->check(gyro, projected_gravity, imu_age_ms, estop_active, extra_vals);
|
||||||
|
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||||
|
safety_triggered_ = true;
|
||||||
|
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||||
|
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||||
|
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3. Populate RuntimeState message
|
||||||
|
sim2real_interfaces::msg::RuntimeState msg;
|
||||||
|
msg.stamp = now_time;
|
||||||
|
msg.sequence = state_sequence_++;
|
||||||
|
msg.source = dry_run_ ? "stub_hw" : "socket_can_hw";
|
||||||
|
|
||||||
|
fresh_count_ = 0;
|
||||||
|
holdover_count_ = 0;
|
||||||
|
stale_max_ = 0;
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
// Stale frames detection & holdover count
|
||||||
|
if (!dry_run_) {
|
||||||
|
if (motor_states_[i].stale_count > 0) {
|
||||||
|
holdover_count_++;
|
||||||
|
stale_max_ = std::max(stale_max_, motor_states_[i].stale_count);
|
||||||
|
} else {
|
||||||
|
fresh_count_++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (dry_run_) {
|
||||||
|
// Mock motor positions tracking target
|
||||||
|
msg.joint_pos[i] = latest_target_[i];
|
||||||
|
msg.joint_vel[i] = 0.0f;
|
||||||
|
msg.joint_torque[i] = 0.0f;
|
||||||
|
msg.update_counts[i] = target_sequence_;
|
||||||
|
} else {
|
||||||
|
msg.joint_pos[i] = motor_states_[i].position;
|
||||||
|
msg.joint_vel[i] = motor_states_[i].velocity;
|
||||||
|
msg.joint_torque[i] = motor_states_[i].torque;
|
||||||
|
msg.update_counts[i] = motor_states_[i].update_count;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msg.imu_gyro = gyro;
|
||||||
|
msg.imu_accel = accel;
|
||||||
|
msg.quat_wxyz = quat;
|
||||||
|
msg.projected_gravity = projected_gravity;
|
||||||
|
msg.imu_age_ms = imu_age_ms;
|
||||||
|
msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness
|
||||||
|
|
||||||
|
msg.odom_age_ms = 0.0f;
|
||||||
|
msg.odom_fresh = false;
|
||||||
|
msg.odom_pos = {0.0f, 0.0f, 0.0f};
|
||||||
|
msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||||
|
msg.odom_linear_vel = {0.0f, 0.0f, 0.0f};
|
||||||
|
msg.odom_angular_vel = {0.0f, 0.0f, 0.0f};
|
||||||
|
msg.odom_local_pos = {0.0f, 0.0f, 0.0f};
|
||||||
|
msg.odom_local_yaw = 0.0f;
|
||||||
|
|
||||||
|
// Populate odom fields from subscriber data
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||||
|
if (odom_fresh_) {
|
||||||
|
double odom_age = (now_time - last_odom_stamp_).seconds() * 1000.0;
|
||||||
|
msg.odom_age_ms = static_cast<float>(odom_age);
|
||||||
|
msg.odom_fresh = (odom_age < 200.0); // 200ms threshold
|
||||||
|
msg.odom_pos = odom_pos_;
|
||||||
|
msg.odom_quat_wxyz = odom_quat_wxyz_;
|
||||||
|
msg.odom_linear_vel = odom_linear_vel_;
|
||||||
|
msg.odom_angular_vel = odom_angular_vel_;
|
||||||
|
msg.odom_local_pos = odom_pos_;
|
||||||
|
// Compute yaw from quaternion
|
||||||
|
float qw = odom_quat_wxyz_[0], qx = odom_quat_wxyz_[1];
|
||||||
|
float qy = odom_quat_wxyz_[2], qz = odom_quat_wxyz_[3];
|
||||||
|
float siny_c = 2.0f * (qw * qz + qx * qy);
|
||||||
|
float cosy_c = 1.0f - 2.0f * (qy * qy + qz * qz);
|
||||||
|
msg.odom_local_yaw = std::atan2(siny_c, cosy_c);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
msg.fresh_count = dry_run_ ? 16 : fresh_count_;
|
||||||
|
msg.holdover_count = dry_run_ ? 0 : holdover_count_;
|
||||||
|
msg.stale_max = dry_run_ ? 0 : stale_max_;
|
||||||
|
|
||||||
|
state_pub_->publish(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
void HardwareBridgeNode::onWriteLoop()
|
||||||
|
{
|
||||||
|
const auto now_time = now();
|
||||||
|
std::array<float, 16> target{};
|
||||||
|
std::string target_source;
|
||||||
|
double age_ms = 0.0;
|
||||||
|
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||||
|
target = latest_target_;
|
||||||
|
target_source = latest_target_source_;
|
||||||
|
|
||||||
|
if (latest_target_stamp_.nanoseconds() > 0) {
|
||||||
|
const auto age_ns = (now_time - latest_target_stamp_).nanoseconds();
|
||||||
|
age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Timeout guard: default stand pose if target is stale
|
||||||
|
if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) {
|
||||||
|
target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||||
|
target_source = "timeout_hold";
|
||||||
|
}
|
||||||
|
|
||||||
|
// Run SafetyMonitor check on incoming target commands
|
||||||
|
std::array<float, 3> gyro{};
|
||||||
|
std::array<float, 3> proj_grav{};
|
||||||
|
bool estop_active = false;
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||||
|
gyro = imu_gyro_;
|
||||||
|
proj_grav = projected_gravity_;
|
||||||
|
}
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||||
|
estop_active = estop_triggered_;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (safety_enabled_ && !safety_triggered_) {
|
||||||
|
auto safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active);
|
||||||
|
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||||
|
safety_triggered_ = true;
|
||||||
|
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||||
|
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||||
|
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||||
|
target = safety_decision.clipped_target;
|
||||||
|
target_source = "safety_clip";
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Override to safety_brake if safety is triggered locally or by estop
|
||||||
|
if (safety_triggered_) {
|
||||||
|
target_source = "safety_brake";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 1. Joint LPF command filtering (200Hz, dt=0.005s)
|
||||||
|
std::array<float, 12> legs_in{};
|
||||||
|
std::array<float, 12> legs_out{};
|
||||||
|
std::array<float, 4> wheels_in{};
|
||||||
|
std::array<float, 4> wheels_out{};
|
||||||
|
|
||||||
|
std::copy(target.begin(), target.begin() + 12, legs_in.begin());
|
||||||
|
std::copy(target.begin() + 12, target.end(), wheels_in.begin());
|
||||||
|
|
||||||
|
lpf_legs_->filter(legs_in.data(), legs_out.data());
|
||||||
|
lpf_wheels_->filter(wheels_in.data(), wheels_out.data());
|
||||||
|
|
||||||
|
std::array<float, 16> filtered_target{};
|
||||||
|
std::copy(legs_out.begin(), legs_out.end(), filtered_target.begin());
|
||||||
|
std::copy(wheels_out.begin(), wheels_out.end(), filtered_target.begin() + 12);
|
||||||
|
|
||||||
|
// 2. Control execution (MIT mode write over CAN)
|
||||||
|
if (!dry_run_) {
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
// Coordinate transform: sim_to_real
|
||||||
|
// real = sign * sim + offset
|
||||||
|
float sim_val = filtered_target[i];
|
||||||
|
float real_val = motors_[i].direction * sim_val + motors_[i].offset;
|
||||||
|
|
||||||
|
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||||
|
|
||||||
|
if (i < 12) {
|
||||||
|
// Leg joints: MIT position control
|
||||||
|
// Kp & Kd depend on whether we are holding pose, running policy, or in safety damping mode
|
||||||
|
double kp_val = sim2real_common::DeploymentContract::kLegKp;
|
||||||
|
double kd_val = sim2real_common::DeploymentContract::kLegKd;
|
||||||
|
|
||||||
|
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||||
|
kp_val = 0.0;
|
||||||
|
kd_val = 2.5; // Leg damping Kd
|
||||||
|
real_val = 0.0; // Set to zero position (sign/offset will be ignored anyway under kp=0)
|
||||||
|
} else if (target_source == "startup_soft_hold") {
|
||||||
|
if (startup_soft_hold_start_time_.nanoseconds() == 0) {
|
||||||
|
startup_soft_hold_start_time_ = now_time;
|
||||||
|
}
|
||||||
|
double elapsed = (now_time - startup_soft_hold_start_time_).seconds();
|
||||||
|
double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp
|
||||||
|
kp_val = sim2real_common::DeploymentContract::kLegHoldKp * kp_scale;
|
||||||
|
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||||
|
} else {
|
||||||
|
startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
|
||||||
|
if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") {
|
||||||
|
kp_val = sim2real_common::DeploymentContract::kLegHoldKp;
|
||||||
|
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0);
|
||||||
|
} else {
|
||||||
|
// Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0)
|
||||||
|
double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign
|
||||||
|
double kd_val = sim2real_common::DeploymentContract::kWheelKd;
|
||||||
|
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||||
|
vel_real = 0.0;
|
||||||
|
kd_val = 2.0; // Wheel damping Kd
|
||||||
|
}
|
||||||
|
writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::enableMotor(int fd, int motor_id)
|
||||||
|
{
|
||||||
|
std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||||
|
return sendCanFrame(fd, ext_id, nullptr, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::disableMotor(int fd, int motor_id)
|
||||||
|
{
|
||||||
|
std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||||
|
std::uint8_t data[8] = {0};
|
||||||
|
return sendCanFrame(fd, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode)
|
||||||
|
{
|
||||||
|
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||||
|
std::uint8_t data[8] = {0};
|
||||||
|
data[0] = PARAM_MODE & 0xFF;
|
||||||
|
data[1] = (PARAM_MODE >> 8) & 0xFF;
|
||||||
|
data[4] = static_cast<std::uint8_t>(mode);
|
||||||
|
return sendCanFrame(fd, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit)
|
||||||
|
{
|
||||||
|
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||||
|
std::uint8_t data[8] = {0};
|
||||||
|
data[0] = param_id & 0xFF;
|
||||||
|
data[1] = (param_id >> 8) & 0xFF;
|
||||||
|
std::memcpy(&data[4], &limit, sizeof(float));
|
||||||
|
return sendCanFrame(fd, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp_val, double kd_val, double torque)
|
||||||
|
{
|
||||||
|
const double P_LIMIT = 4.0 * M_PI;
|
||||||
|
const double V_LIMIT = 44.0;
|
||||||
|
const double T_LIMIT = 17.0;
|
||||||
|
const double KP_LIMIT = 500.0;
|
||||||
|
const double KD_LIMIT = 5.0;
|
||||||
|
|
||||||
|
double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos));
|
||||||
|
double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel));
|
||||||
|
double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val));
|
||||||
|
double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val));
|
||||||
|
double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque));
|
||||||
|
|
||||||
|
std::uint16_t pos_u16 = static_cast<std::uint16_t>(((pos_clamped / P_LIMIT) + 1.0) * 32767.0);
|
||||||
|
std::uint16_t vel_u16 = static_cast<std::uint16_t>(((vel_clamped / V_LIMIT) + 1.0) * 32767.0);
|
||||||
|
std::uint16_t kp_u16 = static_cast<std::uint16_t>((kp_clamped / KP_LIMIT) * 65535.0);
|
||||||
|
std::uint16_t kd_u16 = static_cast<std::uint16_t>((kd_clamped / KD_LIMIT) * 65535.0);
|
||||||
|
std::uint16_t torque_u16 = static_cast<std::uint16_t>(((torque_clamped / T_LIMIT) + 1.0) * 32767.0);
|
||||||
|
|
||||||
|
std::uint8_t data[8];
|
||||||
|
pack_u16_be(&data[0], pos_u16);
|
||||||
|
pack_u16_be(&data[2], vel_u16);
|
||||||
|
pack_u16_be(&data[4], kp_u16);
|
||||||
|
pack_u16_be(&data[6], kd_u16);
|
||||||
|
|
||||||
|
std::uint32_t ext_id = (COMM_OPERATION_CONTROL << 24) | (torque_u16 << 8) | motor_id;
|
||||||
|
return sendCanFrame(fd, ext_id, data, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
|
||||||
|
{
|
||||||
|
struct sockaddr_can addr;
|
||||||
|
struct ifreq ifr;
|
||||||
|
|
||||||
|
if ((fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "Failed to create SocketCAN socket for %s", ifname.c_str());
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set non-blocking mode
|
||||||
|
int flags = ::fcntl(fd, F_GETFL, 0);
|
||||||
|
if (flags < 0 || ::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "Failed to set socket to non-blocking for %s", ifname.c_str());
|
||||||
|
::close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
|
||||||
|
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str());
|
||||||
|
::close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
addr.can_family = AF_CAN;
|
||||||
|
addr.can_ifindex = ifr.ifr_ifindex;
|
||||||
|
|
||||||
|
if (::bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "Failed to bind SocketCAN socket for %s", ifname.c_str());
|
||||||
|
::close(fd);
|
||||||
|
fd = -1;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(), "Successfully bound to SocketCAN interface %s", ifname.c_str());
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc)
|
||||||
|
{
|
||||||
|
if (fd < 0) return false;
|
||||||
|
struct can_frame frame;
|
||||||
|
frame.can_id = can_id | CAN_EFF_FLAG; // Extended frame format (29-bit CAN ID)
|
||||||
|
frame.can_dlc = dlc;
|
||||||
|
if (data) {
|
||||||
|
std::memcpy(frame.data, data, dlc);
|
||||||
|
} else {
|
||||||
|
std::memset(frame.data, 0, 8);
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t bytes_written = ::write(fd, &frame, sizeof(struct can_frame));
|
||||||
|
if (bytes_written != sizeof(struct can_frame)) {
|
||||||
|
int err = errno;
|
||||||
|
// Track errors per bus for recovery logic
|
||||||
|
if (fd == can0_fd_) {
|
||||||
|
can0_error_count_++;
|
||||||
|
if (can0_error_count_ >= kCanErrorThreshold) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "CAN0 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||||
|
can0_error_count_, err, strerror(err));
|
||||||
|
if (!reinitCan(can0_name_, can0_fd_, can0_error_count_)) {
|
||||||
|
RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake.");
|
||||||
|
safety_triggered_ = true;
|
||||||
|
safety_reason_ = "CAN0 bus failure - reinit failed";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else if (fd == can1_fd_) {
|
||||||
|
can1_error_count_++;
|
||||||
|
if (can1_error_count_ >= kCanErrorThreshold) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||||
|
can1_error_count_, err, strerror(err));
|
||||||
|
if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) {
|
||||||
|
RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake.");
|
||||||
|
safety_triggered_ = true;
|
||||||
|
safety_reason_ = "CAN1 bus failure - reinit failed";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
// Reset error count on success
|
||||||
|
if (fd == can0_fd_) can0_error_count_ = 0;
|
||||||
|
else if (fd == can1_fd_) can1_error_count_ = 0;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us)
|
||||||
|
{
|
||||||
|
if (fd < 0) return false;
|
||||||
|
auto* frame = static_cast<struct can_frame*>(frame_ptr);
|
||||||
|
|
||||||
|
if (timeout_us > 0) {
|
||||||
|
struct timeval tv;
|
||||||
|
tv.tv_sec = 0;
|
||||||
|
tv.tv_usec = timeout_us;
|
||||||
|
fd_set rdfs;
|
||||||
|
FD_ZERO(&rdfs);
|
||||||
|
FD_SET(fd, &rdfs);
|
||||||
|
|
||||||
|
int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv);
|
||||||
|
if (ret < 0) {
|
||||||
|
int err = errno;
|
||||||
|
if (err != EINTR) {
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||||
|
"CAN select error (fd=%d): %s", fd, strerror(err));
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (ret == 0) {
|
||||||
|
return false; // timeout, normal
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame));
|
||||||
|
if (bytes_read < 0) {
|
||||||
|
int err = errno;
|
||||||
|
if (err != EAGAIN && err != EWOULDBLOCK) {
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||||
|
"CAN read error (fd=%d): %s", fd, strerror(err));
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return (bytes_read == sizeof(struct can_frame));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count)
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str());
|
||||||
|
if (fd >= 0) {
|
||||||
|
::close(fd);
|
||||||
|
fd = -1;
|
||||||
|
}
|
||||||
|
bool success = initCan(ifname, fd);
|
||||||
|
if (success) {
|
||||||
|
error_count = 0;
|
||||||
|
RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str());
|
||||||
|
}
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace sim2real_hw
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<sim2real_hw::HardwareBridgeNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,16 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(sim2real_interfaces)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(builtin_interfaces REQUIRED)
|
||||||
|
find_package(rosidl_default_generators REQUIRED)
|
||||||
|
find_package(std_msgs REQUIRED)
|
||||||
|
|
||||||
|
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||||
|
"msg/RuntimeState.msg"
|
||||||
|
"msg/RuntimeTarget.msg"
|
||||||
|
DEPENDENCIES builtin_interfaces std_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_export_dependencies(rosidl_default_runtime)
|
||||||
|
ament_package()
|
||||||
@@ -0,0 +1,30 @@
|
|||||||
|
builtin_interfaces/Time stamp
|
||||||
|
uint32 sequence
|
||||||
|
|
||||||
|
float32[16] joint_pos
|
||||||
|
float32[16] joint_vel
|
||||||
|
float32[16] joint_torque
|
||||||
|
|
||||||
|
float32[3] imu_gyro
|
||||||
|
float32[3] imu_accel
|
||||||
|
float32[4] quat_wxyz
|
||||||
|
float32[3] projected_gravity
|
||||||
|
|
||||||
|
float32 imu_age_ms
|
||||||
|
bool imu_fresh
|
||||||
|
|
||||||
|
float32 odom_age_ms
|
||||||
|
bool odom_fresh
|
||||||
|
float32[3] odom_pos
|
||||||
|
float32[4] odom_quat_wxyz
|
||||||
|
float32[3] odom_linear_vel
|
||||||
|
float32[3] odom_angular_vel
|
||||||
|
float32[3] odom_local_pos
|
||||||
|
float32 odom_local_yaw
|
||||||
|
|
||||||
|
uint32 fresh_count
|
||||||
|
uint32 holdover_count
|
||||||
|
uint32 stale_max
|
||||||
|
uint32[16] update_counts
|
||||||
|
|
||||||
|
string source
|
||||||
@@ -0,0 +1,15 @@
|
|||||||
|
builtin_interfaces/Time stamp
|
||||||
|
uint32 sequence
|
||||||
|
|
||||||
|
float32[16] target
|
||||||
|
float32[16] raw_action
|
||||||
|
float32[16] scaled_action
|
||||||
|
float32[3] command
|
||||||
|
float32[3] raw_command
|
||||||
|
|
||||||
|
bool zero_command
|
||||||
|
bool runtime_released
|
||||||
|
float32 release_alpha
|
||||||
|
float32 target_age_ms
|
||||||
|
|
||||||
|
string target_source
|
||||||
@@ -0,0 +1,18 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_interfaces</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>ROS 2 interfaces for the sim2real wheel-leg runtime.</description>
|
||||||
|
<maintainer email="todo@example.com">todo</maintainer>
|
||||||
|
<license>Proprietary</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>builtin_interfaces</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
|
||||||
|
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||||
|
|
||||||
|
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.5)
|
||||||
|
project(sim2real_nav2)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
install(DIRECTORY
|
||||||
|
config
|
||||||
|
launch
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
@@ -0,0 +1,232 @@
|
|||||||
|
amcl:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
alpha1: 0.2
|
||||||
|
alpha2: 0.2
|
||||||
|
alpha3: 0.2
|
||||||
|
alpha4: 0.2
|
||||||
|
alpha5: 0.2
|
||||||
|
base_frame_id: "base_link"
|
||||||
|
beam_skip_distance: 0.5
|
||||||
|
beam_skip_error_threshold: 0.9
|
||||||
|
beam_skip_threshold: 0.3
|
||||||
|
do_beamskip: false
|
||||||
|
global_frame_id: "map"
|
||||||
|
odom_frame_id: "odom"
|
||||||
|
laser_likelihood_max_dist: 2.0
|
||||||
|
laser_max_range: -1.0
|
||||||
|
laser_min_range: -1.0
|
||||||
|
laser_model_type: "likelihood_field"
|
||||||
|
max_beams: 60
|
||||||
|
max_particles: 2000
|
||||||
|
min_particles: 500
|
||||||
|
recovery_alpha_fast: 0.0
|
||||||
|
recovery_alpha_slow: 0.0
|
||||||
|
resample_interval: 1
|
||||||
|
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||||
|
save_pose_rate: 0.5
|
||||||
|
sigma_hit: 0.2
|
||||||
|
transform_tolerance: 1.0
|
||||||
|
update_min_d: 0.25
|
||||||
|
update_min_a: 0.2
|
||||||
|
z_hit: 0.5
|
||||||
|
z_max: 0.05
|
||||||
|
z_rand: 0.5
|
||||||
|
z_short: 0.05
|
||||||
|
scan_topic: "scan"
|
||||||
|
|
||||||
|
bt_navigator:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
global_frame: map
|
||||||
|
robot_base_frame: base_link
|
||||||
|
odom_frame: odom
|
||||||
|
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
|
||||||
|
plugin_lib_names:
|
||||||
|
- nav2_back_up_action_bt_node
|
||||||
|
- nav2_spin_action_bt_node
|
||||||
|
- nav2_wait_action_bt_node
|
||||||
|
- nav2_clear_costmap_service_bt_node
|
||||||
|
- nav2_is_stuck_condition_bt_node
|
||||||
|
- nav2_goal_reached_condition_bt_node
|
||||||
|
- nav2_goal_updated_condition_bt_node
|
||||||
|
- nav2_initial_pose_received_condition_bt_node
|
||||||
|
- nav2_recompute_path_to_pose_action_bt_node
|
||||||
|
- nav2_compute_path_to_pose_action_bt_node
|
||||||
|
- nav2_follow_path_action_bt_node
|
||||||
|
- nav2_rate_controller_bt_node
|
||||||
|
- nav2_distance_controller_bt_node
|
||||||
|
- nav2_speed_controller_bt_node
|
||||||
|
- nav2_truncate_path_action_bt_node
|
||||||
|
- nav2_goal_updater_node
|
||||||
|
- nav2_recovery_node
|
||||||
|
- nav2_pipeline_sequence_node
|
||||||
|
- nav2_round_robin_node
|
||||||
|
- nav2_transform_available_condition_bt_node
|
||||||
|
- nav2_time_expired_condition_bt_node
|
||||||
|
- nav2_distance_traveled_condition_bt_node
|
||||||
|
|
||||||
|
controller_server:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
controller_frequency: 10.0
|
||||||
|
min_x_velocity_threshold: 0.001
|
||||||
|
min_y_velocity_threshold: 0.001
|
||||||
|
min_theta_velocity_threshold: 0.001
|
||||||
|
failure_tolerance: 0.3
|
||||||
|
progress_checker_plugin: "progress_checker"
|
||||||
|
goal_checker_plugins: ["general_goal_checker"]
|
||||||
|
controller_plugins: ["FollowPath"]
|
||||||
|
|
||||||
|
progress_checker:
|
||||||
|
plugin: "nav2_controller::SimpleProgressChecker"
|
||||||
|
required_movement_radius: 0.5
|
||||||
|
movement_time_allowance: 10.0
|
||||||
|
|
||||||
|
general_goal_checker:
|
||||||
|
stateful: true
|
||||||
|
plugin: "nav2_controller::SimpleGoalChecker"
|
||||||
|
xy_goal_tolerance: 0.25
|
||||||
|
yaw_goal_tolerance: 0.25
|
||||||
|
|
||||||
|
FollowPath:
|
||||||
|
plugin: "dwb_core::DWBLocalPlanner"
|
||||||
|
prune_plan: true
|
||||||
|
prune_distance: 1.0
|
||||||
|
debug_trajectory_details: false
|
||||||
|
trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
|
||||||
|
velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator"
|
||||||
|
critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
|
||||||
|
|
||||||
|
# DWB Velocity parameters matching Units
|
||||||
|
min_vel_x: 0.0
|
||||||
|
max_vel_x: 0.6
|
||||||
|
min_vel_y: 0.0
|
||||||
|
max_vel_y: 0.0
|
||||||
|
max_vel_theta: 2.0
|
||||||
|
min_speed_xy: 0.0
|
||||||
|
max_speed_xy: 0.6
|
||||||
|
min_speed_theta: 0.0
|
||||||
|
|
||||||
|
# DWB Acceleration parameters matching Units
|
||||||
|
acc_lim_x: 15.0
|
||||||
|
acc_lim_y: 15.0
|
||||||
|
acc_lim_theta: 12.0
|
||||||
|
decel_lim_x: -15.0
|
||||||
|
decel_lim_y: -15.0
|
||||||
|
decel_lim_theta: -12.0
|
||||||
|
|
||||||
|
# Critics tuning
|
||||||
|
ObstacleFootprint.scale: 0.2
|
||||||
|
PathAlign.scale: 32.0
|
||||||
|
PathAlign.forward_point_distance: 0.1
|
||||||
|
GoalAlign.scale: 24.0
|
||||||
|
GoalAlign.forward_point_distance: 0.1
|
||||||
|
PathDist.scale: 32.0
|
||||||
|
GoalDist.scale: 24.0
|
||||||
|
|
||||||
|
planner_server:
|
||||||
|
ros__parameters:
|
||||||
|
expected_planner_frequency: 1.0
|
||||||
|
use_sim_time: false
|
||||||
|
planner_plugins: ["GridTransition"]
|
||||||
|
GridTransition:
|
||||||
|
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||||
|
tolerance: 0.5
|
||||||
|
use_astar: false
|
||||||
|
allow_unknown: true
|
||||||
|
|
||||||
|
behavior_server:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
recovery_plugins: ["spin", "backup", "wait"]
|
||||||
|
spin:
|
||||||
|
plugin: "nav2_behaviors::Spin"
|
||||||
|
backup:
|
||||||
|
plugin: "nav2_behaviors::Backup"
|
||||||
|
wait:
|
||||||
|
plugin: "nav2_behaviors::Wait"
|
||||||
|
global_frame: odom
|
||||||
|
robot_base_frame: base_link
|
||||||
|
transform_tolerance: 0.1
|
||||||
|
simulate_ahead_time: 2.0
|
||||||
|
max_rotational_vel: 1.0
|
||||||
|
min_rotational_vel: 0.4
|
||||||
|
rotational_acc_lim: 3.2
|
||||||
|
|
||||||
|
global_costmap:
|
||||||
|
global_costmap:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
robot_radius: 0.25
|
||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
publish_frequency: 1.0
|
||||||
|
update_frequency: 1.0
|
||||||
|
global_frame: odom
|
||||||
|
robot_base_frame: base_link
|
||||||
|
rolling_window: true
|
||||||
|
width: 30
|
||||||
|
height: 30
|
||||||
|
resolution: 0.05
|
||||||
|
track_unknown_space: true
|
||||||
|
plugins: ["obstacle_layer", "inflation_layer"]
|
||||||
|
|
||||||
|
obstacle_layer:
|
||||||
|
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||||
|
enabled: true
|
||||||
|
observation_sources: pointcloud
|
||||||
|
pointcloud:
|
||||||
|
topic: /odin1/cloud_slam
|
||||||
|
sensor_frame: base_link
|
||||||
|
data_type: "PointCloud2"
|
||||||
|
clearing: true
|
||||||
|
marking: true
|
||||||
|
max_obstacle_height: 2.0
|
||||||
|
min_obstacle_height: 0.05
|
||||||
|
obstacle_max_range: 2.5
|
||||||
|
obstacle_min_range: 0.1
|
||||||
|
|
||||||
|
inflation_layer:
|
||||||
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
|
enabled: true
|
||||||
|
inflation_radius: 0.6
|
||||||
|
cost_scaling_factor: 4.0
|
||||||
|
|
||||||
|
local_costmap:
|
||||||
|
local_costmap:
|
||||||
|
ros__parameters:
|
||||||
|
use_sim_time: false
|
||||||
|
robot_radius: 0.25
|
||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
publish_frequency: 5.0
|
||||||
|
update_frequency: 5.0
|
||||||
|
global_frame: odom
|
||||||
|
robot_base_frame: base_link
|
||||||
|
rolling_window: true
|
||||||
|
width: 4
|
||||||
|
height: 4
|
||||||
|
resolution: 0.05
|
||||||
|
plugins: ["obstacle_layer", "inflation_layer"]
|
||||||
|
|
||||||
|
obstacle_layer:
|
||||||
|
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||||
|
enabled: true
|
||||||
|
observation_sources: pointcloud
|
||||||
|
pointcloud:
|
||||||
|
topic: /odin1/cloud_slam
|
||||||
|
sensor_frame: base_link
|
||||||
|
data_type: "PointCloud2"
|
||||||
|
clearing: true
|
||||||
|
marking: true
|
||||||
|
max_obstacle_height: 2.0
|
||||||
|
min_obstacle_height: 0.05
|
||||||
|
obstacle_max_range: 2.5
|
||||||
|
obstacle_min_range: 0.1
|
||||||
|
|
||||||
|
inflation_layer:
|
||||||
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
|
enabled: true
|
||||||
|
inflation_radius: 0.4
|
||||||
|
cost_scaling_factor: 4.0
|
||||||
@@ -0,0 +1,146 @@
|
|||||||
|
import os
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
# Get package directories
|
||||||
|
my_share_dir = get_package_share_directory('sim2real_nav2')
|
||||||
|
|
||||||
|
# Declare launch configuration variables
|
||||||
|
params_file_arg = DeclareLaunchArgument(
|
||||||
|
'params_file',
|
||||||
|
default_value=os.path.join(my_share_dir, 'config', 'nav2_params.yaml'),
|
||||||
|
description='Full path to the ROS2 parameters file to use for all launched nodes'
|
||||||
|
)
|
||||||
|
|
||||||
|
params_file = LaunchConfiguration('params_file')
|
||||||
|
|
||||||
|
# Define Nav2 lifecycle nodes to run
|
||||||
|
lifecycle_nodes = ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator',
|
||||||
|
'global_costmap', 'local_costmap', 'amcl']
|
||||||
|
|
||||||
|
# Controller server node
|
||||||
|
controller_server_node = Node(
|
||||||
|
package='nav2_controller',
|
||||||
|
executable='controller_server',
|
||||||
|
name='controller_server',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Planner server node
|
||||||
|
planner_server_node = Node(
|
||||||
|
package='nav2_planner',
|
||||||
|
executable='planner_server',
|
||||||
|
name='planner_server',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Behavior server node (called recovery_server in Galactic, behavior_server in Humble)
|
||||||
|
behavior_server_node = Node(
|
||||||
|
package='nav2_behaviors',
|
||||||
|
executable='behavior_server',
|
||||||
|
name='behavior_server',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# BT Navigator node
|
||||||
|
bt_navigator_node = Node(
|
||||||
|
package='nav2_bt_navigator',
|
||||||
|
executable='bt_navigator',
|
||||||
|
name='bt_navigator',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Global costmap node
|
||||||
|
global_costmap_node = Node(
|
||||||
|
package='nav2_costmap_2d',
|
||||||
|
executable='nav2_costmap_2d',
|
||||||
|
name='global_costmap',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Local costmap node
|
||||||
|
local_costmap_node = Node(
|
||||||
|
package='nav2_costmap_2d',
|
||||||
|
executable='nav2_costmap_2d',
|
||||||
|
name='local_costmap',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# AMCL node (Adaptive Monte Carlo Localization), now receives /scan from pointcloud_to_laserscan
|
||||||
|
amcl_node = Node(
|
||||||
|
package='nav2_amcl',
|
||||||
|
executable='amcl',
|
||||||
|
name='amcl',
|
||||||
|
output='screen',
|
||||||
|
parameters=[params_file]
|
||||||
|
)
|
||||||
|
|
||||||
|
# PointCloud2 to LaserScan converter (AMCL needs LaserScan, LiDAR publishes PointCloud2)
|
||||||
|
pointcloud_to_laserscan_node = Node(
|
||||||
|
package='pointcloud_to_laserscan',
|
||||||
|
executable='pointcloud_to_laserscan_node',
|
||||||
|
name='pointcloud_to_laserscan',
|
||||||
|
output='screen',
|
||||||
|
remappings=[
|
||||||
|
('cloud_in', '/odin1/cloud_slam'),
|
||||||
|
('scan', '/scan')
|
||||||
|
],
|
||||||
|
parameters=[{
|
||||||
|
'target_frame': 'base_link',
|
||||||
|
'transform_tolerance': 0.01,
|
||||||
|
'min_height': 0.05,
|
||||||
|
'max_height': 2.0,
|
||||||
|
'angle_min': -3.14159,
|
||||||
|
'angle_max': 3.14159,
|
||||||
|
'angle_increment': 0.0087, # ~0.5 degrees
|
||||||
|
'scan_time': 0.1,
|
||||||
|
'range_min': 0.1,
|
||||||
|
'range_max': 10.0,
|
||||||
|
'use_inf': True,
|
||||||
|
'inf_epsilon': 1.0,
|
||||||
|
'concurrency_level': 1
|
||||||
|
}]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Lifecycle manager node to transition Nav2 nodes to ACTIVE state
|
||||||
|
lifecycle_manager_node = Node(
|
||||||
|
package='nav2_lifecycle_manager',
|
||||||
|
executable='lifecycle_manager',
|
||||||
|
name='lifecycle_manager_navigation',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'use_sim_time': False,
|
||||||
|
'autostart': True,
|
||||||
|
'node_names': lifecycle_nodes
|
||||||
|
}]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Create launch description
|
||||||
|
ld = LaunchDescription()
|
||||||
|
|
||||||
|
# Set stdout line buffering
|
||||||
|
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'))
|
||||||
|
|
||||||
|
# Add actions
|
||||||
|
ld.add_action(params_file_arg)
|
||||||
|
ld.add_action(controller_server_node)
|
||||||
|
ld.add_action(planner_server_node)
|
||||||
|
ld.add_action(behavior_server_node)
|
||||||
|
ld.add_action(bt_navigator_node)
|
||||||
|
ld.add_action(global_costmap_node)
|
||||||
|
ld.add_action(local_costmap_node)
|
||||||
|
ld.add_action(amcl_node)
|
||||||
|
ld.add_action(pointcloud_to_laserscan_node)
|
||||||
|
ld.add_action(lifecycle_manager_node)
|
||||||
|
|
||||||
|
return ld
|
||||||
@@ -0,0 +1,18 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_nav2</name>
|
||||||
|
<version>0.0.1</version>
|
||||||
|
<description>ROS2 Nav2 configuration package for legged-wheeled quadruped</description>
|
||||||
|
<maintainer email="31560@todo.todo">USER</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>nav2_bringup</exec_depend>
|
||||||
|
<exec_depend>navigation2</exec_depend>
|
||||||
|
<exec_depend>pointcloud_to_laserscan</exec_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,99 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(sim2real_runtime)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(geometry_msgs REQUIRED)
|
||||||
|
find_package(nav_msgs REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(std_msgs REQUIRED)
|
||||||
|
find_package(tf2_ros REQUIRED)
|
||||||
|
find_package(sim2real_common REQUIRED)
|
||||||
|
find_package(sim2real_interfaces REQUIRED)
|
||||||
|
|
||||||
|
# Search for ONNX Runtime headers and library
|
||||||
|
find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h
|
||||||
|
PATHS
|
||||||
|
/usr/include
|
||||||
|
/usr/include/onnxruntime
|
||||||
|
/usr/local/include
|
||||||
|
/usr/local/include/onnxruntime
|
||||||
|
/opt/onnxruntime/include
|
||||||
|
)
|
||||||
|
|
||||||
|
find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||||
|
PATHS
|
||||||
|
/usr/lib
|
||||||
|
/usr/lib/x86_64-linux-gnu
|
||||||
|
/usr/lib/aarch64-linux-gnu
|
||||||
|
/usr/local/lib
|
||||||
|
/opt/onnxruntime/lib
|
||||||
|
)
|
||||||
|
|
||||||
|
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||||
|
|
||||||
|
if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY)
|
||||||
|
message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
add_executable(sim2real_runtime_node
|
||||||
|
src/policy_runtime_node.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
add_executable(odom_relay_node
|
||||||
|
src/odom_relay_node.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(sim2real_runtime_node PRIVATE
|
||||||
|
include
|
||||||
|
${ONNXRUNTIME_INCLUDE_DIR}
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(odom_relay_node PRIVATE include)
|
||||||
|
|
||||||
|
target_link_libraries(sim2real_runtime_node
|
||||||
|
${ONNXRUNTIME_LIBRARY}
|
||||||
|
)
|
||||||
|
|
||||||
|
set_target_properties(sim2real_runtime_node PROPERTIES
|
||||||
|
BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||||
|
INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||||
|
)
|
||||||
|
|
||||||
|
target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17)
|
||||||
|
|
||||||
|
ament_target_dependencies(sim2real_runtime_node
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
rclcpp
|
||||||
|
std_msgs
|
||||||
|
tf2_ros
|
||||||
|
sim2real_common
|
||||||
|
sim2real_interfaces
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_target_dependencies(odom_relay_node
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
rclcpp
|
||||||
|
tf2_ros
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY include/
|
||||||
|
DESTINATION include
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
TARGETS sim2real_runtime_node odom_relay_node
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
PROGRAMS
|
||||||
|
src/remote_uart_node.py
|
||||||
|
src/cmd_mux_node.py
|
||||||
|
src/web_udp_bridge_node.py
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
+32
@@ -0,0 +1,32 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "nav_msgs/msg/odometry.hpp"
|
||||||
|
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||||
|
#include "tf2_ros/transform_broadcaster.h"
|
||||||
|
|
||||||
|
namespace sim2real_runtime
|
||||||
|
{
|
||||||
|
|
||||||
|
/// Subscribes to odin_ros_driver's odometry (e.g. /odin1/odometry),
|
||||||
|
/// remaps child_frame_id to "base_link", republishes on /odom,
|
||||||
|
/// and broadcasts the odom → base_link TF.
|
||||||
|
class OdomRelayNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
OdomRelayNode();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||||
|
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||||
|
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||||
|
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||||
|
|
||||||
|
std::string odom_input_topic_;
|
||||||
|
std::string odom_output_topic_;
|
||||||
|
std::string base_frame_;
|
||||||
|
bool publish_tf_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_runtime
|
||||||
+124
@@ -0,0 +1,124 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <mutex>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
#include "geometry_msgs/msg/twist.hpp"
|
||||||
|
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||||
|
#include "std_msgs/msg/bool.hpp"
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||||
|
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||||
|
#include "sim2real_common/stand_balance_controller.hpp"
|
||||||
|
#include "sim2real_common/safety_monitor.hpp"
|
||||||
|
#include "sim2real_common/runtime_guard.hpp"
|
||||||
|
|
||||||
|
// ONNXRuntime C++ API
|
||||||
|
#include <onnxruntime_cxx_api.h>
|
||||||
|
|
||||||
|
namespace sim2real_runtime
|
||||||
|
{
|
||||||
|
|
||||||
|
class PolicyRuntimeNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
PolicyRuntimeNode();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg);
|
||||||
|
void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||||
|
void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
|
||||||
|
void applyCmdVel(float vx, float vy, float vyaw);
|
||||||
|
void onPolicyLoop();
|
||||||
|
|
||||||
|
std::array<float, 53> buildObservation(
|
||||||
|
const sim2real_interfaces::msg::RuntimeState & state,
|
||||||
|
const std::array<float, 3> & cmd,
|
||||||
|
const std::array<float, 16> & last_actions) const;
|
||||||
|
|
||||||
|
std::array<float, 16> runPolicy(const std::array<float, 53> & obs);
|
||||||
|
bool isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const;
|
||||||
|
bool isCommandActive(const std::array<float, 3> & cmd) const;
|
||||||
|
|
||||||
|
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_pub_;
|
||||||
|
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_sub_;
|
||||||
|
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
||||||
|
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_stamped_sub_;
|
||||||
|
rclcpp::TimerBase::SharedPtr policy_timer_;
|
||||||
|
|
||||||
|
std::mutex mutex_;
|
||||||
|
sim2real_interfaces::msg::RuntimeState latest_state_;
|
||||||
|
bool has_state_{false};
|
||||||
|
std::chrono::steady_clock::time_point last_state_recv_time_{};
|
||||||
|
std::array<float, 3> cmd_{{0.0f, 0.0f, 0.0f}};
|
||||||
|
std::array<float, 3> raw_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||||
|
std::array<float, 16> last_actions_{};
|
||||||
|
std::uint32_t sequence_{0};
|
||||||
|
|
||||||
|
// Startup State Machine
|
||||||
|
enum class StartupState {
|
||||||
|
BOOT_HOLD,
|
||||||
|
STARTUP_SOFT_HOLD,
|
||||||
|
STARTUP_TRANSITION,
|
||||||
|
STARTUP_HOLD_AFTER,
|
||||||
|
RUNTIME
|
||||||
|
};
|
||||||
|
StartupState startup_state_{StartupState::BOOT_HOLD};
|
||||||
|
std::array<float, 16> start_pose_{};
|
||||||
|
std::array<float, 16> startup_delta_{};
|
||||||
|
rclcpp::Time state_start_time_{0, 0, RCL_ROS_TIME};
|
||||||
|
double transition_time_{4.0};
|
||||||
|
double hold_time_{1.0};
|
||||||
|
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||||
|
|
||||||
|
// ONNX Runtime members
|
||||||
|
std::string model_path_{"policies/model_rough.onnx"};
|
||||||
|
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||||
|
std::unique_ptr<Ort::Env> env_;
|
||||||
|
std::unique_ptr<Ort::Session> session_;
|
||||||
|
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||||
|
|
||||||
|
std::vector<std::string> input_names_str_;
|
||||||
|
std::vector<std::string> output_names_str_;
|
||||||
|
std::vector<const char*> input_names_char_;
|
||||||
|
std::vector<const char*> output_names_char_;
|
||||||
|
|
||||||
|
std::vector<std::int64_t> input_shape_;
|
||||||
|
std::vector<std::int64_t> output_shape_;
|
||||||
|
|
||||||
|
// Command filter and release states
|
||||||
|
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||||
|
float release_alpha_{0.0f};
|
||||||
|
float command_release_s_{0.35f};
|
||||||
|
float release_command_hold_s_{0.12f};
|
||||||
|
float release_posture_max_err_{0.35f};
|
||||||
|
float release_target_blend_s_{0.30f};
|
||||||
|
float clip_obs_{100.0f};
|
||||||
|
bool hold_zero_command_pose_{true};
|
||||||
|
bool enable_zero_cmd_suppression_{true};
|
||||||
|
bool require_active_command_to_release_{true};
|
||||||
|
bool zero_cmd_use_yaw_rate_{false};
|
||||||
|
bool runtime_released_{false};
|
||||||
|
float release_active_time_{0.0f};
|
||||||
|
float zero_cmd_lin_thresh_{0.05f};
|
||||||
|
float zero_cmd_yaw_thresh_{0.05f};
|
||||||
|
float zero_yaw_rate_thresh_{0.10f};
|
||||||
|
|
||||||
|
// E-stop and Safety variables
|
||||||
|
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||||
|
std::atomic<bool> estop_triggered_{false};
|
||||||
|
std::atomic<bool> safety_enabled_{true};
|
||||||
|
std::atomic<bool> safety_triggered_{false};
|
||||||
|
std::string safety_reason_{""};
|
||||||
|
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||||
|
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||||
|
|
||||||
|
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace sim2real_runtime
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>sim2real_runtime</name>
|
||||||
|
<version>0.1.0</version>
|
||||||
|
<description>Policy runtime node for sim2real_ros2.</description>
|
||||||
|
<maintainer email="todo@example.com">todo</maintainer>
|
||||||
|
<license>Proprietary</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>nav_msgs</depend>
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<depend>tf2_ros</depend>
|
||||||
|
<depend>sim2real_common</depend>
|
||||||
|
<depend>sim2real_interfaces</depend>
|
||||||
|
<exec_depend>python3-serial</exec_depend>
|
||||||
|
<exec_depend>rclpy</exec_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@@ -0,0 +1,190 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from enum import Enum
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import Twist
|
||||||
|
from rclpy.executors import ExternalShutdownException
|
||||||
|
from rclpy.node import Node
|
||||||
|
from std_msgs.msg import Bool, String
|
||||||
|
|
||||||
|
|
||||||
|
class ControlMode(str, Enum):
|
||||||
|
DISABLED = "DISABLED"
|
||||||
|
REMOTE = "REMOTE"
|
||||||
|
WEB = "WEB"
|
||||||
|
NAV = "NAV"
|
||||||
|
|
||||||
|
|
||||||
|
class CmdMuxNode(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True)
|
||||||
|
|
||||||
|
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper()
|
||||||
|
self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value)
|
||||||
|
self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value)
|
||||||
|
self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value)
|
||||||
|
self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value)
|
||||||
|
self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value)
|
||||||
|
self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value)
|
||||||
|
self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value)
|
||||||
|
self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value)
|
||||||
|
self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value)
|
||||||
|
self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value)
|
||||||
|
|
||||||
|
self.mode = self.parse_mode(self.default_mode)
|
||||||
|
self.estop = False
|
||||||
|
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||||
|
self.web_enabled = self.mode == ControlMode.WEB
|
||||||
|
self.nav_enabled = self.mode == ControlMode.NAV
|
||||||
|
|
||||||
|
self.latest_remote = Twist()
|
||||||
|
self.latest_web = Twist()
|
||||||
|
self.latest_nav = Twist()
|
||||||
|
self.remote_stamp: Optional[rclpy.time.Time] = None
|
||||||
|
self.web_stamp: Optional[rclpy.time.Time] = None
|
||||||
|
self.nav_stamp: Optional[rclpy.time.Time] = None
|
||||||
|
self.last_output = Twist()
|
||||||
|
self.last_pub_time = self.get_clock().now()
|
||||||
|
|
||||||
|
self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10)
|
||||||
|
self.mode_pub = self.create_publisher(String, "control/mode_state", 10)
|
||||||
|
self.status_pub = self.create_publisher(String, "control/mux_status", 10)
|
||||||
|
|
||||||
|
self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10)
|
||||||
|
self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10)
|
||||||
|
self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10)
|
||||||
|
self.create_subscription(String, "control/mode", self.on_mode, 10)
|
||||||
|
self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10)
|
||||||
|
self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10)
|
||||||
|
self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10)
|
||||||
|
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||||
|
|
||||||
|
period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02
|
||||||
|
self.timer = self.create_timer(period, self.on_timer)
|
||||||
|
self.get_logger().info(f"Command mux started in mode {self.mode.value}")
|
||||||
|
|
||||||
|
def parse_mode(self, value: str) -> ControlMode:
|
||||||
|
try:
|
||||||
|
return ControlMode(value.upper())
|
||||||
|
except ValueError:
|
||||||
|
self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED")
|
||||||
|
return ControlMode.DISABLED
|
||||||
|
|
||||||
|
def on_remote(self, msg: Twist) -> None:
|
||||||
|
self.latest_remote = msg
|
||||||
|
self.remote_stamp = self.get_clock().now()
|
||||||
|
|
||||||
|
def on_web(self, msg: Twist) -> None:
|
||||||
|
self.latest_web = msg
|
||||||
|
self.web_stamp = self.get_clock().now()
|
||||||
|
|
||||||
|
def on_nav(self, msg: Twist) -> None:
|
||||||
|
self.latest_nav = msg
|
||||||
|
self.nav_stamp = self.get_clock().now()
|
||||||
|
|
||||||
|
def on_mode(self, msg: String) -> None:
|
||||||
|
new_mode = self.parse_mode(msg.data)
|
||||||
|
if new_mode != self.mode:
|
||||||
|
self.mode = new_mode
|
||||||
|
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||||
|
self.web_enabled = self.mode == ControlMode.WEB
|
||||||
|
self.nav_enabled = self.mode == ControlMode.NAV
|
||||||
|
self.get_logger().info(f"Control mode changed to {self.mode.value}")
|
||||||
|
|
||||||
|
def on_remote_enabled(self, msg: Bool) -> None:
|
||||||
|
self.remote_enabled = bool(msg.data)
|
||||||
|
if self.remote_enabled:
|
||||||
|
self.mode = ControlMode.REMOTE
|
||||||
|
|
||||||
|
def on_web_enabled(self, msg: Bool) -> None:
|
||||||
|
self.web_enabled = bool(msg.data)
|
||||||
|
if self.web_enabled:
|
||||||
|
self.mode = ControlMode.WEB
|
||||||
|
|
||||||
|
def on_nav_enabled(self, msg: Bool) -> None:
|
||||||
|
self.nav_enabled = bool(msg.data)
|
||||||
|
if self.nav_enabled:
|
||||||
|
self.mode = ControlMode.NAV
|
||||||
|
|
||||||
|
def on_estop(self, msg: Bool) -> None:
|
||||||
|
self.estop = bool(msg.data)
|
||||||
|
if self.estop:
|
||||||
|
self.mode = ControlMode.DISABLED
|
||||||
|
|
||||||
|
def on_timer(self) -> None:
|
||||||
|
now = self.get_clock().now()
|
||||||
|
target = Twist()
|
||||||
|
source = "zero"
|
||||||
|
|
||||||
|
if not self.estop:
|
||||||
|
if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now):
|
||||||
|
target = self.latest_remote
|
||||||
|
source = "remote"
|
||||||
|
elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now):
|
||||||
|
target = self.latest_web
|
||||||
|
source = "web"
|
||||||
|
elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now):
|
||||||
|
target = self.latest_nav
|
||||||
|
source = "nav"
|
||||||
|
|
||||||
|
target = self.limit_twist(target)
|
||||||
|
target = self.accel_limit(target, now)
|
||||||
|
self.cmd_pub.publish(target)
|
||||||
|
self.mode_pub.publish(String(data=self.mode.value))
|
||||||
|
self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}"))
|
||||||
|
|
||||||
|
def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool:
|
||||||
|
if stamp is None:
|
||||||
|
return False
|
||||||
|
age_ms = (now - stamp).nanoseconds / 1.0e6
|
||||||
|
return age_ms <= timeout_ms
|
||||||
|
|
||||||
|
def limit_twist(self, msg: Twist) -> Twist:
|
||||||
|
out = Twist()
|
||||||
|
out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx)
|
||||||
|
out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy)
|
||||||
|
out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw)
|
||||||
|
return out
|
||||||
|
|
||||||
|
def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist:
|
||||||
|
dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3)
|
||||||
|
out = Twist()
|
||||||
|
out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt)
|
||||||
|
out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt)
|
||||||
|
out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt)
|
||||||
|
self.last_output = out
|
||||||
|
self.last_pub_time = now
|
||||||
|
return out
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def clamp(value: float, low: float, high: float) -> float:
|
||||||
|
return max(low, min(high, float(value)))
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def step(current: float, target: float, max_delta: float) -> float:
|
||||||
|
delta = target - current
|
||||||
|
if delta > max_delta:
|
||||||
|
return current + max_delta
|
||||||
|
if delta < -max_delta:
|
||||||
|
return current - max_delta
|
||||||
|
return target
|
||||||
|
|
||||||
|
|
||||||
|
def main(args: Optional[list[str]] = None) -> None:
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = CmdMuxNode()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except (KeyboardInterrupt, ExternalShutdownException):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,60 @@
|
|||||||
|
#include "sim2real_runtime/odom_relay_node.hpp"
|
||||||
|
|
||||||
|
namespace sim2real_runtime
|
||||||
|
{
|
||||||
|
|
||||||
|
OdomRelayNode::OdomRelayNode()
|
||||||
|
: Node("odom_relay_node")
|
||||||
|
{
|
||||||
|
odom_input_topic_ = declare_parameter<std::string>("odom_input_topic", "/odin1/odometry");
|
||||||
|
odom_output_topic_ = declare_parameter<std::string>("odom_output_topic", "/odom");
|
||||||
|
base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
|
||||||
|
publish_tf_ = declare_parameter<bool>("publish_tf", true);
|
||||||
|
|
||||||
|
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||||
|
odom_input_topic_, 10,
|
||||||
|
std::bind(&OdomRelayNode::onOdom, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(odom_output_topic_, 10);
|
||||||
|
|
||||||
|
if (publish_tf_) {
|
||||||
|
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(),
|
||||||
|
"Odom relay: %s -> %s (base_frame=%s, publish_tf=%s)",
|
||||||
|
odom_input_topic_.c_str(), odom_output_topic_.c_str(),
|
||||||
|
base_frame_.c_str(), publish_tf_ ? "true" : "false");
|
||||||
|
}
|
||||||
|
|
||||||
|
void OdomRelayNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||||
|
{
|
||||||
|
// Remap child_frame_id and republish
|
||||||
|
auto out_msg = *msg;
|
||||||
|
out_msg.header.frame_id = "odom";
|
||||||
|
out_msg.child_frame_id = base_frame_;
|
||||||
|
odom_pub_->publish(out_msg);
|
||||||
|
|
||||||
|
// Broadcast TF: odom → base_link
|
||||||
|
if (publish_tf_ && tf_broadcaster_) {
|
||||||
|
geometry_msgs::msg::TransformStamped tf;
|
||||||
|
tf.header.stamp = msg->header.stamp;
|
||||||
|
tf.header.frame_id = "odom";
|
||||||
|
tf.child_frame_id = base_frame_;
|
||||||
|
tf.transform.translation.x = msg->pose.pose.position.x;
|
||||||
|
tf.transform.translation.y = msg->pose.pose.position.y;
|
||||||
|
tf.transform.translation.z = msg->pose.pose.position.z;
|
||||||
|
tf.transform.rotation = msg->pose.pose.orientation;
|
||||||
|
tf_broadcaster_->sendTransform(tf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace sim2real_runtime
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<sim2real_runtime::OdomRelayNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,583 @@
|
|||||||
|
#include "sim2real_runtime/policy_runtime_node.hpp"
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
#include "sim2real_common/deployment_contract.hpp"
|
||||||
|
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
|
||||||
|
namespace sim2real_runtime
|
||||||
|
{
|
||||||
|
|
||||||
|
// Named constants for timing and command filtering
|
||||||
|
constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz)
|
||||||
|
constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz)
|
||||||
|
constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz)
|
||||||
|
|
||||||
|
PolicyRuntimeNode::PolicyRuntimeNode()
|
||||||
|
: Node("sim2real_runtime_node")
|
||||||
|
{
|
||||||
|
// 1. Declare and get parameters
|
||||||
|
model_path_ = declare_parameter<std::string>("model_path", "policies/model_rough.onnx");
|
||||||
|
use_cuda_ = declare_parameter<bool>("use_cuda", false); // enable CUDA EP on Orin Nano
|
||||||
|
|
||||||
|
// Safety parameters
|
||||||
|
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||||
|
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||||
|
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||||
|
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||||
|
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||||
|
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||||
|
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||||
|
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||||
|
|
||||||
|
command_release_s_ = static_cast<float>(declare_parameter<double>("command_release_s", 0.35));
|
||||||
|
release_command_hold_s_ = static_cast<float>(declare_parameter<double>("release_command_hold_s", 0.12));
|
||||||
|
release_posture_max_err_ = static_cast<float>(declare_parameter<double>("release_posture_max_err", 0.35));
|
||||||
|
release_target_blend_s_ = static_cast<float>(declare_parameter<double>("release_target_blend_s", 0.30));
|
||||||
|
clip_obs_ = static_cast<float>(declare_parameter<double>("clip_obs", 100.0));
|
||||||
|
hold_zero_command_pose_ = declare_parameter<bool>("hold_zero_command_pose", true);
|
||||||
|
enable_zero_cmd_suppression_ = declare_parameter<bool>("enable_zero_cmd_suppression", true);
|
||||||
|
require_active_command_to_release_ = declare_parameter<bool>("require_active_command_to_release", true);
|
||||||
|
zero_cmd_use_yaw_rate_ = declare_parameter<bool>("zero_cmd_use_yaw_rate", true);
|
||||||
|
runtime_released_ = !require_active_command_to_release_;
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str());
|
||||||
|
|
||||||
|
// Initialize StandBalanceController
|
||||||
|
stand_balance_ = std::make_unique<sim2real_common::StandBalanceController>(0.02);
|
||||||
|
|
||||||
|
// Initialize SafetyMonitor and RuntimeGuard
|
||||||
|
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||||
|
static_cast<float>(max_target_offset),
|
||||||
|
static_cast<float>(max_ang_vel),
|
||||||
|
static_cast<float>(max_tilt_z),
|
||||||
|
clip_to_brake,
|
||||||
|
static_cast<float>(hard_target_offset)
|
||||||
|
);
|
||||||
|
|
||||||
|
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||||
|
static_cast<float>(max_ang_vel + 2.0),
|
||||||
|
static_cast<float>(max_tilt_z),
|
||||||
|
static_cast<float>(imu_age_warn_ms),
|
||||||
|
static_cast<float>(imu_age_stop_ms)
|
||||||
|
);
|
||||||
|
|
||||||
|
// 2. Initialize Ort C++ environment
|
||||||
|
try {
|
||||||
|
env_ = std::make_unique<Ort::Env>(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env");
|
||||||
|
|
||||||
|
Ort::SessionOptions session_options;
|
||||||
|
// single-thread ORIN optimization to prevent thread scheduling jitter
|
||||||
|
session_options.SetIntraOpNumThreads(1);
|
||||||
|
session_options.SetInterOpNumThreads(1);
|
||||||
|
session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
||||||
|
|
||||||
|
// CUDA Execution Provider (Orin Nano GPU acceleration)
|
||||||
|
if (use_cuda_) {
|
||||||
|
try {
|
||||||
|
OrtCUDAProviderOptions cuda_opts{};
|
||||||
|
cuda_opts.device_id = 0;
|
||||||
|
// enable_cuda_graph: false for single-inference RL policy (avoids overhead)
|
||||||
|
session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
||||||
|
RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)");
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
RCLCPP_WARN(get_logger(),
|
||||||
|
"CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.",
|
||||||
|
e.what());
|
||||||
|
use_cuda_ = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
session_ = std::make_unique<Ort::Session>(*env_, model_path_.c_str(), session_options);
|
||||||
|
memory_info_ = std::make_unique<Ort::MemoryInfo>(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU));
|
||||||
|
|
||||||
|
// Get input/output nodes names and shapes
|
||||||
|
Ort::AllocatorWithDefaultOptions allocator;
|
||||||
|
|
||||||
|
std::size_t num_inputs = session_->GetInputCount();
|
||||||
|
for (std::size_t i = 0; i < num_inputs; ++i) {
|
||||||
|
auto name = session_->GetInputNameAllocated(i, allocator);
|
||||||
|
input_names_str_.push_back(std::string(name.get()));
|
||||||
|
}
|
||||||
|
for (const auto& name : input_names_str_) {
|
||||||
|
input_names_char_.push_back(name.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
std::size_t num_outputs = session_->GetOutputCount();
|
||||||
|
for (std::size_t i = 0; i < num_outputs; ++i) {
|
||||||
|
auto name = session_->GetOutputNameAllocated(i, allocator);
|
||||||
|
output_names_str_.push_back(std::string(name.get()));
|
||||||
|
}
|
||||||
|
for (const auto& name : output_names_str_) {
|
||||||
|
output_names_char_.push_back(name.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
auto input_type_info = session_->GetInputTypeInfo(0);
|
||||||
|
auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo();
|
||||||
|
input_shape_ = input_tensor_info.GetShape();
|
||||||
|
if (input_shape_[0] < 0) {
|
||||||
|
input_shape_[0] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto output_type_info = session_->GetOutputTypeInfo(0);
|
||||||
|
auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo();
|
||||||
|
output_shape_ = output_tensor_info.GetShape();
|
||||||
|
if (output_shape_[0] < 0) {
|
||||||
|
output_shape_[0] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Validate output shape matches expected action dimension
|
||||||
|
if (output_shape_.size() < 2 || output_shape_[1] != static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim)) {
|
||||||
|
RCLCPP_FATAL(get_logger(),
|
||||||
|
"ONNX model output dimension mismatch! Expected %ld, got %ld. Wrong model?",
|
||||||
|
static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim),
|
||||||
|
output_shape_.size() >= 2 ? output_shape_[1] : -1);
|
||||||
|
throw std::runtime_error("ONNX model output shape mismatch");
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]",
|
||||||
|
input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what());
|
||||||
|
throw;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 3. Create publishers and subscriptions
|
||||||
|
target_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeTarget>("runtime/target", 10);
|
||||||
|
state_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeState>(
|
||||||
|
"runtime/state", 10,
|
||||||
|
std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1));
|
||||||
|
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
|
||||||
|
"cmd_vel", 10,
|
||||||
|
std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1));
|
||||||
|
cmd_stamped_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
|
||||||
|
"cmd_vel_stamped", 10,
|
||||||
|
std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1));
|
||||||
|
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||||
|
"/safety/estop", 10,
|
||||||
|
std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
// 4. Timer at 50Hz (20ms)
|
||||||
|
policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this));
|
||||||
|
|
||||||
|
last_actions_.fill(0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
latest_state_ = *msg;
|
||||||
|
has_state_ = true;
|
||||||
|
last_state_recv_time_ = std::chrono::steady_clock::now();
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw)
|
||||||
|
{
|
||||||
|
// Velocity saturation limits (consistent with training domain)
|
||||||
|
constexpr float kMaxLinVelX = 0.8f; // m/s
|
||||||
|
constexpr float kMaxLinVelY = 0.3f; // m/s
|
||||||
|
constexpr float kMaxAngVelZ = 0.5f; // rad/s
|
||||||
|
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX);
|
||||||
|
raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY);
|
||||||
|
raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ);
|
||||||
|
cmd_ = raw_cmd_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||||
|
{
|
||||||
|
applyCmdVel(
|
||||||
|
static_cast<float>(msg->linear.x),
|
||||||
|
static_cast<float>(msg->linear.y),
|
||||||
|
static_cast<float>(msg->angular.z));
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
|
||||||
|
{
|
||||||
|
applyCmdVel(
|
||||||
|
static_cast<float>(msg->twist.linear.x),
|
||||||
|
static_cast<float>(msg->twist.linear.y),
|
||||||
|
static_cast<float>(msg->twist.angular.z));
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
estop_triggered_ = msg->data;
|
||||||
|
if (estop_triggered_) {
|
||||||
|
RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!");
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(get_logger(), "E-stop reset.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<float, 53> PolicyRuntimeNode::buildObservation(
|
||||||
|
const sim2real_interfaces::msg::RuntimeState & state,
|
||||||
|
const std::array<float, 3> & cmd,
|
||||||
|
const std::array<float, 16> & last_actions) const
|
||||||
|
{
|
||||||
|
std::array<float, 53> obs{};
|
||||||
|
std::size_t cursor = 0;
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; ++i) {
|
||||||
|
obs[cursor++] = state.imu_gyro[i] * 0.25f;
|
||||||
|
}
|
||||||
|
for (int i = 0; i < 3; ++i) {
|
||||||
|
obs[cursor++] = state.projected_gravity[i];
|
||||||
|
}
|
||||||
|
for (float v : cmd) {
|
||||||
|
obs[cursor++] = v;
|
||||||
|
}
|
||||||
|
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||||
|
obs[cursor++] = state.joint_pos[i] - sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||||
|
}
|
||||||
|
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||||
|
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||||
|
}
|
||||||
|
for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||||
|
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||||
|
}
|
||||||
|
for (float v : last_actions) {
|
||||||
|
obs[cursor++] = v;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clip observations values to ±clip_obs_
|
||||||
|
if (clip_obs_ > 0.0f) {
|
||||||
|
for (float & v : obs) {
|
||||||
|
v = std::clamp(v, -clip_obs_, clip_obs_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return obs;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<float, 16> PolicyRuntimeNode::runPolicy(const std::array<float, 53> & obs)
|
||||||
|
{
|
||||||
|
std::array<float, 16> action{};
|
||||||
|
try {
|
||||||
|
auto input_tensor = Ort::Value::CreateTensor<float>(
|
||||||
|
*memory_info_,
|
||||||
|
const_cast<float*>(obs.data()),
|
||||||
|
obs.size(),
|
||||||
|
input_shape_.data(),
|
||||||
|
input_shape_.size()
|
||||||
|
);
|
||||||
|
|
||||||
|
auto output_tensor = Ort::Value::CreateTensor<float>(
|
||||||
|
*memory_info_,
|
||||||
|
action.data(),
|
||||||
|
action.size(),
|
||||||
|
output_shape_.data(),
|
||||||
|
output_shape_.size()
|
||||||
|
);
|
||||||
|
|
||||||
|
session_->Run(
|
||||||
|
Ort::RunOptions{nullptr},
|
||||||
|
input_names_char_.data(),
|
||||||
|
&input_tensor,
|
||||||
|
1,
|
||||||
|
output_names_char_.data(),
|
||||||
|
&output_tensor,
|
||||||
|
1
|
||||||
|
);
|
||||||
|
} catch (const std::exception& e) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what());
|
||||||
|
action.fill(0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (float& v : action) {
|
||||||
|
v = std::clamp(v, -10.0f, 10.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
return action;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PolicyRuntimeNode::isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const
|
||||||
|
{
|
||||||
|
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||||
|
const bool cmd_is_zero = planar_cmd < zero_cmd_lin_thresh_ && std::abs(cmd[2]) < zero_cmd_yaw_thresh_;
|
||||||
|
if (!zero_cmd_use_yaw_rate_) {
|
||||||
|
return cmd_is_zero;
|
||||||
|
}
|
||||||
|
return cmd_is_zero && std::abs(imu_gyro[2]) < zero_yaw_rate_thresh_;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PolicyRuntimeNode::isCommandActive(const std::array<float, 3> & cmd) const
|
||||||
|
{
|
||||||
|
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||||
|
return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PolicyRuntimeNode::onPolicyLoop()
|
||||||
|
{
|
||||||
|
sim2real_interfaces::msg::RuntimeState state;
|
||||||
|
std::array<float, 3> cmd{};
|
||||||
|
std::array<float, 3> raw_cmd{};
|
||||||
|
std::array<float, 16> last_actions{};
|
||||||
|
bool estop_active = false;
|
||||||
|
bool safety_active = false;
|
||||||
|
double state_age_ms = 0.0;
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
if (!has_state_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
state = latest_state_;
|
||||||
|
cmd = cmd_;
|
||||||
|
raw_cmd = raw_cmd_;
|
||||||
|
last_actions = last_actions_;
|
||||||
|
estop_active = estop_triggered_;
|
||||||
|
safety_active = safety_triggered_;
|
||||||
|
if (last_state_recv_time_.time_since_epoch().count() != 0) {
|
||||||
|
state_age_ms = std::chrono::duration<double, std::milli>(
|
||||||
|
std::chrono::steady_clock::now() - last_state_recv_time_).count();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 1) Run RuntimeGuard check
|
||||||
|
if (safety_enabled_ && !safety_active) {
|
||||||
|
std::vector<float> extra_vals;
|
||||||
|
extra_vals.reserve(48);
|
||||||
|
for (float v : state.joint_pos) extra_vals.push_back(v);
|
||||||
|
for (float v : state.joint_vel) extra_vals.push_back(v);
|
||||||
|
for (float v : last_actions) extra_vals.push_back(v);
|
||||||
|
|
||||||
|
const float effective_imu_age_ms = static_cast<float>(std::max(
|
||||||
|
static_cast<double>(state.imu_age_ms), state_age_ms));
|
||||||
|
|
||||||
|
auto guard_decision = runtime_guard_->check(
|
||||||
|
state.imu_gyro, state.projected_gravity, effective_imu_age_ms, estop_active, extra_vals);
|
||||||
|
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
safety_triggered_ = true;
|
||||||
|
}
|
||||||
|
safety_active = true;
|
||||||
|
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||||
|
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||||
|
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (safety_active) {
|
||||||
|
sim2real_interfaces::msg::RuntimeTarget target;
|
||||||
|
target.stamp = now();
|
||||||
|
target.sequence = sequence_++;
|
||||||
|
target.raw_command = raw_cmd;
|
||||||
|
target.command = cmd;
|
||||||
|
target.raw_action.fill(0.0f);
|
||||||
|
target.scaled_action.fill(0.0f);
|
||||||
|
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||||
|
target.target_source = "safety_brake";
|
||||||
|
target.target_age_ms = 0.0f;
|
||||||
|
target_pub_->publish(target);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
sim2real_interfaces::msg::RuntimeTarget target;
|
||||||
|
target.stamp = now();
|
||||||
|
target.sequence = sequence_++;
|
||||||
|
target.raw_command = raw_cmd;
|
||||||
|
target.raw_action.fill(0.0f);
|
||||||
|
target.scaled_action.fill(0.0f);
|
||||||
|
target.command = cmd;
|
||||||
|
|
||||||
|
const auto now_time = rclcpp::Time(target.stamp);
|
||||||
|
|
||||||
|
if (startup_state_ == StartupState::BOOT_HOLD) {
|
||||||
|
// 1. Initial State Read
|
||||||
|
start_pose_ = state.joint_pos;
|
||||||
|
start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0
|
||||||
|
|
||||||
|
// 2. Shortest periodic delta to stand pose
|
||||||
|
float max_dev = 0.0f;
|
||||||
|
for (std::size_t i = 0; i < 12; ++i) {
|
||||||
|
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i];
|
||||||
|
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||||
|
startup_delta_[i] = delta;
|
||||||
|
max_dev = std::max(max_dev, std::abs(delta));
|
||||||
|
}
|
||||||
|
startup_delta_[12] = startup_delta_[13] = startup_delta_[14] = startup_delta_[15] = 0.0f;
|
||||||
|
|
||||||
|
if (max_dev > 3.0f) {
|
||||||
|
RCLCPP_WARN(get_logger(), "Measured joint dev too large (%f rad > 3.0 rad). Aborting standup transition.", max_dev);
|
||||||
|
target.target = start_pose_;
|
||||||
|
target.target_source = "boot_hold";
|
||||||
|
target_pub_->publish(target);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Adapt transition time: min 2s, max 6s, 1.5s per rad
|
||||||
|
transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0);
|
||||||
|
startup_state_ = StartupState::STARTUP_SOFT_HOLD;
|
||||||
|
state_start_time_ = now_time;
|
||||||
|
RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) {
|
||||||
|
double elapsed = (now_time - state_start_time_).seconds();
|
||||||
|
target.target = start_pose_;
|
||||||
|
target.target_source = "startup_soft_hold";
|
||||||
|
|
||||||
|
if (elapsed >= 1.0) { // 1s soft hold
|
||||||
|
startup_state_ = StartupState::STARTUP_TRANSITION;
|
||||||
|
state_start_time_ = now_time;
|
||||||
|
RCLCPP_INFO(get_logger(), "Transitioning to stand pose...");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (startup_state_ == StartupState::STARTUP_TRANSITION) {
|
||||||
|
double elapsed = (now_time - state_start_time_).seconds();
|
||||||
|
double phase = std::min(1.0, elapsed / transition_time_);
|
||||||
|
|
||||||
|
// Cosine blend interpolation
|
||||||
|
double blend = 0.5 - 0.5 * std::cos(M_PI * phase);
|
||||||
|
for (std::size_t i = 0; i < 16; ++i) {
|
||||||
|
target.target[i] = start_pose_[i] + blend * startup_delta_[i];
|
||||||
|
}
|
||||||
|
target.target_source = "startup_hold";
|
||||||
|
|
||||||
|
if (phase >= 1.0) {
|
||||||
|
// Settle check
|
||||||
|
float max_pos_err = 0.0f;
|
||||||
|
for (std::size_t i = 0; i < 12; ++i) {
|
||||||
|
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i];
|
||||||
|
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||||
|
max_pos_err = std::max(max_pos_err, std::abs(delta));
|
||||||
|
}
|
||||||
|
float max_vel_err = 0.0f;
|
||||||
|
for (std::size_t i = 0; i < 12; ++i) {
|
||||||
|
max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i]));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (max_pos_err <= 0.30f && max_vel_err <= 0.6f) {
|
||||||
|
startup_state_ = StartupState::STARTUP_HOLD_AFTER;
|
||||||
|
state_start_time_ = now_time;
|
||||||
|
RCLCPP_INFO(get_logger(), "Pose settled. Holding for 1.0s...");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) {
|
||||||
|
double elapsed = (now_time - state_start_time_).seconds();
|
||||||
|
|
||||||
|
// Run stand balance controller during holding phase
|
||||||
|
target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd);
|
||||||
|
target.target_source = "startup_hold";
|
||||||
|
|
||||||
|
if (elapsed >= 1.0 && stand_balance_->isStable()) {
|
||||||
|
startup_state_ = StartupState::RUNTIME;
|
||||||
|
RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (startup_state_ == StartupState::RUNTIME) {
|
||||||
|
// Python template uses the command directly in policy obs/release logic.
|
||||||
|
// Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here.
|
||||||
|
filtered_cmd_ = cmd;
|
||||||
|
|
||||||
|
const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array<float, 3>{0.0f, 0.0f, 0.0f});
|
||||||
|
const bool zero_command = isZeroCommand(cmd, state.imu_gyro);
|
||||||
|
|
||||||
|
if (!runtime_released_) {
|
||||||
|
if (require_active_command_to_release_) {
|
||||||
|
if (isCommandActive(cmd)) {
|
||||||
|
release_active_time_ += kPolicyDt;
|
||||||
|
} else {
|
||||||
|
release_active_time_ = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float max_hold_err = 0.0f;
|
||||||
|
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||||
|
max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i]));
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool active_ready = release_active_time_ >= release_command_hold_s_;
|
||||||
|
const bool posture_ready = max_hold_err <= release_posture_max_err_;
|
||||||
|
if (active_ready && posture_ready) {
|
||||||
|
runtime_released_ = true;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
runtime_released_ = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!runtime_released_ || zero_command) {
|
||||||
|
release_alpha_ = 0.0f;
|
||||||
|
target.runtime_released = false;
|
||||||
|
target.release_alpha = 0.0f;
|
||||||
|
target.zero_command = zero_command;
|
||||||
|
target.raw_action.fill(0.0f);
|
||||||
|
target.scaled_action.fill(0.0f);
|
||||||
|
last_actions.fill(0.0f);
|
||||||
|
target.target_source = "runtime_zero_hold";
|
||||||
|
target.target = target_hold;
|
||||||
|
if (!runtime_released_) {
|
||||||
|
target.target_source = "runtime_hold";
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f));
|
||||||
|
target.runtime_released = (release_alpha_ >= 1.0f);
|
||||||
|
target.release_alpha = release_alpha_;
|
||||||
|
target.zero_command = false;
|
||||||
|
target.command = cmd;
|
||||||
|
|
||||||
|
auto raw = runPolicy(buildObservation(state, cmd, last_actions));
|
||||||
|
for (float & v : raw) {
|
||||||
|
v *= release_alpha_;
|
||||||
|
}
|
||||||
|
target.raw_action = raw;
|
||||||
|
|
||||||
|
const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt)));
|
||||||
|
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||||
|
target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i];
|
||||||
|
const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||||
|
target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target;
|
||||||
|
last_actions[i] = raw[i];
|
||||||
|
}
|
||||||
|
target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2) Run SafetyMonitor check on computed target
|
||||||
|
if (safety_enabled_) {
|
||||||
|
auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active);
|
||||||
|
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
safety_triggered_ = true;
|
||||||
|
}
|
||||||
|
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||||
|
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||||
|
|
||||||
|
// Override target to safety_brake damping pose
|
||||||
|
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||||
|
target.target_source = "safety_brake";
|
||||||
|
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||||
|
target.target = safety_decision.clipped_target;
|
||||||
|
target.target_source = "safety_clip";
|
||||||
|
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
target.target_age_ms = 0.0f;
|
||||||
|
|
||||||
|
{
|
||||||
|
std::scoped_lock<std::mutex> lock(mutex_);
|
||||||
|
last_actions_ = last_actions;
|
||||||
|
}
|
||||||
|
|
||||||
|
target_pub_->publish(target);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace sim2real_runtime
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<sim2real_runtime::PolicyRuntimeNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,250 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import serial
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import Twist
|
||||||
|
from rclpy.executors import ExternalShutdownException
|
||||||
|
from rclpy.node import Node
|
||||||
|
from std_msgs.msg import Bool
|
||||||
|
|
||||||
|
SBUS_FRAME_SIZE = 25
|
||||||
|
SBUS_RC_MID = 1024
|
||||||
|
SBUS_AXIS_SCALE = 660.0
|
||||||
|
|
||||||
|
SWITCH_LOW = -1
|
||||||
|
SWITCH_MID = 0
|
||||||
|
SWITCH_HIGH = 1
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class RemoteSwitchState:
|
||||||
|
ch7: int = SWITCH_MID
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class RemoteControlState:
|
||||||
|
ch1: int = 0
|
||||||
|
ch2: int = 0
|
||||||
|
ch3: int = 0
|
||||||
|
ch4: int = 0
|
||||||
|
switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
|
||||||
|
frame_ok: bool = False
|
||||||
|
|
||||||
|
@property
|
||||||
|
def estop_requested(self) -> bool:
|
||||||
|
return self.switches.ch7 == SWITCH_HIGH
|
||||||
|
|
||||||
|
|
||||||
|
class SbusUartReceiver:
|
||||||
|
def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int):
|
||||||
|
self.port = port
|
||||||
|
self.baudrate = int(baudrate)
|
||||||
|
self.timeout = float(timeout)
|
||||||
|
self.axis_deadzone = int(axis_deadzone)
|
||||||
|
self.serial: Optional[serial.Serial] = None
|
||||||
|
self.buffer = bytearray()
|
||||||
|
self.state = RemoteControlState()
|
||||||
|
|
||||||
|
def open(self) -> None:
|
||||||
|
if self.serial and self.serial.is_open:
|
||||||
|
return
|
||||||
|
self.serial = serial.Serial(
|
||||||
|
port=self.port,
|
||||||
|
baudrate=self.baudrate,
|
||||||
|
timeout=self.timeout,
|
||||||
|
bytesize=serial.EIGHTBITS,
|
||||||
|
parity=serial.PARITY_EVEN,
|
||||||
|
stopbits=serial.STOPBITS_TWO,
|
||||||
|
)
|
||||||
|
|
||||||
|
def close(self) -> None:
|
||||||
|
if self.serial and self.serial.is_open:
|
||||||
|
self.serial.close()
|
||||||
|
|
||||||
|
def poll(self) -> RemoteControlState:
|
||||||
|
if not self.serial or not self.serial.is_open:
|
||||||
|
raise RuntimeError("remote uart is not open")
|
||||||
|
|
||||||
|
waiting = self.serial.in_waiting
|
||||||
|
if waiting:
|
||||||
|
self.buffer.extend(self.serial.read(waiting))
|
||||||
|
|
||||||
|
while len(self.buffer) >= SBUS_FRAME_SIZE:
|
||||||
|
start_idx = self.buffer.find(0x0F)
|
||||||
|
if start_idx < 0:
|
||||||
|
self.buffer.clear()
|
||||||
|
break
|
||||||
|
if start_idx > 0:
|
||||||
|
del self.buffer[:start_idx]
|
||||||
|
if len(self.buffer) < SBUS_FRAME_SIZE:
|
||||||
|
break
|
||||||
|
frame = bytes(self.buffer[:SBUS_FRAME_SIZE])
|
||||||
|
del self.buffer[:SBUS_FRAME_SIZE]
|
||||||
|
parsed = self._parse_frame(frame)
|
||||||
|
if parsed is not None:
|
||||||
|
self.state = parsed
|
||||||
|
return self.state
|
||||||
|
|
||||||
|
def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
|
||||||
|
if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
|
||||||
|
return None
|
||||||
|
|
||||||
|
channels = [0] * 16
|
||||||
|
channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
|
||||||
|
channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
|
||||||
|
channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
|
||||||
|
channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
|
||||||
|
channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
|
||||||
|
channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
|
||||||
|
channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
|
||||||
|
channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
|
||||||
|
channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
|
||||||
|
channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
|
||||||
|
|
||||||
|
if channels[0] < 100:
|
||||||
|
return None
|
||||||
|
|
||||||
|
state = RemoteControlState(
|
||||||
|
ch1=self._normalize_axis(channels[0]),
|
||||||
|
ch2=self._normalize_axis(channels[1]),
|
||||||
|
ch3=self._normalize_axis(channels[3]),
|
||||||
|
ch4=self._normalize_axis(channels[2]),
|
||||||
|
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||||
|
frame_ok=True,
|
||||||
|
)
|
||||||
|
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||||
|
return None
|
||||||
|
return state
|
||||||
|
|
||||||
|
def _normalize_axis(self, value: int) -> int:
|
||||||
|
mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
|
||||||
|
return 0 if abs(mapped) <= self.axis_deadzone else mapped
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _decode_switch(value: int) -> int:
|
||||||
|
if value < 500:
|
||||||
|
return SWITCH_LOW
|
||||||
|
if value > 1500:
|
||||||
|
return SWITCH_HIGH
|
||||||
|
return SWITCH_MID
|
||||||
|
|
||||||
|
|
||||||
|
class RemoteUartNode(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True)
|
||||||
|
|
||||||
|
self.enabled = bool(self.declare_parameter("remote_enabled", True).value)
|
||||||
|
self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value)
|
||||||
|
self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value)
|
||||||
|
self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value)
|
||||||
|
self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value)
|
||||||
|
self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value)
|
||||||
|
self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0)
|
||||||
|
self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value)
|
||||||
|
self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value)
|
||||||
|
self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value)
|
||||||
|
self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value)
|
||||||
|
self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value)
|
||||||
|
self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value)
|
||||||
|
self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value)
|
||||||
|
self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value)
|
||||||
|
self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value)
|
||||||
|
|
||||||
|
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||||
|
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||||
|
self.receiver: Optional[SbusUartReceiver] = None
|
||||||
|
self.estop_published = False
|
||||||
|
self.open_error_logged = False
|
||||||
|
|
||||||
|
if self.enabled:
|
||||||
|
self.receiver = SbusUartReceiver(
|
||||||
|
port=self.port,
|
||||||
|
baudrate=self.baudrate,
|
||||||
|
timeout=self.timeout,
|
||||||
|
axis_deadzone=self.axis_deadzone,
|
||||||
|
)
|
||||||
|
try:
|
||||||
|
self.receiver.open()
|
||||||
|
self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud")
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}")
|
||||||
|
self.open_error_logged = True
|
||||||
|
else:
|
||||||
|
self.get_logger().warn("Remote UART node is disabled by parameter")
|
||||||
|
|
||||||
|
period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02
|
||||||
|
self.timer = self.create_timer(period, self.on_timer)
|
||||||
|
|
||||||
|
def destroy_node(self) -> bool:
|
||||||
|
if self.receiver is not None:
|
||||||
|
self.receiver.close()
|
||||||
|
return super().destroy_node()
|
||||||
|
|
||||||
|
def on_timer(self) -> None:
|
||||||
|
if not self.enabled or self.receiver is None:
|
||||||
|
return
|
||||||
|
|
||||||
|
try:
|
||||||
|
if not self.receiver.serial or not self.receiver.serial.is_open:
|
||||||
|
self.receiver.open()
|
||||||
|
state = self.receiver.poll()
|
||||||
|
except Exception as exc:
|
||||||
|
if not self.open_error_logged:
|
||||||
|
self.get_logger().error(f"Remote UART poll failed: {exc}")
|
||||||
|
self.open_error_logged = True
|
||||||
|
return
|
||||||
|
|
||||||
|
self.open_error_logged = False
|
||||||
|
|
||||||
|
if state.estop_requested:
|
||||||
|
if not self.estop_published or not self.estop_latch:
|
||||||
|
self.estop_pub.publish(Bool(data=True))
|
||||||
|
self.get_logger().warn("Remote E-stop requested by CH7 high")
|
||||||
|
self.estop_published = True
|
||||||
|
self.publish_zero_cmd()
|
||||||
|
return
|
||||||
|
|
||||||
|
if not self.estop_latch and self.estop_published:
|
||||||
|
self.estop_pub.publish(Bool(data=False))
|
||||||
|
self.estop_published = False
|
||||||
|
|
||||||
|
active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||||
|
if active or self.publish_inactive_zero:
|
||||||
|
cmd = Twist()
|
||||||
|
cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
|
||||||
|
cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
|
||||||
|
cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
|
||||||
|
self.cmd_pub.publish(cmd)
|
||||||
|
|
||||||
|
def publish_zero_cmd(self) -> None:
|
||||||
|
self.cmd_pub.publish(Twist())
|
||||||
|
|
||||||
|
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||||
|
if abs(raw_value) <= self.active_threshold:
|
||||||
|
return 0.0
|
||||||
|
scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
|
||||||
|
if invert:
|
||||||
|
scaled = -scaled
|
||||||
|
return float(scaled * limit)
|
||||||
|
|
||||||
|
|
||||||
|
def main(args: Optional[list[str]] = None) -> None:
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = RemoteUartNode()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except (KeyboardInterrupt, ExternalShutdownException):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,261 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import socket
|
||||||
|
from typing import Any, Optional
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import Twist
|
||||||
|
from rclpy.executors import ExternalShutdownException
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sim2real_interfaces.msg import RuntimeState, RuntimeTarget
|
||||||
|
from std_msgs.msg import Bool, String
|
||||||
|
|
||||||
|
|
||||||
|
class WebUdpBridgeNode(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True)
|
||||||
|
|
||||||
|
self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value)
|
||||||
|
self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value)
|
||||||
|
self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value)
|
||||||
|
self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value)
|
||||||
|
self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value)
|
||||||
|
self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value)
|
||||||
|
self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value)
|
||||||
|
self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value)
|
||||||
|
self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value)
|
||||||
|
self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value)
|
||||||
|
self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value)
|
||||||
|
self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value)
|
||||||
|
|
||||||
|
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
self.sock.setblocking(False)
|
||||||
|
self.sock.bind((self.listen_host, self.listen_port))
|
||||||
|
|
||||||
|
self.client_addr: Optional[tuple[str, int]] = None
|
||||||
|
if self.remote_host:
|
||||||
|
self.client_addr = (self.remote_host, self.remote_port)
|
||||||
|
|
||||||
|
self.latest_target: Optional[RuntimeTarget] = None
|
||||||
|
self.latest_state: Optional[RuntimeState] = None
|
||||||
|
self.latest_cmd = Twist()
|
||||||
|
self.latest_mode = "UNKNOWN"
|
||||||
|
self.latest_mux_status = ""
|
||||||
|
self.estop = False
|
||||||
|
self.web_enabled = False
|
||||||
|
self.last_cmd_time = self.get_clock().now()
|
||||||
|
self.timeout_estop_sent = False
|
||||||
|
|
||||||
|
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10)
|
||||||
|
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||||
|
self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10)
|
||||||
|
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||||
|
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||||
|
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||||
|
|
||||||
|
self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10)
|
||||||
|
self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10)
|
||||||
|
self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10)
|
||||||
|
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||||
|
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||||
|
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
|
||||||
|
|
||||||
|
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||||
|
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||||
|
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||||
|
|
||||||
|
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||||
|
|
||||||
|
def on_target(self, msg: RuntimeTarget) -> None:
|
||||||
|
self.latest_target = msg
|
||||||
|
|
||||||
|
def on_state(self, msg: RuntimeState) -> None:
|
||||||
|
self.latest_state = msg
|
||||||
|
|
||||||
|
def on_cmd_vel(self, msg: Twist) -> None:
|
||||||
|
self.latest_cmd = msg
|
||||||
|
|
||||||
|
def on_estop(self, msg: Bool) -> None:
|
||||||
|
self.estop = bool(msg.data)
|
||||||
|
|
||||||
|
def on_mode_state(self, msg: String) -> None:
|
||||||
|
self.latest_mode = msg.data
|
||||||
|
|
||||||
|
def on_mux_status(self, msg: String) -> None:
|
||||||
|
self.latest_mux_status = msg.data
|
||||||
|
|
||||||
|
def on_rx_timer(self) -> None:
|
||||||
|
if not self.enabled:
|
||||||
|
return
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
data, addr = self.sock.recvfrom(self.max_packet_bytes)
|
||||||
|
except BlockingIOError:
|
||||||
|
break
|
||||||
|
except OSError as exc:
|
||||||
|
self.get_logger().warn(f"UDP receive failed: {exc}")
|
||||||
|
break
|
||||||
|
self.client_addr = addr
|
||||||
|
try:
|
||||||
|
payload = json.loads(data.decode("utf-8"))
|
||||||
|
self.handle_packet(payload)
|
||||||
|
except Exception as exc:
|
||||||
|
self.send_packet({"type": "error", "message": str(exc)})
|
||||||
|
|
||||||
|
def handle_packet(self, payload: dict[str, Any]) -> None:
|
||||||
|
msg_type = str(payload.get("type", "")).lower()
|
||||||
|
if msg_type == "cmd_vel":
|
||||||
|
cmd = self.parse_twist(payload)
|
||||||
|
self.cmd_pub.publish(cmd)
|
||||||
|
self.last_cmd_time = self.get_clock().now()
|
||||||
|
self.timeout_estop_sent = False
|
||||||
|
elif msg_type == "zero":
|
||||||
|
self.cmd_pub.publish(Twist())
|
||||||
|
self.last_cmd_time = self.get_clock().now()
|
||||||
|
elif msg_type == "estop":
|
||||||
|
self.estop_pub.publish(Bool(data=bool(payload.get("data", True))))
|
||||||
|
elif msg_type == "mode":
|
||||||
|
mode = str(payload.get("mode", "DISABLED")).upper()
|
||||||
|
self.mode_pub.publish(String(data=mode))
|
||||||
|
self.web_enabled = mode == "WEB"
|
||||||
|
self.web_enabled_pub.publish(Bool(data=mode == "WEB"))
|
||||||
|
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
|
||||||
|
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
|
||||||
|
elif msg_type == "web_enable":
|
||||||
|
self.web_enabled = bool(payload.get("data", False))
|
||||||
|
self.web_enabled_pub.publish(Bool(data=self.web_enabled))
|
||||||
|
if self.web_enabled:
|
||||||
|
self.mode_pub.publish(String(data="WEB"))
|
||||||
|
elif msg_type == "remote_enable":
|
||||||
|
enabled = bool(payload.get("data", False))
|
||||||
|
self.remote_enabled_pub.publish(Bool(data=enabled))
|
||||||
|
if enabled:
|
||||||
|
self.mode_pub.publish(String(data="REMOTE"))
|
||||||
|
elif msg_type == "nav_enable":
|
||||||
|
enabled = bool(payload.get("data", False))
|
||||||
|
self.nav_enabled_pub.publish(Bool(data=enabled))
|
||||||
|
if enabled:
|
||||||
|
self.mode_pub.publish(String(data="NAV"))
|
||||||
|
elif msg_type == "ping":
|
||||||
|
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||||
|
else:
|
||||||
|
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||||
|
|
||||||
|
def parse_twist(self, payload: dict[str, Any]) -> Twist:
|
||||||
|
cmd = Twist()
|
||||||
|
linear = payload.get("linear", {}) or {}
|
||||||
|
angular = payload.get("angular", {}) or {}
|
||||||
|
cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx)
|
||||||
|
cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy)
|
||||||
|
cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw)
|
||||||
|
return cmd
|
||||||
|
|
||||||
|
def on_guard_timer(self) -> None:
|
||||||
|
age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6
|
||||||
|
if age_ms > self.cmd_timeout_ms:
|
||||||
|
self.cmd_pub.publish(Twist())
|
||||||
|
if self.estop_on_timeout and not self.timeout_estop_sent:
|
||||||
|
self.estop_pub.publish(Bool(data=True))
|
||||||
|
self.timeout_estop_sent = True
|
||||||
|
|
||||||
|
def on_state_timer(self) -> None:
|
||||||
|
if not self.enabled:
|
||||||
|
return
|
||||||
|
self.send_packet(self.build_state_packet())
|
||||||
|
|
||||||
|
def build_state_packet(self) -> dict[str, Any]:
|
||||||
|
target = self.latest_target
|
||||||
|
state = self.latest_state
|
||||||
|
packet: dict[str, Any] = {
|
||||||
|
"type": "state",
|
||||||
|
"stamp": self.now_sec(),
|
||||||
|
"mode": self.latest_mode,
|
||||||
|
"mux_status": self.latest_mux_status,
|
||||||
|
"estop": self.estop,
|
||||||
|
"web_enabled": self.web_enabled,
|
||||||
|
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||||
|
"runtime": {},
|
||||||
|
"robot": {},
|
||||||
|
}
|
||||||
|
if target is not None:
|
||||||
|
packet["runtime"] = {
|
||||||
|
"target_source": str(target.target_source),
|
||||||
|
"zero_command": bool(target.zero_command),
|
||||||
|
"runtime_released": bool(target.runtime_released),
|
||||||
|
"release_alpha": self._f(target.release_alpha),
|
||||||
|
"command": [self._f(v) for v in target.command],
|
||||||
|
"raw_command": [self._f(v) for v in target.raw_command],
|
||||||
|
}
|
||||||
|
if state is not None:
|
||||||
|
packet["robot"] = {
|
||||||
|
"joint_pos": [self._f(v) for v in state.joint_pos],
|
||||||
|
"joint_vel": [self._f(v) for v in state.joint_vel],
|
||||||
|
"joint_torque": [self._f(v) for v in state.joint_torque],
|
||||||
|
"imu_gyro": [self._f(v) for v in state.imu_gyro],
|
||||||
|
"imu_accel": [self._f(v) for v in state.imu_accel],
|
||||||
|
"projected_gravity": [self._f(v) for v in state.projected_gravity],
|
||||||
|
"quat_wxyz": [self._f(v) for v in state.quat_wxyz],
|
||||||
|
"imu_age_ms": self._f(state.imu_age_ms),
|
||||||
|
"imu_fresh": bool(state.imu_fresh),
|
||||||
|
"odom_age_ms": self._f(state.odom_age_ms),
|
||||||
|
"odom_fresh": bool(state.odom_fresh),
|
||||||
|
"odom_local_pos": [self._f(v) for v in state.odom_local_pos],
|
||||||
|
"odom_local_yaw": self._f(state.odom_local_yaw),
|
||||||
|
"fresh_count": int(state.fresh_count),
|
||||||
|
"holdover_count": int(state.holdover_count),
|
||||||
|
"stale_max": int(state.stale_max),
|
||||||
|
"update_counts": [int(v) for v in state.update_counts],
|
||||||
|
}
|
||||||
|
return packet
|
||||||
|
|
||||||
|
def send_packet(self, payload: dict[str, Any]) -> None:
|
||||||
|
if self.client_addr is None:
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||||
|
self.sock.sendto(data, self.client_addr)
|
||||||
|
except OSError as exc:
|
||||||
|
self.get_logger().warn(f"UDP send failed: {exc}")
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _f(v: Any) -> float:
|
||||||
|
try:
|
||||||
|
f = float(v)
|
||||||
|
if f != f:
|
||||||
|
return 0.0
|
||||||
|
return round(f, 6)
|
||||||
|
except (TypeError, ValueError):
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
def now_sec(self) -> float:
|
||||||
|
return self.get_clock().now().nanoseconds / 1.0e9
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def twist_to_dict(msg: Twist) -> dict[str, Any]:
|
||||||
|
return {
|
||||||
|
"linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z},
|
||||||
|
"angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z},
|
||||||
|
}
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def clamp(value: float, low: float, high: float) -> float:
|
||||||
|
return max(low, min(high, value))
|
||||||
|
|
||||||
|
|
||||||
|
def main(args: Optional[list[str]] = None) -> None:
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = WebUdpBridgeNode()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
except (KeyboardInterrupt, ExternalShutdownException):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
|
||||||
|
# Exit immediately if a command exits with a non-zero status
|
||||||
|
set -e
|
||||||
|
|
||||||
|
# Define color codes for pretty output
|
||||||
|
GREEN='\033[0;32m'
|
||||||
|
YELLOW='\033[1;33m'
|
||||||
|
RED='\033[0;31m'
|
||||||
|
NC='\033[0m' # No Color
|
||||||
|
|
||||||
|
echo -e "${YELLOW}====================================================${NC}"
|
||||||
|
echo -e "${GREEN} Starting Sim2Real Locomotion ROS2 Stack ${NC}"
|
||||||
|
echo -e "${YELLOW}====================================================${NC}"
|
||||||
|
|
||||||
|
# 1. Source ROS2 Humble environment
|
||||||
|
if [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||||
|
echo -e "[System] Sourcing ROS2 Humble..."
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
else
|
||||||
|
echo -e "${RED}[Error] ROS2 Humble not found. Please install ROS2 Humble first.${NC}"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# 2. Check if local workspace is compiled and source it
|
||||||
|
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||||
|
cd "$SCRIPT_DIR"
|
||||||
|
|
||||||
|
if [ -f "install/setup.bash" ]; then
|
||||||
|
echo -e "[Workspace] Sourcing local workspace..."
|
||||||
|
source install/setup.bash
|
||||||
|
elif [ -f "../../install/setup.bash" ]; then
|
||||||
|
echo -e "[Workspace] Sourcing parent install/setup.bash..."
|
||||||
|
source ../../install/setup.bash
|
||||||
|
else
|
||||||
|
echo -e "${YELLOW}[Warning] install/setup.bash not found. Attempting to build the workspace first...${NC}"
|
||||||
|
if command -v colcon &> /dev/null; then
|
||||||
|
echo -e "[Build] Running colcon build..."
|
||||||
|
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||||
|
source install/setup.bash
|
||||||
|
else
|
||||||
|
echo -e "${RED}[Error] 'colcon' tool not found. Please compile the workspace manually before running.${NC}"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
# 3. Check for SocketCAN interfaces (in non-dry-run mode)
|
||||||
|
# Reading dry_run parameter from yaml config
|
||||||
|
if [ -f "src/sim2real_bringup/config/runtime.yaml" ]; then
|
||||||
|
# Use sed for portability (busybox-compatible, avoids GNU grep -oP dependency)
|
||||||
|
DRY_RUN=$(sed -n 's/^[[:space:]]*dry_run:[[:space:]]*//p' src/sim2real_bringup/config/runtime.yaml | head -n 1 || echo "true")
|
||||||
|
# Trim trailing whitespace/newlines
|
||||||
|
DRY_RUN=$(echo "$DRY_RUN" | tr -d '[:space:]')
|
||||||
|
else
|
||||||
|
DRY_RUN="true"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ "$DRY_RUN" = "false" ]; then
|
||||||
|
echo -e "[Network] Checking CAN interfaces..."
|
||||||
|
if ip link show can0 &> /dev/null && ip link show can1 &> /dev/null; then
|
||||||
|
echo -e "[Network] can0 and can1 interfaces detected."
|
||||||
|
else
|
||||||
|
echo -e "${YELLOW}[Warning] CAN interfaces (can0/can1) not fully active.${NC}"
|
||||||
|
echo -e "To configure CAN interfaces, run:"
|
||||||
|
echo -e " sudo ip link set can0 up type can bitrate 1000000"
|
||||||
|
echo -e " sudo ip link set can1 up type can bitrate 1000000"
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
echo -e "${YELLOW}[Dry-Run] Running in Dry-Run mode. SocketCAN will not be accessed.${NC}"
|
||||||
|
fi
|
||||||
|
|
||||||
|
# 4. Run the ROS2 Launch file
|
||||||
|
echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}"
|
||||||
|
ros2 launch sim2real_bringup sim2real_system.launch.py "$@"
|
||||||
|
|
||||||
@@ -0,0 +1,107 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import socket
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
STATE_LOCK = threading.Lock()
|
||||||
|
LATEST_STATE: dict = {"type": "state", "connected": False}
|
||||||
|
NANO_ADDR: tuple[str, int]
|
||||||
|
UDP_SOCK: socket.socket
|
||||||
|
|
||||||
|
|
||||||
|
class Handler(SimpleHTTPRequestHandler):
|
||||||
|
def do_GET(self) -> None:
|
||||||
|
if self.path == "/api/state":
|
||||||
|
with STATE_LOCK:
|
||||||
|
data = json.dumps(LATEST_STATE).encode("utf-8")
|
||||||
|
self._json(200, data)
|
||||||
|
return
|
||||||
|
super().do_GET()
|
||||||
|
|
||||||
|
def do_POST(self) -> None:
|
||||||
|
if self.path not in ("/api/control", "/api/state"):
|
||||||
|
self.send_error(404)
|
||||||
|
return
|
||||||
|
length = int(self.headers.get("Content-Length", "0"))
|
||||||
|
body = self.rfile.read(length) if length else b"{}"
|
||||||
|
try:
|
||||||
|
payload = json.loads(body.decode("utf-8"))
|
||||||
|
send_udp(payload)
|
||||||
|
self._json(200, b'{"ok":true}')
|
||||||
|
except Exception as exc:
|
||||||
|
self._json(400, json.dumps({"ok": False, "error": str(exc)}).encode())
|
||||||
|
|
||||||
|
def _json(self, code: int, data: bytes) -> None:
|
||||||
|
self.send_response(code)
|
||||||
|
self.send_header("Content-Type", "application/json")
|
||||||
|
self.send_header("Content-Length", str(len(data)))
|
||||||
|
self.send_header("Access-Control-Allow-Origin", "*")
|
||||||
|
self.end_headers()
|
||||||
|
self.wfile.write(data)
|
||||||
|
|
||||||
|
def log_message(self, format: str, *args: object) -> None:
|
||||||
|
return
|
||||||
|
|
||||||
|
|
||||||
|
def send_udp(payload: dict) -> None:
|
||||||
|
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||||
|
UDP_SOCK.sendto(data, NANO_ADDR)
|
||||||
|
|
||||||
|
|
||||||
|
def udp_rx_loop(sock: socket.socket) -> None:
|
||||||
|
global LATEST_STATE
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
data, _ = sock.recvfrom(65535)
|
||||||
|
payload = json.loads(data.decode("utf-8"))
|
||||||
|
payload["connected"] = True
|
||||||
|
payload["local_receive_time"] = time.time()
|
||||||
|
with STATE_LOCK:
|
||||||
|
LATEST_STATE = payload
|
||||||
|
except Exception:
|
||||||
|
time.sleep(0.01)
|
||||||
|
|
||||||
|
|
||||||
|
def heartbeat_loop() -> None:
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
send_udp({"type": "ping", "stamp": time.time()})
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
time.sleep(0.5)
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
global NANO_ADDR, UDP_SOCK
|
||||||
|
parser = argparse.ArgumentParser(description="Windows local web debug UI for sim2real_ros2")
|
||||||
|
parser.add_argument("--nano-host", required=True, help="Nano IP address")
|
||||||
|
parser.add_argument("--nano-port", type=int, default=15000, help="Nano UDP listen port")
|
||||||
|
parser.add_argument("--listen-host", default="0.0.0.0", help="Local HTTP host")
|
||||||
|
parser.add_argument("--http-port", type=int, default=8088, help="Local HTTP port")
|
||||||
|
parser.add_argument("--udp-port", type=int, default=15001, help="Local UDP receive port")
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
NANO_ADDR = (args.nano_host, args.nano_port)
|
||||||
|
UDP_SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
UDP_SOCK.bind(("0.0.0.0", args.udp_port))
|
||||||
|
|
||||||
|
threading.Thread(target=udp_rx_loop, args=(UDP_SOCK,), daemon=True).start()
|
||||||
|
threading.Thread(target=heartbeat_loop, daemon=True).start()
|
||||||
|
|
||||||
|
static_dir = Path(__file__).resolve().parent / "static"
|
||||||
|
handler = lambda *a, **kw: Handler(*a, directory=str(static_dir), **kw)
|
||||||
|
httpd = ThreadingHTTPServer((args.listen_host, args.http_port), handler)
|
||||||
|
print(f"Open http://127.0.0.1:{args.http_port}")
|
||||||
|
print(f"UDP Nano={args.nano_host}:{args.nano_port} local={args.udp_port}")
|
||||||
|
httpd.serve_forever()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,259 @@
|
|||||||
|
'use strict';
|
||||||
|
|
||||||
|
const JOINT_NAMES = [
|
||||||
|
'FL_H_ABD','FL_H_PIT','FL_KNEE',
|
||||||
|
'FR_H_ABD','FR_H_PIT','FR_KNEE',
|
||||||
|
'RL_H_ABD','RL_H_PIT','RL_KNEE',
|
||||||
|
'RR_H_ABD','RR_H_PIT','RR_KNEE',
|
||||||
|
'FL_WHEEL','FR_WHEEL','RL_WHEEL','RR_WHEEL',
|
||||||
|
];
|
||||||
|
|
||||||
|
const $ = id => document.getElementById(id);
|
||||||
|
const cmd = { vx: 0, vy: 0, yaw: 0 };
|
||||||
|
let cmdSendTimer = null;
|
||||||
|
let currentMode = 'UNKNOWN';
|
||||||
|
|
||||||
|
// ── API ──────────────────────────────────────────────────────────────────────
|
||||||
|
async function post(payload) {
|
||||||
|
try {
|
||||||
|
await fetch('/api/control', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify(payload),
|
||||||
|
});
|
||||||
|
} catch (e) {
|
||||||
|
appendEvent('API_ERROR', e.message, 'bad');
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function sendCmd() {
|
||||||
|
post({
|
||||||
|
type: 'cmd_vel',
|
||||||
|
linear: { x: cmd.vx, y: cmd.vy, z: 0 },
|
||||||
|
angular: { x: 0, y: 0, z: cmd.yaw },
|
||||||
|
});
|
||||||
|
$('cmd-display').textContent =
|
||||||
|
`vx=${cmd.vx.toFixed(2)} vy=${cmd.vy.toFixed(2)} yaw=${cmd.yaw.toFixed(2)}`;
|
||||||
|
}
|
||||||
|
|
||||||
|
function zeroAll() {
|
||||||
|
cmd.vx = 0; cmd.vy = 0; cmd.yaw = 0;
|
||||||
|
$('cmd-vx').value = 0;
|
||||||
|
$('cmd-vy').value = 0;
|
||||||
|
$('cmd-yaw').value = 0;
|
||||||
|
$('cmd-vx-v').textContent = '0.00';
|
||||||
|
$('cmd-vy-v').textContent = '0.00';
|
||||||
|
$('cmd-yaw-v').textContent = '0.00';
|
||||||
|
$('cmd-display').textContent = 'vx=0.00 vy=0.00 yaw=0.00';
|
||||||
|
$('stick').style.transform = 'translate(-50%, -50%)';
|
||||||
|
post({ type: 'zero' });
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── Buttons ──────────────────────────────────────────────────────────────────
|
||||||
|
function setMode(mode) {
|
||||||
|
if (mode === 'WEB' && !confirm('确认切换到 WEB 控制？\n请确认机器人安全且速度为 0。')) return;
|
||||||
|
post({ type: 'mode', mode });
|
||||||
|
appendEvent('MODE_SET', `→ ${mode}`, 'ok');
|
||||||
|
}
|
||||||
|
|
||||||
|
function highlightMode(mode) {
|
||||||
|
for (const m of ['DISABLED', 'REMOTE', 'WEB', 'NAV']) {
|
||||||
|
const btn = $('btn-' + m.toLowerCase());
|
||||||
|
if (btn) btn.classList.toggle('active-mode', m === mode);
|
||||||
|
}
|
||||||
|
const el = $('stage');
|
||||||
|
if (el) {
|
||||||
|
el.textContent = mode;
|
||||||
|
el.className = 'stage ' + mode;
|
||||||
|
}
|
||||||
|
currentMode = mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
$('btn-disabled').onclick = () => { zeroAll(); setMode('DISABLED'); };
|
||||||
|
$('btn-remote').onclick = () => setMode('REMOTE');
|
||||||
|
$('btn-web').onclick = () => setMode('WEB');
|
||||||
|
$('btn-nav').onclick = () => setMode('NAV');
|
||||||
|
$('btn-zero').onclick = zeroAll;
|
||||||
|
$('btn-estop').onclick = () => {
|
||||||
|
if (confirm('确认触发软急停？')) {
|
||||||
|
post({ type: 'estop', data: true });
|
||||||
|
zeroAll();
|
||||||
|
appendEvent('ESTOP', '软急停已触发', 'bad');
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// ── Sliders ──────────────────────────────────────────────────────────────────
|
||||||
|
for (const [id, key] of [['cmd-vx','vx'],['cmd-vy','vy'],['cmd-yaw','yaw']]) {
|
||||||
|
$(id).addEventListener('input', e => {
|
||||||
|
cmd[key] = parseFloat(e.target.value);
|
||||||
|
$(id + '-v').textContent = cmd[key].toFixed(2);
|
||||||
|
if (currentMode === 'WEB') sendCmd();
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── Joystick ─────────────────────────────────────────────────────────────────
|
||||||
|
const joystick = $('joystick');
|
||||||
|
const stick = $('stick');
|
||||||
|
let dragging = false;
|
||||||
|
|
||||||
|
function updateJoystick(clientX, clientY) {
|
||||||
|
const rect = joystick.getBoundingClientRect();
|
||||||
|
const cx = rect.left + rect.width / 2;
|
||||||
|
const cy = rect.top + rect.height / 2;
|
||||||
|
const maxR = rect.width * 0.42;
|
||||||
|
let dx = clientX - cx;
|
||||||
|
let dy = clientY - cy;
|
||||||
|
const dist = Math.hypot(dx, dy);
|
||||||
|
if (dist > maxR) { dx = dx / dist * maxR; dy = dy / dist * maxR; }
|
||||||
|
stick.style.transform = `translate(calc(-50% + ${dx}px), calc(-50% + ${dy}px))`;
|
||||||
|
cmd.vx = parseFloat((-(dy / maxR) * 0.8).toFixed(3));
|
||||||
|
cmd.vy = parseFloat(( (dx / maxR) * 0.3).toFixed(3));
|
||||||
|
$('cmd-vx').value = cmd.vx;
|
||||||
|
$('cmd-vy').value = cmd.vy;
|
||||||
|
$('cmd-vx-v').textContent = cmd.vx.toFixed(2);
|
||||||
|
$('cmd-vy-v').textContent = cmd.vy.toFixed(2);
|
||||||
|
if (currentMode === 'WEB') sendCmd();
|
||||||
|
}
|
||||||
|
|
||||||
|
joystick.addEventListener('pointerdown', e => {
|
||||||
|
dragging = true;
|
||||||
|
joystick.setPointerCapture(e.pointerId);
|
||||||
|
updateJoystick(e.clientX, e.clientY);
|
||||||
|
});
|
||||||
|
joystick.addEventListener('pointermove', e => { if (dragging) updateJoystick(e.clientX, e.clientY); });
|
||||||
|
joystick.addEventListener('pointerup', () => { dragging = false; zeroAll(); });
|
||||||
|
joystick.addEventListener('pointercancel', () => { dragging = false; zeroAll(); });
|
||||||
|
|
||||||
|
// ── Joints grid init ─────────────────────────────────────────────────────────
|
||||||
|
function initJointsGrid() {
|
||||||
|
const grid = $('joints-grid');
|
||||||
|
if (!grid) return;
|
||||||
|
grid.innerHTML = JOINT_NAMES.map((name, i) => `
|
||||||
|
<div class="motor-row" id="mi-${i}">
|
||||||
|
<span class="stale" id="ms-${i}" style="color:#ef4444">●</span>
|
||||||
|
<span class="name">${name}</span>
|
||||||
|
<span class="val pos" id="mp-${i}">0.00</span>
|
||||||
|
<span class="val vel" id="mv-${i}">0.00</span>
|
||||||
|
<span class="val tau" id="mt-${i}">0.00</span>
|
||||||
|
</div>`).join('');
|
||||||
|
}
|
||||||
|
|
||||||
|
function updateJointsGrid(robot) {
|
||||||
|
if (!robot) return;
|
||||||
|
const pos = robot.joint_pos || [];
|
||||||
|
const vel = robot.joint_vel || [];
|
||||||
|
const tau = robot.joint_torque || [];
|
||||||
|
const upd = robot.update_counts || [];
|
||||||
|
for (let i = 0; i < 16; i++) {
|
||||||
|
const dot = $('ms-' + i);
|
||||||
|
const cnt = upd[i] ?? 0;
|
||||||
|
if (dot) dot.style.color = cnt > 0 ? '#30d158' : '#ef4444';
|
||||||
|
const p = $('mp-' + i); if (p) p.textContent = (pos[i] || 0).toFixed(2);
|
||||||
|
const v = $('mv-' + i); if (v) v.textContent = (vel[i] || 0).toFixed(2);
|
||||||
|
const t = $('mt-' + i);
|
||||||
|
if (t) {
|
||||||
|
t.textContent = (tau[i] || 0).toFixed(2);
|
||||||
|
t.style.color = Math.abs(tau[i] || 0) > 16 ? '#ff453a' : '#ff9f0a';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── State polling ─────────────────────────────────────────────────────────────
|
||||||
|
function setText(id, text, cls) {
|
||||||
|
const el = $(id);
|
||||||
|
if (!el) return;
|
||||||
|
el.textContent = text;
|
||||||
|
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||||
|
}
|
||||||
|
|
||||||
|
function applyState(data) {
|
||||||
|
const connected = data.connected &&
|
||||||
|
(!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||||
|
|
||||||
|
const stage = $('stage');
|
||||||
|
if (stage) {
|
||||||
|
if (!connected) {
|
||||||
|
stage.textContent = 'DISCONNECTED';
|
||||||
|
stage.className = 'stage DISCONNECTED';
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const mode = data.mode || 'UNKNOWN';
|
||||||
|
if (mode !== currentMode) highlightMode(mode);
|
||||||
|
|
||||||
|
const rt = data.runtime || {};
|
||||||
|
const src = rt.target_source || '--';
|
||||||
|
const srcCls = src === 'safety_brake' ? 'bad'
|
||||||
|
: src === 'runtime_policy' ? 'ok'
|
||||||
|
: src === 'runtime_zero_hold' ? 'active' : '';
|
||||||
|
setText('d-source', src, srcCls);
|
||||||
|
setText('d-released', String(rt.runtime_released ?? '--'),
|
||||||
|
rt.runtime_released ? 'ok' : '');
|
||||||
|
setText('d-alpha', (rt.release_alpha ?? '--') !== '--'
|
||||||
|
? Number(rt.release_alpha).toFixed(2) : '--');
|
||||||
|
setText('d-zero', String(rt.zero_command ?? '--'));
|
||||||
|
setText('d-estop', String(data.estop ?? '--'),
|
||||||
|
data.estop ? 'bad' : 'ok');
|
||||||
|
setText('d-mux', data.mux_status || '--');
|
||||||
|
|
||||||
|
const robot = data.robot || {};
|
||||||
|
const imuAge = robot.imu_age_ms ?? null;
|
||||||
|
setText('d-imu-fresh', String(robot.imu_fresh ?? '--'),
|
||||||
|
robot.imu_fresh ? 'ok' : 'bad');
|
||||||
|
setText('d-imu-age', imuAge !== null ? imuAge.toFixed(1) : '--',
|
||||||
|
imuAge !== null ? (imuAge > 200 ? 'bad' : imuAge > 60 ? 'warn' : 'ok') : '');
|
||||||
|
|
||||||
|
const grav = robot.projected_gravity;
|
||||||
|
setText('d-gravity', grav ? grav.map(v => Number(v).toFixed(2)).join(', ') : '--',
|
||||||
|
grav && grav[2] < -0.5 ? 'ok' : 'warn');
|
||||||
|
|
||||||
|
setText('d-holdover', String(robot.holdover_count ?? '--'),
|
||||||
|
(robot.holdover_count || 0) > 10 ? 'warn' : '');
|
||||||
|
|
||||||
|
const odomAge = robot.odom_age_ms ?? null;
|
||||||
|
setText('d-odom-age', odomAge !== null ? odomAge.toFixed(1) : '--',
|
||||||
|
odomAge !== null ? (odomAge > 500 ? 'bad' : odomAge > 200 ? 'warn' : 'ok') : '');
|
||||||
|
|
||||||
|
const lp = robot.odom_local_pos;
|
||||||
|
setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--');
|
||||||
|
|
||||||
|
const cv = data.cmd_vel || {};
|
||||||
|
const lin = cv.linear || {};
|
||||||
|
const ang = cv.angular || {};
|
||||||
|
setText('cv-vx', (lin.x ?? 0).toFixed(3));
|
||||||
|
setText('cv-vy', (lin.y ?? 0).toFixed(3));
|
||||||
|
setText('cv-yaw', (ang.z ?? 0).toFixed(3));
|
||||||
|
|
||||||
|
updateJointsGrid(robot);
|
||||||
|
}
|
||||||
|
|
||||||
|
async function poll() {
|
||||||
|
try {
|
||||||
|
const res = await fetch('/api/state');
|
||||||
|
const data = await res.json();
|
||||||
|
applyState(data);
|
||||||
|
} catch (_) {}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── WEB mode heartbeat ────────────────────────────────────────────────────────
|
||||||
|
setInterval(() => {
|
||||||
|
if (currentMode === 'WEB' && !dragging) sendCmd();
|
||||||
|
}, 50);
|
||||||
|
|
||||||
|
// ── Event log ─────────────────────────────────────────────────────────────────
|
||||||
|
function appendEvent(kind, detail, cls) {
|
||||||
|
const el = $('events-log');
|
||||||
|
if (!el) return;
|
||||||
|
const div = document.createElement('div');
|
||||||
|
const t = new Date().toLocaleTimeString();
|
||||||
|
div.innerHTML = `<span class="ev-t">${t}</span> <span class="ev-${cls || 'ok'}">${kind}</span> <span style="color:#8e8e93">${detail || ''}</span>`;
|
||||||
|
el.appendChild(div);
|
||||||
|
while (el.children.length > 200) el.removeChild(el.firstChild);
|
||||||
|
el.scrollTop = el.scrollHeight;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||||
|
initJointsGrid();
|
||||||
|
setInterval(poll, 100);
|
||||||
|
appendEvent('READY', '页面已加载，等待 Nano 连接', 'ok');
|
||||||
@@ -0,0 +1,98 @@
|
|||||||
|
<!doctype html>
|
||||||
|
<html lang="zh-CN" data-theme="dark">
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||||
|
<title>sim2real ROS2 控制台</title>
|
||||||
|
<link rel="stylesheet" href="style.css">
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<header class="glass-panel top-bar">
|
||||||
|
<div class="top-bar-left">
|
||||||
|
<h1>sim2real ROS2</h1>
|
||||||
|
<span class="stage" id="stage">DISCONNECTED</span>
|
||||||
|
</div>
|
||||||
|
<div class="top-bar-center">
|
||||||
|
<span class="label">控制模式</span>
|
||||||
|
<button class="btn" id="btn-disabled">DISABLED</button>
|
||||||
|
<button class="btn btn-remote" id="btn-remote">REMOTE</button>
|
||||||
|
<button class="btn btn-web" id="btn-web">WEB</button>
|
||||||
|
<button class="btn btn-nav" id="btn-nav">NAV</button>
|
||||||
|
<div class="divider"></div>
|
||||||
|
<button class="btn" id="btn-zero">速度归零</button>
|
||||||
|
</div>
|
||||||
|
<div class="top-bar-right">
|
||||||
|
<button class="btn btn-danger" id="btn-estop">软急停</button>
|
||||||
|
</div>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<div class="glass-panel side-panel left-panel">
|
||||||
|
<div class="panel-section">
|
||||||
|
<h2 class="panel-title">运行状态</h2>
|
||||||
|
<div class="diag-row"><span class="diag-label">target_source</span><span class="diag-value" id="d-source">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">runtime_released</span><span class="diag-value" id="d-released">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">release_alpha</span><span class="diag-value" id="d-alpha">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">zero_command</span><span class="diag-value" id="d-zero">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">estop</span><span class="diag-value" id="d-estop">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">mux</span><span class="diag-value" id="d-mux">--</span></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="panel-section">
|
||||||
|
<h2 class="panel-title">IMU & 里程计</h2>
|
||||||
|
<div class="diag-row"><span class="diag-label">IMU fresh</span><span class="diag-value" id="d-imu-fresh">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">IMU age ms</span><span class="diag-value" id="d-imu-age">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">projected_gravity</span><span class="diag-value" id="d-gravity">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">holdover</span><span class="diag-value" id="d-holdover">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">odom age ms</span><span class="diag-value" id="d-odom-age">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">odom local pos</span><span class="diag-value" id="d-odom-pos">--</span></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="panel-section flex-1">
|
||||||
|
<h2 class="panel-title">关节状态 (16轴)</h2>
|
||||||
|
<div id="joints-grid" class="motors-grid-list"></div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="glass-panel side-panel right-panel">
|
||||||
|
<div class="panel-section">
|
||||||
|
<h2 class="panel-title">Web 手动控制</h2>
|
||||||
|
<div class="joystick-area">
|
||||||
|
<div class="joystick" id="joystick"><div id="stick"></div></div>
|
||||||
|
<p class="hint">拖动控制前后(vx)和横移(vy)，松开归零</p>
|
||||||
|
</div>
|
||||||
|
<div class="slider-group">
|
||||||
|
<div class="slider-row">
|
||||||
|
<span class="slider-label">vx</span>
|
||||||
|
<input type="range" id="cmd-vx" class="glass-slider" min="-0.8" max="0.8" step="0.01" value="0">
|
||||||
|
<span class="slider-val" id="cmd-vx-v">0.00</span>
|
||||||
|
</div>
|
||||||
|
<div class="slider-row">
|
||||||
|
<span class="slider-label">vy</span>
|
||||||
|
<input type="range" id="cmd-vy" class="glass-slider" min="-0.3" max="0.3" step="0.01" value="0">
|
||||||
|
<span class="slider-val" id="cmd-vy-v">0.00</span>
|
||||||
|
</div>
|
||||||
|
<div class="slider-row">
|
||||||
|
<span class="slider-label">yaw</span>
|
||||||
|
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.5" max="0.5" step="0.01" value="0">
|
||||||
|
<span class="slider-val" id="cmd-yaw-v">0.00</span>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="cmd-display" id="cmd-display">vx=0.00 vy=0.00 yaw=0.00</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="panel-section">
|
||||||
|
<h2 class="panel-title">当前输出 /cmd_vel</h2>
|
||||||
|
<div class="diag-row"><span class="diag-label">linear.x</span><span class="diag-value" id="cv-vx">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">linear.y</span><span class="diag-value" id="cv-vy">--</span></div>
|
||||||
|
<div class="diag-row"><span class="diag-label">angular.z</span><span class="diag-value" id="cv-yaw">--</span></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="panel-section log-section flex-1">
|
||||||
|
<h2 class="panel-title">事件流</h2>
|
||||||
|
<div id="events-log" class="log"></div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<script src="app.js"></script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@@ -0,0 +1,263 @@
|
|||||||
|
/* sim2real ROS2 Web Debug — Apple Glass Design */
|
||||||
|
:root {
|
||||||
|
--bg-primary: #000000;
|
||||||
|
--glass-bg: rgba(20, 20, 22, 0.65);
|
||||||
|
--glass-border: rgba(255, 255, 255, 0.12);
|
||||||
|
--glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
|
||||||
|
--text-primary: #ffffff;
|
||||||
|
--text-secondary: #ebebf5;
|
||||||
|
--text-tertiary: #8e8e93;
|
||||||
|
--accent: #0a84ff;
|
||||||
|
--accent-hover: #409cff;
|
||||||
|
--success: #30d158;
|
||||||
|
--warning: #ffd60a;
|
||||||
|
--danger: #ff453a;
|
||||||
|
--blur-amount: 24px;
|
||||||
|
--saturation: 180%;
|
||||||
|
--spring: cubic-bezier(0.4, 0, 0.2, 1);
|
||||||
|
--panel-radius: 16px;
|
||||||
|
--font-family: -apple-system, BlinkMacSystemFont, "SF Pro Display", "PingFang SC", sans-serif;
|
||||||
|
}
|
||||||
|
|
||||||
|
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||||
|
|
||||||
|
body {
|
||||||
|
font-family: var(--font-family);
|
||||||
|
background: radial-gradient(circle at top left, #1a1a24 0%, #000000 100%);
|
||||||
|
color: var(--text-primary);
|
||||||
|
-webkit-font-smoothing: antialiased;
|
||||||
|
min-height: 100vh;
|
||||||
|
overflow-x: hidden;
|
||||||
|
}
|
||||||
|
|
||||||
|
.glass-panel {
|
||||||
|
background: var(--glass-bg);
|
||||||
|
backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||||
|
-webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||||
|
border: 0.5px solid var(--glass-border);
|
||||||
|
box-shadow: var(--glass-shadow);
|
||||||
|
z-index: 50;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Top Bar */
|
||||||
|
.top-bar {
|
||||||
|
position: fixed;
|
||||||
|
top: 16px;
|
||||||
|
left: 50%;
|
||||||
|
transform: translateX(-50%);
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: space-between;
|
||||||
|
padding: 8px 16px;
|
||||||
|
border-radius: 24px;
|
||||||
|
width: 96%;
|
||||||
|
max-width: 1400px;
|
||||||
|
gap: 16px;
|
||||||
|
}
|
||||||
|
.top-bar-left, .top-bar-center, .top-bar-right {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
gap: 10px;
|
||||||
|
}
|
||||||
|
.top-bar-center { flex: 1; justify-content: center; }
|
||||||
|
.top-bar h1 {
|
||||||
|
font-size: 16px;
|
||||||
|
font-weight: 600;
|
||||||
|
background: linear-gradient(45deg, #fff, #8e8e93);
|
||||||
|
-webkit-background-clip: text;
|
||||||
|
-webkit-text-fill-color: transparent;
|
||||||
|
}
|
||||||
|
.divider { width: 1px; height: 24px; background: var(--glass-border); margin: 0 4px; }
|
||||||
|
|
||||||
|
/* Stage badge */
|
||||||
|
.stage {
|
||||||
|
padding: 4px 10px;
|
||||||
|
border-radius: 12px;
|
||||||
|
font-size: 11px;
|
||||||
|
font-weight: 700;
|
||||||
|
text-transform: uppercase;
|
||||||
|
letter-spacing: 0.5px;
|
||||||
|
background: rgba(255,255,255,0.1);
|
||||||
|
color: var(--text-secondary);
|
||||||
|
}
|
||||||
|
.stage.DISCONNECTED { background: rgba(142,142,147,0.3); color: #aaa; }
|
||||||
|
.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
|
||||||
|
.stage.REMOTE { background: rgba(48,209,88,0.3); color: #8deda7; }
|
||||||
|
.stage.WEB { background: rgba(0,122,255,0.3); color: #82c4ff; }
|
||||||
|
.stage.NAV { background: rgba(255,214,10,0.3); color: #ffe680; }
|
||||||
|
.stage.DISABLED { background: rgba(142,142,147,0.25); color: #aaa; }
|
||||||
|
.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
|
||||||
|
|
||||||
|
/* Buttons */
|
||||||
|
.btn {
|
||||||
|
background: rgba(255,255,255,0.08);
|
||||||
|
border: 1px solid rgba(255,255,255,0.1);
|
||||||
|
border-radius: 8px;
|
||||||
|
color: var(--text-primary);
|
||||||
|
font-size: 12px;
|
||||||
|
font-weight: 500;
|
||||||
|
padding: 6px 12px;
|
||||||
|
cursor: pointer;
|
||||||
|
transition: all 0.2s var(--spring);
|
||||||
|
font-family: inherit;
|
||||||
|
}
|
||||||
|
.btn:hover:not(:disabled) { background: rgba(255,255,255,0.15); transform: translateY(-1px); }
|
||||||
|
.btn:active:not(:disabled) { transform: translateY(1px); }
|
||||||
|
.btn:disabled { opacity: 0.5; cursor: not-allowed; }
|
||||||
|
.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
|
||||||
|
.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; }
|
||||||
|
.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; }
|
||||||
|
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
|
||||||
|
.btn.active-mode { box-shadow: 0 0 0 2px white; }
|
||||||
|
|
||||||
|
.label { font-size: 11px; color: var(--text-tertiary); }
|
||||||
|
|
||||||
|
/* Side panels */
|
||||||
|
.side-panel {
|
||||||
|
position: fixed;
|
||||||
|
top: 80px;
|
||||||
|
bottom: 20px;
|
||||||
|
width: 340px;
|
||||||
|
border-radius: var(--panel-radius);
|
||||||
|
display: flex;
|
||||||
|
flex-direction: column;
|
||||||
|
overflow: hidden;
|
||||||
|
}
|
||||||
|
.left-panel { left: 2%; }
|
||||||
|
.right-panel { right: 2%; }
|
||||||
|
.flex-1 { flex: 1; min-height: 0; overflow: hidden; display: flex; flex-direction: column; }
|
||||||
|
|
||||||
|
.panel-section {
|
||||||
|
padding: 14px 16px;
|
||||||
|
border-bottom: 0.5px solid var(--glass-border);
|
||||||
|
}
|
||||||
|
.panel-section:last-child { border-bottom: none; }
|
||||||
|
|
||||||
|
.panel-title {
|
||||||
|
font-size: 11px;
|
||||||
|
font-weight: 700;
|
||||||
|
color: var(--text-tertiary);
|
||||||
|
text-transform: uppercase;
|
||||||
|
letter-spacing: 0.5px;
|
||||||
|
margin-bottom: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Diag rows */
|
||||||
|
.diag-row {
|
||||||
|
display: flex;
|
||||||
|
justify-content: space-between;
|
||||||
|
align-items: center;
|
||||||
|
padding: 4px 6px;
|
||||||
|
background: rgba(0,0,0,0.2);
|
||||||
|
border-radius: 4px;
|
||||||
|
margin-bottom: 3px;
|
||||||
|
}
|
||||||
|
.diag-label { font-size: 11px; color: var(--text-tertiary); }
|
||||||
|
.diag-value { font-size: 11px; font-family: monospace; color: var(--text-primary); font-weight: 600; }
|
||||||
|
.diag-value.ok { color: var(--success); }
|
||||||
|
.diag-value.warn { color: var(--warning); }
|
||||||
|
.diag-value.bad { color: var(--danger); }
|
||||||
|
.diag-value.active { color: #82c4ff; }
|
||||||
|
|
||||||
|
/* Joints grid */
|
||||||
|
.motors-grid-list {
|
||||||
|
display: flex;
|
||||||
|
flex-direction: column;
|
||||||
|
gap: 2px;
|
||||||
|
overflow-y: auto;
|
||||||
|
flex: 1;
|
||||||
|
}
|
||||||
|
.motor-row {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
gap: 4px;
|
||||||
|
padding: 3px 6px;
|
||||||
|
background: rgba(0,0,0,0.25);
|
||||||
|
border-radius: 5px;
|
||||||
|
}
|
||||||
|
.motor-row .name { font-size: 10px; color: var(--text-secondary); width: 72px; font-family: monospace; flex-shrink: 0; }
|
||||||
|
.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; flex: 1; }
|
||||||
|
.motor-row .val.pos { color: #0a84ff; }
|
||||||
|
.motor-row .val.vel { color: #30d158; }
|
||||||
|
.motor-row .val.tau { color: #ff9f0a; }
|
||||||
|
.motor-row .stale { font-size: 9px; width: 8px; flex-shrink: 0; }
|
||||||
|
|
||||||
|
/* Joystick */
|
||||||
|
.joystick-area { display: flex; flex-direction: column; align-items: center; gap: 8px; margin-bottom: 12px; }
|
||||||
|
.joystick {
|
||||||
|
position: relative;
|
||||||
|
width: 180px;
|
||||||
|
height: 180px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: radial-gradient(circle, rgba(10,132,255,0.2), rgba(10,132,255,0.05));
|
||||||
|
border: 1px solid rgba(10,132,255,0.3);
|
||||||
|
touch-action: none;
|
||||||
|
flex-shrink: 0;
|
||||||
|
}
|
||||||
|
#stick {
|
||||||
|
position: absolute;
|
||||||
|
left: 50%; top: 50%;
|
||||||
|
width: 56px; height: 56px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: linear-gradient(135deg, #30d158, #0a84ff);
|
||||||
|
transform: translate(-50%, -50%);
|
||||||
|
box-shadow: 0 8px 24px rgba(0,0,0,0.4);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Sliders */
|
||||||
|
.slider-group { display: flex; flex-direction: column; gap: 8px; }
|
||||||
|
.slider-row { display: flex; align-items: center; gap: 8px; }
|
||||||
|
.slider-label { font-size: 11px; color: var(--text-tertiary); width: 28px; font-family: monospace; }
|
||||||
|
.slider-val { font-size: 11px; color: var(--accent); font-family: monospace; width: 38px; text-align: right; }
|
||||||
|
.glass-slider {
|
||||||
|
flex: 1;
|
||||||
|
-webkit-appearance: none;
|
||||||
|
height: 4px;
|
||||||
|
border-radius: 2px;
|
||||||
|
background: rgba(255,255,255,0.2);
|
||||||
|
outline: none;
|
||||||
|
}
|
||||||
|
.glass-slider::-webkit-slider-thumb {
|
||||||
|
-webkit-appearance: none;
|
||||||
|
width: 14px; height: 14px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: white;
|
||||||
|
cursor: pointer;
|
||||||
|
box-shadow: 0 2px 4px rgba(0,0,0,0.5);
|
||||||
|
}
|
||||||
|
.cmd-display {
|
||||||
|
margin-top: 8px;
|
||||||
|
padding: 6px 10px;
|
||||||
|
background: rgba(0,0,0,0.3);
|
||||||
|
border-radius: 6px;
|
||||||
|
font-family: monospace;
|
||||||
|
font-size: 12px;
|
||||||
|
color: var(--accent);
|
||||||
|
text-align: center;
|
||||||
|
}
|
||||||
|
.hint { font-size: 11px; color: var(--text-tertiary); text-align: center; }
|
||||||
|
|
||||||
|
/* Log */
|
||||||
|
.log-section { flex: 1; overflow: hidden; display: flex; flex-direction: column; }
|
||||||
|
.log {
|
||||||
|
flex: 1;
|
||||||
|
background: rgba(0,0,0,0.4);
|
||||||
|
border: 1px solid rgba(255,255,255,0.05);
|
||||||
|
border-radius: 6px;
|
||||||
|
padding: 8px;
|
||||||
|
font-family: monospace;
|
||||||
|
font-size: 11px;
|
||||||
|
color: var(--text-secondary);
|
||||||
|
overflow-y: auto;
|
||||||
|
}
|
||||||
|
.log div { margin-bottom: 2px; line-height: 1.4; }
|
||||||
|
.ev-t { color: var(--text-tertiary); margin-right: 4px; }
|
||||||
|
.ev-ok { color: var(--success); }
|
||||||
|
.ev-warn { color: var(--warning); }
|
||||||
|
.ev-bad { color: var(--danger); }
|
||||||
|
|
||||||
|
@media (max-width: 960px) {
|
||||||
|
.side-panel { position: relative; top: auto; width: 100%; left: 0; right: 0; border-radius: 0; height: auto; }
|
||||||
|
.top-bar { width: 100%; border-radius: 0; top: 0; }
|
||||||
|
body { padding-top: 56px; }
|
||||||
|
}
|
||||||
@@ -39,6 +39,7 @@ RC_WheelLeg/
|
|||||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||||
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
||||||
|
- [x] 整理 Python Sim2Real v2 与 ROS 2/C++ 初版
|
||||||
- [ ] 核对比赛机械与仿真模型参数
|
- [ ] 核对比赛机械与仿真模型参数
|
||||||
- [ ] 整理 URDF/MJCF 机器人描述
|
- [ ] 整理 URDF/MJCF 机器人描述
|
||||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||||
|
|||||||
Reference in New Issue
Block a user