[real] 整理 ROS 2 v2 导航原型
This commit is contained in:
@@ -16,6 +16,7 @@
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
|
||||
| `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
|
||||
|
||||
> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
|
||||
|
||||
@@ -34,6 +35,14 @@
|
||||
- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
|
||||
- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
|
||||
|
||||
## `v0.11.0` 的 ROS 2 Sim2Real v2
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2`，保持 `v0.10.0` 的 ROS 2/C++ 控制契约。
|
||||
- 增加 `simple_nav_node.py`、PCD 点击工具、任务点/任务序列配置和 Web 导航控制入口。
|
||||
- 默认命令源从遥控切换为 `NAV`，加入简单导航状态、PCD 位姿和地图显示链路。
|
||||
- 原始 `map1.pcd`、`map6.pcd` 分别约 49.05 MiB、44.24 MiB，归档时确定性抽样到 10 MB 以下并记录哈希。
|
||||
- 原始快照中的 Odin 驱动仍为空目录；排除设备运行日志和开发草稿。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
- 机械 CAD 不变。
|
||||
|
||||
@@ -10,7 +10,8 @@
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
└─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
└─ sim2real_ros2_v2/ # ROS 2 导航原型及后续演进
|
||||
```
|
||||
|
||||
## 数据流
|
||||
@@ -32,7 +33,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ Sim2Real 初版对应 `v0.10.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++ 初版对应 `v0.10.0`，简单导航与 PCD 导航原型对应 `v0.11.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -32,6 +32,12 @@ ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200
|
||||
|
||||
原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
## `sim2real_ros2_v2`
|
||||
|
||||
ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。该版本对应重排主线的 `v0.11.0`。
|
||||
|
||||
本阶段的两份大体积 PCD 已确定性抽样，Odin 驱动仍为外部依赖；详细边界见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
[](../../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 Sim2Real v2
|
||||
|
||||
本目录归档 `real/sim2real_ros2_v2`，对应重排主线的 `v0.11.0`。该阶段在 ROS 2/C++ 初版基础上增加简单导航、PCD 交互定位、任务点/任务序列和 Web 导航调试。
|
||||
|
||||
本工程保留当前 `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 驱动从后续站姿调参版本开始随工程归档。
|
||||
- 原始 `map1.pcd` 和 `map6.pcd` 已确定性抽样到 10 MB 以下，点数和哈希见 [`map/README.md`](map/README.md)。
|
||||
@@ -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_v2
|
||||
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_v2
|
||||
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_v2
|
||||
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_v2
|
||||
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_v2
|
||||
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_v2
|
||||
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
|
||||
@@ -0,0 +1,10 @@
|
||||
# `v0.11.0` 导航点云
|
||||
|
||||
本目录保存 `sim2real_ros2_v2` 简单导航原型引用的两份预览点云。为避免大体积 ASCII PCD 进入 Git 历史，使用 `tools/nav_tools/downsample_ascii_pcd.py` 做确定性等步长抽样，坐标值和原始顺序不做修改。
|
||||
|
||||
| 文件 | 原始点数 | 抽样步长 | 抽样点数 | 抽样大小 | 原始 SHA-256 | 抽样 SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- | --- |
|
||||
| `map1.pcd` | 1,604,103 | 6 | 267,350 | 8,572,407 B | `FBAD5DBA9AD15042FBC1702BDAAD5F7A3819B678E00558CFEC8BC54479A578AE` | `5F4AF5178E7F0D1AEAD7A2F454F6CB1524EF0F53AF0544FD415A8FB6A384000A` |
|
||||
| `map6.pcd` | 1,446,503 | 5 | 289,301 | 9,278,742 B | `77DEB28D739760C58E9C08B589CFD52C2A6CB2981CB3B51156BBCC26EFC4FA9F` | `792C11023A2F91415378580276F87BE103AD7B22A214D24D9FC1B6928EB875C4` |
|
||||
|
||||
抽样点云适合路线编辑、可视化和代码演示，不等价于原始高密度建图数据。需要复现实机定位精度时，应从原始备份恢复对应 PCD 并核对原始哈希。
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
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_v2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已引用该包。
|
||||
|
||||
因此 `v0.11.0` 主要记录简单导航与 PCD 导航原型，仍需另行提供兼容的 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()
|
||||
+103
@@ -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,101 @@
|
||||
/**:
|
||||
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: "NAV"
|
||||
cmd_mux_output_hz: 50.0
|
||||
cmd_mux_remote_timeout_ms: 250.0
|
||||
cmd_mux_web_timeout_ms: 300.0
|
||||
cmd_mux_nav_timeout_ms: 500.0
|
||||
cmd_mux_max_vx: 0.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_http_host: "0.0.0.0"
|
||||
web_http_port: 18080
|
||||
web_static_dir: "tools/win_web_debug/static"
|
||||
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
|
||||
|
||||
# Simple navigation parameters
|
||||
nav_map_frame: "map"
|
||||
nav_base_frame: "base_link"
|
||||
nav_control_hz: 20.0
|
||||
nav_goal_tolerance: 0.20
|
||||
nav_yaw_stop_threshold: 0.80
|
||||
nav_max_vx: 0.45
|
||||
nav_max_wz: 0.80
|
||||
nav_kp_dist: 0.80
|
||||
nav_kp_yaw: 1.80
|
||||
nav_goals_file: src/sim2real_nav2/config/maps/factory_a/goals.yaml
|
||||
nav_missions_file: src/sim2real_nav2/config/maps/factory_a/missions.yaml
|
||||
pcd_nav_file: map/map1.pcd
|
||||
pcd_floor_z_min: -1.6
|
||||
pcd_floor_z_max: 0.4
|
||||
pcd_sample_step: 25
|
||||
pcd_robot_radius: 0.18
|
||||
+144
@@ -0,0 +1,144 @@
|
||||
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='false',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
launch_remote_arg = DeclareLaunchArgument(
|
||||
'launch_remote',
|
||||
default_value='true',
|
||||
description='Whether to launch the SBUS UART remote control node'
|
||||
)
|
||||
|
||||
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||
'launch_web_bridge',
|
||||
default_value='true',
|
||||
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||
)
|
||||
|
||||
launch_simple_nav_arg = DeclareLaunchArgument(
|
||||
'launch_simple_nav',
|
||||
default_value='true',
|
||||
description='Whether to launch the simple waypoint navigation node'
|
||||
)
|
||||
|
||||
# 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,
|
||||
launch_simple_nav_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="simple_nav_node.py",
|
||||
name="sim2real_simple_nav_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_simple_nav')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="odom_relay_node",
|
||||
name="odom_relay_node",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"odom_input_topic": "/odin1/odometry",
|
||||
"odom_output_topic": "/odom",
|
||||
"base_frame": "base_link",
|
||||
"publish_tf": True,
|
||||
}],
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||
),
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
@@ -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,16 @@
|
||||
frame_id: map
|
||||
goals:
|
||||
dock:
|
||||
position: [0.0, 0.0, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "起始点/回充点"
|
||||
|
||||
test_p1:
|
||||
position: [1.5, 0.0, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "测试点1"
|
||||
|
||||
test_p2:
|
||||
position: [1.5, -1.0, 0.0]
|
||||
yaw_deg: -90.0
|
||||
description: "测试点2"
|
||||
+9
@@ -0,0 +1,9 @@
|
||||
missions:
|
||||
test_roundtrip:
|
||||
description: "测试点1到点2再回起点"
|
||||
loop: false
|
||||
goals:
|
||||
- dock
|
||||
- test_p1
|
||||
- test_p2
|
||||
- dock
|
||||
@@ -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,101 @@
|
||||
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
|
||||
src/simple_nav_node.py
|
||||
src/pcd_nav_click_tool.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,26 @@
|
||||
<?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-matplotlib</exec_depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<exec_depend>python3-yaml</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,170 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Circle, FancyArrow
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from tf2_ros import Buffer, TransformException, TransformListener
|
||||
|
||||
|
||||
class PcdNavClickTool(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("pcd_nav_click_tool")
|
||||
|
||||
self.map_frame = str(self.declare_parameter("nav_map_frame", "map").value)
|
||||
self.base_frame = str(self.declare_parameter("nav_base_frame", "base_link").value)
|
||||
self.pcd_file = str(self.declare_parameter("pcd_nav_file", "").value)
|
||||
self.floor_z_min = float(self.declare_parameter("pcd_floor_z_min", -1.6).value)
|
||||
self.floor_z_max = float(self.declare_parameter("pcd_floor_z_max", 0.4).value)
|
||||
self.sample_step = max(1, int(self.declare_parameter("pcd_sample_step", 25).value))
|
||||
self.robot_radius = float(self.declare_parameter("pcd_robot_radius", 0.18).value)
|
||||
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
self.cmd_pub = self.create_publisher(String, "/simple_nav/cmd", 10)
|
||||
|
||||
self.xy_points = self.load_filtered_pcd(Path(self.pcd_file))
|
||||
self.current_target: Optional[tuple[float, float]] = None
|
||||
|
||||
def load_filtered_pcd(self, path: Path) -> list[tuple[float, float]]:
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"PCD file not found: {path}")
|
||||
|
||||
points: list[tuple[float, float]] = []
|
||||
data_started = False
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
stripped = line.strip()
|
||||
if not stripped:
|
||||
continue
|
||||
if data_started:
|
||||
parts = stripped.split()
|
||||
if len(parts) < 3:
|
||||
continue
|
||||
try:
|
||||
x = float(parts[0])
|
||||
y = float(parts[1])
|
||||
z = float(parts[2])
|
||||
except ValueError:
|
||||
continue
|
||||
if self.floor_z_min <= z <= self.floor_z_max:
|
||||
points.append((x, y))
|
||||
elif stripped.upper().startswith("DATA"):
|
||||
if "ascii" not in stripped.lower():
|
||||
raise RuntimeError("Only ASCII PCD is supported by this simple tool")
|
||||
data_started = True
|
||||
|
||||
if not points:
|
||||
raise RuntimeError("No usable floor points after z filtering")
|
||||
return points[:: self.sample_step]
|
||||
|
||||
def lookup_pose(self) -> Optional[tuple[float, float, float]]:
|
||||
try:
|
||||
transform = self.tf_buffer.lookup_transform(self.map_frame, self.base_frame, rclpy.time.Time())
|
||||
except TransformException:
|
||||
return None
|
||||
t = transform.transform.translation
|
||||
q = transform.transform.rotation
|
||||
yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w)
|
||||
return float(t.x), float(t.y), float(yaw)
|
||||
|
||||
def publish_command(self, text: str) -> None:
|
||||
self.cmd_pub.publish(String(data=text))
|
||||
self.get_logger().info(text)
|
||||
|
||||
def on_click(self, event) -> None:
|
||||
if event.inaxes is None or event.xdata is None or event.ydata is None:
|
||||
return
|
||||
x = float(event.xdata)
|
||||
y = float(event.ydata)
|
||||
self.current_target = (x, y)
|
||||
self.publish_command(f"go {x:.3f} {y:.3f}")
|
||||
self.refresh_plot()
|
||||
|
||||
def on_key(self, event) -> None:
|
||||
if event.key == "r":
|
||||
pose = self.lookup_pose()
|
||||
if pose is not None:
|
||||
x, y, _ = pose
|
||||
self.publish_command("record p_click")
|
||||
self.current_target = (x, y)
|
||||
self.refresh_plot()
|
||||
elif event.key == "s":
|
||||
self.publish_command("stop")
|
||||
elif event.key == "escape":
|
||||
plt.close("all")
|
||||
|
||||
def refresh_plot(self) -> None:
|
||||
self.ax.clear()
|
||||
xs = [p[0] for p in self.xy_points]
|
||||
ys = [p[1] for p in self.xy_points]
|
||||
self.ax.scatter(xs, ys, s=1, c="black", alpha=0.35)
|
||||
|
||||
pose = self.lookup_pose()
|
||||
if pose is not None:
|
||||
rx, ry, ryaw = pose
|
||||
self.ax.add_patch(Circle((rx, ry), self.robot_radius, color="tab:blue", alpha=0.8))
|
||||
self.ax.add_patch(
|
||||
FancyArrow(
|
||||
rx,
|
||||
ry,
|
||||
0.35 * math.cos(ryaw),
|
||||
0.35 * math.sin(ryaw),
|
||||
width=0.03,
|
||||
color="tab:blue",
|
||||
)
|
||||
)
|
||||
self.ax.text(rx, ry, f" robot\n({rx:.2f},{ry:.2f})", color="tab:blue")
|
||||
|
||||
if self.current_target is not None:
|
||||
gx, gy = self.current_target
|
||||
self.ax.scatter([gx], [gy], s=80, c="tab:red", marker="x")
|
||||
self.ax.text(gx, gy, f" goal\n({gx:.2f},{gy:.2f})", color="tab:red")
|
||||
|
||||
self.ax.set_title(
|
||||
"PCD 2D Click Nav\nLeft click: go absolute | r: record | s: stop | Esc: quit"
|
||||
)
|
||||
self.ax.set_xlabel("map x")
|
||||
self.ax.set_ylabel("map y")
|
||||
self.ax.set_aspect("equal", adjustable="box")
|
||||
self.ax.grid(True, alpha=0.2)
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def run(self) -> None:
|
||||
self.fig, self.ax = plt.subplots(figsize=(10, 8))
|
||||
self.fig.canvas.mpl_connect("button_press_event", self.on_click)
|
||||
self.fig.canvas.mpl_connect("key_press_event", self.on_key)
|
||||
self.refresh_plot()
|
||||
|
||||
timer = self.create_timer(0.5, self.refresh_plot)
|
||||
try:
|
||||
plt.show()
|
||||
finally:
|
||||
timer.cancel()
|
||||
|
||||
@staticmethod
|
||||
def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
|
||||
siny_cosp = 2.0 * (w * z + x * y)
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
return math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = PcdNavClickTool()
|
||||
try:
|
||||
node.run()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -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,302 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Optional
|
||||
|
||||
import rclpy
|
||||
import yaml
|
||||
from geometry_msgs.msg import PoseStamped, Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from tf2_ros import Buffer, TransformException, TransformListener
|
||||
|
||||
|
||||
class SimpleNavNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_simple_nav_node")
|
||||
|
||||
self.map_frame = str(self.declare_parameter("nav_map_frame", "map").value)
|
||||
self.base_frame = str(self.declare_parameter("nav_base_frame", "base_link").value)
|
||||
self.control_hz = float(self.declare_parameter("nav_control_hz", 20.0).value)
|
||||
self.goal_tolerance = float(self.declare_parameter("nav_goal_tolerance", 0.20).value)
|
||||
self.yaw_stop_threshold = float(self.declare_parameter("nav_yaw_stop_threshold", 0.80).value)
|
||||
self.max_vx = float(self.declare_parameter("nav_max_vx", 0.45).value)
|
||||
self.max_wz = float(self.declare_parameter("nav_max_wz", 0.8).value)
|
||||
self.kp_dist = float(self.declare_parameter("nav_kp_dist", 0.8).value)
|
||||
self.kp_yaw = float(self.declare_parameter("nav_kp_yaw", 1.8).value)
|
||||
self.goals_file = str(self.declare_parameter("nav_goals_file", "").value)
|
||||
self.missions_file = str(self.declare_parameter("nav_missions_file", "").value)
|
||||
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_nav", 10)
|
||||
self.goal_pose_pub = self.create_publisher(PoseStamped, "simple_nav/goal_pose", 10)
|
||||
self.status_pub = self.create_publisher(String, "simple_nav/status", 10)
|
||||
self.record_pub = self.create_publisher(String, "simple_nav/recorded_pose", 10)
|
||||
self.create_subscription(String, "simple_nav/cmd", self.on_command, 10)
|
||||
|
||||
self.goals = self._load_goals(self.goals_file)
|
||||
self.missions = self._load_missions(self.missions_file)
|
||||
self.active_goal_name: Optional[str] = None
|
||||
self.active_goal_xy: Optional[tuple[float, float]] = None
|
||||
self.active_mission_name: Optional[str] = None
|
||||
self.active_mission_goals: list[str] = []
|
||||
self.active_mission_index = 0
|
||||
self.pose_waiting_reported = False
|
||||
|
||||
period = 1.0 / self.control_hz if self.control_hz > 0.0 else 0.05
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
self.get_logger().info(
|
||||
f"Simple nav started (map_frame={self.map_frame}, base_frame={self.base_frame}, goals={len(self.goals)}, missions={len(self.missions)})"
|
||||
)
|
||||
|
||||
def _load_yaml(self, path_value: str) -> dict[str, Any]:
|
||||
if not path_value:
|
||||
return {}
|
||||
path = Path(path_value).expanduser()
|
||||
if not path.exists():
|
||||
self.get_logger().warn(f"YAML file not found: {path}")
|
||||
return {}
|
||||
try:
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
data = yaml.safe_load(f) or {}
|
||||
return data if isinstance(data, dict) else {}
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Failed to load YAML {path}: {exc}")
|
||||
return {}
|
||||
|
||||
def _load_goals(self, path_value: str) -> dict[str, dict[str, float]]:
|
||||
data = self._load_yaml(path_value)
|
||||
raw_goals = data.get("goals", {})
|
||||
parsed: dict[str, dict[str, float]] = {}
|
||||
if not isinstance(raw_goals, dict):
|
||||
return parsed
|
||||
for name, spec in raw_goals.items():
|
||||
if not isinstance(spec, dict):
|
||||
continue
|
||||
pos = spec.get("position", [0.0, 0.0, 0.0])
|
||||
if not isinstance(pos, list) or len(pos) < 2:
|
||||
continue
|
||||
try:
|
||||
parsed[name] = {"x": float(pos[0]), "y": float(pos[1])}
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
return parsed
|
||||
|
||||
def _load_missions(self, path_value: str) -> dict[str, list[str]]:
|
||||
data = self._load_yaml(path_value)
|
||||
raw_missions = data.get("missions", {})
|
||||
parsed: dict[str, list[str]] = {}
|
||||
if not isinstance(raw_missions, dict):
|
||||
return parsed
|
||||
for name, spec in raw_missions.items():
|
||||
if not isinstance(spec, dict):
|
||||
continue
|
||||
goals = spec.get("goals", [])
|
||||
if isinstance(goals, list) and all(isinstance(item, str) for item in goals) and goals:
|
||||
parsed[name] = goals
|
||||
return parsed
|
||||
|
||||
def on_command(self, msg: String) -> None:
|
||||
command = msg.data.strip()
|
||||
if not command:
|
||||
return
|
||||
parts = command.split()
|
||||
op = parts[0].lower()
|
||||
|
||||
if op == "stop":
|
||||
self.cancel_navigation("manual stop")
|
||||
elif op == "record" and len(parts) >= 2:
|
||||
self.record_current_pose(parts[1])
|
||||
elif op == "goto" and len(parts) >= 2:
|
||||
self.start_goal(parts[1])
|
||||
elif op == "go" and len(parts) >= 3:
|
||||
try:
|
||||
self.start_direct_goal(float(parts[1]), float(parts[2]), f"direct({parts[1]},{parts[2]})")
|
||||
except ValueError:
|
||||
self.publish_status("go expects numeric x y")
|
||||
elif op == "go_rel" and len(parts) >= 3:
|
||||
try:
|
||||
self.start_relative_goal(float(parts[1]), float(parts[2]))
|
||||
except ValueError:
|
||||
self.publish_status("go_rel expects numeric dx dy")
|
||||
elif op == "run" and len(parts) >= 2:
|
||||
self.start_mission(parts[1])
|
||||
elif op == "reload":
|
||||
self.goals = self._load_goals(self.goals_file)
|
||||
self.missions = self._load_missions(self.missions_file)
|
||||
self.publish_status(f"reloaded goals={len(self.goals)} missions={len(self.missions)}")
|
||||
else:
|
||||
self.publish_status(f"unknown or incomplete command: {command}")
|
||||
|
||||
def start_goal(self, goal_name: str) -> None:
|
||||
goal = self.goals.get(goal_name)
|
||||
if goal is None:
|
||||
self.publish_status(f"goal not found: {goal_name}")
|
||||
return
|
||||
self.active_mission_name = None
|
||||
self.active_mission_goals = []
|
||||
self.active_mission_index = 0
|
||||
self.start_direct_goal(goal["x"], goal["y"], goal_name)
|
||||
|
||||
def start_direct_goal(self, x: float, y: float, goal_name: str) -> None:
|
||||
self.active_goal_name = goal_name
|
||||
self.active_goal_xy = (x, y)
|
||||
self.pose_waiting_reported = False
|
||||
self.publish_goal_pose(x, y)
|
||||
self.publish_status(f"nav target set in {self.map_frame}: {goal_name} -> ({x:.2f}, {y:.2f})")
|
||||
|
||||
def start_relative_goal(self, dx: float, dy: float) -> None:
|
||||
pose = self.lookup_pose()
|
||||
if pose is None:
|
||||
self.publish_status("go_rel failed: pose unavailable")
|
||||
return
|
||||
rx, ry, ryaw = pose
|
||||
gx = rx + math.cos(ryaw) * dx - math.sin(ryaw) * dy
|
||||
gy = ry + math.sin(ryaw) * dx + math.cos(ryaw) * dy
|
||||
self.active_mission_name = None
|
||||
self.active_mission_goals = []
|
||||
self.active_mission_index = 0
|
||||
self.start_direct_goal(gx, gy, f"relative(dx={dx:.2f},dy={dy:.2f})")
|
||||
|
||||
def start_mission(self, mission_name: str) -> None:
|
||||
goals = self.missions.get(mission_name)
|
||||
if goals is None:
|
||||
self.publish_status(f"mission not found: {mission_name}")
|
||||
return
|
||||
self.active_mission_name = mission_name
|
||||
self.active_mission_goals = list(goals)
|
||||
self.active_mission_index = 0
|
||||
self._activate_mission_goal()
|
||||
|
||||
def _activate_mission_goal(self) -> None:
|
||||
if self.active_mission_index >= len(self.active_mission_goals):
|
||||
self.cancel_navigation("mission complete")
|
||||
return
|
||||
goal_name = self.active_mission_goals[self.active_mission_index]
|
||||
goal = self.goals.get(goal_name)
|
||||
if goal is None:
|
||||
self.cancel_navigation(f"mission goal missing: {goal_name}")
|
||||
return
|
||||
self.active_goal_name = goal_name
|
||||
self.active_goal_xy = (goal["x"], goal["y"])
|
||||
self.publish_goal_pose(goal["x"], goal["y"])
|
||||
self.publish_status(
|
||||
f"mission {self.active_mission_name}: waypoint {self.active_mission_index + 1}/{len(self.active_mission_goals)} -> {goal_name}"
|
||||
)
|
||||
|
||||
def record_current_pose(self, name: str) -> None:
|
||||
pose = self.lookup_pose()
|
||||
if pose is None:
|
||||
self.publish_status("record failed: pose unavailable")
|
||||
return
|
||||
x, y, yaw = pose
|
||||
text = f"{name}: frame={self.map_frame}, x={x:.3f}, y={y:.3f}, yaw_deg={math.degrees(yaw):.1f}"
|
||||
self.record_pub.publish(String(data=text))
|
||||
self.publish_status(f"recorded {text}")
|
||||
|
||||
def lookup_pose(self) -> Optional[tuple[float, float, float]]:
|
||||
try:
|
||||
transform = self.tf_buffer.lookup_transform(self.map_frame, self.base_frame, rclpy.time.Time())
|
||||
except TransformException:
|
||||
return None
|
||||
t = transform.transform.translation
|
||||
q = transform.transform.rotation
|
||||
yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w)
|
||||
return float(t.x), float(t.y), float(yaw)
|
||||
|
||||
def on_timer(self) -> None:
|
||||
if self.active_goal_xy is None:
|
||||
return
|
||||
pose = self.lookup_pose()
|
||||
if pose is None:
|
||||
if not self.pose_waiting_reported:
|
||||
self.publish_status(f"pose unavailable: waiting for TF {self.map_frame} -> {self.base_frame}")
|
||||
self.pose_waiting_reported = True
|
||||
self.cmd_pub.publish(Twist())
|
||||
return
|
||||
if self.pose_waiting_reported:
|
||||
self.publish_status(f"pose available: TF {self.map_frame} -> {self.base_frame} restored")
|
||||
self.pose_waiting_reported = False
|
||||
|
||||
rx, ry, ryaw = pose
|
||||
gx, gy = self.active_goal_xy
|
||||
dx = gx - rx
|
||||
dy = gy - ry
|
||||
dist = math.hypot(dx, dy)
|
||||
|
||||
if dist < self.goal_tolerance:
|
||||
reached_name = self.active_goal_name or "goal"
|
||||
self.cmd_pub.publish(Twist())
|
||||
if self.active_mission_name is not None:
|
||||
self.publish_status(f"reached {reached_name}")
|
||||
self.active_mission_index += 1
|
||||
self._activate_mission_goal()
|
||||
else:
|
||||
self.cancel_navigation(f"reached {reached_name}")
|
||||
return
|
||||
|
||||
target_yaw = math.atan2(dy, dx)
|
||||
yaw_err = self.normalize_angle(target_yaw - ryaw)
|
||||
cmd = Twist()
|
||||
cmd.angular.z = self.clamp(self.kp_yaw * yaw_err, -self.max_wz, self.max_wz)
|
||||
if abs(yaw_err) <= self.yaw_stop_threshold:
|
||||
cmd.linear.x = self.clamp(self.kp_dist * dist * math.cos(yaw_err), 0.0, self.max_vx)
|
||||
self.cmd_pub.publish(cmd)
|
||||
|
||||
def cancel_navigation(self, reason: str) -> None:
|
||||
self.active_goal_name = None
|
||||
self.active_goal_xy = None
|
||||
self.active_mission_name = None
|
||||
self.active_mission_goals = []
|
||||
self.active_mission_index = 0
|
||||
self.cmd_pub.publish(Twist())
|
||||
self.publish_status(f"navigation stopped: {reason}")
|
||||
|
||||
def publish_goal_pose(self, x: float, y: float) -> None:
|
||||
msg = PoseStamped()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self.map_frame
|
||||
msg.pose.position.x = x
|
||||
msg.pose.position.y = y
|
||||
msg.pose.orientation.w = 1.0
|
||||
self.goal_pose_pub.publish(msg)
|
||||
|
||||
def publish_status(self, text: str) -> None:
|
||||
self.status_pub.publish(String(data=text))
|
||||
self.get_logger().info(text)
|
||||
|
||||
@staticmethod
|
||||
def quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
|
||||
siny_cosp = 2.0 * (w * z + x * y)
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
return math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
@staticmethod
|
||||
def normalize_angle(angle: float) -> float:
|
||||
return (angle + math.pi) % (2.0 * math.pi) - math.pi
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, float(value)))
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SimpleNavNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,442 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
import threading
|
||||
from http import HTTPStatus
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
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
|
||||
from tf2_ros import Buffer, TransformException, TransformListener
|
||||
|
||||
|
||||
class WebUdpBridgeNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value)
|
||||
self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value)
|
||||
self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value)
|
||||
self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value)
|
||||
self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value)
|
||||
self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value)
|
||||
self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value)
|
||||
self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value)
|
||||
self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value)
|
||||
self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value)
|
||||
|
||||
self.http_host = str(self.declare_parameter("web_http_host", "0.0.0.0").value)
|
||||
self.http_port = int(self.declare_parameter("web_http_port", 18080).value)
|
||||
self.web_static_dir = str(self.declare_parameter("web_static_dir", "").value)
|
||||
self.nav_map_frame = str(self.declare_parameter("nav_map_frame", "map").value)
|
||||
self.nav_base_frame = str(self.declare_parameter("nav_base_frame", "base_link").value)
|
||||
self.pcd_nav_file = str(self.declare_parameter("pcd_nav_file", "").value)
|
||||
self.pcd_floor_z_min = float(self.declare_parameter("pcd_floor_z_min", -1.6).value)
|
||||
self.pcd_floor_z_max = float(self.declare_parameter("pcd_floor_z_max", 0.4).value)
|
||||
self.pcd_sample_step = max(1, int(self.declare_parameter("pcd_sample_step", 25).value))
|
||||
|
||||
self.tf_buffer = Buffer()
|
||||
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||
|
||||
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.latest_nav_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.nav_cmd_pub = self.create_publisher(String, "/simple_nav/cmd", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10)
|
||||
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||
|
||||
self.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.create_subscription(String, "simple_nav/status", self.on_nav_status, 10)
|
||||
|
||||
self.map_points = self.load_filtered_pcd(Path(self.pcd_nav_file)) if self.pcd_nav_file else []
|
||||
self.http_server: Optional[ThreadingHTTPServer] = None
|
||||
self.http_thread: Optional[threading.Thread] = None
|
||||
self.static_dir = Path(self.web_static_dir) if self.web_static_dir else Path(__file__).resolve().parents[3] / "tools" / "win_web_debug" / "static"
|
||||
self.start_http_server()
|
||||
|
||||
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||
|
||||
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||
self.get_logger().info(f"Web HTTP UI serving on http://{self.http_host}:{self.http_port}")
|
||||
|
||||
def load_filtered_pcd(self, path: Path) -> list[list[float]]:
|
||||
if not path.exists():
|
||||
self.get_logger().warn(f"PCD file not found: {path}")
|
||||
return []
|
||||
|
||||
points: list[list[float]] = []
|
||||
data_started = False
|
||||
with path.open("r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
stripped = line.strip()
|
||||
if not stripped:
|
||||
continue
|
||||
if data_started:
|
||||
parts = stripped.split()
|
||||
if len(parts) < 3:
|
||||
continue
|
||||
try:
|
||||
x = float(parts[0])
|
||||
y = float(parts[1])
|
||||
z = float(parts[2])
|
||||
except ValueError:
|
||||
continue
|
||||
if self.pcd_floor_z_min <= z <= self.pcd_floor_z_max:
|
||||
points.append([round(x, 3), round(y, 3)])
|
||||
elif stripped.upper().startswith("DATA"):
|
||||
data_started = True
|
||||
return points[:: self.pcd_sample_step]
|
||||
|
||||
def lookup_pose(self) -> Optional[dict[str, float]]:
|
||||
try:
|
||||
transform = self.tf_buffer.lookup_transform(self.nav_map_frame, self.nav_base_frame, rclpy.time.Time())
|
||||
except TransformException:
|
||||
return None
|
||||
t = transform.transform.translation
|
||||
q = transform.transform.rotation
|
||||
yaw = self.quaternion_to_yaw(q.x, q.y, q.z, q.w)
|
||||
return {"x": round(float(t.x), 3), "y": round(float(t.y), 3), "yaw": round(float(yaw), 6)}
|
||||
|
||||
def start_http_server(self) -> None:
|
||||
node = self
|
||||
static_dir = self.static_dir
|
||||
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
def do_GET(self):
|
||||
if self.path in ["/", "/index.html"]:
|
||||
self.serve_file(static_dir / "index.html", "text/html; charset=utf-8")
|
||||
elif self.path == "/app.js":
|
||||
self.serve_file(static_dir / "app.js", "application/javascript; charset=utf-8")
|
||||
elif self.path == "/style.css":
|
||||
self.serve_file(static_dir / "style.css", "text/css; charset=utf-8")
|
||||
elif self.path == "/api/state":
|
||||
self.send_json(node.build_state_packet())
|
||||
elif self.path == "/api/map":
|
||||
self.send_json({
|
||||
"points": node.map_points,
|
||||
"map_frame": node.nav_map_frame,
|
||||
"base_frame": node.nav_base_frame,
|
||||
"pose": node.lookup_pose(),
|
||||
})
|
||||
else:
|
||||
self.send_error(HTTPStatus.NOT_FOUND)
|
||||
|
||||
def do_POST(self):
|
||||
if self.path != "/api/control":
|
||||
self.send_error(HTTPStatus.NOT_FOUND)
|
||||
return
|
||||
try:
|
||||
length = int(self.headers.get("Content-Length", "0"))
|
||||
raw = self.rfile.read(length)
|
||||
payload = json.loads(raw.decode("utf-8"))
|
||||
node.handle_http_control(payload)
|
||||
self.send_json({"ok": True})
|
||||
except Exception as exc:
|
||||
self.send_json({"ok": False, "error": str(exc)}, status=HTTPStatus.BAD_REQUEST)
|
||||
|
||||
def serve_file(self, path: Path, content_type: str):
|
||||
if not path.exists():
|
||||
self.send_error(HTTPStatus.NOT_FOUND)
|
||||
return
|
||||
data = path.read_bytes()
|
||||
self.send_response(HTTPStatus.OK)
|
||||
self.send_header("Content-Type", content_type)
|
||||
self.send_header("Content-Length", str(len(data)))
|
||||
self.end_headers()
|
||||
self.wfile.write(data)
|
||||
|
||||
def send_json(self, payload: dict[str, Any], status: HTTPStatus = HTTPStatus.OK):
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
self.send_response(status)
|
||||
self.send_header("Content-Type", "application/json")
|
||||
self.send_header("Content-Length", str(len(data)))
|
||||
self.end_headers()
|
||||
self.wfile.write(data)
|
||||
|
||||
def log_message(self, format: str, *args):
|
||||
return
|
||||
|
||||
self.http_server = ThreadingHTTPServer((self.http_host, self.http_port), Handler)
|
||||
self.http_thread = threading.Thread(target=self.http_server.serve_forever, daemon=True)
|
||||
self.http_thread.start()
|
||||
|
||||
def handle_http_control(self, payload: dict[str, Any]) -> None:
|
||||
msg_type = str(payload.get("type", "")).lower()
|
||||
if msg_type in {"cmd_vel", "zero", "estop", "mode", "web_enable", "remote_enable", "nav_enable", "ping", "nav_cmd", "go_to", "go_rel"}:
|
||||
self.handle_packet(payload)
|
||||
return
|
||||
raise ValueError(f"unknown control type: {msg_type}")
|
||||
|
||||
def on_target(self, msg: RuntimeTarget) -> None:
|
||||
self.latest_target = msg
|
||||
|
||||
def on_state(self, msg: RuntimeState) -> None:
|
||||
self.latest_state = msg
|
||||
|
||||
def on_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_nav_status(self, msg: String) -> None:
|
||||
self.latest_nav_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.set_control_mode("WEB")
|
||||
self.cmd_pub.publish(cmd)
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
elif msg_type == "zero":
|
||||
self.cmd_pub.publish(Twist())
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
elif msg_type == "estop":
|
||||
self.estop_pub.publish(Bool(data=bool(payload.get("data", True))))
|
||||
elif msg_type == "mode":
|
||||
mode = str(payload.get("mode", "DISABLED")).upper()
|
||||
self.set_control_mode(mode)
|
||||
elif msg_type == "web_enable":
|
||||
self.web_enabled = bool(payload.get("data", False))
|
||||
self.web_enabled_pub.publish(Bool(data=self.web_enabled))
|
||||
if self.web_enabled:
|
||||
self.set_control_mode("WEB")
|
||||
elif msg_type == "remote_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.remote_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.set_control_mode("REMOTE")
|
||||
elif msg_type == "nav_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.nav_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.set_control_mode("NAV")
|
||||
elif msg_type == "nav_cmd":
|
||||
command = str(payload.get("command", "")).strip()
|
||||
if command:
|
||||
self.set_control_mode("NAV")
|
||||
self.nav_cmd_pub.publish(String(data=command))
|
||||
elif msg_type == "go_to":
|
||||
x = float(payload.get("x", 0.0))
|
||||
y = float(payload.get("y", 0.0))
|
||||
self.set_control_mode("NAV")
|
||||
self.nav_cmd_pub.publish(String(data=f"go {x:.3f} {y:.3f}"))
|
||||
elif msg_type == "go_rel":
|
||||
dx = float(payload.get("dx", 0.0))
|
||||
dy = float(payload.get("dy", 0.0))
|
||||
self.set_control_mode("NAV")
|
||||
self.nav_cmd_pub.publish(String(data=f"go_rel {dx:.3f} {dy:.3f}"))
|
||||
elif msg_type == "ping":
|
||||
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||
elif msg_type == "map_request":
|
||||
self.send_packet({
|
||||
"type": "map",
|
||||
"points": self.map_points,
|
||||
"map_frame": self.nav_map_frame,
|
||||
"base_frame": self.nav_base_frame,
|
||||
"pose": self.lookup_pose(),
|
||||
})
|
||||
else:
|
||||
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||
|
||||
def set_control_mode(self, mode: str) -> None:
|
||||
mode = str(mode).upper()
|
||||
self.mode_pub.publish(String(data=mode))
|
||||
self.web_enabled = mode == "WEB"
|
||||
self.web_enabled_pub.publish(Bool(data=mode == "WEB"))
|
||||
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
|
||||
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
|
||||
|
||||
def parse_twist(self, payload: dict[str, Any]) -> Twist:
|
||||
cmd = Twist()
|
||||
linear = payload.get("linear", {}) or {}
|
||||
angular = payload.get("angular", {}) or {}
|
||||
cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx)
|
||||
cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy)
|
||||
cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw)
|
||||
return cmd
|
||||
|
||||
def on_guard_timer(self) -> None:
|
||||
age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6
|
||||
if age_ms > self.cmd_timeout_ms:
|
||||
self.cmd_pub.publish(Twist())
|
||||
if self.estop_on_timeout and not self.timeout_estop_sent:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.timeout_estop_sent = True
|
||||
|
||||
def on_state_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
self.send_packet(self.build_state_packet())
|
||||
|
||||
def build_state_packet(self) -> dict[str, Any]:
|
||||
target = self.latest_target
|
||||
state = self.latest_state
|
||||
packet: dict[str, Any] = {
|
||||
"type": "state",
|
||||
"stamp": self.now_sec(),
|
||||
"mode": self.latest_mode,
|
||||
"mux_status": self.latest_mux_status,
|
||||
"nav_status": self.latest_nav_status,
|
||||
"estop": self.estop,
|
||||
"web_enabled": self.web_enabled,
|
||||
"connected": True,
|
||||
"local_receive_time": self.now_sec(),
|
||||
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||
"runtime": {},
|
||||
"robot": {},
|
||||
"nav": {"pose": self.lookup_pose(), "map_frame": self.nav_map_frame, "base_frame": self.nav_base_frame},
|
||||
}
|
||||
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 quaternion_to_yaw(x: float, y: float, z: float, w: float) -> float:
|
||||
siny_cosp = 2.0 * (w * z + x * y)
|
||||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||||
return math.atan2(siny_cosp, cosy_cosp)
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, value))
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = WebUdpBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
if node.http_server is not None:
|
||||
node.http_server.shutdown()
|
||||
node.http_server.server_close()
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -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,120 @@
|
||||
#!/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
|
||||
if self.path == "/api/map":
|
||||
send_udp({"type": "map_request", "stamp": time.time()})
|
||||
deadline = time.time() + 1.0
|
||||
while time.time() < deadline:
|
||||
with STATE_LOCK:
|
||||
maybe_map = LATEST_STATE.get("map")
|
||||
if isinstance(maybe_map, dict):
|
||||
data = json.dumps(maybe_map).encode("utf-8")
|
||||
self._json(200, data)
|
||||
return
|
||||
time.sleep(0.02)
|
||||
self._json(200, b'{"points":[],"pose":null}')
|
||||
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,426 @@
|
||||
'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 currentMode = 'UNKNOWN';
|
||||
let dragging = false;
|
||||
let mapModel = null;
|
||||
let latestPose = null;
|
||||
let lastGoal = null;
|
||||
let pendingModePromise = null;
|
||||
|
||||
const mapCanvas = $('map-canvas');
|
||||
const mapCtx = mapCanvas ? mapCanvas.getContext('2d') : null;
|
||||
|
||||
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' });
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
async function setMode(mode) {
|
||||
if (mode === 'WEB') {
|
||||
const ok = confirm('Confirm switch to WEB control?\nMake sure the robot is safe and velocity is zero.');
|
||||
if (!ok) return false;
|
||||
}
|
||||
await post({ type: 'mode', mode });
|
||||
highlightMode(mode);
|
||||
appendEvent('MODE_SET', `-> ${mode}`, 'ok');
|
||||
return true;
|
||||
}
|
||||
|
||||
async function ensureMode(mode) {
|
||||
if (currentMode === mode) return true;
|
||||
if (pendingModePromise) {
|
||||
const activeMode = await pendingModePromise;
|
||||
if (activeMode === mode) return true;
|
||||
}
|
||||
pendingModePromise = (async () => {
|
||||
const ok = await setMode(mode);
|
||||
return ok ? mode : currentMode;
|
||||
})();
|
||||
const resolvedMode = await pendingModePromise;
|
||||
pendingModePromise = null;
|
||||
return resolvedMode === mode;
|
||||
}
|
||||
|
||||
function worldToCanvas(x, y) {
|
||||
if (!mapModel) return null;
|
||||
const { minX, minY, scale, pad, drawH } = mapModel;
|
||||
return {
|
||||
x: pad + (x - minX) * scale,
|
||||
y: pad + drawH - (y - minY) * scale,
|
||||
};
|
||||
}
|
||||
|
||||
function canvasToWorld(px, py) {
|
||||
if (!mapModel) return null;
|
||||
const { minX, minY, scale, pad, drawH } = mapModel;
|
||||
return {
|
||||
x: minX + (px - pad) / scale,
|
||||
y: minY + (drawH - (py - pad)) / scale,
|
||||
};
|
||||
}
|
||||
|
||||
function buildMapModel(points) {
|
||||
if (!points || !points.length || !mapCanvas) return null;
|
||||
let minX = Infinity;
|
||||
let minY = Infinity;
|
||||
let maxX = -Infinity;
|
||||
let maxY = -Infinity;
|
||||
for (const [x, y] of points) {
|
||||
if (x < minX) minX = x;
|
||||
if (y < minY) minY = y;
|
||||
if (x > maxX) maxX = x;
|
||||
if (y > maxY) maxY = y;
|
||||
}
|
||||
const pad = 20;
|
||||
const usableW = mapCanvas.width - pad * 2;
|
||||
const usableH = mapCanvas.height - pad * 2;
|
||||
const spanX = Math.max(maxX - minX, 1e-6);
|
||||
const spanY = Math.max(maxY - minY, 1e-6);
|
||||
const scale = Math.min(usableW / spanX, usableH / spanY);
|
||||
return { minX, minY, maxX, maxY, scale, pad, drawH: usableH };
|
||||
}
|
||||
|
||||
function drawMap() {
|
||||
if (!mapCtx || !mapCanvas) return;
|
||||
mapCtx.clearRect(0, 0, mapCanvas.width, mapCanvas.height);
|
||||
mapCtx.fillStyle = '#0b1020';
|
||||
mapCtx.fillRect(0, 0, mapCanvas.width, mapCanvas.height);
|
||||
|
||||
if (!mapModel) {
|
||||
mapCtx.fillStyle = '#9ca3af';
|
||||
mapCtx.font = '16px sans-serif';
|
||||
mapCtx.fillText('Map not loaded', 24, 32);
|
||||
return;
|
||||
}
|
||||
|
||||
mapCtx.fillStyle = 'rgba(255,255,255,0.28)';
|
||||
for (const [x, y] of mapModel.points) {
|
||||
const p = worldToCanvas(x, y);
|
||||
if (!p) continue;
|
||||
mapCtx.fillRect(p.x, p.y, 1.5, 1.5);
|
||||
}
|
||||
|
||||
if (latestPose) {
|
||||
const p = worldToCanvas(latestPose.x, latestPose.y);
|
||||
if (p) {
|
||||
mapCtx.fillStyle = '#3b82f6';
|
||||
mapCtx.beginPath();
|
||||
mapCtx.arc(p.x, p.y, 6, 0, Math.PI * 2);
|
||||
mapCtx.fill();
|
||||
mapCtx.strokeStyle = '#60a5fa';
|
||||
mapCtx.lineWidth = 2;
|
||||
mapCtx.beginPath();
|
||||
mapCtx.moveTo(p.x, p.y);
|
||||
mapCtx.lineTo(p.x + Math.cos(latestPose.yaw) * 18, p.y - Math.sin(latestPose.yaw) * 18);
|
||||
mapCtx.stroke();
|
||||
}
|
||||
}
|
||||
|
||||
if (lastGoal) {
|
||||
const p = worldToCanvas(lastGoal.x, lastGoal.y);
|
||||
if (p) {
|
||||
mapCtx.strokeStyle = '#ef4444';
|
||||
mapCtx.lineWidth = 2;
|
||||
mapCtx.beginPath();
|
||||
mapCtx.moveTo(p.x - 7, p.y - 7);
|
||||
mapCtx.lineTo(p.x + 7, p.y + 7);
|
||||
mapCtx.moveTo(p.x + 7, p.y - 7);
|
||||
mapCtx.lineTo(p.x - 7, p.y + 7);
|
||||
mapCtx.stroke();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async function fetchMap() {
|
||||
try {
|
||||
const res = await fetch('/api/map');
|
||||
const data = await res.json();
|
||||
const points = data.points || [];
|
||||
mapModel = buildMapModel(points);
|
||||
if (mapModel) mapModel.points = points;
|
||||
if (data.pose) latestPose = data.pose;
|
||||
drawMap();
|
||||
appendEvent('MAP', `loaded ${points.length} filtered points`, 'ok');
|
||||
} catch (e) {
|
||||
appendEvent('MAP_ERROR', e.message, 'bad');
|
||||
}
|
||||
}
|
||||
|
||||
function onMapClick(event) {
|
||||
if (!mapCanvas || !mapModel) return;
|
||||
const rect = mapCanvas.getBoundingClientRect();
|
||||
const px = (event.clientX - rect.left) * (mapCanvas.width / rect.width);
|
||||
const py = (event.clientY - rect.top) * (mapCanvas.height / rect.height);
|
||||
const world = canvasToWorld(px, py);
|
||||
if (!world) return;
|
||||
lastGoal = world;
|
||||
drawMap();
|
||||
ensureMode('NAV').then(ok => {
|
||||
if (!ok) return;
|
||||
post({ type: 'go_to', x: world.x, y: world.y });
|
||||
appendEvent('NAV_GO', `x=${world.x.toFixed(2)} y=${world.y.toFixed(2)}`, 'ok');
|
||||
});
|
||||
}
|
||||
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
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';
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
function applyState(data) {
|
||||
const connected = data.connected && (!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||
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)}` : '--');
|
||||
setText('d-nav-status', data.nav_status || '--');
|
||||
|
||||
const nav = data.nav || {};
|
||||
if (nav.pose) {
|
||||
latestPose = nav.pose;
|
||||
setText('d-nav-pose', `x=${nav.pose.x.toFixed(2)} y=${nav.pose.y.toFixed(2)} yaw=${(nav.pose.yaw * 57.2958).toFixed(1)}deg`);
|
||||
drawMap();
|
||||
}
|
||||
|
||||
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 (_) {}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
$('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('Confirm soft e-stop?')) {
|
||||
post({ type: 'estop', data: true });
|
||||
zeroAll();
|
||||
appendEvent('ESTOP', 'soft e-stop triggered', 'bad');
|
||||
}
|
||||
};
|
||||
$('btn-refresh-map').onclick = fetchMap;
|
||||
$('btn-record').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'nav_cmd', command: 'record web_p1' });
|
||||
});
|
||||
$('btn-stop-nav').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'nav_cmd', command: 'stop' });
|
||||
});
|
||||
$('btn-go-rel').onclick = () => ensureMode('NAV').then(ok => {
|
||||
if (ok) post({ type: 'go_rel', dx: 0.3, dy: 0.0 });
|
||||
});
|
||||
|
||||
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);
|
||||
ensureMode('WEB').then(ok => {
|
||||
if (ok) sendCmd();
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
const joystick = $('joystick');
|
||||
const stick = $('stick');
|
||||
|
||||
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 => {
|
||||
ensureMode('WEB').then(ok => {
|
||||
if (!ok) {
|
||||
dragging = false;
|
||||
zeroAll();
|
||||
return;
|
||||
}
|
||||
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();
|
||||
});
|
||||
|
||||
if (mapCanvas) mapCanvas.addEventListener('click', onMapClick);
|
||||
|
||||
setInterval(() => {
|
||||
if (currentMode === 'WEB' && !dragging) sendCmd();
|
||||
}, 50);
|
||||
|
||||
initJointsGrid();
|
||||
setInterval(poll, 100);
|
||||
fetchMap();
|
||||
drawMap();
|
||||
appendEvent('READY', 'page loaded, waiting for bridge state', 'ok');
|
||||
@@ -0,0 +1,116 @@
|
||||
<!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 class="diag-row"><span class="diag-label">nav pose</span><span class="diag-value" id="d-nav-pose">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">nav status</span><span class="diag-value" id="d-nav-status">--</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>
|
||||
|
||||
<main class="glass-panel center-panel">
|
||||
<div class="panel-section map-section">
|
||||
<div class="map-header">
|
||||
<h2 class="panel-title">PCD 2D 导航</h2>
|
||||
<div class="map-actions">
|
||||
<button class="btn" id="btn-refresh-map">刷新地图</button>
|
||||
<button class="btn" id="btn-record">记录当前位置</button>
|
||||
<button class="btn" id="btn-stop-nav">停止导航</button>
|
||||
<button class="btn" id="btn-go-rel">前进 0.3m</button>
|
||||
</div>
|
||||
</div>
|
||||
<canvas id="map-canvas" width="900" height="680"></canvas>
|
||||
<div class="map-help">左键点击地图发送绝对目标；蓝点是当前机器人，红叉是最近目标。</div>
|
||||
</div>
|
||||
</main>
|
||||
|
||||
<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,299 @@
|
||||
/* 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 {
|
||||
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: 1700px;
|
||||
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 {
|
||||
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); }
|
||||
|
||||
.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-panel,
|
||||
.center-panel {
|
||||
position: fixed;
|
||||
top: 80px;
|
||||
bottom: 20px;
|
||||
border-radius: var(--panel-radius);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
.left-panel { left: 2%; width: 320px; }
|
||||
.right-panel { right: 2%; width: 320px; }
|
||||
.center-panel { left: calc(2% + 340px); right: calc(2% + 340px); }
|
||||
.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-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; }
|
||||
|
||||
.map-section {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 10px;
|
||||
height: 100%;
|
||||
}
|
||||
.map-header {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
gap: 12px;
|
||||
}
|
||||
.map-actions {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
gap: 8px;
|
||||
}
|
||||
#map-canvas {
|
||||
width: 100%;
|
||||
height: calc(100% - 56px);
|
||||
min-height: 520px;
|
||||
background: rgba(6, 10, 18, 0.95);
|
||||
border: 1px solid rgba(255,255,255,0.08);
|
||||
border-radius: 10px;
|
||||
}
|
||||
.map-help {
|
||||
font-size: 12px;
|
||||
color: var(--text-tertiary);
|
||||
}
|
||||
|
||||
.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-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);
|
||||
}
|
||||
|
||||
.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-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: 1400px) {
|
||||
.center-panel { left: 34%; right: 34%; }
|
||||
}
|
||||
|
||||
@media (max-width: 1100px) {
|
||||
.top-bar { width: 100%; border-radius: 0; top: 0; }
|
||||
body { padding-top: 56px; }
|
||||
.side-panel, .center-panel {
|
||||
position: relative;
|
||||
top: auto;
|
||||
left: auto;
|
||||
right: auto;
|
||||
bottom: auto;
|
||||
width: 100%;
|
||||
border-radius: 0;
|
||||
}
|
||||
.center-panel { min-height: 700px; }
|
||||
#map-canvas { min-height: 420px; }
|
||||
}
|
||||
@@ -39,7 +39,7 @@ RC_WheelLeg/
|
||||
- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
|
||||
- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
|
||||
- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
|
||||
- [x] 整理 Python Sim2Real v2 与 ROS 2/C++ 初版
|
||||
- [x] 整理 Python Sim2Real v2、ROS 2/C++ 初版与导航原型
|
||||
- [ ] 核对比赛机械与仿真模型参数
|
||||
- [ ] 整理 URDF/MJCF 机器人描述
|
||||
- [ ] 整理后续统一训练、ROS 2 和比赛版本
|
||||
|
||||
Reference in New Issue
Block a user