Compare commits
9 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
117b02bec7 | ||
|
zeitvex
|
196044b993 | ||
|
zeitvex
|
645819a65f | ||
|
zeitvex
|
1b551f974b | ||
|
zeitvex
|
765173676f | ||
|
zeitvex
|
dc7a2c2a67 | ||
|
zeitvex
|
c8aa490b37 | ||
|
zeitvex
|
302b6acc64 | ||
|
zeitvex
|
3bf6eb0c4c |
@@ -29,6 +29,9 @@ log/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
|
||||
!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
|
||||
!05_software/real/sim2real_ros2_v3/src/odin_ros_driver/lib/*.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
|
||||
@@ -14,6 +14,83 @@
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
|
||||
| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
|
||||
| `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
|
||||
| `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 |
|
||||
| `v0.12.0` | 里程计导航联调 | 纯里程计 fallback、A_min 路线、TF 冲突保护和 model_9600 |
|
||||
| `v1.0.0` | ROS 2 v3 比赛最终部署 | last_not_slalom_1050、model_6800/model_84、最终路线和触控屏 |
|
||||
| `v1.0.1` | 比赛成果媒体补充 | 最终机器人图片与 1050 分比赛视频 |
|
||||
| `v1.0.2` | 文档一致性修正 | 统一历史 Tag、当前快照和成果媒体的描述 |
|
||||
| `v1.1.0` | 三代目录规范化 | 恢复无后缀初版、保留 v2 里程计版、明确最终比赛 v3，并校准训练 README |
|
||||
|
||||
> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排后已正式归入 `v1.0.0`。
|
||||
|
||||
## `v0.9.0` 的 Python Sim2Real v2
|
||||
|
||||
- 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。
|
||||
- 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。
|
||||
- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
|
||||
- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
|
||||
|
||||
## `v0.10.0` 的 ROS 2/C++ Sim2Real 初版
|
||||
|
||||
- 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。
|
||||
- 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。
|
||||
- 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。
|
||||
- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
|
||||
- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
|
||||
|
||||
## `v0.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.11.1` 的 Odin、TensorRT 与站姿调参
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，在同一 `sim2real_ros2_v2` 目录中记录真实差异。
|
||||
- 首次随部署工程保留完整 Odin ROS 驱动、Apache-2.0 许可证、设备标定参数和预编译 SDK 静态库。
|
||||
- 策略运行时增加 TensorRT、ONNX 回退、Rough/Crawl 模式切换、事件日志和更完整的电机失效诊断。
|
||||
- 默认 Rough 策略为 `NEWmodel_1900`，默认站姿为髋俯仰 `0.670`、膝关节 `-1.390`；Crawl 配置使用 IK 后端。
|
||||
- `map_b.pcd` 从 1,080,047 点确定性抽样为 270,012 点，并保留原始和抽样哈希。
|
||||
- 排除嵌套 Git、Odin 运行日志、缓存、开发草稿和未被配置引用的候选策略。
|
||||
|
||||
## `v0.12.0` 的里程计导航联调
|
||||
|
||||
- 归档 `real/sim2real_ros2_v2(odom)`，继续沿用 `sim2real_ros2_v2` 目录的线性演进。
|
||||
- 将 Rough 策略切换为 `model_9600`，默认站姿恢复为髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- Odin `custom_map_mode` 固定为纯里程计，加入 odom 新鲜度、外部 map/odom TF 冲突和任务结束交接保护。
|
||||
- 增加 A_min 路线、PCD 地图编辑工具和三份抽样点云；原始大 PCD 不直接进入 Git。
|
||||
- 保留完整 Odin 驱动、标定参数和 SDK 静态库；未找到的 `map_a.bin` 仍不伪造，重定位闭环不在本 Tag 声称已复现。
|
||||
|
||||
## `v1.0.0` 的比赛最终部署
|
||||
|
||||
- 将原始 `sim2real_ros2_v2(last_not_slalom_1050)` 正式整理为 `real/sim2real_ros2_v3`；这是第三版、最终 ROS 2/C++ 真机工程，`1050` 是比赛成绩，不是模型编号。
|
||||
- Rough 使用 `model_6800`，Wall 使用 `model_84`，Crawl 按比赛配置使用 IK 后端。
|
||||
- 保留最终五份路线、1 号场地抽样 PCD、Odin 驱动、CAN 硬件桥、命令仲裁、导航和 Orin 触控屏 UI。
|
||||
- 最终配置默认命令源为 `NAV`、定位模式为 `relocal`，但真实 Odin `1hao.bin` 不在备份中，重定位闭环需要从比赛设备补回。
|
||||
- 排除嵌套 Git、日志、备份、候选策略、构建产物和开发草稿；TensorRT engine 仅代表比赛机环境。
|
||||
|
||||
## `v1.0.1` 的比赛成果媒体补充
|
||||
|
||||
- 保持 `v1.0.0` 的比赛最终代码和部署内容不变。
|
||||
- 补充最终机器人图片和比赛视频，成绩为 1050 分、第七名（前 5%）。
|
||||
- 代码复现可查看 `v1.0.0`，包含成果媒体的对应快照可查看 `v1.0.1`。
|
||||
|
||||
## `v1.0.2` 的文档一致性修正
|
||||
|
||||
- 统一 ROS 2 历史 Tag、当前工作树和媒体补丁的说明。
|
||||
- 该版本仅修正文档，没有改变训练或真机运行代码。
|
||||
|
||||
## `v1.1.0` 的目录与说明规范化
|
||||
|
||||
- 从 `v0.10.0` 恢复无后缀 `sim2real_ros2` 初版快照。
|
||||
- `sim2real_ros2_v2` 保持 `v0.12.0` 里程计联调快照。
|
||||
- 将 `last_not_slalom_1050` 最终比赛部署正式命名为 `sim2real_ros2_v3`。
|
||||
- 依据当前源码重新校准 `rc_mjlab` README 中的物理步长、控制频率、环境数、执行器、地形、奖励和随机化说明。
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
|
||||
+11
-3
@@ -8,9 +8,15 @@
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
└─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
├─ sim2real_ros2_v2/ # ROS 2 导航原型及里程计演进
|
||||
└─ sim2real_ros2_v3/ # 最终比赛 ROS 2/C++ 部署
|
||||
```
|
||||
|
||||
当前工作树按架构大版本同时保留三个 ROS 2 目录：无后缀目录是初版，`_v2` 是第二版演进的最终里程计快照，`_v3` 是 `last_not_slalom_1050` 最终比赛部署。各目录内部的小阶段仍可通过对应 Tag 恢复。
|
||||
|
||||
## 数据流
|
||||
|
||||
```text
|
||||
@@ -23,12 +29,14 @@ MJCF + mjlab task
|
||||
|
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
+----> Python Sim2Real ----> 电机 / IMU
|
||||
+----> Python Sim2Real / v2 ----> 电机 / IMU
|
||||
|
|
||||
+----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
|
||||
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`，纯里程计导航联调对应 `v0.12.0`，1050 分比赛最终部署对应 `v1.0.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# 第一代真机控制
|
||||
# 真机控制版本演进
|
||||
|
||||
本目录保存 16DOF 轮足机器人的早期真机控制实现。
|
||||
本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
@@ -20,6 +20,30 @@
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_v2`
|
||||
|
||||
Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。
|
||||
|
||||
部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
|
||||
|
||||
## ROS 2/C++ 版本线
|
||||
|
||||
### `sim2real_ros2`（初版，`v0.10.0`）
|
||||
|
||||
无后缀目录固定表示 ROS 2/C++ Sim2Real 初版：将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。原始快照未随工程保存 Odin ROS 2 驱动源码，依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
### `sim2real_ros2_v2`（`v0.11.0`～`v0.12.0`）
|
||||
|
||||
在 `v0.11.0` 中，该目录是 ROS 2 Sim2Real v2 导航原型，增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。
|
||||
|
||||
`v0.11.1` 在同一路径继续演进，首次归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。
|
||||
|
||||
`v0.12.0` 仍在同一路径上形成里程计导航联调快照：固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。当前该目录保持 `v0.12.0` 快照，阶段说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
|
||||
|
||||
### `sim2real_ros2_v3`（最终比赛版，`v1.0.0`）
|
||||
|
||||
第三版来自原始目录 `sim2real_ros2_v2(last_not_slalom_1050)`，整理时正式命名为 `sim2real_ros2_v3`。它是 1050 分比赛最终部署，包含 `model_6800` Rough、`model_84` Wall、最终路线、完整 Odin 驱动、CAN 和触控屏。部署说明见 [`sim2real_ros2_v3/README.md`](sim2real_ros2_v3/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
[](../../06_assets/videos/early_sim2real.mp4)
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
.colcon/
|
||||
.vscode/
|
||||
compile_commands.json
|
||||
@@ -0,0 +1,178 @@
|
||||
# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
|
||||
|
||||
本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
|
||||
|
||||
---
|
||||
|
||||
## 1. 前提条件与环境
|
||||
|
||||
### 硬件
|
||||
* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。
|
||||
* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。
|
||||
* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。
|
||||
|
||||
### 主机依赖
|
||||
* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。
|
||||
* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。
|
||||
* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。
|
||||
* **库与 ROS2 包**：
|
||||
* `libyaml-cpp-dev`
|
||||
* `libeigen3-dev`
|
||||
* `libusb-1.0-0-dev`（Odin USB 传感器通信）
|
||||
* `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理）
|
||||
* `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器）
|
||||
* `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用）
|
||||
* `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理）
|
||||
* `can-utils`（SocketCAN 验证工具）
|
||||
|
||||
---
|
||||
|
||||
## 2. 本地编译与部署
|
||||
|
||||
按以下步骤在主机系统上编译运行整个栈：
|
||||
|
||||
### 步骤 1：安装系统依赖
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \
|
||||
libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge ros-humble-pcl-conversions
|
||||
```
|
||||
|
||||
### 步骤 2：下载 ONNXRuntime C++ SDK
|
||||
策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录：
|
||||
|
||||
```bash
|
||||
# 创建目录
|
||||
sudo mkdir -p /opt/onnxruntime
|
||||
cd /opt
|
||||
|
||||
# 针对 Jetson Orin (ARM64 / aarch64)：
|
||||
sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz
|
||||
sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
|
||||
# 或标准桌面仿真 (x86_64 / amd64)：
|
||||
# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz
|
||||
# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
|
||||
```
|
||||
|
||||
导出 CMake 辅助变量：
|
||||
```bash
|
||||
export ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
```
|
||||
|
||||
### 步骤 3：构建工作区
|
||||
进入包含 `src/` 的本包根目录，运行 `colcon`：
|
||||
```bash
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
### 步骤 4：配置 SocketCAN 接口
|
||||
启动前，以 1 Mbps 波特率激活 CAN 接口：
|
||||
```bash
|
||||
sudo ip link set can0 up type can bitrate 1000000
|
||||
sudo ip link set can1 up type can bitrate 1000000
|
||||
```
|
||||
使用 `ifconfig` 或 `ip link` 验证接口已启动。
|
||||
|
||||
### 步骤 5：启动节点
|
||||
使启动脚本可执行并运行：
|
||||
```bash
|
||||
chmod +x start_sim2real.sh
|
||||
./start_sim2real.sh
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. Docker 部署（推荐）
|
||||
|
||||
强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。
|
||||
|
||||
### 步骤 1：构建镜像
|
||||
确保在 `sim2real_ros2` 目录中（包含 `Dockerfile`）：
|
||||
```bash
|
||||
# 使用标准 docker build：
|
||||
docker build -t sim2real_ros2:latest .
|
||||
|
||||
# 或使用 Docker Compose：
|
||||
docker compose build
|
||||
```
|
||||
|
||||
### 步骤 2：运行容器
|
||||
对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度：
|
||||
|
||||
```bash
|
||||
# 选项 A：手动运行
|
||||
docker run -it \
|
||||
--network host \
|
||||
--privileged \
|
||||
--cap-add=sys_nice \
|
||||
--volume=/dev:/dev \
|
||||
--shm-size=2g \
|
||||
--name sim2real_ros2_run \
|
||||
sim2real_ros2:latest
|
||||
|
||||
# 选项 B：通过 Docker Compose 运行（最简单）
|
||||
docker compose up -d
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 系统拓扑与话题
|
||||
|
||||
控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互：
|
||||
|
||||
* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。
|
||||
* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。
|
||||
* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。
|
||||
* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。
|
||||
* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。
|
||||
* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。
|
||||
|
||||
### TF 树
|
||||
```
|
||||
odom ──→ base_link （由 odom_relay_node 广播）
|
||||
map ──→ odom （由 AMCL / Odin SLAM 发布，取决于运行模式）
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 集成 ROS 2 导航与传感器驱动
|
||||
|
||||
### USB 设备权限（Odin 传感器）
|
||||
要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则：
|
||||
|
||||
```bash
|
||||
# 1. 添加 udev 规则
|
||||
echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules
|
||||
|
||||
# 2. 重新加载 udev 规则并重新插拔传感器
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
### 集成启动参数
|
||||
统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
|
||||
|
||||
* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
|
||||
* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
|
||||
|
||||
#### 1. 完整真实硬件闭环（默认）
|
||||
启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 2. Dry-Run / 仿真航点测试
|
||||
在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true
|
||||
```
|
||||
|
||||
#### 3. 仅运动控制（无导航）
|
||||
禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）：
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
|
||||
```
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
# 使用 ROS2 官方 Humble 基础镜像
|
||||
FROM ros:humble-ros-base-jammy
|
||||
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
|
||||
# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
can-utils \
|
||||
libyaml-cpp-dev \
|
||||
libeigen3-dev \
|
||||
libusb-1.0-0-dev \
|
||||
libpcl-dev \
|
||||
libopencv-dev \
|
||||
ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup \
|
||||
ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge \
|
||||
ros-humble-pcl-conversions \
|
||||
wget \
|
||||
tar \
|
||||
python3-pip \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# ============================================================================
|
||||
# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
|
||||
# ============================================================================
|
||||
# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
|
||||
# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
|
||||
WORKDIR /opt
|
||||
RUN ARCH=$(uname -m) && \
|
||||
if [ "$ARCH" = "aarch64" ]; then \
|
||||
echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
|
||||
pip3 install --no-cache-dir onnxruntime-gpu && \
|
||||
SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
|
||||
mkdir -p onnxruntime/include onnxruntime/lib && \
|
||||
cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
|
||||
cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
|
||||
echo "[ONNX] CUDA ONNX Runtime installed."; \
|
||||
else \
|
||||
echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
|
||||
wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
|
||||
rm onnxruntime-linux-x64-1.16.3.tgz; \
|
||||
fi
|
||||
|
||||
ENV ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
|
||||
# 创建工作空间，将所有 C++ 源码包拷入
|
||||
WORKDIR /sim2real_ws/src
|
||||
COPY src/sim2real_bringup sim2real_bringup
|
||||
COPY src/sim2real_common sim2real_common
|
||||
COPY src/sim2real_hw sim2real_hw
|
||||
COPY src/sim2real_interfaces sim2real_interfaces
|
||||
COPY src/sim2real_runtime sim2real_runtime
|
||||
COPY src/odin_ros_driver odin_ros_driver
|
||||
COPY src/sim2real_nav2 sim2real_nav2
|
||||
|
||||
# 拷贝策略文件与运行脚本
|
||||
WORKDIR /sim2real_ws
|
||||
COPY policies policies
|
||||
COPY start_sim2real.sh start_sim2real.sh
|
||||
RUN chmod +x start_sim2real.sh
|
||||
|
||||
# 编译 ROS2 工作空间
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
RUN source /opt/ros/humble/setup.bash && \
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
|
||||
# 拷贝 Docker 入口脚本并设置
|
||||
COPY docker_entrypoint.sh /docker_entrypoint.sh
|
||||
RUN chmod +x /docker_entrypoint.sh
|
||||
|
||||
ENTRYPOINT ["/docker_entrypoint.sh"]
|
||||
CMD ["./start_sim2real.sh"]
|
||||
@@ -0,0 +1,74 @@
|
||||
# ROS 2/C++ Sim2Real 初版
|
||||
|
||||
本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
|
||||
|
||||
本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
|
||||
|
||||
- `53D` 策略观测契约不变
|
||||
- `16D` 动作契约不变
|
||||
- `50Hz` 策略循环与训练对齐
|
||||
- `200Hz` 电机循环为专用 C++ 热路径
|
||||
- ROS 2 作为导航、TF、诊断和启动管理的系统集成层
|
||||
|
||||
## 工作区布局
|
||||
|
||||
- `src/sim2real_interfaces`
|
||||
硬件桥接与策略运行时共享的 ROS 2 消息定义。
|
||||
- `src/sim2real_common`
|
||||
共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。
|
||||
- `src/sim2real_hw`
|
||||
面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。
|
||||
- `src/sim2real_runtime`
|
||||
策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。
|
||||
同时包含 `odom_relay_node`（里程计中继与 TF 广播）。
|
||||
- `src/sim2real_nav2`
|
||||
ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。
|
||||
- `src/sim2real_bringup`
|
||||
统一启动文件与运行时参数配置。
|
||||
- `src/odin_ros_driver`
|
||||
Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。
|
||||
- `docs`
|
||||
架构说明与迁移计划。
|
||||
|
||||
## 目标架构
|
||||
|
||||
```text
|
||||
Odin / IMU / Odom ---> sim2real_hw ---> sim2real_runtime ---> sim2real_hw
|
||||
| | |
|
||||
v v v
|
||||
RuntimeState RuntimeTarget 电机 CAN 指令
|
||||
| |
|
||||
+-------> 诊断 / 遥测
|
||||
|
||||
Nav2 / cmd_vel ------------------------------> sim2real_runtime
|
||||
(经 odom_relay_node 提供 odom→base_link TF)
|
||||
```
|
||||
|
||||
## 当前状态
|
||||
|
||||
已完成 Phase 0-5 的全部迁移：
|
||||
|
||||
1. ✅ 冻结部署契约（deployment_contract.hpp）
|
||||
2. ✅ ROS 2 包结构搭建
|
||||
3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
|
||||
4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
|
||||
5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
|
||||
|
||||
## 契约来源
|
||||
|
||||
迁移过程中以下文件被视为真值源：
|
||||
|
||||
- `sim2real/deployment_manifest.yaml`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
## 注意事项
|
||||
|
||||
- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
|
||||
- Windows 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
|
||||
- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。
|
||||
@@ -0,0 +1,22 @@
|
||||
version: '3.8'
|
||||
|
||||
services:
|
||||
sim2real_ros2:
|
||||
build:
|
||||
context: .
|
||||
dockerfile: Dockerfile
|
||||
container_name: sim2real_ros2_node
|
||||
runtime: nvidia
|
||||
network_mode: host
|
||||
privileged: true
|
||||
stdin_open: true
|
||||
tty: true
|
||||
environment:
|
||||
- NVIDIA_VISIBLE_DEVICES=all
|
||||
- NVIDIA_DRIVER_CAPABILITIES=compute,utility
|
||||
cap_add:
|
||||
- SYS_NICE
|
||||
shm_size: '2gb'
|
||||
volumes:
|
||||
- /dev:/dev
|
||||
restart: unless-stopped
|
||||
@@ -0,0 +1,12 @@
|
||||
#!/bin/bash
|
||||
set -e
|
||||
|
||||
# Source ROS2 Humble environment
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
# Source workspace install setup if compiled
|
||||
if [ -f "/sim2real_ws/install/setup.bash" ]; then
|
||||
source /sim2real_ws/install/setup.bash
|
||||
fi
|
||||
|
||||
exec "$@"
|
||||
@@ -0,0 +1,102 @@
|
||||
# sim2real_ros2 架构说明
|
||||
|
||||
## 设计目标
|
||||
|
||||
- 保留已验证的 RL 部署契约不变
|
||||
- 将低延迟循环从 Python 迁移至 C++
|
||||
- 暴露标准 ROS 2 接口用于导航和系统集成
|
||||
- 保持安全边界独立于策略正确性
|
||||
|
||||
## 各包职责
|
||||
|
||||
### `sim2real_interfaces`（接口消息）
|
||||
|
||||
定义最小化的运行时消息：
|
||||
|
||||
- `RuntimeState`
|
||||
硬件桥接发布的归一化运行时状态快照
|
||||
- `RuntimeTarget`
|
||||
策略运行时发送至硬件桥接的最新策略目标
|
||||
|
||||
### `sim2real_common`（共享常量）
|
||||
|
||||
存储编译期常量和部署契约辅助：
|
||||
|
||||
- 观测维度和字段布局
|
||||
- 动作维度和轮子索引
|
||||
- 关节顺序和默认站姿
|
||||
- 动作缩放因子和默认循环频率
|
||||
- Mahony 姿态滤波器
|
||||
- 站立平衡控制器
|
||||
- 安全监控器（SafetyMonitor / RuntimeGuard）
|
||||
|
||||
### `sim2real_hw`（硬件桥接）
|
||||
|
||||
拥有硬件侧执行循环和安全边界：
|
||||
|
||||
- RobStride CAN 收发
|
||||
- IMU 与 Odin 状态采集
|
||||
- 电机丢帧检测与保活逻辑（holdover）
|
||||
- 看门狗与阻尼刹车
|
||||
- 发布 `RuntimeState`
|
||||
- 订阅 `RuntimeTarget`
|
||||
- 订阅 `/odom` 里程计数据
|
||||
|
||||
目标热路径：
|
||||
|
||||
- 以 `200Hz` 频率读取状态
|
||||
- 应用最新安全目标
|
||||
- 超时或安全违规时立即停机
|
||||
|
||||
### `sim2real_runtime`（策略运行时）
|
||||
|
||||
拥有策略侧执行：
|
||||
|
||||
- 订阅 `RuntimeState`
|
||||
- 按当前部署契约精确构建 `53D` 观测
|
||||
- 以 `50Hz` 运行 ONNXRuntime 推理
|
||||
- 对 raw_action 做 `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
- 仲裁命令来源：estop > safety_hold > startup > navigation > web
|
||||
|
||||
同时包含：
|
||||
- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
|
||||
|
||||
### `sim2real_nav2`（导航配置）
|
||||
|
||||
拥有：
|
||||
|
||||
- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
|
||||
- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
|
||||
|
||||
### `sim2real_bringup`（启动管理）
|
||||
|
||||
拥有：
|
||||
|
||||
- 参数文件
|
||||
- 启动组合
|
||||
- 运行时模式选择
|
||||
- 集成 odin_ros_driver、sim2real_nav2 的条件启动
|
||||
|
||||
## 迁移规则
|
||||
|
||||
1. 优化之前先冻结当前契约
|
||||
2. 先迁移传输和循环结构，再调整控制算法
|
||||
3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
|
||||
4. 按段测量延迟：
|
||||
- 观测延迟
|
||||
- 策略推理延迟
|
||||
- 目标传输延迟
|
||||
- 执行器响应延迟
|
||||
|
||||
## 首个里程碑
|
||||
|
||||
首个里程碑不是"机器人在 ROS 2 下行走"，而是：
|
||||
|
||||
1. `sim2real_hw` 发布稳定的 `RuntimeState`
|
||||
2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
|
||||
3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
|
||||
4. `sim2real_hw` 消费最新目标并执行超时刹车
|
||||
5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
|
||||
|
||||
> ✅ 以上里程碑已全部完成。
|
||||
@@ -0,0 +1,80 @@
|
||||
# 迁移计划
|
||||
|
||||
## Phase 1: 硬件核心迁移 ✅ 已完成
|
||||
|
||||
将当前高频热路径从 Python 迁出。
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/interface/motor_driver.py`
|
||||
- `sim2real/interface/motor_mapping.py`
|
||||
- `sim2real/interface/imu_client.py`
|
||||
- `sim2real/safety/runtime_guard.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- C++ SocketCAN 电机总线封装
|
||||
- C++ 状态缓存
|
||||
- target 超时保活（timeout hold）
|
||||
- 阻尼刹车 / 急停通路
|
||||
- 发布 `RuntimeState`
|
||||
|
||||
## Phase 2: 策略运行时迁移 ✅ 已完成
|
||||
|
||||
吸收的源文件：
|
||||
|
||||
- `sim2real/policy/policy_runner.py`
|
||||
- `sim2real/interface/real_io.py`
|
||||
- `sim2real/web/session.py`
|
||||
|
||||
交付物：
|
||||
|
||||
- 精确的 `53D` 观测构造器
|
||||
- ONNXRuntime C++ 推理封装
|
||||
- `50Hz` 策略定时器
|
||||
- 命令平滑与来源仲裁
|
||||
- raw_action `[-10, 10]` 安全裁剪
|
||||
- 发布 `RuntimeTarget`
|
||||
|
||||
## Phase 3: ROS 2 系统集成 ✅ 已完成
|
||||
|
||||
参考的源项目：
|
||||
|
||||
- `00_ reference/odin_ros_driver`
|
||||
- `00_ reference/EDULITE_A3/el_a3_ros`
|
||||
- `00_ reference/rl_sar`
|
||||
|
||||
交付物：
|
||||
|
||||
- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
|
||||
- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
|
||||
- 诊断话题
|
||||
- rosbag/foxglove 可观测性
|
||||
|
||||
## Phase 4: 导航集成 ✅ 已完成
|
||||
|
||||
目标：
|
||||
|
||||
- 导航通过 ROS 2 发送身体速度指令
|
||||
- RL 运行时保持为 locomotion 控制器
|
||||
- 看门狗和安全边界始终在导航之下
|
||||
|
||||
规则：
|
||||
|
||||
- 导航绝不直接写电机指令
|
||||
- 策略契约在重新训练前保持不变
|
||||
- 任何新增历史项或里程计项必须版本化
|
||||
|
||||
## 当前状态
|
||||
|
||||
所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
|
||||
|
||||
| 组件 | 节点 | 说明 |
|
||||
|------|------|------|
|
||||
| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
|
||||
| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
|
||||
| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
|
||||
| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
|
||||
| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
|
||||
| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
|
||||
@@ -0,0 +1,495 @@
|
||||
# sim2real_ros2 遥控器调用说明
|
||||
|
||||
本文档说明如何在 `sim2real_ros2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
|
||||
|
||||
## 1. 当前接入关系
|
||||
|
||||
遥控器节点位于：
|
||||
|
||||
```text
|
||||
src/sim2real_runtime/src/remote_uart_node.py
|
||||
```
|
||||
|
||||
该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
|
||||
|
||||
| 输入 | 输出 | 作用 |
|
||||
|---|---|---|
|
||||
| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
|
||||
| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
|
||||
|
||||
策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
|
||||
|
||||
## 2. 通道映射
|
||||
|
||||
通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
|
||||
|
||||
| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
|
||||
|---|---|---|---:|
|
||||
| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
|
||||
| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
|
||||
| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
|
||||
| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
|
||||
|
||||
默认方向反转配置：
|
||||
|
||||
| 参数 | 默认值 | 含义 |
|
||||
|---|---:|---|
|
||||
| `remote_invert_vx` | `true` | 反转前后方向 |
|
||||
| `remote_invert_vy` | `false` | 不反转横移方向 |
|
||||
| `remote_invert_yaw` | `true` | 反转转向方向 |
|
||||
|
||||
## 3. 参数位置
|
||||
|
||||
遥控器参数在：
|
||||
|
||||
```text
|
||||
src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
当前默认参数：
|
||||
|
||||
```yaml
|
||||
remote_enabled: true
|
||||
remote_port: "/dev/ttyACM0"
|
||||
remote_baudrate: 100000
|
||||
remote_timeout: 0.02
|
||||
remote_axis_deadzone: 50
|
||||
remote_active_threshold: 50
|
||||
remote_axis_full_scale: 660.0
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
```
|
||||
|
||||
如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
或改成实际设备路径。
|
||||
|
||||
## 4. 启动前检查
|
||||
|
||||
### 4.1 确认串口存在
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
如果使用默认配置，应能看到：
|
||||
|
||||
```bash
|
||||
/dev/ttyACM0
|
||||
```
|
||||
|
||||
### 4.2 确认串口权限
|
||||
|
||||
如果节点提示串口权限不足，可以临时执行：
|
||||
|
||||
```bash
|
||||
sudo chmod 666 /dev/ttyACM0
|
||||
```
|
||||
|
||||
更推荐的长期方式是把当前用户加入 `dialout` 组：
|
||||
|
||||
```bash
|
||||
sudo usermod -aG dialout $USER
|
||||
```
|
||||
|
||||
然后重新登录。
|
||||
|
||||
### 4.3 确认 Python serial 依赖
|
||||
|
||||
节点依赖 `pyserial`。如果系统没有安装：
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install -y python3-serial
|
||||
```
|
||||
|
||||
## 5. 构建
|
||||
|
||||
如果刚修改过代码或参数，建议重新构建相关包：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
|
||||
```
|
||||
|
||||
构建完成后 source 环境：
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
确认可执行节点存在：
|
||||
|
||||
```bash
|
||||
ros2 pkg executables sim2real_runtime
|
||||
```
|
||||
|
||||
应包含：
|
||||
|
||||
```text
|
||||
sim2real_runtime remote_uart_node.py
|
||||
```
|
||||
|
||||
## 6. 推荐启动方式
|
||||
|
||||
### 6.1 启动完整系统，不启动 Nav2
|
||||
|
||||
这是你当前常用方式：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
|
||||
|
||||
等价完整写法：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
|
||||
```
|
||||
|
||||
### 6.2 不启动遥控器
|
||||
|
||||
如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
|
||||
```
|
||||
|
||||
## 7. 单独启动遥控器节点
|
||||
|
||||
如果系统已经在运行，只想单独测试遥控器节点：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
如果要临时指定串口：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args \
|
||||
--params-file src/sim2real_bringup/config/runtime.yaml \
|
||||
-p remote_port:=/dev/ttyUSB0
|
||||
```
|
||||
|
||||
## 8. 执行后会产生什么效果
|
||||
|
||||
启动以下命令后：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
系统会产生以下效果。
|
||||
|
||||
### 8.1 启动硬件桥接节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开 `can0` 和 `can1`。
|
||||
2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
|
||||
3. 设置电机 MIT 模式。
|
||||
4. 设置电机速度限制和力矩限制。
|
||||
5. 以 `200Hz` 运行硬件读写循环。
|
||||
6. 发布 `/runtime/state`。
|
||||
7. 订阅 `/runtime/target` 执行策略目标。
|
||||
|
||||
### 8.2 启动策略运行节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_runtime_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 加载 ONNX 策略模型。
|
||||
2. 订阅 `/runtime/state`。
|
||||
3. 订阅 `/cmd_vel`。
|
||||
4. 订阅 `/safety/estop`。
|
||||
5. 执行启动站立流程：
|
||||
- `boot_hold`
|
||||
- `startup_soft_hold`
|
||||
- `startup_hold`
|
||||
- `runtime_zero_hold`
|
||||
- `runtime_policy`
|
||||
6. 以 `50Hz` 发布 `/runtime/target`。
|
||||
|
||||
### 8.3 启动遥控器节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开默认串口 `/dev/ttyACM0`。
|
||||
2. 以 `50Hz` 轮询 SBUS 数据。
|
||||
3. 遥控器摇杆居中时持续发布零速度：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = 0.0
|
||||
linear.y = 0.0
|
||||
angular.z = 0.0
|
||||
```
|
||||
|
||||
4. 推动遥控器时发布非零速度，例如：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = vx
|
||||
linear.y = vy
|
||||
angular.z = yaw
|
||||
```
|
||||
|
||||
5. 当 CH7 打到高位时发布：
|
||||
|
||||
```text
|
||||
/safety/estop: true
|
||||
```
|
||||
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
|
||||
### 8.4 机器人行为效果
|
||||
|
||||
正常启动后，机器人不会立即按策略行走，而是按阶段执行：
|
||||
|
||||
1. 电机使能。
|
||||
2. 读取当前关节位置。
|
||||
3. 软保持当前姿态。
|
||||
4. 平滑过渡到默认站立姿态。
|
||||
5. 稳定后进入 runtime。
|
||||
6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
|
||||
7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
|
||||
|
||||
也就是说：
|
||||
|
||||
| 遥控器状态 | 机器人效果 |
|
||||
|---|---|
|
||||
| 摇杆居中 | 站立保持，不主动行走 |
|
||||
| CH2 前后推动 | 前进/后退 |
|
||||
| CH4 左右推动 | 横向移动 |
|
||||
| CH1 左右推动 | 原地转向 |
|
||||
| CH7 高位 | 软件急停，进入安全刹车 |
|
||||
|
||||
## 9. 如何确认遥控器已经生效
|
||||
|
||||
### 9.1 查看节点是否存在
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
```
|
||||
|
||||
应看到：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
/sim2real_runtime_node
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
### 9.2 查看 `/cmd_vel`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
|
||||
|
||||
### 9.3 查看 `/safety/estop`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
|
||||
CH7 高位时应看到：
|
||||
|
||||
```yaml
|
||||
data: true
|
||||
```
|
||||
|
||||
### 9.4 查看策略目标阶段
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
常见输出含义：
|
||||
|
||||
| `target_source` | 含义 |
|
||||
|---|---|
|
||||
| `boot_hold` | 刚启动，保持初始姿态 |
|
||||
| `startup_soft_hold` | 启动软保持 |
|
||||
| `startup_hold` | 正在站立或站立后保持 |
|
||||
| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
|
||||
| `runtime_policy` | 遥控器有输入，策略已经介入 |
|
||||
| `safety_brake` | 安全刹车 |
|
||||
| `timeout_hold` | 目标超时，硬件保持默认姿态 |
|
||||
|
||||
### 9.5 查看完整目标状态
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /runtime/target
|
||||
```
|
||||
|
||||
重点关注字段：
|
||||
|
||||
```yaml
|
||||
target_source:
|
||||
zero_command:
|
||||
runtime_released:
|
||||
release_alpha:
|
||||
command:
|
||||
raw_command:
|
||||
```
|
||||
|
||||
如果遥控器摇杆有输入，通常会看到：
|
||||
|
||||
```yaml
|
||||
target_source: runtime_policy
|
||||
zero_command: false
|
||||
runtime_released: true
|
||||
release_alpha: 1.0
|
||||
```
|
||||
|
||||
## 10. 常见问题
|
||||
|
||||
### 10.1 启动后提示无法打开串口
|
||||
|
||||
可能原因：
|
||||
|
||||
1. 串口路径不对。
|
||||
2. 权限不足。
|
||||
3. 设备没有插好。
|
||||
4. 设备被其他程序占用。
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
修改 `runtime.yaml`：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
### 10.2 `/cmd_vel` 没有变化
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
如果节点存在但无变化，可能是：
|
||||
|
||||
1. 遥控器没有输出 SBUS。
|
||||
2. 串口波特率不对。
|
||||
3. SBUS 接线错误。
|
||||
4. 遥控器通道未校准。
|
||||
5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
|
||||
|
||||
### 10.3 摇杆方向反了
|
||||
|
||||
修改：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
```
|
||||
|
||||
例如前后方向反了，就切换：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: false
|
||||
```
|
||||
|
||||
### 10.4 急停后不恢复
|
||||
|
||||
当前配置：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: true
|
||||
```
|
||||
|
||||
这表示急停锁存。触发后建议：
|
||||
|
||||
1. 先确认机器人物理安全。
|
||||
2. 停止 launch。
|
||||
3. 将 CH7 打回安全位置。
|
||||
4. 重新启动系统。
|
||||
|
||||
如果需要非锁存模式，可以改为：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: false
|
||||
```
|
||||
|
||||
但实机调试时更建议使用锁存模式。
|
||||
|
||||
## 11. 快速验证命令清单
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
如果只测遥控器，不启动电机系统：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
@@ -0,0 +1,246 @@
|
||||
# sim2real_ros2 Web UDP 调试说明
|
||||
|
||||
本文档说明本次新增的最小 Web 调试链路。
|
||||
|
||||
## 1. 架构
|
||||
|
||||
```text
|
||||
Windows 本地浏览器/HTTP 服务
|
||||
|
|
||||
| UDP JSON
|
||||
v
|
||||
Nano: sim2real_web_udp_bridge_node.py
|
||||
|
|
||||
| ROS 2 topics
|
||||
v
|
||||
sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
|
||||
```
|
||||
|
||||
Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
|
||||
|
||||
## 2. 新增 ROS2 节点
|
||||
|
||||
### `remote_uart_node.py`
|
||||
|
||||
遥控器节点现在发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
```
|
||||
|
||||
不再直接发布 `/cmd_vel`。
|
||||
|
||||
通道触发阈值改为：
|
||||
|
||||
```yaml
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
```
|
||||
|
||||
只有通道归一化值绝对值大于 `40` 才认为是有效输入。
|
||||
|
||||
### `cmd_mux_node.py`
|
||||
|
||||
输入：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
/cmd_vel_web
|
||||
/cmd_vel_nav
|
||||
/control/mode
|
||||
/remote/enabled
|
||||
/web/enabled
|
||||
/nav/enabled
|
||||
/safety/estop
|
||||
```
|
||||
|
||||
输出：
|
||||
|
||||
```text
|
||||
/cmd_vel
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
控制模式：
|
||||
|
||||
```text
|
||||
DISABLED
|
||||
REMOTE
|
||||
WEB
|
||||
NAV
|
||||
```
|
||||
|
||||
急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
|
||||
|
||||
### `web_udp_bridge_node.py`
|
||||
|
||||
Nano 端 UDP 监听：
|
||||
|
||||
```text
|
||||
0.0.0.0:15000
|
||||
```
|
||||
|
||||
发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_web
|
||||
/safety/estop
|
||||
/control/mode
|
||||
/web/enabled
|
||||
/remote/enabled
|
||||
/nav/enabled
|
||||
```
|
||||
|
||||
订阅并回传状态：
|
||||
|
||||
```text
|
||||
/runtime/state
|
||||
/runtime/target
|
||||
/cmd_vel
|
||||
/safety/estop
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
## 3. Nano 启动
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认会启动：
|
||||
|
||||
```text
|
||||
sim2real_remote_uart_node
|
||||
sim2real_cmd_mux_node
|
||||
sim2real_web_udp_bridge_node
|
||||
```
|
||||
|
||||
如果不想启动 Web UDP bridge：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
|
||||
```
|
||||
|
||||
## 4. Windows 本地 Web 启动
|
||||
|
||||
把目录复制到 Windows 或通过共享目录访问：
|
||||
|
||||
```text
|
||||
tools/win_web_debug
|
||||
```
|
||||
|
||||
在 Windows 上安装 Python 3 后运行：
|
||||
|
||||
```bash
|
||||
python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
|
||||
```
|
||||
|
||||
浏览器打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
|
||||
## 5. UDP 命令格式
|
||||
|
||||
### 切换模式
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"REMOTE"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"WEB"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"DISABLED"}
|
||||
```
|
||||
|
||||
### Web 速度控制
|
||||
|
||||
```json
|
||||
{
|
||||
"type": "cmd_vel",
|
||||
"linear": {"x": 0.2, "y": 0.0, "z": 0.0},
|
||||
"angular": {"x": 0.0, "y": 0.0, "z": 0.1}
|
||||
}
|
||||
```
|
||||
|
||||
Nano 端会再次限幅：
|
||||
|
||||
```text
|
||||
vx <= ±0.8 m/s
|
||||
vy <= ±0.3 m/s
|
||||
yaw <= ±0.5 rad/s
|
||||
```
|
||||
|
||||
### 零速度
|
||||
|
||||
```json
|
||||
{"type":"zero"}
|
||||
```
|
||||
|
||||
### 软急停
|
||||
|
||||
```json
|
||||
{"type":"estop","data":true}
|
||||
```
|
||||
|
||||
## 6. 安全保护
|
||||
|
||||
当前最小版本已经包含：
|
||||
|
||||
1. 遥控器误触发阈值：`40`。
|
||||
2. 遥控器/Web/Nav 互斥控制模式。
|
||||
3. `cmd_mux` 二次限幅。
|
||||
4. `cmd_mux` 加速度限制。
|
||||
5. Web UDP 超时自动发布零速度。
|
||||
6. 急停优先级最高。
|
||||
7. Web 页面切换到 `WEB` 模式需要确认。
|
||||
8. Web 松开虚拟摇杆会自动发送零速度。
|
||||
|
||||
建议实机调试流程：
|
||||
|
||||
1. 先点击 `DISABLED`。
|
||||
2. 确认 `/cmd_vel` 为零。
|
||||
3. 如果使用遥控器，点击 `REMOTE`。
|
||||
4. 如果使用 Web，点击 `WEB` 并确认周围安全。
|
||||
5. 一旦异常，立即点击 `软急停`。
|
||||
|
||||
## 7. 验证命令
|
||||
|
||||
查看最终输出速度：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
查看遥控器输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_remote
|
||||
```
|
||||
|
||||
查看 Web 输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_web
|
||||
```
|
||||
|
||||
查看当前仲裁模式：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /control/mode_state
|
||||
```
|
||||
|
||||
查看策略状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
@@ -0,0 +1,20 @@
|
||||
Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150,
|
||||
0.002390, -0.000340, -1.000000, -0.011850,
|
||||
0.999960, -0.009160, 0.002390, 0.005360,
|
||||
0.000000, 0.000000, 0.000000, 1.000000]
|
||||
cam_0:
|
||||
image_width: 1600
|
||||
image_height: 1296
|
||||
k2: 0.000656
|
||||
k3: -0.028961
|
||||
k4: 0.045390
|
||||
k5: -0.064513
|
||||
k6: 0.038735
|
||||
k7: -0.009903
|
||||
p1: 0.000000
|
||||
p2: 0.000000
|
||||
A11: 736.894262
|
||||
A12: -0.161150
|
||||
A22: 736.611354
|
||||
u0: 806.125535
|
||||
v0: 639.650710
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
# Odin 驱动依赖占位
|
||||
|
||||
`real/sim2real_ros2` 原始快照中的 `src/odin_ros_driver` 为空目录，但启动文件、Dockerfile 和 `sim2real_bringup` 已经引用该包。
|
||||
|
||||
因此 `v0.10.0` 记录的是 ROS 2/C++ 迁移初版，不能仅凭本目录宣称 Odin 驱动可独立构建。兼容的 Odin ROS 2 驱动源码从后续版本开始随工程归档。
|
||||
@@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,103 @@
|
||||
# =============================================================================
|
||||
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||
# =============================================================================
|
||||
#
|
||||
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||
#
|
||||
# =============================================================================
|
||||
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||
- {name: projected_gravity, dim: 3}
|
||||
- {name: command, dim: 3}
|
||||
- {name: joint_pos_rel, dim: 12}
|
||||
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||
- {name: last_actions, dim: 16}
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
fl_hip_abduction: [1, 1]
|
||||
fl_hip_pitch: [1, 2]
|
||||
fl_knee: [1, 3]
|
||||
fl_wheel: [1, 4]
|
||||
fr_hip_abduction: [1, 5]
|
||||
fr_hip_pitch: [1, 6]
|
||||
fr_knee: [1, 7]
|
||||
fr_wheel: [1, 8]
|
||||
rl_hip_abduction: [2, 1]
|
||||
rl_hip_pitch: [2, 2]
|
||||
rl_knee: [2, 3]
|
||||
rl_wheel: [2, 4]
|
||||
rr_hip_abduction: [2, 5]
|
||||
rr_hip_pitch: [2, 6]
|
||||
rr_knee: [2, 7]
|
||||
rr_wheel: [2, 8]
|
||||
direction_map:
|
||||
fl_hip_abduction: -1
|
||||
fl_hip_pitch: -1
|
||||
fl_knee: -1
|
||||
fl_wheel: -1
|
||||
fr_hip_abduction: -1
|
||||
fr_hip_pitch: 1
|
||||
fr_knee: 1
|
||||
fr_wheel: 1
|
||||
rl_hip_abduction: 1
|
||||
rl_hip_pitch: -1
|
||||
rl_knee: -1
|
||||
rl_wheel: -1
|
||||
rr_hip_abduction: 1
|
||||
rr_hip_pitch: 1
|
||||
rr_knee: 1
|
||||
rr_wheel: 1
|
||||
zero_offset_map:
|
||||
fl_hip_abduction: 0.003
|
||||
fl_hip_pitch: 0.030
|
||||
fl_knee: 0.028
|
||||
fl_wheel: 0.0
|
||||
fr_hip_abduction: 0.004
|
||||
fr_hip_pitch: 0.038
|
||||
fr_knee: 0.011
|
||||
fr_wheel: 0.0
|
||||
rl_hip_abduction: 0.019
|
||||
rl_hip_pitch: -0.034
|
||||
rl_knee: 0.025
|
||||
rl_wheel: 0.0
|
||||
rr_hip_abduction: -0.001
|
||||
rr_hip_pitch: 0.039
|
||||
rr_knee: 0.018
|
||||
rr_wheel: 0.0
|
||||
@@ -0,0 +1,80 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
policy_hz: 50.0
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
model_path: policies/model_rough.onnx
|
||||
use_cuda: true # 启用 CUDA Execution Provider（Orin Nano GPU 加速）
|
||||
contract_file: deployment_contract.yaml
|
||||
dry_run: false
|
||||
can0_name: "can0"
|
||||
can1_name: "can1"
|
||||
imu_topic: "/odin1/imu"
|
||||
odom_topic: "/odom"
|
||||
|
||||
# Remote UART / SBUS parameters, aligned with the Python deployment
|
||||
remote_enabled: true
|
||||
remote_port: "/dev/ttyACM0"
|
||||
remote_baudrate: 100000
|
||||
remote_timeout: 0.02
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
remote_axis_full_scale: 660.0
|
||||
remote_max_vx: 0.8
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
|
||||
# Command mux parameters
|
||||
cmd_mux_default_mode: "REMOTE"
|
||||
cmd_mux_output_hz: 50.0
|
||||
cmd_mux_remote_timeout_ms: 250.0
|
||||
cmd_mux_web_timeout_ms: 300.0
|
||||
cmd_mux_nav_timeout_ms: 500.0
|
||||
cmd_mux_max_vx: 0.8
|
||||
cmd_mux_max_vy: 0.3
|
||||
cmd_mux_max_yaw_rate: 0.5
|
||||
cmd_mux_max_vx_acc: 1.0
|
||||
cmd_mux_max_vy_acc: 1.0
|
||||
cmd_mux_max_yaw_acc: 1.5
|
||||
|
||||
# Windows/Nano Web UDP bridge parameters
|
||||
web_bridge_enabled: true
|
||||
web_udp_listen_host: "0.0.0.0"
|
||||
web_udp_listen_port: 15000
|
||||
web_udp_remote_host: ""
|
||||
web_udp_remote_port: 15001
|
||||
web_udp_state_hz: 20.0
|
||||
web_udp_cmd_timeout_ms: 300.0
|
||||
web_udp_max_packet_bytes: 8192
|
||||
web_udp_max_vx: 0.8
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.5
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 0.6
|
||||
hard_target_offset: 2.0
|
||||
max_ang_vel: 10.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 200.0
|
||||
|
||||
# Policy alignment with the Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: true
|
||||
@@ -0,0 +1,129 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
runtime_params = ParameterFile(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("sim2real_bringup"),
|
||||
"config",
|
||||
"runtime.yaml",
|
||||
]),
|
||||
allow_substs=True,
|
||||
)
|
||||
|
||||
# Declare launch configurations
|
||||
launch_driver_arg = DeclareLaunchArgument(
|
||||
'launch_driver',
|
||||
default_value='true',
|
||||
description='Whether to launch the odin_ros_driver sensor node'
|
||||
)
|
||||
|
||||
launch_nav2_arg = DeclareLaunchArgument(
|
||||
'launch_nav2',
|
||||
default_value='true',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
launch_remote_arg = DeclareLaunchArgument(
|
||||
'launch_remote',
|
||||
default_value='true',
|
||||
description='Whether to launch the SBUS UART remote control node'
|
||||
)
|
||||
|
||||
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||
'launch_web_bridge',
|
||||
default_value='true',
|
||||
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||
)
|
||||
|
||||
# Include odin_ros_driver launch
|
||||
driver_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('odin_ros_driver'),
|
||||
'launch',
|
||||
'odin1_ros2.launch.py'
|
||||
])
|
||||
),
|
||||
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||
)
|
||||
|
||||
# Include sim2real_nav2 launch
|
||||
nav2_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('sim2real_nav2'),
|
||||
'launch',
|
||||
'nav2.launch.py'
|
||||
])
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('launch_nav2'))
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
launch_driver_arg,
|
||||
launch_nav2_arg,
|
||||
launch_remote_arg,
|
||||
launch_web_bridge_arg,
|
||||
Node(
|
||||
package="sim2real_hw",
|
||||
executable="sim2real_hw_node",
|
||||
name="sim2real_hw_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="cmd_mux_node.py",
|
||||
name="sim2real_cmd_mux_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="web_udp_bridge_node.py",
|
||||
name="sim2real_web_udp_bridge_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="remote_uart_node.py",
|
||||
name="sim2real_remote_uart_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_remote')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="odom_relay_node",
|
||||
name="odom_relay_node",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"odom_input_topic": "/odin1/odometry",
|
||||
"odom_output_topic": "/odom",
|
||||
"base_frame": "base_link",
|
||||
"publish_tf": True,
|
||||
}],
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||
),
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Launch and configuration package for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>sim2real_common</exec_depend>
|
||||
<exec_depend>sim2real_hw</exec_depend>
|
||||
<exec_depend>sim2real_interfaces</exec_depend>
|
||||
<exec_depend>sim2real_runtime</exec_depend>
|
||||
<exec_depend>sim2real_nav2</exec_depend>
|
||||
<exec_depend>odin_ros_driver</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_common)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} INTERFACE)
|
||||
target_include_directories(${PROJECT_NAME} INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_package()
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
struct DeploymentContract
|
||||
{
|
||||
static constexpr std::size_t kObsDim = 53;
|
||||
static constexpr std::size_t kActionDim = 16;
|
||||
static constexpr std::size_t kLegJointCount = 12;
|
||||
static constexpr std::size_t kWheelCount = 4;
|
||||
static constexpr double kPolicyHz = 50.0;
|
||||
static constexpr double kMotorHz = 200.0;
|
||||
static constexpr double kStatusHz = 10.0;
|
||||
|
||||
static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
|
||||
static constexpr float kLegKp = 50.0f;
|
||||
static constexpr float kLegKd = 1.5f;
|
||||
static constexpr float kLegHoldKp = 80.0f;
|
||||
static constexpr float kLegHoldKd = 4.0f;
|
||||
static constexpr float kWheelKd = 1.0f;
|
||||
|
||||
static constexpr std::array<int, 16> kCanBusMap = {
|
||||
1, 1, 1, // fl legs
|
||||
1, 1, 1, // fr legs
|
||||
2, 2, 2, // rl legs
|
||||
2, 2, 2, // rr legs
|
||||
1, 1, 2, 2 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<int, 16> kCanIdMap = {
|
||||
1, 2, 3, // fl legs
|
||||
5, 6, 7, // fr legs
|
||||
1, 2, 3, // rl legs
|
||||
5, 6, 7, // rr legs
|
||||
4, 8, 4, 8 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDirectionMap = {
|
||||
-1.0f, -1.0f, -1.0f, // fl
|
||||
-1.0f, 1.0f, 1.0f, // fr
|
||||
1.0f, -1.0f, -1.0f, // rl
|
||||
1.0f, 1.0f, 1.0f, // rr
|
||||
-1.0f, 1.0f, -1.0f, 1.0f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kZeroOffsetMap = {
|
||||
0.003f, 0.030f, 0.028f, // fl
|
||||
0.004f, 0.038f, 0.011f, // fr
|
||||
0.019f, -0.034f, 0.025f, // rl
|
||||
-0.001f, 0.039f, 0.018f, // rr
|
||||
0.000f, 0.000f, 0.000f, 0.000f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kActionScale = {
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
5.0f, 5.0f, 5.0f, 5.0f
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.9f, -1.8f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f
|
||||
};
|
||||
};
|
||||
|
||||
static constexpr std::array<const char *, 16> kJointLabels = {
|
||||
"fl_hip_abduction",
|
||||
"fl_hip_pitch",
|
||||
"fl_knee",
|
||||
"fr_hip_abduction",
|
||||
"fr_hip_pitch",
|
||||
"fr_knee",
|
||||
"rl_hip_abduction",
|
||||
"rl_hip_pitch",
|
||||
"rl_knee",
|
||||
"rr_hip_abduction",
|
||||
"rr_hip_pitch",
|
||||
"rr_knee",
|
||||
"fl_wheel",
|
||||
"fr_wheel",
|
||||
"rl_wheel",
|
||||
"rr_wheel"
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+55
@@ -0,0 +1,55 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class LowPassFilter
|
||||
{
|
||||
public:
|
||||
LowPassFilter(double cutoff_freq, double dt, std::size_t dim)
|
||||
: dim_(dim), initialized_(false)
|
||||
{
|
||||
alpha_ = static_cast<float>(1.0 - std::exp(-2.0 * M_PI * cutoff_freq * dt));
|
||||
y_prev_.resize(dim, 0.0f);
|
||||
}
|
||||
|
||||
void filter(const float* x, float* y)
|
||||
{
|
||||
if (!initialized_) {
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y_prev_[i] = x[i];
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
for (std::size_t i = 0; i < dim_; ++i) {
|
||||
y[i] = alpha_ * x[i] + (1.0f - alpha_) * y_prev_[i];
|
||||
y_prev_[i] = y[i];
|
||||
}
|
||||
}
|
||||
|
||||
void filter(const std::vector<float>& x, std::vector<float>& y)
|
||||
{
|
||||
filter(x.data(), y.data());
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
private:
|
||||
float alpha_;
|
||||
std::size_t dim_;
|
||||
bool initialized_;
|
||||
std::vector<float> y_prev_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
// Helper to calculate gravity orientation from quaternion [w, x, y, z]
|
||||
inline std::array<float, 3> get_gravity_orientation(const std::array<float, 4>& quat_wxyz)
|
||||
{
|
||||
float qw = quat_wxyz[0];
|
||||
float qx = quat_wxyz[1];
|
||||
float qy = quat_wxyz[2];
|
||||
float qz = quat_wxyz[3];
|
||||
|
||||
float gx = 2.0f * (-qz * qx + qw * qy);
|
||||
float gy = -2.0f * (qz * qy + qw * qx);
|
||||
float gz = 1.0f - 2.0f * (qw * qw + qz * qz);
|
||||
return {gx, gy, gz};
|
||||
}
|
||||
|
||||
// Helper to create quaternion from acceleration vector
|
||||
inline std::array<float, 4> quat_from_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
if (norm_a < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
// Ref gravity vector is [0.0, 0.0, 1.0]
|
||||
float cross_x = -ay;
|
||||
float cross_y = ax;
|
||||
float cross_z = 0.0f;
|
||||
float dot = az;
|
||||
|
||||
if (dot < -0.999999f) {
|
||||
return {0.0f, 1.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
float s = std::sqrt((1.0f + dot) * 2.0f);
|
||||
std::array<float, 4> q = {
|
||||
s * 0.5f,
|
||||
cross_x / s,
|
||||
cross_y / s,
|
||||
cross_z / s
|
||||
};
|
||||
|
||||
float norm_q = std::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||
if (norm_q < 1e-9f) {
|
||||
return {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
q[0] /= norm_q;
|
||||
q[1] /= norm_q;
|
||||
q[2] /= norm_q;
|
||||
q[3] /= norm_q;
|
||||
|
||||
return q;
|
||||
}
|
||||
|
||||
class MahonyFilter
|
||||
{
|
||||
public:
|
||||
MahonyFilter(float kp = 2.0f, float ki = 0.0f)
|
||||
: kp_(kp), ki_(ki)
|
||||
{
|
||||
q_ = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
void reset_with_accel(const std::array<float, 3>& accel)
|
||||
{
|
||||
q_ = quat_from_accel(accel);
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
std::array<float, 4> update(const std::array<float, 3>& accel, const std::array<float, 3>& gyro, float dt)
|
||||
{
|
||||
float norm_a = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]);
|
||||
std::array<float, 3> gyro_corr = gyro;
|
||||
|
||||
if (norm_a > 1e-6f) {
|
||||
float ax = accel[0] / norm_a;
|
||||
float ay = accel[1] / norm_a;
|
||||
float az = accel[2] / norm_a;
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float vx = 2.0f * (qx * qz - qw * qy);
|
||||
float vy = 2.0f * (qw * qx + qy * qz);
|
||||
float vz = qw * qw - qx * qx - qy * qy + qz * qz;
|
||||
|
||||
// Error = cross(a, v)
|
||||
float ex = ay * vz - az * vy;
|
||||
float ey = az * vx - ax * vz;
|
||||
float ez = ax * vy - ay * vx;
|
||||
|
||||
if (ki_ > 0.0f) {
|
||||
e_int_[0] += ex * dt;
|
||||
e_int_[1] += ey * dt;
|
||||
e_int_[2] += ez * dt;
|
||||
} else {
|
||||
e_int_ = {0.0f, 0.0f, 0.0f};
|
||||
}
|
||||
|
||||
gyro_corr[0] += kp_ * ex + ki_ * e_int_[0];
|
||||
gyro_corr[1] += kp_ * ey + ki_ * e_int_[1];
|
||||
gyro_corr[2] += kp_ * ez + ki_ * e_int_[2];
|
||||
}
|
||||
|
||||
float qw = q_[0];
|
||||
float qx = q_[1];
|
||||
float qy = q_[2];
|
||||
float qz = q_[3];
|
||||
|
||||
float q_dot_w = 0.5f * (-qx * gyro_corr[0] - qy * gyro_corr[1] - qz * gyro_corr[2]);
|
||||
float q_dot_x = 0.5f * ( qw * gyro_corr[0] + qy * gyro_corr[2] - qz * gyro_corr[1]);
|
||||
float q_dot_y = 0.5f * ( qw * gyro_corr[1] - qx * gyro_corr[2] + qz * gyro_corr[0]);
|
||||
float q_dot_z = 0.5f * ( qw * gyro_corr[2] + qx * gyro_corr[1] - qy * gyro_corr[0]);
|
||||
|
||||
q_[0] += q_dot_w * dt;
|
||||
q_[1] += q_dot_x * dt;
|
||||
q_[2] += q_dot_y * dt;
|
||||
q_[3] += q_dot_z * dt;
|
||||
|
||||
float norm_q = std::sqrt(q_[0]*q_[0] + q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3]) + 1e-9f;
|
||||
q_[0] /= norm_q;
|
||||
q_[1] /= norm_q;
|
||||
q_[2] /= norm_q;
|
||||
q_[3] /= norm_q;
|
||||
|
||||
return q_;
|
||||
}
|
||||
|
||||
const std::array<float, 4>& get_q() const { return q_; }
|
||||
|
||||
private:
|
||||
float kp_;
|
||||
float ki_;
|
||||
std::array<float, 4> q_;
|
||||
std::array<float, 3> e_int_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class GuardLevel : int {
|
||||
OK = 0,
|
||||
WARN = 1,
|
||||
STOP = 2
|
||||
};
|
||||
|
||||
struct GuardDecision {
|
||||
GuardLevel level{GuardLevel::OK};
|
||||
std::string reason;
|
||||
};
|
||||
|
||||
class RuntimeGuard {
|
||||
public:
|
||||
RuntimeGuard(
|
||||
float max_ang_vel = 12.0f,
|
||||
float max_tilt_z = -0.30f,
|
||||
float imu_age_warn_ms = 60.0f,
|
||||
float imu_age_stop_ms = 200.0f)
|
||||
: max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
imu_age_warn_ms_(imu_age_warn_ms),
|
||||
imu_age_stop_ms_(imu_age_stop_ms)
|
||||
{}
|
||||
|
||||
GuardDecision check(
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
float imu_age_ms,
|
||||
bool estop_triggered,
|
||||
const std::vector<float>& extra_vals = {})
|
||||
{
|
||||
GuardDecision decision;
|
||||
|
||||
// 1) user E-stop
|
||||
if (estop_triggered) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 2) NaN/Inf check
|
||||
for (float v : imu_gyro) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in imu_gyro";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : projected_gravity) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in projected_gravity";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
for (float v : extra_vals) {
|
||||
if (std::isnan(v) || std::isinf(v)) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "NaN/Inf detected in checked values";
|
||||
return decision;
|
||||
}
|
||||
}
|
||||
|
||||
// 3) IMU stale
|
||||
if (imu_age_ms > imu_age_stop_ms_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "IMU stale " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
bool warned_imu = (imu_age_ms > imu_age_warn_ms_);
|
||||
|
||||
// 4) Tilt check
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "tilt: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// 5) Angular velocity check
|
||||
float ang_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_norm > max_ang_vel_) {
|
||||
decision.level = GuardLevel::STOP;
|
||||
decision.reason = "ang_vel overflow: |w|=" + std::to_string(ang_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
if (warned_imu) {
|
||||
decision.level = GuardLevel::WARN;
|
||||
decision.reason = "IMU age " + std::to_string(imu_age_ms) + "ms";
|
||||
return decision;
|
||||
}
|
||||
|
||||
decision.level = GuardLevel::OK;
|
||||
return decision;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
float imu_age_warn_ms_;
|
||||
float imu_age_stop_ms_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+119
@@ -0,0 +1,119 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
enum class SafetyLevel : int {
|
||||
NORMAL = 0,
|
||||
CLIP = 1,
|
||||
BRAKE = 2,
|
||||
ESTOP = 3
|
||||
};
|
||||
|
||||
struct SafetyDecision {
|
||||
SafetyLevel level{SafetyLevel::NORMAL};
|
||||
std::string message;
|
||||
std::array<float, 16> clipped_target{};
|
||||
};
|
||||
|
||||
class SafetyMonitor {
|
||||
public:
|
||||
SafetyMonitor(
|
||||
float max_target_offset = 0.6f,
|
||||
float max_ang_vel = 10.0f,
|
||||
float max_tilt_z = -0.3f,
|
||||
int clip_to_brake = 0,
|
||||
float hard_target_offset = 1.2f)
|
||||
: max_target_offset_(max_target_offset),
|
||||
max_ang_vel_(max_ang_vel),
|
||||
max_tilt_z_(max_tilt_z),
|
||||
clip_to_brake_(clip_to_brake),
|
||||
hard_target_offset_(hard_target_offset),
|
||||
consecutive_clips_(0)
|
||||
{}
|
||||
|
||||
SafetyDecision check(
|
||||
const std::array<float, 16>& target_pose,
|
||||
const std::array<float, 16>& default_pose,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
bool estop_triggered)
|
||||
{
|
||||
SafetyDecision decision;
|
||||
decision.clipped_target = target_pose;
|
||||
|
||||
if (estop_triggered) {
|
||||
decision.level = SafetyLevel::ESTOP;
|
||||
decision.message = "user E-stop";
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Tilt check (g_z should be ~ -1.0, if it is > max_tilt_z e.g. -0.3, it is tilted)
|
||||
if (projected_gravity[2] > max_tilt_z_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "tilt detected: g_z=" + std::to_string(projected_gravity[2]);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Angular velocity norm check
|
||||
float ang_vel_norm = std::sqrt(imu_gyro[0] * imu_gyro[0] + imu_gyro[1] * imu_gyro[1] + imu_gyro[2] * imu_gyro[2]);
|
||||
if (ang_vel_norm > max_ang_vel_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "angular velocity overflow: |w|=" + std::to_string(ang_vel_norm);
|
||||
return decision;
|
||||
}
|
||||
|
||||
// Offset check
|
||||
bool needs_clip = false;
|
||||
float max_offset = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) { // check leg joint offsets from default pose
|
||||
float offset = target_pose[i] - default_pose[i];
|
||||
max_offset = std::max(max_offset, std::abs(offset));
|
||||
if (std::abs(offset) > max_target_offset_) {
|
||||
needs_clip = true;
|
||||
float clipped_val = std::clamp(offset, -max_target_offset_, max_target_offset_);
|
||||
decision.clipped_target[i] = default_pose[i] + clipped_val;
|
||||
}
|
||||
}
|
||||
|
||||
if (needs_clip) {
|
||||
consecutive_clips_++;
|
||||
if (hard_target_offset_ > 0.0f && max_offset > hard_target_offset_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "target leg offset exceeds hard limit: " + std::to_string(max_offset);
|
||||
return decision;
|
||||
}
|
||||
if (clip_to_brake_ > 0 && consecutive_clips_ >= clip_to_brake_) {
|
||||
decision.level = SafetyLevel::BRAKE;
|
||||
decision.message = "clipped " + std::to_string(consecutive_clips_) + " frames in a row";
|
||||
return decision;
|
||||
}
|
||||
decision.level = SafetyLevel::CLIP;
|
||||
decision.message = "target leg offset out of range";
|
||||
return decision;
|
||||
}
|
||||
|
||||
consecutive_clips_ = 0;
|
||||
decision.level = SafetyLevel::NORMAL;
|
||||
return decision;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
consecutive_clips_ = 0;
|
||||
}
|
||||
|
||||
private:
|
||||
float max_target_offset_;
|
||||
float max_ang_vel_;
|
||||
float max_tilt_z_;
|
||||
int clip_to_brake_;
|
||||
float hard_target_offset_;
|
||||
int consecutive_clips_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class StandBalanceController
|
||||
{
|
||||
public:
|
||||
StandBalanceController(double control_dt = 0.02)
|
||||
: control_dt_(control_dt)
|
||||
{
|
||||
profile_h_ = {0.157f, 0.248f, 0.311f, 0.366f, 0.411f, 0.448f};
|
||||
profile_hip_ = {1.5f, 1.2f, 1.0f, 0.8f, 0.6f, 0.4f};
|
||||
profile_knee_ = {-2.5f, -2.1f, -1.8f, -1.5f, -1.2f, -0.9f};
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
}
|
||||
|
||||
std::array<float, 16> computeTarget(
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
float hip_base = 0.9f;
|
||||
float knee_base = -1.8f;
|
||||
estimateBaseLegPose(hip_base, knee_base);
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
estimateRollPitch(projected_gravity, roll, pitch);
|
||||
|
||||
float roll_rate = imu_gyro[0];
|
||||
float pitch_rate = imu_gyro[1];
|
||||
|
||||
float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate;
|
||||
|
||||
float lateral_lean = lateral_lean_gain_ * cmd[1];
|
||||
|
||||
std::array<float, 16> target{};
|
||||
for (int leg_idx = 0; leg_idx < 4; ++leg_idx) {
|
||||
float side = (leg_idx == 0 || leg_idx == 2) ? 1.0f : -1.0f;
|
||||
|
||||
target[leg_idx * 3 + 0] = std::clamp(side * roll_corr + lateral_lean, -hip_abduction_clip_, hip_abduction_clip_);
|
||||
target[leg_idx * 3 + 1] = std::clamp(hip_base, hip_pitch_clip_[0], hip_pitch_clip_[1]);
|
||||
target[leg_idx * 3 + 2] = std::clamp(knee_base, knee_clip_[0], knee_clip_[1]);
|
||||
}
|
||||
// wheels 0
|
||||
target[12] = target[13] = target[14] = target[15] = 0.0f;
|
||||
|
||||
bool stable = (std::abs(roll * 180.0f / static_cast<float>(M_PI)) <= stable_roll_deg_) &&
|
||||
(std::abs(pitch * 180.0f / static_cast<float>(M_PI)) <= stable_pitch_deg_) &&
|
||||
(std::max(std::abs(roll_rate * 180.0f / static_cast<float>(M_PI)), std::abs(pitch_rate * 180.0f / static_cast<float>(M_PI))) <= stable_gyro_deg_s_);
|
||||
|
||||
stable_time_ = stable ? (stable_time_ + static_cast<float>(control_dt_)) : 0.0f;
|
||||
|
||||
return target;
|
||||
}
|
||||
|
||||
bool isStable() const
|
||||
{
|
||||
return stable_time_ >= enter_hold_s_;
|
||||
}
|
||||
|
||||
private:
|
||||
void estimateRollPitch(const std::array<float, 3>& projected_gravity, float& roll, float& pitch)
|
||||
{
|
||||
float gx = projected_gravity[0];
|
||||
float gy = projected_gravity[1];
|
||||
float gz = projected_gravity[2];
|
||||
roll = std::atan2(-gy, std::max(1e-6f, -gz));
|
||||
pitch = std::atan2(gx, std::sqrt(std::max(1e-6f, gy * gy + gz * gz)));
|
||||
}
|
||||
|
||||
void estimateBaseLegPose(float& hip, float& knee)
|
||||
{
|
||||
float h_clamp = std::clamp(height_, profile_h_.front(), profile_h_.back());
|
||||
hip = interpolate(h_clamp, profile_h_, profile_hip_);
|
||||
knee = interpolate(h_clamp, profile_h_, profile_knee_);
|
||||
}
|
||||
|
||||
float interpolate(float x, const std::vector<float>& xp, const std::vector<float>& fp)
|
||||
{
|
||||
if (x <= xp.front()) return fp.front();
|
||||
if (x >= xp.back()) return fp.back();
|
||||
for (std::size_t i = 0; i < xp.size() - 1; ++i) {
|
||||
if (x >= xp[i] && x <= xp[i+1]) {
|
||||
float f = (x - xp[i]) / (xp[i+1] - xp[i]);
|
||||
return fp[i] + f * (fp[i+1] - fp[i]);
|
||||
}
|
||||
}
|
||||
return fp.back();
|
||||
}
|
||||
|
||||
double control_dt_;
|
||||
float height_{0.33f};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
float hip_abduction_clip_{0.45f};
|
||||
std::array<float, 2> hip_pitch_clip_{-1.0f, 2.5f};
|
||||
std::array<float, 2> knee_clip_{-2.6f, -0.3f};
|
||||
float stable_roll_deg_{6.0f};
|
||||
float stable_pitch_deg_{8.0f};
|
||||
float stable_gyro_deg_s_{45.0f};
|
||||
float enter_hold_s_{1.0f};
|
||||
|
||||
std::vector<float> profile_h_;
|
||||
std::vector<float> profile_hip_;
|
||||
std::vector<float> profile_knee_;
|
||||
|
||||
float stable_time_{0.0f};
|
||||
};
|
||||
|
||||
} // namespace sim2real_common
|
||||
@@ -0,0 +1,10 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_common</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Shared constants and deployment contract helpers for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
</package>
|
||||
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_hw)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
add_executable(sim2real_hw_node
|
||||
src/hardware_bridge_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_hw_node PRIVATE include)
|
||||
target_compile_features(sim2real_hw_node PRIVATE cxx_std_17)
|
||||
ament_target_dependencies(sim2real_hw_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
nav_msgs
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_hw_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+156
@@ -0,0 +1,156 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/low_pass_filter.hpp"
|
||||
#include "sim2real_common/mahony_filter.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
int id; // motor CAN id
|
||||
float direction;
|
||||
float offset;
|
||||
};
|
||||
|
||||
struct MotorStateInternal
|
||||
{
|
||||
float position{0.0f};
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{0};
|
||||
// Hold-over state
|
||||
float last_valid_pos{0.0f};
|
||||
float last_valid_vel{0.0f};
|
||||
float last_valid_torque{0.0f};
|
||||
std::uint32_t prev_update_count{0};
|
||||
bool has_valid_data{false};
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
HardwareBridgeNode();
|
||||
~HardwareBridgeNode();
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
bool initCan(const std::string& ifname, int& fd);
|
||||
bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc);
|
||||
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
|
||||
rclcpp::TimerBase::SharedPtr read_timer_;
|
||||
rclcpp::TimerBase::SharedPtr write_timer_;
|
||||
|
||||
std::mutex target_mutex_;
|
||||
std::array<float, 16> latest_target_{};
|
||||
std::array<float, 16> latest_raw_action_{};
|
||||
std::string latest_target_source_{"boot_hold"};
|
||||
rclcpp::Time latest_target_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
|
||||
// SocketCAN file descriptors
|
||||
int can0_fd_{-1};
|
||||
int can1_fd_{-1};
|
||||
std::string can0_name_{"can0"};
|
||||
std::string can1_name_{"can1"};
|
||||
|
||||
// CAN error recovery
|
||||
static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit
|
||||
int can0_error_count_{0};
|
||||
int can1_error_count_{0};
|
||||
bool reinitCan(const std::string& ifname, int& fd, int& error_count);
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<MotorConfig, 16> motors_;
|
||||
std::array<MotorStateInternal, 16> motor_states_;
|
||||
|
||||
// IMU state
|
||||
std::mutex imu_mutex_;
|
||||
std::array<float, 3> imu_gyro_{};
|
||||
std::array<float, 3> imu_accel_{};
|
||||
std::array<float, 3> projected_gravity_{0.0f, 0.0f, -1.0f};
|
||||
bool imu_fresh_{false};
|
||||
rclcpp::Time last_imu_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::chrono::steady_clock::time_point last_imu_recv_time_{};
|
||||
bool has_received_imu_{false};
|
||||
std::array<float, 3> imu_gravity_sum_{0.0f, 0.0f, 0.0f};
|
||||
std::uint32_t imu_gravity_sample_count_{0};
|
||||
static constexpr std::uint32_t kImuGravityAlignSamples = 50;
|
||||
|
||||
// Odom state
|
||||
std::mutex odom_mutex_;
|
||||
rclcpp::Time last_odom_stamp_{0, 0, RCL_ROS_TIME};
|
||||
std::array<float, 3> odom_pos_{};
|
||||
std::array<float, 4> odom_quat_wxyz_{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
std::array<float, 3> odom_linear_vel_{};
|
||||
std::array<float, 3> odom_angular_vel_{};
|
||||
bool odom_fresh_{false};
|
||||
|
||||
// Filters and Estimators
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_legs_;
|
||||
std::unique_ptr<sim2real_common::LowPassFilter> lpf_wheels_;
|
||||
std::unique_ptr<sim2real_common::MahonyFilter> mahony_filter_;
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
bool mahony_initialized_{false};
|
||||
rclcpp::Time last_read_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time startup_soft_hold_start_time_{0, 0, RCL_ROS_TIME};
|
||||
|
||||
// Telemetry
|
||||
std::uint32_t fresh_count_{0};
|
||||
std::uint32_t holdover_count_{0};
|
||||
std::uint32_t stale_max_{0};
|
||||
std::uint32_t holdover_events_total_{0};
|
||||
|
||||
bool dry_run_{false};
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_hw</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Hardware bridge and safety boundary for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
</package>
|
||||
@@ -0,0 +1,852 @@
|
||||
#include "sim2real_hw/hardware_bridge_node.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
#include <net/if.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
// Protocol constants
|
||||
const std::uint32_t COMM_ENABLE = 3;
|
||||
const std::uint32_t COMM_DISABLE = 4;
|
||||
const std::uint32_t COMM_WRITE_PARAMETER = 18;
|
||||
const std::uint32_t COMM_OPERATION_CONTROL = 1;
|
||||
const std::uint32_t COMM_SET_ZERO_POSITION = 6;
|
||||
const std::uint16_t PARAM_MODE = 0x7005;
|
||||
const std::uint16_t PARAM_VELOCITY_LIMIT = 0x7017;
|
||||
const std::uint16_t PARAM_TORQUE_LIMIT = 0x700B;
|
||||
const std::uint8_t HOST_ID = 0xFD;
|
||||
|
||||
inline void pack_u16_be(std::uint8_t* buf, std::uint16_t val)
|
||||
{
|
||||
buf[0] = (val >> 8) & 0xFF;
|
||||
buf[1] = val & 0xFF;
|
||||
}
|
||||
|
||||
inline float nearest_periodic(float val, float ref)
|
||||
{
|
||||
float diff = val - ref;
|
||||
float wrapped = diff - 2.0f * static_cast<float>(M_PI) * std::floor((diff + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
return ref + wrapped;
|
||||
}
|
||||
|
||||
HardwareBridgeNode::HardwareBridgeNode()
|
||||
: Node("sim2real_hw_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
target_timeout_ms_ = declare_parameter<double>("target_timeout_ms", 150.0);
|
||||
can0_name_ = declare_parameter<std::string>("can0_name", "can0");
|
||||
can1_name_ = declare_parameter<std::string>("can1_name", "can1");
|
||||
dry_run_ = declare_parameter<bool>("dry_run", true); // Default to dry-run for safety
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Initializing hardware bridge node (Dry run: %s)", dry_run_ ? "true" : "false");
|
||||
if (safety_enabled_) {
|
||||
RCLCPP_INFO(get_logger(), "Safety monitoring is ENABLED (tilt threshold: %f, ang_vel threshold: %f)", max_tilt_z, max_ang_vel);
|
||||
} else {
|
||||
RCLCPP_WARN(get_logger(), "Safety monitoring is DISABLED!");
|
||||
}
|
||||
|
||||
// 2. Set up logical motors mapping matching contract
|
||||
// Mapping index in array matches joint ordering in kJointLabels
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motors_[i].direction = sim2real_common::DeploymentContract::kDirectionMap[i];
|
||||
motors_[i].offset = sim2real_common::DeploymentContract::kZeroOffsetMap[i];
|
||||
motors_[i].bus = sim2real_common::DeploymentContract::kCanBusMap[i];
|
||||
motors_[i].id = sim2real_common::DeploymentContract::kCanIdMap[i];
|
||||
}
|
||||
|
||||
// 3. Initialize filters & safety monitors
|
||||
lpf_legs_ = std::make_unique<sim2real_common::LowPassFilter>(5.0, 0.005, 12);
|
||||
lpf_wheels_ = std::make_unique<sim2real_common::LowPassFilter>(15.0, 0.005, 4);
|
||||
mahony_filter_ = std::make_unique<sim2real_common::MahonyFilter>(2.0f, 0.0f);
|
||||
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0), // slightly higher limit for runtime guard stop
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 4. Initialize CAN sockets if not in dry-run
|
||||
if (!dry_run_) {
|
||||
if (!initCan(can0_name_, can0_fd_) || !initCan(can1_name_, can1_fd_)) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN initialization failed! Falling back to dry-run.");
|
||||
dry_run_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 5. Initialize motor target states
|
||||
latest_target_ = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
latest_raw_action_.fill(0.0f);
|
||||
|
||||
// 6. Set up ROS publishers & subscriptions
|
||||
state_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeState>("runtime/state", 10);
|
||||
target_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeTarget>(
|
||||
"runtime/target", 10,
|
||||
std::bind(&HardwareBridgeNode::onTarget, this, std::placeholders::_1));
|
||||
std::string imu_topic = declare_parameter<std::string>("imu_topic", "/odin1/imu");
|
||||
imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
|
||||
imu_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onImu, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&HardwareBridgeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// Odom subscription
|
||||
std::string odom_topic = declare_parameter<std::string>("odom_topic", "/odom");
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_topic, 10,
|
||||
std::bind(&HardwareBridgeNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
// 7. Enable motors on total startup
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Enabling RobStride motors...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
enableMotor(fd, motors_[i].id);
|
||||
setModeRaw(fd, motors_[i].id, 0); // MIT Mode
|
||||
writeLimit(fd, motors_[i].id, PARAM_VELOCITY_LIMIT, 20.0f);
|
||||
writeLimit(fd, motors_[i].id, PARAM_TORQUE_LIMIT, 17.0f);
|
||||
}
|
||||
}
|
||||
|
||||
// 8. Timers at 200Hz (5ms)
|
||||
read_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onReadLoop, this));
|
||||
write_timer_ = create_wall_timer(5ms, std::bind(&HardwareBridgeNode::onWriteLoop, this));
|
||||
}
|
||||
|
||||
HardwareBridgeNode::~HardwareBridgeNode()
|
||||
{
|
||||
if (!dry_run_) {
|
||||
RCLCPP_INFO(get_logger(), "Disabling RobStride motors on shutdown...");
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
disableMotor(fd, motors_[i].id);
|
||||
}
|
||||
if (can0_fd_ >= 0) {
|
||||
if (::close(can0_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can0 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
if (can1_fd_ >= 0) {
|
||||
if (::close(can1_fd_) < 0) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to close can1 socket: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
latest_target_ = msg->target;
|
||||
latest_raw_action_ = msg->raw_action;
|
||||
latest_target_source_ = msg->target_source;
|
||||
latest_target_stamp_ = rclcpp::Time(msg->stamp);
|
||||
target_sequence_ = msg->sequence;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onImu(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
|
||||
const float gyro_x = static_cast<float>(msg->angular_velocity.x);
|
||||
const float gyro_y = static_cast<float>(msg->angular_velocity.y);
|
||||
const float gyro_z = static_cast<float>(msg->angular_velocity.z);
|
||||
|
||||
const float accel_x = static_cast<float>(msg->linear_acceleration.x);
|
||||
const float accel_y = static_cast<float>(msg->linear_acceleration.y);
|
||||
const float accel_z = static_cast<float>(msg->linear_acceleration.z);
|
||||
|
||||
imu_gyro_ = {gyro_x, gyro_y, gyro_z};
|
||||
imu_accel_ = {accel_x, accel_y, accel_z};
|
||||
if (!mahony_initialized_ && imu_gravity_sample_count_ < kImuGravityAlignSamples) {
|
||||
imu_gravity_sum_[0] += accel_x;
|
||||
imu_gravity_sum_[1] += accel_y;
|
||||
imu_gravity_sum_[2] += accel_z;
|
||||
imu_gravity_sample_count_++;
|
||||
}
|
||||
// Track both ROS header time and local receive time. The local steady clock
|
||||
// is used for stale detection so scheduler jitter or device timestamp quirks
|
||||
// don't falsely trip the runtime guard.
|
||||
last_imu_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
last_imu_recv_time_ = std::chrono::steady_clock::now();
|
||||
has_received_imu_ = true;
|
||||
imu_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! Physical E-stop received over /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "Physical E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
last_odom_stamp_ = rclcpp::Time(msg->header.stamp);
|
||||
|
||||
odom_pos_[0] = static_cast<float>(msg->pose.pose.position.x);
|
||||
odom_pos_[1] = static_cast<float>(msg->pose.pose.position.y);
|
||||
odom_pos_[2] = static_cast<float>(msg->pose.pose.position.z);
|
||||
|
||||
odom_quat_wxyz_[0] = static_cast<float>(msg->pose.pose.orientation.w);
|
||||
odom_quat_wxyz_[1] = static_cast<float>(msg->pose.pose.orientation.x);
|
||||
odom_quat_wxyz_[2] = static_cast<float>(msg->pose.pose.orientation.y);
|
||||
odom_quat_wxyz_[3] = static_cast<float>(msg->pose.pose.orientation.z);
|
||||
|
||||
odom_linear_vel_[0] = static_cast<float>(msg->twist.twist.linear.x);
|
||||
odom_linear_vel_[1] = static_cast<float>(msg->twist.twist.linear.y);
|
||||
odom_linear_vel_[2] = static_cast<float>(msg->twist.twist.linear.z);
|
||||
|
||||
odom_angular_vel_[0] = static_cast<float>(msg->twist.twist.angular.x);
|
||||
odom_angular_vel_[1] = static_cast<float>(msg->twist.twist.angular.y);
|
||||
odom_angular_vel_[2] = static_cast<float>(msg->twist.twist.angular.z);
|
||||
|
||||
odom_fresh_ = true;
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onReadLoop()
|
||||
{
|
||||
// 1. Process CAN messages (only if CAN is open)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
motor_states_[i].stale_count++;
|
||||
}
|
||||
|
||||
struct can_frame frame;
|
||||
// Process can0 (bus 1)
|
||||
while (readCanFrame(can0_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) { // Status Frame
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 1 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
// Apply motor mapping: real_to_sim
|
||||
// real = sign * sim + offset -> sim = (real - offset) / sign
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Process can1 (bus 2)
|
||||
while (readCanFrame(can1_fd_, &frame, 50)) {
|
||||
if (!(frame.can_id & CAN_EFF_FLAG)) continue;
|
||||
std::uint32_t comm_type = (frame.can_id >> 24) & 0x1F;
|
||||
if (comm_type == 2) {
|
||||
std::uint32_t extra_data = (frame.can_id >> 8) & 0xFFFF;
|
||||
int motor_id = extra_data & 0xFF;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motors_[i].bus == 2 && motors_[i].id == motor_id) {
|
||||
std::uint16_t p_u16 = (frame.data[0] << 8) | frame.data[1];
|
||||
std::uint16_t v_u16 = (frame.data[2] << 8) | frame.data[3];
|
||||
std::uint16_t t_u16 = (frame.data[4] << 8) | frame.data[5];
|
||||
std::uint16_t temp_u16 = (frame.data[6] << 8) | frame.data[7];
|
||||
|
||||
double pos_raw = (static_cast<double>(p_u16) / 32767.0 - 1.0) * (4.0 * M_PI);
|
||||
double vel_raw = (static_cast<double>(v_u16) / 32767.0 - 1.0) * 44.0;
|
||||
double torque_raw = (static_cast<double>(t_u16) / 32767.0 - 1.0) * 17.0;
|
||||
|
||||
float pos_sim = (static_cast<float>(pos_raw) - motors_[i].offset) / motors_[i].direction;
|
||||
float vel_sim = static_cast<float>(vel_raw) / motors_[i].direction;
|
||||
float torque_sim = static_cast<float>(torque_raw) / motors_[i].direction;
|
||||
|
||||
if (i < 12) {
|
||||
pos_sim = nearest_periodic(pos_sim, sim2real_common::DeploymentContract::kDefaultDofPos[i]);
|
||||
}
|
||||
|
||||
motor_states_[i].position = pos_sim;
|
||||
motor_states_[i].velocity = vel_sim;
|
||||
motor_states_[i].torque = torque_sim;
|
||||
motor_states_[i].temperature = static_cast<float>(temp_u16) * 0.1f;
|
||||
motor_states_[i].update_count++;
|
||||
motor_states_[i].stale_count = 0;
|
||||
// Update hold-over valid data
|
||||
motor_states_[i].last_valid_pos = pos_sim;
|
||||
motor_states_[i].last_valid_vel = vel_sim;
|
||||
motor_states_[i].last_valid_torque = torque_sim;
|
||||
motor_states_[i].has_valid_data = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Hold-over: apply last valid data for stale motors
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
if (motor_states_[i].stale_count >= kHoldoverThreshold && motor_states_[i].has_valid_data) {
|
||||
motor_states_[i].position = motor_states_[i].last_valid_pos;
|
||||
motor_states_[i].velocity = motor_states_[i].last_valid_vel;
|
||||
motor_states_[i].torque = motor_states_[i].last_valid_torque;
|
||||
holdover_events_total_++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 2. Fetch IMU data & update MahonyFilter
|
||||
auto now_time = now();
|
||||
double dt = 0.005;
|
||||
if (last_read_time_.nanoseconds() > 0) {
|
||||
dt = (now_time - last_read_time_).seconds();
|
||||
if (dt <= 0.0 || dt > 0.5) {
|
||||
dt = 0.005;
|
||||
}
|
||||
}
|
||||
last_read_time_ = now_time;
|
||||
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> accel{0.0f, 0.0f, 9.81f};
|
||||
bool imu_fresh = false;
|
||||
double imu_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
accel = imu_accel_;
|
||||
imu_fresh = imu_fresh_;
|
||||
imu_fresh_ = false;
|
||||
|
||||
if (has_received_imu_) {
|
||||
const auto age = std::chrono::steady_clock::now() - last_imu_recv_time_;
|
||||
imu_age_ms = std::chrono::duration<double, std::milli>(age).count();
|
||||
} else if (last_imu_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - last_imu_stamp_).nanoseconds();
|
||||
imu_age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 4> quat{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
if (imu_fresh || mahony_initialized_) {
|
||||
if (!mahony_initialized_) {
|
||||
std::array<float, 3> gravity_init = accel;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
if (imu_gravity_sample_count_ >= kImuGravityAlignSamples) {
|
||||
gravity_init = {
|
||||
imu_gravity_sum_[0] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[1] / static_cast<float>(imu_gravity_sample_count_),
|
||||
imu_gravity_sum_[2] / static_cast<float>(imu_gravity_sample_count_)
|
||||
};
|
||||
}
|
||||
}
|
||||
mahony_filter_->reset_with_accel(gravity_init);
|
||||
mahony_initialized_ = true;
|
||||
}
|
||||
quat = mahony_filter_->update(accel, gyro, static_cast<float>(dt));
|
||||
}
|
||||
std::array<float, 3> projected_gravity = sim2real_common::get_gravity_orientation(quat);
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
projected_gravity_ = projected_gravity;
|
||||
}
|
||||
|
||||
// Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(32);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
extra_vals.push_back(motor_states_[i].position);
|
||||
extra_vals.push_back(motor_states_[i].velocity);
|
||||
}
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
auto guard_decision = runtime_guard_->check(gyro, projected_gravity, imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
// 3. Populate RuntimeState message
|
||||
sim2real_interfaces::msg::RuntimeState msg;
|
||||
msg.stamp = now_time;
|
||||
msg.sequence = state_sequence_++;
|
||||
msg.source = dry_run_ ? "stub_hw" : "socket_can_hw";
|
||||
|
||||
fresh_count_ = 0;
|
||||
holdover_count_ = 0;
|
||||
stale_max_ = 0;
|
||||
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Stale frames detection & holdover count
|
||||
if (!dry_run_) {
|
||||
if (motor_states_[i].stale_count > 0) {
|
||||
holdover_count_++;
|
||||
stale_max_ = std::max(stale_max_, motor_states_[i].stale_count);
|
||||
} else {
|
||||
fresh_count_++;
|
||||
}
|
||||
}
|
||||
|
||||
if (dry_run_) {
|
||||
// Mock motor positions tracking target
|
||||
msg.joint_pos[i] = latest_target_[i];
|
||||
msg.joint_vel[i] = 0.0f;
|
||||
msg.joint_torque[i] = 0.0f;
|
||||
msg.update_counts[i] = target_sequence_;
|
||||
} else {
|
||||
msg.joint_pos[i] = motor_states_[i].position;
|
||||
msg.joint_vel[i] = motor_states_[i].velocity;
|
||||
msg.joint_torque[i] = motor_states_[i].torque;
|
||||
msg.update_counts[i] = motor_states_[i].update_count;
|
||||
}
|
||||
}
|
||||
|
||||
msg.imu_gyro = gyro;
|
||||
msg.imu_accel = accel;
|
||||
msg.quat_wxyz = quat;
|
||||
msg.projected_gravity = projected_gravity;
|
||||
msg.imu_age_ms = imu_age_ms;
|
||||
msg.imu_fresh = imu_fresh || (imu_age_ms < 60.0); // Allow brief staleness
|
||||
|
||||
msg.odom_age_ms = 0.0f;
|
||||
msg.odom_fresh = false;
|
||||
msg.odom_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_quat_wxyz = {1.0f, 0.0f, 0.0f, 0.0f};
|
||||
msg.odom_linear_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_angular_vel = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_pos = {0.0f, 0.0f, 0.0f};
|
||||
msg.odom_local_yaw = 0.0f;
|
||||
|
||||
// Populate odom fields from subscriber data
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(odom_mutex_);
|
||||
if (odom_fresh_) {
|
||||
double odom_age = (now_time - last_odom_stamp_).seconds() * 1000.0;
|
||||
msg.odom_age_ms = static_cast<float>(odom_age);
|
||||
msg.odom_fresh = (odom_age < 200.0); // 200ms threshold
|
||||
msg.odom_pos = odom_pos_;
|
||||
msg.odom_quat_wxyz = odom_quat_wxyz_;
|
||||
msg.odom_linear_vel = odom_linear_vel_;
|
||||
msg.odom_angular_vel = odom_angular_vel_;
|
||||
msg.odom_local_pos = odom_pos_;
|
||||
// Compute yaw from quaternion
|
||||
float qw = odom_quat_wxyz_[0], qx = odom_quat_wxyz_[1];
|
||||
float qy = odom_quat_wxyz_[2], qz = odom_quat_wxyz_[3];
|
||||
float siny_c = 2.0f * (qw * qz + qx * qy);
|
||||
float cosy_c = 1.0f - 2.0f * (qy * qy + qz * qz);
|
||||
msg.odom_local_yaw = std::atan2(siny_c, cosy_c);
|
||||
}
|
||||
}
|
||||
|
||||
msg.fresh_count = dry_run_ ? 16 : fresh_count_;
|
||||
msg.holdover_count = dry_run_ ? 0 : holdover_count_;
|
||||
msg.stale_max = dry_run_ ? 0 : stale_max_;
|
||||
|
||||
state_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void HardwareBridgeNode::onWriteLoop()
|
||||
{
|
||||
const auto now_time = now();
|
||||
std::array<float, 16> target{};
|
||||
std::string target_source;
|
||||
double age_ms = 0.0;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
target = latest_target_;
|
||||
target_source = latest_target_source_;
|
||||
|
||||
if (latest_target_stamp_.nanoseconds() > 0) {
|
||||
const auto age_ns = (now_time - latest_target_stamp_).nanoseconds();
|
||||
age_ms = age_ns > 0 ? static_cast<double>(age_ns) / 1.0e6 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Timeout guard: default stand pose if target is stale
|
||||
if (latest_target_stamp_.nanoseconds() == 0 || age_ms > target_timeout_ms_) {
|
||||
target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target_source = "timeout_hold";
|
||||
}
|
||||
|
||||
// Run SafetyMonitor check on incoming target commands
|
||||
std::array<float, 3> gyro{};
|
||||
std::array<float, 3> proj_grav{};
|
||||
bool estop_active = false;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(imu_mutex_);
|
||||
gyro = imu_gyro_;
|
||||
proj_grav = projected_gravity_;
|
||||
}
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(target_mutex_);
|
||||
estop_active = estop_triggered_;
|
||||
}
|
||||
|
||||
if (safety_enabled_ && !safety_triggered_) {
|
||||
auto safety_decision = safety_monitor_->check(target, sim2real_common::DeploymentContract::kDefaultDofPos, gyro, proj_grav, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY TRIGGERED: %s", safety_reason_.c_str());
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target = safety_decision.clipped_target;
|
||||
target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped.");
|
||||
}
|
||||
}
|
||||
|
||||
// Override to safety_brake if safety is triggered locally or by estop
|
||||
if (safety_triggered_) {
|
||||
target_source = "safety_brake";
|
||||
}
|
||||
|
||||
// 1. Joint LPF command filtering (200Hz, dt=0.005s)
|
||||
std::array<float, 12> legs_in{};
|
||||
std::array<float, 12> legs_out{};
|
||||
std::array<float, 4> wheels_in{};
|
||||
std::array<float, 4> wheels_out{};
|
||||
|
||||
std::copy(target.begin(), target.begin() + 12, legs_in.begin());
|
||||
std::copy(target.begin() + 12, target.end(), wheels_in.begin());
|
||||
|
||||
lpf_legs_->filter(legs_in.data(), legs_out.data());
|
||||
lpf_wheels_->filter(wheels_in.data(), wheels_out.data());
|
||||
|
||||
std::array<float, 16> filtered_target{};
|
||||
std::copy(legs_out.begin(), legs_out.end(), filtered_target.begin());
|
||||
std::copy(wheels_out.begin(), wheels_out.end(), filtered_target.begin() + 12);
|
||||
|
||||
// 2. Control execution (MIT mode write over CAN)
|
||||
if (!dry_run_) {
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
// Coordinate transform: sim_to_real
|
||||
// real = sign * sim + offset
|
||||
float sim_val = filtered_target[i];
|
||||
float real_val = motors_[i].direction * sim_val + motors_[i].offset;
|
||||
|
||||
int fd = (motors_[i].bus == 1) ? can0_fd_ : can1_fd_;
|
||||
|
||||
if (i < 12) {
|
||||
// Leg joints: MIT position control
|
||||
// Kp & Kd depend on whether we are holding pose, running policy, or in safety damping mode
|
||||
double kp_val = sim2real_common::DeploymentContract::kLegKp;
|
||||
double kd_val = sim2real_common::DeploymentContract::kLegKd;
|
||||
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
kp_val = 0.0;
|
||||
kd_val = 2.5; // Leg damping Kd
|
||||
real_val = 0.0; // Set to zero position (sign/offset will be ignored anyway under kp=0)
|
||||
} else if (target_source == "startup_soft_hold") {
|
||||
if (startup_soft_hold_start_time_.nanoseconds() == 0) {
|
||||
startup_soft_hold_start_time_ = now_time;
|
||||
}
|
||||
double elapsed = (now_time - startup_soft_hold_start_time_).seconds();
|
||||
double kp_scale = 0.125 + (1.0 - 0.125) * std::min(1.0, elapsed / 1.0); // 1.0s ramp
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp * kp_scale;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
} else {
|
||||
startup_soft_hold_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
|
||||
if (target_source == "timeout_hold" || target_source == "boot_hold" || target_source == "runtime_zero_hold" || target_source == "startup_hold") {
|
||||
kp_val = sim2real_common::DeploymentContract::kLegHoldKp;
|
||||
kd_val = sim2real_common::DeploymentContract::kLegHoldKd;
|
||||
}
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, real_val, 0.0, kp_val, kd_val, 0.0);
|
||||
} else {
|
||||
// Wheel joints: MIT velocity control (Kp = 0, Kd = kWheelKd, velocity = target, position = 0)
|
||||
double vel_real = motors_[i].direction * sim_val; // Wheels actions are in velocity, apply sign
|
||||
double kd_val = sim2real_common::DeploymentContract::kWheelKd;
|
||||
if (target_source == "safety_brake" || target_source == "safety_estop") {
|
||||
vel_real = 0.0;
|
||||
kd_val = 2.0; // Wheel damping Kd
|
||||
}
|
||||
writeOperationFrame(fd, motors_[i].id, 0.0, vel_real, 0.0, kd_val, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::enableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_ENABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, nullptr, 0);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::disableMotor(int fd, int motor_id)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_DISABLE << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::setModeRaw(int fd, int motor_id, std::int8_t mode)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = PARAM_MODE & 0xFF;
|
||||
data[1] = (PARAM_MODE >> 8) & 0xFF;
|
||||
data[4] = static_cast<std::uint8_t>(mode);
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit)
|
||||
{
|
||||
std::uint32_t ext_id = (COMM_WRITE_PARAMETER << 24) | (HOST_ID << 8) | motor_id;
|
||||
std::uint8_t data[8] = {0};
|
||||
data[0] = param_id & 0xFF;
|
||||
data[1] = (param_id >> 8) & 0xFF;
|
||||
std::memcpy(&data[4], &limit, sizeof(float));
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp_val, double kd_val, double torque)
|
||||
{
|
||||
const double P_LIMIT = 4.0 * M_PI;
|
||||
const double V_LIMIT = 44.0;
|
||||
const double T_LIMIT = 17.0;
|
||||
const double KP_LIMIT = 500.0;
|
||||
const double KD_LIMIT = 5.0;
|
||||
|
||||
double pos_clamped = std::max(-P_LIMIT, std::min(P_LIMIT, pos));
|
||||
double vel_clamped = std::max(-V_LIMIT, std::min(V_LIMIT, vel));
|
||||
double kp_clamped = std::max(0.0, std::min(KP_LIMIT, kp_val));
|
||||
double kd_clamped = std::max(0.0, std::min(KD_LIMIT, kd_val));
|
||||
double torque_clamped = std::max(-T_LIMIT, std::min(T_LIMIT, torque));
|
||||
|
||||
std::uint16_t pos_u16 = static_cast<std::uint16_t>(((pos_clamped / P_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t vel_u16 = static_cast<std::uint16_t>(((vel_clamped / V_LIMIT) + 1.0) * 32767.0);
|
||||
std::uint16_t kp_u16 = static_cast<std::uint16_t>((kp_clamped / KP_LIMIT) * 65535.0);
|
||||
std::uint16_t kd_u16 = static_cast<std::uint16_t>((kd_clamped / KD_LIMIT) * 65535.0);
|
||||
std::uint16_t torque_u16 = static_cast<std::uint16_t>(((torque_clamped / T_LIMIT) + 1.0) * 32767.0);
|
||||
|
||||
std::uint8_t data[8];
|
||||
pack_u16_be(&data[0], pos_u16);
|
||||
pack_u16_be(&data[2], vel_u16);
|
||||
pack_u16_be(&data[4], kp_u16);
|
||||
pack_u16_be(&data[6], kd_u16);
|
||||
|
||||
std::uint32_t ext_id = (COMM_OPERATION_CONTROL << 24) | (torque_u16 << 8) | motor_id;
|
||||
return sendCanFrame(fd, ext_id, data, 8);
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::initCan(const std::string& ifname, int& fd)
|
||||
{
|
||||
struct sockaddr_can addr;
|
||||
struct ifreq ifr;
|
||||
|
||||
if ((fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to create SocketCAN socket for %s", ifname.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set non-blocking mode
|
||||
int flags = ::fcntl(fd, F_GETFL, 0);
|
||||
if (flags < 0 || ::fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to set socket to non-blocking for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::strncpy(ifr.ifr_name, ifname.c_str(), IFNAMSIZ - 1);
|
||||
if (::ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to ioctl SIOCGIFINDEX for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
if (::bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to bind SocketCAN socket for %s", ifname.c_str());
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully bound to SocketCAN interface %s", ifname.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
struct can_frame frame;
|
||||
frame.can_id = can_id | CAN_EFF_FLAG; // Extended frame format (29-bit CAN ID)
|
||||
frame.can_dlc = dlc;
|
||||
if (data) {
|
||||
std::memcpy(frame.data, data, dlc);
|
||||
} else {
|
||||
std::memset(frame.data, 0, 8);
|
||||
}
|
||||
|
||||
ssize_t bytes_written = ::write(fd, &frame, sizeof(struct can_frame));
|
||||
if (bytes_written != sizeof(struct can_frame)) {
|
||||
int err = errno;
|
||||
// Track errors per bus for recovery logic
|
||||
if (fd == can0_fd_) {
|
||||
can0_error_count_++;
|
||||
if (can0_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN0 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can0_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can0_name_, can0_fd_, can0_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN0 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN0 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
} else if (fd == can1_fd_) {
|
||||
can1_error_count_++;
|
||||
if (can1_error_count_ >= kCanErrorThreshold) {
|
||||
RCLCPP_ERROR(get_logger(), "CAN1 write: %d consecutive errors (errno=%d: %s). Attempting reinit.",
|
||||
can1_error_count_, err, strerror(err));
|
||||
if (!reinitCan(can1_name_, can1_fd_, can1_error_count_)) {
|
||||
RCLCPP_FATAL(get_logger(), "CAN1 reinit failed! Triggering safety brake.");
|
||||
safety_triggered_ = true;
|
||||
safety_reason_ = "CAN1 bus failure - reinit failed";
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// Reset error count on success
|
||||
if (fd == can0_fd_) can0_error_count_ = 0;
|
||||
else if (fd == can1_fd_) can1_error_count_ = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::readCanFrame(int fd, void* frame_ptr, int timeout_us)
|
||||
{
|
||||
if (fd < 0) return false;
|
||||
auto* frame = static_cast<struct can_frame*>(frame_ptr);
|
||||
|
||||
if (timeout_us > 0) {
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = timeout_us;
|
||||
fd_set rdfs;
|
||||
FD_ZERO(&rdfs);
|
||||
FD_SET(fd, &rdfs);
|
||||
|
||||
int ret = ::select(fd + 1, &rdfs, nullptr, nullptr, &tv);
|
||||
if (ret < 0) {
|
||||
int err = errno;
|
||||
if (err != EINTR) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN select error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (ret == 0) {
|
||||
return false; // timeout, normal
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t bytes_read = ::read(fd, frame, sizeof(struct can_frame));
|
||||
if (bytes_read < 0) {
|
||||
int err = errno;
|
||||
if (err != EAGAIN && err != EWOULDBLOCK) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000,
|
||||
"CAN read error (fd=%d): %s", fd, strerror(err));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return (bytes_read == sizeof(struct can_frame));
|
||||
}
|
||||
|
||||
bool HardwareBridgeNode::reinitCan(const std::string& ifname, int& fd, int& error_count)
|
||||
{
|
||||
RCLCPP_WARN(get_logger(), "Attempting to reinitialize CAN interface: %s", ifname.c_str());
|
||||
if (fd >= 0) {
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
bool success = initCan(ifname, fd);
|
||||
if (success) {
|
||||
error_count = 0;
|
||||
RCLCPP_INFO(get_logger(), "CAN interface %s reinitialized successfully.", ifname.c_str());
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
} // namespace sim2real_hw
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_hw::HardwareBridgeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_interfaces)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/RuntimeState.msg"
|
||||
"msg/RuntimeTarget.msg"
|
||||
DEPENDENCIES builtin_interfaces std_msgs
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@@ -0,0 +1,30 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] joint_pos
|
||||
float32[16] joint_vel
|
||||
float32[16] joint_torque
|
||||
|
||||
float32[3] imu_gyro
|
||||
float32[3] imu_accel
|
||||
float32[4] quat_wxyz
|
||||
float32[3] projected_gravity
|
||||
|
||||
float32 imu_age_ms
|
||||
bool imu_fresh
|
||||
|
||||
float32 odom_age_ms
|
||||
bool odom_fresh
|
||||
float32[3] odom_pos
|
||||
float32[4] odom_quat_wxyz
|
||||
float32[3] odom_linear_vel
|
||||
float32[3] odom_angular_vel
|
||||
float32[3] odom_local_pos
|
||||
float32 odom_local_yaw
|
||||
|
||||
uint32 fresh_count
|
||||
uint32 holdover_count
|
||||
uint32 stale_max
|
||||
uint32[16] update_counts
|
||||
|
||||
string source
|
||||
@@ -0,0 +1,15 @@
|
||||
builtin_interfaces/Time stamp
|
||||
uint32 sequence
|
||||
|
||||
float32[16] target
|
||||
float32[16] raw_action
|
||||
float32[16] scaled_action
|
||||
float32[3] command
|
||||
float32[3] raw_command
|
||||
|
||||
bool zero_command
|
||||
bool runtime_released
|
||||
float32 release_alpha
|
||||
float32 target_age_ms
|
||||
|
||||
string target_source
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_interfaces</name>
|
||||
<version>0.1.0</version>
|
||||
<description>ROS 2 interfaces for the sim2real wheel-leg runtime.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
|
||||
<depend>builtin_interfaces</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(sim2real_nav2)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY
|
||||
config
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,232 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
odom_frame_id: "odom"
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: -1.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
transform_tolerance: 1.0
|
||||
update_min_d: 0.25
|
||||
update_min_a: 0.2
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: "scan"
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_frame: odom
|
||||
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
|
||||
plugin_lib_names:
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_recompute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node
|
||||
- nav2_recovery_node
|
||||
- nav2_pipeline_sequence_node
|
||||
- nav2_round_robin_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 10.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.001
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
prune_plan: true
|
||||
prune_distance: 1.0
|
||||
debug_trajectory_details: false
|
||||
trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
|
||||
velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator"
|
||||
critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
|
||||
|
||||
# DWB Velocity parameters matching Units
|
||||
min_vel_x: 0.0
|
||||
max_vel_x: 0.6
|
||||
min_vel_y: 0.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 2.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.6
|
||||
min_speed_theta: 0.0
|
||||
|
||||
# DWB Acceleration parameters matching Units
|
||||
acc_lim_x: 15.0
|
||||
acc_lim_y: 15.0
|
||||
acc_lim_theta: 12.0
|
||||
decel_lim_x: -15.0
|
||||
decel_lim_y: -15.0
|
||||
decel_lim_theta: -12.0
|
||||
|
||||
# Critics tuning
|
||||
ObstacleFootprint.scale: 0.2
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: false
|
||||
planner_plugins: ["GridTransition"]
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors::Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors::Backup"
|
||||
wait:
|
||||
plugin: "nav2_behaviors::Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 1.0
|
||||
update_frequency: 1.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 30
|
||||
height: 30
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.6
|
||||
cost_scaling_factor: 4.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 5.0
|
||||
update_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 4
|
||||
height: 4
|
||||
resolution: 0.05
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.4
|
||||
cost_scaling_factor: 4.0
|
||||
@@ -0,0 +1,146 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directories
|
||||
my_share_dir = get_package_share_directory('sim2real_nav2')
|
||||
|
||||
# Declare launch configuration variables
|
||||
params_file_arg = DeclareLaunchArgument(
|
||||
'params_file',
|
||||
default_value=os.path.join(my_share_dir, 'config', 'nav2_params.yaml'),
|
||||
description='Full path to the ROS2 parameters file to use for all launched nodes'
|
||||
)
|
||||
|
||||
params_file = LaunchConfiguration('params_file')
|
||||
|
||||
# Define Nav2 lifecycle nodes to run
|
||||
lifecycle_nodes = ['controller_server', 'planner_server', 'behavior_server', 'bt_navigator',
|
||||
'global_costmap', 'local_costmap', 'amcl']
|
||||
|
||||
# Controller server node
|
||||
controller_server_node = Node(
|
||||
package='nav2_controller',
|
||||
executable='controller_server',
|
||||
name='controller_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Planner server node
|
||||
planner_server_node = Node(
|
||||
package='nav2_planner',
|
||||
executable='planner_server',
|
||||
name='planner_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Behavior server node (called recovery_server in Galactic, behavior_server in Humble)
|
||||
behavior_server_node = Node(
|
||||
package='nav2_behaviors',
|
||||
executable='behavior_server',
|
||||
name='behavior_server',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# BT Navigator node
|
||||
bt_navigator_node = Node(
|
||||
package='nav2_bt_navigator',
|
||||
executable='bt_navigator',
|
||||
name='bt_navigator',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Global costmap node
|
||||
global_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='global_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# Local costmap node
|
||||
local_costmap_node = Node(
|
||||
package='nav2_costmap_2d',
|
||||
executable='nav2_costmap_2d',
|
||||
name='local_costmap',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# AMCL node (Adaptive Monte Carlo Localization), now receives /scan from pointcloud_to_laserscan
|
||||
amcl_node = Node(
|
||||
package='nav2_amcl',
|
||||
executable='amcl',
|
||||
name='amcl',
|
||||
output='screen',
|
||||
parameters=[params_file]
|
||||
)
|
||||
|
||||
# PointCloud2 to LaserScan converter (AMCL needs LaserScan, LiDAR publishes PointCloud2)
|
||||
pointcloud_to_laserscan_node = Node(
|
||||
package='pointcloud_to_laserscan',
|
||||
executable='pointcloud_to_laserscan_node',
|
||||
name='pointcloud_to_laserscan',
|
||||
output='screen',
|
||||
remappings=[
|
||||
('cloud_in', '/odin1/cloud_slam'),
|
||||
('scan', '/scan')
|
||||
],
|
||||
parameters=[{
|
||||
'target_frame': 'base_link',
|
||||
'transform_tolerance': 0.01,
|
||||
'min_height': 0.05,
|
||||
'max_height': 2.0,
|
||||
'angle_min': -3.14159,
|
||||
'angle_max': 3.14159,
|
||||
'angle_increment': 0.0087, # ~0.5 degrees
|
||||
'scan_time': 0.1,
|
||||
'range_min': 0.1,
|
||||
'range_max': 10.0,
|
||||
'use_inf': True,
|
||||
'inf_epsilon': 1.0,
|
||||
'concurrency_level': 1
|
||||
}]
|
||||
)
|
||||
|
||||
# Lifecycle manager node to transition Nav2 nodes to ACTIVE state
|
||||
lifecycle_manager_node = Node(
|
||||
package='nav2_lifecycle_manager',
|
||||
executable='lifecycle_manager',
|
||||
name='lifecycle_manager_navigation',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': False,
|
||||
'autostart': True,
|
||||
'node_names': lifecycle_nodes
|
||||
}]
|
||||
)
|
||||
|
||||
# Create launch description
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Set stdout line buffering
|
||||
ld.add_action(SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'))
|
||||
|
||||
# Add actions
|
||||
ld.add_action(params_file_arg)
|
||||
ld.add_action(controller_server_node)
|
||||
ld.add_action(planner_server_node)
|
||||
ld.add_action(behavior_server_node)
|
||||
ld.add_action(bt_navigator_node)
|
||||
ld.add_action(global_costmap_node)
|
||||
ld.add_action(local_costmap_node)
|
||||
ld.add_action(amcl_node)
|
||||
ld.add_action(pointcloud_to_laserscan_node)
|
||||
ld.add_action(lifecycle_manager_node)
|
||||
|
||||
return ld
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_nav2</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 Nav2 configuration package for legged-wheeled quadruped</description>
|
||||
<maintainer email="31560@todo.todo">USER</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
<exec_depend>navigation2</exec_depend>
|
||||
<exec_depend>pointcloud_to_laserscan</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,99 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_runtime)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(sim2real_common REQUIRED)
|
||||
find_package(sim2real_interfaces REQUIRED)
|
||||
|
||||
# Search for ONNX Runtime headers and library
|
||||
find_path(ONNXRUNTIME_INCLUDE_DIR onnxruntime_cxx_api.h
|
||||
PATHS
|
||||
/usr/include
|
||||
/usr/include/onnxruntime
|
||||
/usr/local/include
|
||||
/usr/local/include/onnxruntime
|
||||
/opt/onnxruntime/include
|
||||
)
|
||||
|
||||
find_library(ONNXRUNTIME_LIBRARY NAMES onnxruntime
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/lib/x86_64-linux-gnu
|
||||
/usr/lib/aarch64-linux-gnu
|
||||
/usr/local/lib
|
||||
/opt/onnxruntime/lib
|
||||
)
|
||||
|
||||
get_filename_component(ONNXRUNTIME_LIBRARY_DIR ${ONNXRUNTIME_LIBRARY} DIRECTORY)
|
||||
|
||||
if(NOT ONNXRUNTIME_INCLUDE_DIR OR NOT ONNXRUNTIME_LIBRARY)
|
||||
message(FATAL_ERROR "ONNX Runtime not found! Please install it or specify include/library paths.")
|
||||
endif()
|
||||
|
||||
add_executable(sim2real_runtime_node
|
||||
src/policy_runtime_node.cpp
|
||||
)
|
||||
|
||||
add_executable(odom_relay_node
|
||||
src/odom_relay_node.cpp
|
||||
)
|
||||
|
||||
target_include_directories(sim2real_runtime_node PRIVATE
|
||||
include
|
||||
${ONNXRUNTIME_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
target_include_directories(odom_relay_node PRIVATE include)
|
||||
|
||||
target_link_libraries(sim2real_runtime_node
|
||||
${ONNXRUNTIME_LIBRARY}
|
||||
)
|
||||
|
||||
set_target_properties(sim2real_runtime_node PROPERTIES
|
||||
BUILD_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
INSTALL_RPATH "${ONNXRUNTIME_LIBRARY_DIR}"
|
||||
)
|
||||
|
||||
target_compile_features(sim2real_runtime_node PRIVATE cxx_std_17)
|
||||
|
||||
ament_target_dependencies(sim2real_runtime_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
std_msgs
|
||||
tf2_ros
|
||||
sim2real_common
|
||||
sim2real_interfaces
|
||||
)
|
||||
|
||||
ament_target_dependencies(odom_relay_node
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
rclcpp
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS sim2real_runtime_node odom_relay_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
PROGRAMS
|
||||
src/remote_uart_node.py
|
||||
src/cmd_mux_node.py
|
||||
src/web_udp_bridge_node.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
/// Subscribes to odin_ros_driver's odometry (e.g. /odin1/odometry),
|
||||
/// remaps child_frame_id to "base_link", republishes on /odom,
|
||||
/// and broadcasts the odom → base_link TF.
|
||||
class OdomRelayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
OdomRelayNode();
|
||||
|
||||
private:
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
std::string odom_input_topic_;
|
||||
std::string odom_output_topic_;
|
||||
std::string base_frame_;
|
||||
bool publish_tf_;
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
+124
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <mutex>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "std_msgs/msg/bool.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/stand_balance_controller.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
// ONNXRuntime C++ API
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
class PolicyRuntimeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
PolicyRuntimeNode();
|
||||
|
||||
private:
|
||||
void onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg);
|
||||
void onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
|
||||
void applyCmdVel(float vx, float vy, float vyaw);
|
||||
void onPolicyLoop();
|
||||
|
||||
std::array<float, 53> buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const;
|
||||
|
||||
std::array<float, 16> runPolicy(const std::array<float, 53> & obs);
|
||||
bool isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const;
|
||||
bool isCommandActive(const std::array<float, 3> & cmd) const;
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_stamped_sub_;
|
||||
rclcpp::TimerBase::SharedPtr policy_timer_;
|
||||
|
||||
std::mutex mutex_;
|
||||
sim2real_interfaces::msg::RuntimeState latest_state_;
|
||||
bool has_state_{false};
|
||||
std::chrono::steady_clock::time_point last_state_recv_time_{};
|
||||
std::array<float, 3> cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 3> raw_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
std::array<float, 16> last_actions_{};
|
||||
std::uint32_t sequence_{0};
|
||||
|
||||
// Startup State Machine
|
||||
enum class StartupState {
|
||||
BOOT_HOLD,
|
||||
STARTUP_SOFT_HOLD,
|
||||
STARTUP_TRANSITION,
|
||||
STARTUP_HOLD_AFTER,
|
||||
RUNTIME
|
||||
};
|
||||
StartupState startup_state_{StartupState::BOOT_HOLD};
|
||||
std::array<float, 16> start_pose_{};
|
||||
std::array<float, 16> startup_delta_{};
|
||||
rclcpp::Time state_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double transition_time_{4.0};
|
||||
double hold_time_{1.0};
|
||||
std::unique_ptr<sim2real_common::StandBalanceController> stand_balance_;
|
||||
|
||||
// ONNX Runtime members
|
||||
std::string model_path_{"policies/model_rough.onnx"};
|
||||
bool use_cuda_{false}; // enable CUDA Execution Provider on Orin Nano
|
||||
std::unique_ptr<Ort::Env> env_;
|
||||
std::unique_ptr<Ort::Session> session_;
|
||||
std::unique_ptr<Ort::MemoryInfo> memory_info_;
|
||||
|
||||
std::vector<std::string> input_names_str_;
|
||||
std::vector<std::string> output_names_str_;
|
||||
std::vector<const char*> input_names_char_;
|
||||
std::vector<const char*> output_names_char_;
|
||||
|
||||
std::vector<std::int64_t> input_shape_;
|
||||
std::vector<std::int64_t> output_shape_;
|
||||
|
||||
// Command filter and release states
|
||||
std::array<float, 3> filtered_cmd_{{0.0f, 0.0f, 0.0f}};
|
||||
float release_alpha_{0.0f};
|
||||
float command_release_s_{0.35f};
|
||||
float release_command_hold_s_{0.12f};
|
||||
float release_posture_max_err_{0.35f};
|
||||
float release_target_blend_s_{0.30f};
|
||||
float clip_obs_{100.0f};
|
||||
bool hold_zero_command_pose_{true};
|
||||
bool enable_zero_cmd_suppression_{true};
|
||||
bool require_active_command_to_release_{true};
|
||||
bool zero_cmd_use_yaw_rate_{false};
|
||||
bool runtime_released_{false};
|
||||
float release_active_time_{0.0f};
|
||||
float zero_cmd_lin_thresh_{0.05f};
|
||||
float zero_cmd_yaw_thresh_{0.05f};
|
||||
float zero_yaw_rate_thresh_{0.10f};
|
||||
|
||||
// E-stop and Safety variables
|
||||
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr estop_sub_;
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
std::unique_ptr<sim2real_common::SafetyMonitor> safety_monitor_;
|
||||
std::unique_ptr<sim2real_common::RuntimeGuard> runtime_guard_;
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_runtime</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Policy runtime node for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>sim2real_common</depend>
|
||||
<depend>sim2real_interfaces</depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,190 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from enum import Enum
|
||||
from typing import Optional
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
|
||||
class ControlMode(str, Enum):
|
||||
DISABLED = "DISABLED"
|
||||
REMOTE = "REMOTE"
|
||||
WEB = "WEB"
|
||||
NAV = "NAV"
|
||||
|
||||
|
||||
class CmdMuxNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_cmd_mux_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.default_mode = str(self.declare_parameter("cmd_mux_default_mode", "REMOTE").value).upper()
|
||||
self.output_hz = float(self.declare_parameter("cmd_mux_output_hz", 50.0).value)
|
||||
self.remote_timeout_ms = float(self.declare_parameter("cmd_mux_remote_timeout_ms", 250.0).value)
|
||||
self.web_timeout_ms = float(self.declare_parameter("cmd_mux_web_timeout_ms", 300.0).value)
|
||||
self.nav_timeout_ms = float(self.declare_parameter("cmd_mux_nav_timeout_ms", 500.0).value)
|
||||
self.max_vx = float(self.declare_parameter("cmd_mux_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("cmd_mux_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("cmd_mux_max_yaw_rate", 0.5).value)
|
||||
self.max_vx_acc = float(self.declare_parameter("cmd_mux_max_vx_acc", 1.0).value)
|
||||
self.max_vy_acc = float(self.declare_parameter("cmd_mux_max_vy_acc", 1.0).value)
|
||||
self.max_yaw_acc = float(self.declare_parameter("cmd_mux_max_yaw_acc", 1.5).value)
|
||||
|
||||
self.mode = self.parse_mode(self.default_mode)
|
||||
self.estop = False
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
|
||||
self.latest_remote = Twist()
|
||||
self.latest_web = Twist()
|
||||
self.latest_nav = Twist()
|
||||
self.remote_stamp: Optional[rclpy.time.Time] = None
|
||||
self.web_stamp: Optional[rclpy.time.Time] = None
|
||||
self.nav_stamp: Optional[rclpy.time.Time] = None
|
||||
self.last_output = Twist()
|
||||
self.last_pub_time = self.get_clock().now()
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode_state", 10)
|
||||
self.status_pub = self.create_publisher(String, "control/mux_status", 10)
|
||||
|
||||
self.create_subscription(Twist, "cmd_vel_remote", self.on_remote, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_web", self.on_web, 10)
|
||||
self.create_subscription(Twist, "cmd_vel_nav", self.on_nav, 10)
|
||||
self.create_subscription(String, "control/mode", self.on_mode, 10)
|
||||
self.create_subscription(Bool, "remote/enabled", self.on_remote_enabled, 10)
|
||||
self.create_subscription(Bool, "web/enabled", self.on_web_enabled, 10)
|
||||
self.create_subscription(Bool, "nav/enabled", self.on_nav_enabled, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
|
||||
period = 1.0 / self.output_hz if self.output_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
self.get_logger().info(f"Command mux started in mode {self.mode.value}")
|
||||
|
||||
def parse_mode(self, value: str) -> ControlMode:
|
||||
try:
|
||||
return ControlMode(value.upper())
|
||||
except ValueError:
|
||||
self.get_logger().warn(f"Unknown control mode '{value}', using DISABLED")
|
||||
return ControlMode.DISABLED
|
||||
|
||||
def on_remote(self, msg: Twist) -> None:
|
||||
self.latest_remote = msg
|
||||
self.remote_stamp = self.get_clock().now()
|
||||
|
||||
def on_web(self, msg: Twist) -> None:
|
||||
self.latest_web = msg
|
||||
self.web_stamp = self.get_clock().now()
|
||||
|
||||
def on_nav(self, msg: Twist) -> None:
|
||||
self.latest_nav = msg
|
||||
self.nav_stamp = self.get_clock().now()
|
||||
|
||||
def on_mode(self, msg: String) -> None:
|
||||
new_mode = self.parse_mode(msg.data)
|
||||
if new_mode != self.mode:
|
||||
self.mode = new_mode
|
||||
self.remote_enabled = self.mode == ControlMode.REMOTE
|
||||
self.web_enabled = self.mode == ControlMode.WEB
|
||||
self.nav_enabled = self.mode == ControlMode.NAV
|
||||
self.get_logger().info(f"Control mode changed to {self.mode.value}")
|
||||
|
||||
def on_remote_enabled(self, msg: Bool) -> None:
|
||||
self.remote_enabled = bool(msg.data)
|
||||
if self.remote_enabled:
|
||||
self.mode = ControlMode.REMOTE
|
||||
|
||||
def on_web_enabled(self, msg: Bool) -> None:
|
||||
self.web_enabled = bool(msg.data)
|
||||
if self.web_enabled:
|
||||
self.mode = ControlMode.WEB
|
||||
|
||||
def on_nav_enabled(self, msg: Bool) -> None:
|
||||
self.nav_enabled = bool(msg.data)
|
||||
if self.nav_enabled:
|
||||
self.mode = ControlMode.NAV
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
if self.estop:
|
||||
self.mode = ControlMode.DISABLED
|
||||
|
||||
def on_timer(self) -> None:
|
||||
now = self.get_clock().now()
|
||||
target = Twist()
|
||||
source = "zero"
|
||||
|
||||
if not self.estop:
|
||||
if self.mode == ControlMode.REMOTE and self.remote_enabled and self.is_fresh(self.remote_stamp, self.remote_timeout_ms, now):
|
||||
target = self.latest_remote
|
||||
source = "remote"
|
||||
elif self.mode == ControlMode.WEB and self.web_enabled and self.is_fresh(self.web_stamp, self.web_timeout_ms, now):
|
||||
target = self.latest_web
|
||||
source = "web"
|
||||
elif self.mode == ControlMode.NAV and self.nav_enabled and self.is_fresh(self.nav_stamp, self.nav_timeout_ms, now):
|
||||
target = self.latest_nav
|
||||
source = "nav"
|
||||
|
||||
target = self.limit_twist(target)
|
||||
target = self.accel_limit(target, now)
|
||||
self.cmd_pub.publish(target)
|
||||
self.mode_pub.publish(String(data=self.mode.value))
|
||||
self.status_pub.publish(String(data=f"mode={self.mode.value},source={source},estop={self.estop}"))
|
||||
|
||||
def is_fresh(self, stamp: Optional[rclpy.time.Time], timeout_ms: float, now: rclpy.time.Time) -> bool:
|
||||
if stamp is None:
|
||||
return False
|
||||
age_ms = (now - stamp).nanoseconds / 1.0e6
|
||||
return age_ms <= timeout_ms
|
||||
|
||||
def limit_twist(self, msg: Twist) -> Twist:
|
||||
out = Twist()
|
||||
out.linear.x = self.clamp(msg.linear.x, -self.max_vx, self.max_vx)
|
||||
out.linear.y = self.clamp(msg.linear.y, -self.max_vy, self.max_vy)
|
||||
out.angular.z = self.clamp(msg.angular.z, -self.max_yaw, self.max_yaw)
|
||||
return out
|
||||
|
||||
def accel_limit(self, target: Twist, now: rclpy.time.Time) -> Twist:
|
||||
dt = max((now - self.last_pub_time).nanoseconds / 1.0e9, 1.0e-3)
|
||||
out = Twist()
|
||||
out.linear.x = self.step(self.last_output.linear.x, target.linear.x, self.max_vx_acc * dt)
|
||||
out.linear.y = self.step(self.last_output.linear.y, target.linear.y, self.max_vy_acc * dt)
|
||||
out.angular.z = self.step(self.last_output.angular.z, target.angular.z, self.max_yaw_acc * dt)
|
||||
self.last_output = out
|
||||
self.last_pub_time = now
|
||||
return out
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, float(value)))
|
||||
|
||||
@staticmethod
|
||||
def step(current: float, target: float, max_delta: float) -> float:
|
||||
delta = target - current
|
||||
if delta > max_delta:
|
||||
return current + max_delta
|
||||
if delta < -max_delta:
|
||||
return current - max_delta
|
||||
return target
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = CmdMuxNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,60 @@
|
||||
#include "sim2real_runtime/odom_relay_node.hpp"
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
OdomRelayNode::OdomRelayNode()
|
||||
: Node("odom_relay_node")
|
||||
{
|
||||
odom_input_topic_ = declare_parameter<std::string>("odom_input_topic", "/odin1/odometry");
|
||||
odom_output_topic_ = declare_parameter<std::string>("odom_output_topic", "/odom");
|
||||
base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
|
||||
publish_tf_ = declare_parameter<bool>("publish_tf", true);
|
||||
|
||||
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_input_topic_, 10,
|
||||
std::bind(&OdomRelayNode::onOdom, this, std::placeholders::_1));
|
||||
|
||||
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(odom_output_topic_, 10);
|
||||
|
||||
if (publish_tf_) {
|
||||
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"Odom relay: %s -> %s (base_frame=%s, publish_tf=%s)",
|
||||
odom_input_topic_.c_str(), odom_output_topic_.c_str(),
|
||||
base_frame_.c_str(), publish_tf_ ? "true" : "false");
|
||||
}
|
||||
|
||||
void OdomRelayNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
// Remap child_frame_id and republish
|
||||
auto out_msg = *msg;
|
||||
out_msg.header.frame_id = "odom";
|
||||
out_msg.child_frame_id = base_frame_;
|
||||
odom_pub_->publish(out_msg);
|
||||
|
||||
// Broadcast TF: odom → base_link
|
||||
if (publish_tf_ && tf_broadcaster_) {
|
||||
geometry_msgs::msg::TransformStamped tf;
|
||||
tf.header.stamp = msg->header.stamp;
|
||||
tf.header.frame_id = "odom";
|
||||
tf.child_frame_id = base_frame_;
|
||||
tf.transform.translation.x = msg->pose.pose.position.x;
|
||||
tf.transform.translation.y = msg->pose.pose.position.y;
|
||||
tf.transform.translation.z = msg->pose.pose.position.z;
|
||||
tf.transform.rotation = msg->pose.pose.orientation;
|
||||
tf_broadcaster_->sendTransform(tf);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::OdomRelayNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,583 @@
|
||||
#include "sim2real_runtime/policy_runtime_node.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace sim2real_runtime
|
||||
{
|
||||
|
||||
// Named constants for timing and command filtering
|
||||
constexpr float kCmdAccelLimitXY = 0.02f; // m/s per step (at 50Hz)
|
||||
constexpr float kCmdAccelLimitYaw = 0.03f; // rad/s per step (at 50Hz)
|
||||
constexpr float kPolicyDt = 0.02f; // policy loop period (50Hz)
|
||||
|
||||
PolicyRuntimeNode::PolicyRuntimeNode()
|
||||
: Node("sim2real_runtime_node")
|
||||
{
|
||||
// 1. Declare and get parameters
|
||||
model_path_ = declare_parameter<std::string>("model_path", "policies/model_rough.onnx");
|
||||
use_cuda_ = declare_parameter<bool>("use_cuda", false); // enable CUDA EP on Orin Nano
|
||||
|
||||
// Safety parameters
|
||||
safety_enabled_ = declare_parameter<bool>("safety_enabled", true);
|
||||
double max_target_offset = declare_parameter<double>("max_target_offset", 0.6);
|
||||
double hard_target_offset = declare_parameter<double>("hard_target_offset", 1.2);
|
||||
double max_ang_vel = declare_parameter<double>("max_ang_vel", 10.0);
|
||||
double max_tilt_z = declare_parameter<double>("max_tilt_z", -0.3);
|
||||
int clip_to_brake = declare_parameter<int>("clip_to_brake", 0);
|
||||
double imu_age_warn_ms = declare_parameter<double>("imu_age_warn_ms", 60.0);
|
||||
double imu_age_stop_ms = declare_parameter<double>("imu_age_stop_ms", 200.0);
|
||||
|
||||
command_release_s_ = static_cast<float>(declare_parameter<double>("command_release_s", 0.35));
|
||||
release_command_hold_s_ = static_cast<float>(declare_parameter<double>("release_command_hold_s", 0.12));
|
||||
release_posture_max_err_ = static_cast<float>(declare_parameter<double>("release_posture_max_err", 0.35));
|
||||
release_target_blend_s_ = static_cast<float>(declare_parameter<double>("release_target_blend_s", 0.30));
|
||||
clip_obs_ = static_cast<float>(declare_parameter<double>("clip_obs", 100.0));
|
||||
hold_zero_command_pose_ = declare_parameter<bool>("hold_zero_command_pose", true);
|
||||
enable_zero_cmd_suppression_ = declare_parameter<bool>("enable_zero_cmd_suppression", true);
|
||||
require_active_command_to_release_ = declare_parameter<bool>("require_active_command_to_release", true);
|
||||
zero_cmd_use_yaw_rate_ = declare_parameter<bool>("zero_cmd_use_yaw_rate", true);
|
||||
runtime_released_ = !require_active_command_to_release_;
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Loading ONNX policy model from: %s", model_path_.c_str());
|
||||
|
||||
// Initialize StandBalanceController
|
||||
stand_balance_ = std::make_unique<sim2real_common::StandBalanceController>(0.02);
|
||||
|
||||
// Initialize SafetyMonitor and RuntimeGuard
|
||||
safety_monitor_ = std::make_unique<sim2real_common::SafetyMonitor>(
|
||||
static_cast<float>(max_target_offset),
|
||||
static_cast<float>(max_ang_vel),
|
||||
static_cast<float>(max_tilt_z),
|
||||
clip_to_brake,
|
||||
static_cast<float>(hard_target_offset)
|
||||
);
|
||||
|
||||
runtime_guard_ = std::make_unique<sim2real_common::RuntimeGuard>(
|
||||
static_cast<float>(max_ang_vel + 2.0),
|
||||
static_cast<float>(max_tilt_z),
|
||||
static_cast<float>(imu_age_warn_ms),
|
||||
static_cast<float>(imu_age_stop_ms)
|
||||
);
|
||||
|
||||
// 2. Initialize Ort C++ environment
|
||||
try {
|
||||
env_ = std::make_unique<Ort::Env>(ORT_LOGGING_LEVEL_WARNING, "sim2real_onnx_env");
|
||||
|
||||
Ort::SessionOptions session_options;
|
||||
// single-thread ORIN optimization to prevent thread scheduling jitter
|
||||
session_options.SetIntraOpNumThreads(1);
|
||||
session_options.SetInterOpNumThreads(1);
|
||||
session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
||||
|
||||
// CUDA Execution Provider (Orin Nano GPU acceleration)
|
||||
if (use_cuda_) {
|
||||
try {
|
||||
OrtCUDAProviderOptions cuda_opts{};
|
||||
cuda_opts.device_id = 0;
|
||||
// enable_cuda_graph: false for single-inference RL policy (avoids overhead)
|
||||
session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
||||
RCLCPP_INFO(get_logger(), "CUDA Execution Provider enabled (device 0)");
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_WARN(get_logger(),
|
||||
"CUDA EP init failed (ONNX Runtime built without CUDA?): %s. Falling back to CPU.",
|
||||
e.what());
|
||||
use_cuda_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
session_ = std::make_unique<Ort::Session>(*env_, model_path_.c_str(), session_options);
|
||||
memory_info_ = std::make_unique<Ort::MemoryInfo>(Ort::MemoryInfo::CreateCpu(OrtDeviceAllocator, OrtMemTypeCPU));
|
||||
|
||||
// Get input/output nodes names and shapes
|
||||
Ort::AllocatorWithDefaultOptions allocator;
|
||||
|
||||
std::size_t num_inputs = session_->GetInputCount();
|
||||
for (std::size_t i = 0; i < num_inputs; ++i) {
|
||||
auto name = session_->GetInputNameAllocated(i, allocator);
|
||||
input_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : input_names_str_) {
|
||||
input_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
std::size_t num_outputs = session_->GetOutputCount();
|
||||
for (std::size_t i = 0; i < num_outputs; ++i) {
|
||||
auto name = session_->GetOutputNameAllocated(i, allocator);
|
||||
output_names_str_.push_back(std::string(name.get()));
|
||||
}
|
||||
for (const auto& name : output_names_str_) {
|
||||
output_names_char_.push_back(name.c_str());
|
||||
}
|
||||
|
||||
auto input_type_info = session_->GetInputTypeInfo(0);
|
||||
auto input_tensor_info = input_type_info.GetTensorTypeAndShapeInfo();
|
||||
input_shape_ = input_tensor_info.GetShape();
|
||||
if (input_shape_[0] < 0) {
|
||||
input_shape_[0] = 1;
|
||||
}
|
||||
|
||||
auto output_type_info = session_->GetOutputTypeInfo(0);
|
||||
auto output_tensor_info = output_type_info.GetTensorTypeAndShapeInfo();
|
||||
output_shape_ = output_tensor_info.GetShape();
|
||||
if (output_shape_[0] < 0) {
|
||||
output_shape_[0] = 1;
|
||||
}
|
||||
|
||||
// Validate output shape matches expected action dimension
|
||||
if (output_shape_.size() < 2 || output_shape_[1] != static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim)) {
|
||||
RCLCPP_FATAL(get_logger(),
|
||||
"ONNX model output dimension mismatch! Expected %ld, got %ld. Wrong model?",
|
||||
static_cast<std::int64_t>(sim2real_common::DeploymentContract::kActionDim),
|
||||
output_shape_.size() >= 2 ? output_shape_[1] : -1);
|
||||
throw std::runtime_error("ONNX model output shape mismatch");
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Successfully loaded ONNX policy model. Input shape: [%ld, %ld], Output shape: [%ld, %ld]",
|
||||
input_shape_[0], input_shape_[1], output_shape_[0], output_shape_[1]);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_FATAL(get_logger(), "Failed to load ONNX model: %s", e.what());
|
||||
throw;
|
||||
}
|
||||
|
||||
// 3. Create publishers and subscriptions
|
||||
target_pub_ = create_publisher<sim2real_interfaces::msg::RuntimeTarget>("runtime/target", 10);
|
||||
state_sub_ = create_subscription<sim2real_interfaces::msg::RuntimeState>(
|
||||
"runtime/state", 10,
|
||||
std::bind(&PolicyRuntimeNode::onState, this, std::placeholders::_1));
|
||||
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
|
||||
"cmd_vel", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVel, this, std::placeholders::_1));
|
||||
cmd_stamped_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
|
||||
"cmd_vel_stamped", 10,
|
||||
std::bind(&PolicyRuntimeNode::onCmdVelStamped, this, std::placeholders::_1));
|
||||
estop_sub_ = create_subscription<std_msgs::msg::Bool>(
|
||||
"/safety/estop", 10,
|
||||
std::bind(&PolicyRuntimeNode::onEstop, this, std::placeholders::_1));
|
||||
|
||||
// 4. Timer at 50Hz (20ms)
|
||||
policy_timer_ = create_wall_timer(20ms, std::bind(&PolicyRuntimeNode::onPolicyLoop, this));
|
||||
|
||||
last_actions_.fill(0.0f);
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onState(const sim2real_interfaces::msg::RuntimeState::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
latest_state_ = *msg;
|
||||
has_state_ = true;
|
||||
last_state_recv_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::applyCmdVel(float vx, float vy, float vyaw)
|
||||
{
|
||||
// Velocity saturation limits (consistent with training domain)
|
||||
constexpr float kMaxLinVelX = 0.8f; // m/s
|
||||
constexpr float kMaxLinVelY = 0.3f; // m/s
|
||||
constexpr float kMaxAngVelZ = 0.5f; // rad/s
|
||||
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
raw_cmd_[0] = std::clamp(vx, -kMaxLinVelX, kMaxLinVelX);
|
||||
raw_cmd_[1] = std::clamp(vy, -kMaxLinVelY, kMaxLinVelY);
|
||||
raw_cmd_[2] = std::clamp(vyaw, -kMaxAngVelZ, kMaxAngVelZ);
|
||||
cmd_ = raw_cmd_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVel(const geometry_msgs::msg::Twist::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->linear.x),
|
||||
static_cast<float>(msg->linear.y),
|
||||
static_cast<float>(msg->angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onCmdVelStamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
|
||||
{
|
||||
applyCmdVel(
|
||||
static_cast<float>(msg->twist.linear.x),
|
||||
static_cast<float>(msg->twist.linear.y),
|
||||
static_cast<float>(msg->twist.angular.z));
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onEstop(const std_msgs::msg::Bool::SharedPtr msg)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
estop_triggered_ = msg->data;
|
||||
if (estop_triggered_) {
|
||||
RCLCPP_WARN(get_logger(), "!!! E-stop triggered via /safety/estop !!!");
|
||||
} else {
|
||||
RCLCPP_INFO(get_logger(), "E-stop reset.");
|
||||
}
|
||||
}
|
||||
|
||||
std::array<float, 53> PolicyRuntimeNode::buildObservation(
|
||||
const sim2real_interfaces::msg::RuntimeState & state,
|
||||
const std::array<float, 3> & cmd,
|
||||
const std::array<float, 16> & last_actions) const
|
||||
{
|
||||
std::array<float, 53> obs{};
|
||||
std::size_t cursor = 0;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.imu_gyro[i] * 0.25f;
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
obs[cursor++] = state.projected_gravity[i];
|
||||
}
|
||||
for (float v : cmd) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_pos[i] - sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
}
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (std::size_t i = 12; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
obs[cursor++] = state.joint_vel[i] * 0.05f;
|
||||
}
|
||||
for (float v : last_actions) {
|
||||
obs[cursor++] = v;
|
||||
}
|
||||
|
||||
// Clip observations values to ±clip_obs_
|
||||
if (clip_obs_ > 0.0f) {
|
||||
for (float & v : obs) {
|
||||
v = std::clamp(v, -clip_obs_, clip_obs_);
|
||||
}
|
||||
}
|
||||
|
||||
return obs;
|
||||
}
|
||||
|
||||
std::array<float, 16> PolicyRuntimeNode::runPolicy(const std::array<float, 53> & obs)
|
||||
{
|
||||
std::array<float, 16> action{};
|
||||
try {
|
||||
auto input_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
const_cast<float*>(obs.data()),
|
||||
obs.size(),
|
||||
input_shape_.data(),
|
||||
input_shape_.size()
|
||||
);
|
||||
|
||||
auto output_tensor = Ort::Value::CreateTensor<float>(
|
||||
*memory_info_,
|
||||
action.data(),
|
||||
action.size(),
|
||||
output_shape_.data(),
|
||||
output_shape_.size()
|
||||
);
|
||||
|
||||
session_->Run(
|
||||
Ort::RunOptions{nullptr},
|
||||
input_names_char_.data(),
|
||||
&input_tensor,
|
||||
1,
|
||||
output_names_char_.data(),
|
||||
&output_tensor,
|
||||
1
|
||||
);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(get_logger(), "ONNX Runtime inference exception: %s", e.what());
|
||||
action.fill(0.0f);
|
||||
}
|
||||
|
||||
for (float& v : action) {
|
||||
v = std::clamp(v, -10.0f, 10.0f);
|
||||
}
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isZeroCommand(const std::array<float, 3> & cmd, const std::array<float, 3> & imu_gyro) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
const bool cmd_is_zero = planar_cmd < zero_cmd_lin_thresh_ && std::abs(cmd[2]) < zero_cmd_yaw_thresh_;
|
||||
if (!zero_cmd_use_yaw_rate_) {
|
||||
return cmd_is_zero;
|
||||
}
|
||||
return cmd_is_zero && std::abs(imu_gyro[2]) < zero_yaw_rate_thresh_;
|
||||
}
|
||||
|
||||
bool PolicyRuntimeNode::isCommandActive(const std::array<float, 3> & cmd) const
|
||||
{
|
||||
const float planar_cmd = std::sqrt(cmd[0] * cmd[0] + cmd[1] * cmd[1]);
|
||||
return planar_cmd >= zero_cmd_lin_thresh_ || std::abs(cmd[2]) >= zero_cmd_yaw_thresh_;
|
||||
}
|
||||
|
||||
void PolicyRuntimeNode::onPolicyLoop()
|
||||
{
|
||||
sim2real_interfaces::msg::RuntimeState state;
|
||||
std::array<float, 3> cmd{};
|
||||
std::array<float, 3> raw_cmd{};
|
||||
std::array<float, 16> last_actions{};
|
||||
bool estop_active = false;
|
||||
bool safety_active = false;
|
||||
double state_age_ms = 0.0;
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!has_state_) {
|
||||
return;
|
||||
}
|
||||
state = latest_state_;
|
||||
cmd = cmd_;
|
||||
raw_cmd = raw_cmd_;
|
||||
last_actions = last_actions_;
|
||||
estop_active = estop_triggered_;
|
||||
safety_active = safety_triggered_;
|
||||
if (last_state_recv_time_.time_since_epoch().count() != 0) {
|
||||
state_age_ms = std::chrono::duration<double, std::milli>(
|
||||
std::chrono::steady_clock::now() - last_state_recv_time_).count();
|
||||
}
|
||||
}
|
||||
|
||||
// 1) Run RuntimeGuard check
|
||||
if (safety_enabled_ && !safety_active) {
|
||||
std::vector<float> extra_vals;
|
||||
extra_vals.reserve(48);
|
||||
for (float v : state.joint_pos) extra_vals.push_back(v);
|
||||
for (float v : state.joint_vel) extra_vals.push_back(v);
|
||||
for (float v : last_actions) extra_vals.push_back(v);
|
||||
|
||||
const float effective_imu_age_ms = static_cast<float>(std::max(
|
||||
static_cast<double>(state.imu_age_ms), state_age_ms));
|
||||
|
||||
auto guard_decision = runtime_guard_->check(
|
||||
state.imu_gyro, state.projected_gravity, effective_imu_age_ms, estop_active, extra_vals);
|
||||
if (guard_decision.level == sim2real_common::GuardLevel::STOP) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_active = true;
|
||||
safety_reason_ = "Runtime Guard Stop: " + guard_decision.reason;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
} else if (guard_decision.level == sim2real_common::GuardLevel::WARN) {
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Guard Warning in Policy Runtime: %s", guard_decision.reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
if (safety_active) {
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.command = cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
target.target_age_ms = 0.0f;
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
sim2real_interfaces::msg::RuntimeTarget target;
|
||||
target.stamp = now();
|
||||
target.sequence = sequence_++;
|
||||
target.raw_command = raw_cmd;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
target.command = cmd;
|
||||
|
||||
const auto now_time = rclcpp::Time(target.stamp);
|
||||
|
||||
if (startup_state_ == StartupState::BOOT_HOLD) {
|
||||
// 1. Initial State Read
|
||||
start_pose_ = state.joint_pos;
|
||||
start_pose_[12] = start_pose_[13] = start_pose_[14] = start_pose_[15] = 0.0f; // Wheel starts at 0
|
||||
|
||||
// 2. Shortest periodic delta to stand pose
|
||||
float max_dev = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - start_pose_[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
startup_delta_[i] = delta;
|
||||
max_dev = std::max(max_dev, std::abs(delta));
|
||||
}
|
||||
startup_delta_[12] = startup_delta_[13] = startup_delta_[14] = startup_delta_[15] = 0.0f;
|
||||
|
||||
if (max_dev > 3.0f) {
|
||||
RCLCPP_WARN(get_logger(), "Measured joint dev too large (%f rad > 3.0 rad). Aborting standup transition.", max_dev);
|
||||
target.target = start_pose_;
|
||||
target.target_source = "boot_hold";
|
||||
target_pub_->publish(target);
|
||||
return;
|
||||
}
|
||||
|
||||
// Adapt transition time: min 2s, max 6s, 1.5s per rad
|
||||
transition_time_ = std::clamp(max_dev * 1.5, 2.0, 6.0);
|
||||
startup_state_ = StartupState::STARTUP_SOFT_HOLD;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence started. Starting dev: %f rad, transition time: %f s", max_dev, transition_time_);
|
||||
}
|
||||
|
||||
if (startup_state_ == StartupState::STARTUP_SOFT_HOLD) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
target.target = start_pose_;
|
||||
target.target_source = "startup_soft_hold";
|
||||
|
||||
if (elapsed >= 1.0) { // 1s soft hold
|
||||
startup_state_ = StartupState::STARTUP_TRANSITION;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Transitioning to stand pose...");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_TRANSITION) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
double phase = std::min(1.0, elapsed / transition_time_);
|
||||
|
||||
// Cosine blend interpolation
|
||||
double blend = 0.5 - 0.5 * std::cos(M_PI * phase);
|
||||
for (std::size_t i = 0; i < 16; ++i) {
|
||||
target.target[i] = start_pose_[i] + blend * startup_delta_[i];
|
||||
}
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (phase >= 1.0) {
|
||||
// Settle check
|
||||
float max_pos_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
float delta = sim2real_common::DeploymentContract::kDefaultDofPos[i] - state.joint_pos[i];
|
||||
delta = delta - 2.0f * static_cast<float>(M_PI) * std::floor((delta + static_cast<float>(M_PI)) / (2.0f * static_cast<float>(M_PI)));
|
||||
max_pos_err = std::max(max_pos_err, std::abs(delta));
|
||||
}
|
||||
float max_vel_err = 0.0f;
|
||||
for (std::size_t i = 0; i < 12; ++i) {
|
||||
max_vel_err = std::max(max_vel_err, std::abs(state.joint_vel[i]));
|
||||
}
|
||||
|
||||
if (max_pos_err <= 0.30f && max_vel_err <= 0.6f) {
|
||||
startup_state_ = StartupState::STARTUP_HOLD_AFTER;
|
||||
state_start_time_ = now_time;
|
||||
RCLCPP_INFO(get_logger(), "Pose settled. Holding for 1.0s...");
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::STARTUP_HOLD_AFTER) {
|
||||
double elapsed = (now_time - state_start_time_).seconds();
|
||||
|
||||
// Run stand balance controller during holding phase
|
||||
target.target = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, cmd);
|
||||
target.target_source = "startup_hold";
|
||||
|
||||
if (elapsed >= 1.0 && stand_balance_->isStable()) {
|
||||
startup_state_ = StartupState::RUNTIME;
|
||||
RCLCPP_INFO(get_logger(), "Standup sequence completed. Entering Policy RUNTIME mode!");
|
||||
}
|
||||
}
|
||||
else if (startup_state_ == StartupState::RUNTIME) {
|
||||
// Python template uses the command directly in policy obs/release logic.
|
||||
// Upstream cmd mux may already smooth it, so do not apply an extra runtime filter here.
|
||||
filtered_cmd_ = cmd;
|
||||
|
||||
const auto target_hold = stand_balance_->computeTarget(state.projected_gravity, state.imu_gyro, std::array<float, 3>{0.0f, 0.0f, 0.0f});
|
||||
const bool zero_command = isZeroCommand(cmd, state.imu_gyro);
|
||||
|
||||
if (!runtime_released_) {
|
||||
if (require_active_command_to_release_) {
|
||||
if (isCommandActive(cmd)) {
|
||||
release_active_time_ += kPolicyDt;
|
||||
} else {
|
||||
release_active_time_ = 0.0f;
|
||||
}
|
||||
|
||||
float max_hold_err = 0.0f;
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kLegJointCount; ++i) {
|
||||
max_hold_err = std::max(max_hold_err, std::abs(state.joint_pos[i] - target_hold[i]));
|
||||
}
|
||||
|
||||
const bool active_ready = release_active_time_ >= release_command_hold_s_;
|
||||
const bool posture_ready = max_hold_err <= release_posture_max_err_;
|
||||
if (active_ready && posture_ready) {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
} else {
|
||||
runtime_released_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!runtime_released_ || zero_command) {
|
||||
release_alpha_ = 0.0f;
|
||||
target.runtime_released = false;
|
||||
target.release_alpha = 0.0f;
|
||||
target.zero_command = zero_command;
|
||||
target.raw_action.fill(0.0f);
|
||||
target.scaled_action.fill(0.0f);
|
||||
last_actions.fill(0.0f);
|
||||
target.target_source = "runtime_zero_hold";
|
||||
target.target = target_hold;
|
||||
if (!runtime_released_) {
|
||||
target.target_source = "runtime_hold";
|
||||
}
|
||||
} else {
|
||||
release_alpha_ = std::min(1.0f, release_alpha_ + kPolicyDt / std::max(command_release_s_, 1.0e-3f));
|
||||
target.runtime_released = (release_alpha_ >= 1.0f);
|
||||
target.release_alpha = release_alpha_;
|
||||
target.zero_command = false;
|
||||
target.command = cmd;
|
||||
|
||||
auto raw = runPolicy(buildObservation(state, cmd, last_actions));
|
||||
for (float & v : raw) {
|
||||
v *= release_alpha_;
|
||||
}
|
||||
target.raw_action = raw;
|
||||
|
||||
const float blend = std::min(1.0f, release_alpha_ * (command_release_s_ / std::max(release_target_blend_s_, kPolicyDt)));
|
||||
for (std::size_t i = 0; i < sim2real_common::DeploymentContract::kActionDim; ++i) {
|
||||
target.scaled_action[i] = raw[i] * sim2real_common::DeploymentContract::kActionScale[i];
|
||||
const float policy_target = target.scaled_action[i] + sim2real_common::DeploymentContract::kDefaultDofPos[i];
|
||||
target.target[i] = (1.0f - blend) * target_hold[i] + blend * policy_target;
|
||||
last_actions[i] = raw[i];
|
||||
}
|
||||
target.target_source = blend < 0.999f ? "runtime_blend" : "runtime_policy";
|
||||
}
|
||||
}
|
||||
|
||||
// 2) Run SafetyMonitor check on computed target
|
||||
if (safety_enabled_) {
|
||||
auto safety_decision = safety_monitor_->check(target.target, sim2real_common::DeploymentContract::kDefaultDofPos, state.imu_gyro, state.projected_gravity, estop_active);
|
||||
if (safety_decision.level == sim2real_common::SafetyLevel::ESTOP || safety_decision.level == sim2real_common::SafetyLevel::BRAKE) {
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
safety_triggered_ = true;
|
||||
}
|
||||
safety_reason_ = "Safety Monitor Stop: " + safety_decision.message;
|
||||
RCLCPP_ERROR(get_logger(), "SAFETY STOP TRIGGERED in Policy Runtime: %s", safety_reason_.c_str());
|
||||
|
||||
// Override target to safety_brake damping pose
|
||||
target.target = sim2real_common::DeploymentContract::kDefaultDofPos;
|
||||
target.target_source = "safety_brake";
|
||||
} else if (safety_decision.level == sim2real_common::SafetyLevel::CLIP) {
|
||||
target.target = safety_decision.clipped_target;
|
||||
target.target_source = "safety_clip";
|
||||
RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "Safety Monitor: Joint target clipped in Policy Runtime.");
|
||||
}
|
||||
}
|
||||
|
||||
target.target_age_ms = 0.0f;
|
||||
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
last_actions_ = last_actions;
|
||||
}
|
||||
|
||||
target_pub_->publish(target);
|
||||
}
|
||||
|
||||
} // namespace sim2real_runtime
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<sim2real_runtime::PolicyRuntimeNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,250 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional
|
||||
|
||||
import serial
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
SBUS_FRAME_SIZE = 25
|
||||
SBUS_RC_MID = 1024
|
||||
SBUS_AXIS_SCALE = 660.0
|
||||
|
||||
SWITCH_LOW = -1
|
||||
SWITCH_MID = 0
|
||||
SWITCH_HIGH = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteSwitchState:
|
||||
ch7: int = SWITCH_MID
|
||||
|
||||
|
||||
@dataclass
|
||||
class RemoteControlState:
|
||||
ch1: int = 0
|
||||
ch2: int = 0
|
||||
ch3: int = 0
|
||||
ch4: int = 0
|
||||
switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
|
||||
frame_ok: bool = False
|
||||
|
||||
@property
|
||||
def estop_requested(self) -> bool:
|
||||
return self.switches.ch7 == SWITCH_HIGH
|
||||
|
||||
|
||||
class SbusUartReceiver:
|
||||
def __init__(self, port: str, baudrate: int, timeout: float, axis_deadzone: int):
|
||||
self.port = port
|
||||
self.baudrate = int(baudrate)
|
||||
self.timeout = float(timeout)
|
||||
self.axis_deadzone = int(axis_deadzone)
|
||||
self.serial: Optional[serial.Serial] = None
|
||||
self.buffer = bytearray()
|
||||
self.state = RemoteControlState()
|
||||
|
||||
def open(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
return
|
||||
self.serial = serial.Serial(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
bytesize=serial.EIGHTBITS,
|
||||
parity=serial.PARITY_EVEN,
|
||||
stopbits=serial.STOPBITS_TWO,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
if self.serial and self.serial.is_open:
|
||||
self.serial.close()
|
||||
|
||||
def poll(self) -> RemoteControlState:
|
||||
if not self.serial or not self.serial.is_open:
|
||||
raise RuntimeError("remote uart is not open")
|
||||
|
||||
waiting = self.serial.in_waiting
|
||||
if waiting:
|
||||
self.buffer.extend(self.serial.read(waiting))
|
||||
|
||||
while len(self.buffer) >= SBUS_FRAME_SIZE:
|
||||
start_idx = self.buffer.find(0x0F)
|
||||
if start_idx < 0:
|
||||
self.buffer.clear()
|
||||
break
|
||||
if start_idx > 0:
|
||||
del self.buffer[:start_idx]
|
||||
if len(self.buffer) < SBUS_FRAME_SIZE:
|
||||
break
|
||||
frame = bytes(self.buffer[:SBUS_FRAME_SIZE])
|
||||
del self.buffer[:SBUS_FRAME_SIZE]
|
||||
parsed = self._parse_frame(frame)
|
||||
if parsed is not None:
|
||||
self.state = parsed
|
||||
return self.state
|
||||
|
||||
def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
|
||||
if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
|
||||
return None
|
||||
|
||||
channels = [0] * 16
|
||||
channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
|
||||
channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
|
||||
channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
|
||||
channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
|
||||
channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
|
||||
channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
|
||||
channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
|
||||
channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
|
||||
channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
|
||||
channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
|
||||
|
||||
if channels[0] < 100:
|
||||
return None
|
||||
|
||||
state = RemoteControlState(
|
||||
ch1=self._normalize_axis(channels[0]),
|
||||
ch2=self._normalize_axis(channels[1]),
|
||||
ch3=self._normalize_axis(channels[3]),
|
||||
ch4=self._normalize_axis(channels[2]),
|
||||
switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
|
||||
frame_ok=True,
|
||||
)
|
||||
if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
|
||||
return None
|
||||
return state
|
||||
|
||||
def _normalize_axis(self, value: int) -> int:
|
||||
mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
|
||||
return 0 if abs(mapped) <= self.axis_deadzone else mapped
|
||||
|
||||
@staticmethod
|
||||
def _decode_switch(value: int) -> int:
|
||||
if value < 500:
|
||||
return SWITCH_LOW
|
||||
if value > 1500:
|
||||
return SWITCH_HIGH
|
||||
return SWITCH_MID
|
||||
|
||||
|
||||
class RemoteUartNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_remote_uart_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("remote_enabled", True).value)
|
||||
self.port = str(self.declare_parameter("remote_port", "/dev/ttyACM0").value)
|
||||
self.baudrate = int(self.declare_parameter("remote_baudrate", 100000).value)
|
||||
self.timeout = float(self.declare_parameter("remote_timeout", 0.02).value)
|
||||
self.axis_deadzone = int(self.declare_parameter("remote_axis_deadzone", 40).value)
|
||||
self.active_threshold = int(self.declare_parameter("remote_active_threshold", 40).value)
|
||||
self.axis_full_scale = max(float(self.declare_parameter("remote_axis_full_scale", 660.0).value), 1.0)
|
||||
self.max_vx = float(self.declare_parameter("remote_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("remote_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("remote_max_yaw_rate", 0.5).value)
|
||||
self.invert_vx = bool(self.declare_parameter("remote_invert_vx", True).value)
|
||||
self.invert_vy = bool(self.declare_parameter("remote_invert_vy", False).value)
|
||||
self.invert_yaw = bool(self.declare_parameter("remote_invert_yaw", True).value)
|
||||
self.publish_inactive_zero = bool(self.declare_parameter("remote_publish_inactive_zero", True).value)
|
||||
self.estop_latch = bool(self.declare_parameter("remote_estop_latch", True).value)
|
||||
self.poll_hz = float(self.declare_parameter("remote_poll_hz", 50.0).value)
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_remote", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.receiver: Optional[SbusUartReceiver] = None
|
||||
self.estop_published = False
|
||||
self.open_error_logged = False
|
||||
|
||||
if self.enabled:
|
||||
self.receiver = SbusUartReceiver(
|
||||
port=self.port,
|
||||
baudrate=self.baudrate,
|
||||
timeout=self.timeout,
|
||||
axis_deadzone=self.axis_deadzone,
|
||||
)
|
||||
try:
|
||||
self.receiver.open()
|
||||
self.get_logger().info(f"Remote UART opened on {self.port} at {self.baudrate} baud")
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"Failed to open remote UART {self.port}: {exc}")
|
||||
self.open_error_logged = True
|
||||
else:
|
||||
self.get_logger().warn("Remote UART node is disabled by parameter")
|
||||
|
||||
period = 1.0 / self.poll_hz if self.poll_hz > 0.0 else 0.02
|
||||
self.timer = self.create_timer(period, self.on_timer)
|
||||
|
||||
def destroy_node(self) -> bool:
|
||||
if self.receiver is not None:
|
||||
self.receiver.close()
|
||||
return super().destroy_node()
|
||||
|
||||
def on_timer(self) -> None:
|
||||
if not self.enabled or self.receiver is None:
|
||||
return
|
||||
|
||||
try:
|
||||
if not self.receiver.serial or not self.receiver.serial.is_open:
|
||||
self.receiver.open()
|
||||
state = self.receiver.poll()
|
||||
except Exception as exc:
|
||||
if not self.open_error_logged:
|
||||
self.get_logger().error(f"Remote UART poll failed: {exc}")
|
||||
self.open_error_logged = True
|
||||
return
|
||||
|
||||
self.open_error_logged = False
|
||||
|
||||
if state.estop_requested:
|
||||
if not self.estop_published or not self.estop_latch:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.get_logger().warn("Remote E-stop requested by CH7 high")
|
||||
self.estop_published = True
|
||||
self.publish_zero_cmd()
|
||||
return
|
||||
|
||||
if not self.estop_latch and self.estop_published:
|
||||
self.estop_pub.publish(Bool(data=False))
|
||||
self.estop_published = False
|
||||
|
||||
active = any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
|
||||
if active or self.publish_inactive_zero:
|
||||
cmd = Twist()
|
||||
cmd.linear.x = self.axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
|
||||
cmd.linear.y = self.axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
|
||||
cmd.angular.z = self.axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
|
||||
self.cmd_pub.publish(cmd)
|
||||
|
||||
def publish_zero_cmd(self) -> None:
|
||||
self.cmd_pub.publish(Twist())
|
||||
|
||||
def axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
|
||||
if abs(raw_value) <= self.active_threshold:
|
||||
return 0.0
|
||||
scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
|
||||
if invert:
|
||||
scaled = -scaled
|
||||
return float(scaled * limit)
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = RemoteUartNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,261 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import socket
|
||||
from typing import Any, Optional
|
||||
|
||||
import rclpy
|
||||
from geometry_msgs.msg import Twist
|
||||
from rclpy.executors import ExternalShutdownException
|
||||
from rclpy.node import Node
|
||||
from sim2real_interfaces.msg import RuntimeState, RuntimeTarget
|
||||
from std_msgs.msg import Bool, String
|
||||
|
||||
|
||||
class WebUdpBridgeNode(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim2real_web_udp_bridge_node", allow_undeclared_parameters=True)
|
||||
|
||||
self.enabled = bool(self.declare_parameter("web_bridge_enabled", True).value)
|
||||
self.listen_host = str(self.declare_parameter("web_udp_listen_host", "0.0.0.0").value)
|
||||
self.listen_port = int(self.declare_parameter("web_udp_listen_port", 15000).value)
|
||||
self.remote_host = str(self.declare_parameter("web_udp_remote_host", "").value)
|
||||
self.remote_port = int(self.declare_parameter("web_udp_remote_port", 15001).value)
|
||||
self.state_hz = float(self.declare_parameter("web_udp_state_hz", 20.0).value)
|
||||
self.cmd_timeout_ms = float(self.declare_parameter("web_udp_cmd_timeout_ms", 300.0).value)
|
||||
self.max_packet_bytes = int(self.declare_parameter("web_udp_max_packet_bytes", 8192).value)
|
||||
self.max_vx = float(self.declare_parameter("web_udp_max_vx", 0.8).value)
|
||||
self.max_vy = float(self.declare_parameter("web_udp_max_vy", 0.3).value)
|
||||
self.max_yaw = float(self.declare_parameter("web_udp_max_yaw_rate", 0.5).value)
|
||||
self.estop_on_timeout = bool(self.declare_parameter("web_udp_estop_on_timeout", False).value)
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.sock.setblocking(False)
|
||||
self.sock.bind((self.listen_host, self.listen_port))
|
||||
|
||||
self.client_addr: Optional[tuple[str, int]] = None
|
||||
if self.remote_host:
|
||||
self.client_addr = (self.remote_host, self.remote_port)
|
||||
|
||||
self.latest_target: Optional[RuntimeTarget] = None
|
||||
self.latest_state: Optional[RuntimeState] = None
|
||||
self.latest_cmd = Twist()
|
||||
self.latest_mode = "UNKNOWN"
|
||||
self.latest_mux_status = ""
|
||||
self.estop = False
|
||||
self.web_enabled = False
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
|
||||
self.cmd_pub = self.create_publisher(Twist, "cmd_vel_web", 10)
|
||||
self.estop_pub = self.create_publisher(Bool, "/safety/estop", 10)
|
||||
self.web_enabled_pub = self.create_publisher(Bool, "web/enabled", 10)
|
||||
self.remote_enabled_pub = self.create_publisher(Bool, "remote/enabled", 10)
|
||||
self.nav_enabled_pub = self.create_publisher(Bool, "nav/enabled", 10)
|
||||
self.mode_pub = self.create_publisher(String, "control/mode", 10)
|
||||
|
||||
self.create_subscription(RuntimeTarget, "runtime/target", self.on_target, 10)
|
||||
self.create_subscription(RuntimeState, "runtime/state", self.on_state, 10)
|
||||
self.create_subscription(Twist, "cmd_vel", self.on_cmd_vel, 10)
|
||||
self.create_subscription(Bool, "/safety/estop", self.on_estop, 10)
|
||||
self.create_subscription(String, "control/mode_state", self.on_mode_state, 10)
|
||||
self.create_subscription(String, "control/mux_status", self.on_mux_status, 10)
|
||||
|
||||
self.rx_timer = self.create_timer(0.01, self.on_rx_timer)
|
||||
self.state_timer = self.create_timer(1.0 / self.state_hz if self.state_hz > 0.0 else 0.05, self.on_state_timer)
|
||||
self.guard_timer = self.create_timer(0.05, self.on_guard_timer)
|
||||
|
||||
self.get_logger().info(f"Web UDP bridge listening on {self.listen_host}:{self.listen_port}")
|
||||
|
||||
def on_target(self, msg: RuntimeTarget) -> None:
|
||||
self.latest_target = msg
|
||||
|
||||
def on_state(self, msg: RuntimeState) -> None:
|
||||
self.latest_state = msg
|
||||
|
||||
def on_cmd_vel(self, msg: Twist) -> None:
|
||||
self.latest_cmd = msg
|
||||
|
||||
def on_estop(self, msg: Bool) -> None:
|
||||
self.estop = bool(msg.data)
|
||||
|
||||
def on_mode_state(self, msg: String) -> None:
|
||||
self.latest_mode = msg.data
|
||||
|
||||
def on_mux_status(self, msg: String) -> None:
|
||||
self.latest_mux_status = msg.data
|
||||
|
||||
def on_rx_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
while True:
|
||||
try:
|
||||
data, addr = self.sock.recvfrom(self.max_packet_bytes)
|
||||
except BlockingIOError:
|
||||
break
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP receive failed: {exc}")
|
||||
break
|
||||
self.client_addr = addr
|
||||
try:
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
self.handle_packet(payload)
|
||||
except Exception as exc:
|
||||
self.send_packet({"type": "error", "message": str(exc)})
|
||||
|
||||
def handle_packet(self, payload: dict[str, Any]) -> None:
|
||||
msg_type = str(payload.get("type", "")).lower()
|
||||
if msg_type == "cmd_vel":
|
||||
cmd = self.parse_twist(payload)
|
||||
self.cmd_pub.publish(cmd)
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
self.timeout_estop_sent = False
|
||||
elif msg_type == "zero":
|
||||
self.cmd_pub.publish(Twist())
|
||||
self.last_cmd_time = self.get_clock().now()
|
||||
elif msg_type == "estop":
|
||||
self.estop_pub.publish(Bool(data=bool(payload.get("data", True))))
|
||||
elif msg_type == "mode":
|
||||
mode = str(payload.get("mode", "DISABLED")).upper()
|
||||
self.mode_pub.publish(String(data=mode))
|
||||
self.web_enabled = mode == "WEB"
|
||||
self.web_enabled_pub.publish(Bool(data=mode == "WEB"))
|
||||
self.remote_enabled_pub.publish(Bool(data=mode == "REMOTE"))
|
||||
self.nav_enabled_pub.publish(Bool(data=mode == "NAV"))
|
||||
elif msg_type == "web_enable":
|
||||
self.web_enabled = bool(payload.get("data", False))
|
||||
self.web_enabled_pub.publish(Bool(data=self.web_enabled))
|
||||
if self.web_enabled:
|
||||
self.mode_pub.publish(String(data="WEB"))
|
||||
elif msg_type == "remote_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.remote_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="REMOTE"))
|
||||
elif msg_type == "nav_enable":
|
||||
enabled = bool(payload.get("data", False))
|
||||
self.nav_enabled_pub.publish(Bool(data=enabled))
|
||||
if enabled:
|
||||
self.mode_pub.publish(String(data="NAV"))
|
||||
elif msg_type == "ping":
|
||||
self.send_packet({"type": "pong", "stamp": self.now_sec()})
|
||||
else:
|
||||
self.send_packet({"type": "error", "message": f"unknown packet type: {msg_type}"})
|
||||
|
||||
def parse_twist(self, payload: dict[str, Any]) -> Twist:
|
||||
cmd = Twist()
|
||||
linear = payload.get("linear", {}) or {}
|
||||
angular = payload.get("angular", {}) or {}
|
||||
cmd.linear.x = self.clamp(float(linear.get("x", 0.0)), -self.max_vx, self.max_vx)
|
||||
cmd.linear.y = self.clamp(float(linear.get("y", 0.0)), -self.max_vy, self.max_vy)
|
||||
cmd.angular.z = self.clamp(float(angular.get("z", 0.0)), -self.max_yaw, self.max_yaw)
|
||||
return cmd
|
||||
|
||||
def on_guard_timer(self) -> None:
|
||||
age_ms = (self.get_clock().now() - self.last_cmd_time).nanoseconds / 1.0e6
|
||||
if age_ms > self.cmd_timeout_ms:
|
||||
self.cmd_pub.publish(Twist())
|
||||
if self.estop_on_timeout and not self.timeout_estop_sent:
|
||||
self.estop_pub.publish(Bool(data=True))
|
||||
self.timeout_estop_sent = True
|
||||
|
||||
def on_state_timer(self) -> None:
|
||||
if not self.enabled:
|
||||
return
|
||||
self.send_packet(self.build_state_packet())
|
||||
|
||||
def build_state_packet(self) -> dict[str, Any]:
|
||||
target = self.latest_target
|
||||
state = self.latest_state
|
||||
packet: dict[str, Any] = {
|
||||
"type": "state",
|
||||
"stamp": self.now_sec(),
|
||||
"mode": self.latest_mode,
|
||||
"mux_status": self.latest_mux_status,
|
||||
"estop": self.estop,
|
||||
"web_enabled": self.web_enabled,
|
||||
"cmd_vel": self.twist_to_dict(self.latest_cmd),
|
||||
"runtime": {},
|
||||
"robot": {},
|
||||
}
|
||||
if target is not None:
|
||||
packet["runtime"] = {
|
||||
"target_source": str(target.target_source),
|
||||
"zero_command": bool(target.zero_command),
|
||||
"runtime_released": bool(target.runtime_released),
|
||||
"release_alpha": self._f(target.release_alpha),
|
||||
"command": [self._f(v) for v in target.command],
|
||||
"raw_command": [self._f(v) for v in target.raw_command],
|
||||
}
|
||||
if state is not None:
|
||||
packet["robot"] = {
|
||||
"joint_pos": [self._f(v) for v in state.joint_pos],
|
||||
"joint_vel": [self._f(v) for v in state.joint_vel],
|
||||
"joint_torque": [self._f(v) for v in state.joint_torque],
|
||||
"imu_gyro": [self._f(v) for v in state.imu_gyro],
|
||||
"imu_accel": [self._f(v) for v in state.imu_accel],
|
||||
"projected_gravity": [self._f(v) for v in state.projected_gravity],
|
||||
"quat_wxyz": [self._f(v) for v in state.quat_wxyz],
|
||||
"imu_age_ms": self._f(state.imu_age_ms),
|
||||
"imu_fresh": bool(state.imu_fresh),
|
||||
"odom_age_ms": self._f(state.odom_age_ms),
|
||||
"odom_fresh": bool(state.odom_fresh),
|
||||
"odom_local_pos": [self._f(v) for v in state.odom_local_pos],
|
||||
"odom_local_yaw": self._f(state.odom_local_yaw),
|
||||
"fresh_count": int(state.fresh_count),
|
||||
"holdover_count": int(state.holdover_count),
|
||||
"stale_max": int(state.stale_max),
|
||||
"update_counts": [int(v) for v in state.update_counts],
|
||||
}
|
||||
return packet
|
||||
|
||||
def send_packet(self, payload: dict[str, Any]) -> None:
|
||||
if self.client_addr is None:
|
||||
return
|
||||
try:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
self.sock.sendto(data, self.client_addr)
|
||||
except OSError as exc:
|
||||
self.get_logger().warn(f"UDP send failed: {exc}")
|
||||
|
||||
@staticmethod
|
||||
def _f(v: Any) -> float:
|
||||
try:
|
||||
f = float(v)
|
||||
if f != f:
|
||||
return 0.0
|
||||
return round(f, 6)
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
|
||||
def now_sec(self) -> float:
|
||||
return self.get_clock().now().nanoseconds / 1.0e9
|
||||
|
||||
@staticmethod
|
||||
def twist_to_dict(msg: Twist) -> dict[str, Any]:
|
||||
return {
|
||||
"linear": {"x": msg.linear.x, "y": msg.linear.y, "z": msg.linear.z},
|
||||
"angular": {"x": msg.angular.x, "y": msg.angular.y, "z": msg.angular.z},
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def clamp(value: float, low: float, high: float) -> float:
|
||||
return max(low, min(high, value))
|
||||
|
||||
|
||||
def main(args: Optional[list[str]] = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = WebUdpBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except (KeyboardInterrupt, ExternalShutdownException):
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,75 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Exit immediately if a command exits with a non-zero status
|
||||
set -e
|
||||
|
||||
# Define color codes for pretty output
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
RED='\033[0;31m'
|
||||
NC='\033[0m' # No Color
|
||||
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
echo -e "${GREEN} Starting Sim2Real Locomotion ROS2 Stack ${NC}"
|
||||
echo -e "${YELLOW}====================================================${NC}"
|
||||
|
||||
# 1. Source ROS2 Humble environment
|
||||
if [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
echo -e "[System] Sourcing ROS2 Humble..."
|
||||
source /opt/ros/humble/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] ROS2 Humble not found. Please install ROS2 Humble first.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# 2. Check if local workspace is compiled and source it
|
||||
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||
cd "$SCRIPT_DIR"
|
||||
|
||||
if [ -f "install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing local workspace..."
|
||||
source install/setup.bash
|
||||
elif [ -f "../../install/setup.bash" ]; then
|
||||
echo -e "[Workspace] Sourcing parent install/setup.bash..."
|
||||
source ../../install/setup.bash
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] install/setup.bash not found. Attempting to build the workspace first...${NC}"
|
||||
if command -v colcon &> /dev/null; then
|
||||
echo -e "[Build] Running colcon build..."
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
source install/setup.bash
|
||||
else
|
||||
echo -e "${RED}[Error] 'colcon' tool not found. Please compile the workspace manually before running.${NC}"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
|
||||
# 3. Check for SocketCAN interfaces (in non-dry-run mode)
|
||||
# Reading dry_run parameter from yaml config
|
||||
if [ -f "src/sim2real_bringup/config/runtime.yaml" ]; then
|
||||
# Use sed for portability (busybox-compatible, avoids GNU grep -oP dependency)
|
||||
DRY_RUN=$(sed -n 's/^[[:space:]]*dry_run:[[:space:]]*//p' src/sim2real_bringup/config/runtime.yaml | head -n 1 || echo "true")
|
||||
# Trim trailing whitespace/newlines
|
||||
DRY_RUN=$(echo "$DRY_RUN" | tr -d '[:space:]')
|
||||
else
|
||||
DRY_RUN="true"
|
||||
fi
|
||||
|
||||
if [ "$DRY_RUN" = "false" ]; then
|
||||
echo -e "[Network] Checking CAN interfaces..."
|
||||
if ip link show can0 &> /dev/null && ip link show can1 &> /dev/null; then
|
||||
echo -e "[Network] can0 and can1 interfaces detected."
|
||||
else
|
||||
echo -e "${YELLOW}[Warning] CAN interfaces (can0/can1) not fully active.${NC}"
|
||||
echo -e "To configure CAN interfaces, run:"
|
||||
echo -e " sudo ip link set can0 up type can bitrate 1000000"
|
||||
echo -e " sudo ip link set can1 up type can bitrate 1000000"
|
||||
fi
|
||||
else
|
||||
echo -e "${YELLOW}[Dry-Run] Running in Dry-Run mode. SocketCAN will not be accessed.${NC}"
|
||||
fi
|
||||
|
||||
# 4. Run the ROS2 Launch file
|
||||
echo -e "${GREEN}[Launch] Starting sim2real launch file...${NC}"
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py "$@"
|
||||
|
||||
@@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
STATE_LOCK = threading.Lock()
|
||||
LATEST_STATE: dict = {"type": "state", "connected": False}
|
||||
NANO_ADDR: tuple[str, int]
|
||||
UDP_SOCK: socket.socket
|
||||
|
||||
|
||||
class Handler(SimpleHTTPRequestHandler):
|
||||
def do_GET(self) -> None:
|
||||
if self.path == "/api/state":
|
||||
with STATE_LOCK:
|
||||
data = json.dumps(LATEST_STATE).encode("utf-8")
|
||||
self._json(200, data)
|
||||
return
|
||||
super().do_GET()
|
||||
|
||||
def do_POST(self) -> None:
|
||||
if self.path not in ("/api/control", "/api/state"):
|
||||
self.send_error(404)
|
||||
return
|
||||
length = int(self.headers.get("Content-Length", "0"))
|
||||
body = self.rfile.read(length) if length else b"{}"
|
||||
try:
|
||||
payload = json.loads(body.decode("utf-8"))
|
||||
send_udp(payload)
|
||||
self._json(200, b'{"ok":true}')
|
||||
except Exception as exc:
|
||||
self._json(400, json.dumps({"ok": False, "error": str(exc)}).encode())
|
||||
|
||||
def _json(self, code: int, data: bytes) -> None:
|
||||
self.send_response(code)
|
||||
self.send_header("Content-Type", "application/json")
|
||||
self.send_header("Content-Length", str(len(data)))
|
||||
self.send_header("Access-Control-Allow-Origin", "*")
|
||||
self.end_headers()
|
||||
self.wfile.write(data)
|
||||
|
||||
def log_message(self, format: str, *args: object) -> None:
|
||||
return
|
||||
|
||||
|
||||
def send_udp(payload: dict) -> None:
|
||||
data = json.dumps(payload, separators=(",", ":")).encode("utf-8")
|
||||
UDP_SOCK.sendto(data, NANO_ADDR)
|
||||
|
||||
|
||||
def udp_rx_loop(sock: socket.socket) -> None:
|
||||
global LATEST_STATE
|
||||
while True:
|
||||
try:
|
||||
data, _ = sock.recvfrom(65535)
|
||||
payload = json.loads(data.decode("utf-8"))
|
||||
payload["connected"] = True
|
||||
payload["local_receive_time"] = time.time()
|
||||
with STATE_LOCK:
|
||||
LATEST_STATE = payload
|
||||
except Exception:
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
def heartbeat_loop() -> None:
|
||||
while True:
|
||||
try:
|
||||
send_udp({"type": "ping", "stamp": time.time()})
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.5)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
global NANO_ADDR, UDP_SOCK
|
||||
parser = argparse.ArgumentParser(description="Windows local web debug UI for sim2real_ros2")
|
||||
parser.add_argument("--nano-host", required=True, help="Nano IP address")
|
||||
parser.add_argument("--nano-port", type=int, default=15000, help="Nano UDP listen port")
|
||||
parser.add_argument("--listen-host", default="0.0.0.0", help="Local HTTP host")
|
||||
parser.add_argument("--http-port", type=int, default=8088, help="Local HTTP port")
|
||||
parser.add_argument("--udp-port", type=int, default=15001, help="Local UDP receive port")
|
||||
args = parser.parse_args()
|
||||
|
||||
NANO_ADDR = (args.nano_host, args.nano_port)
|
||||
UDP_SOCK = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
UDP_SOCK.bind(("0.0.0.0", args.udp_port))
|
||||
|
||||
threading.Thread(target=udp_rx_loop, args=(UDP_SOCK,), daemon=True).start()
|
||||
threading.Thread(target=heartbeat_loop, daemon=True).start()
|
||||
|
||||
static_dir = Path(__file__).resolve().parent / "static"
|
||||
handler = lambda *a, **kw: Handler(*a, directory=str(static_dir), **kw)
|
||||
httpd = ThreadingHTTPServer((args.listen_host, args.http_port), handler)
|
||||
print(f"Open http://127.0.0.1:{args.http_port}")
|
||||
print(f"UDP Nano={args.nano_host}:{args.nano_port} local={args.udp_port}")
|
||||
httpd.serve_forever()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,259 @@
|
||||
'use strict';
|
||||
|
||||
const JOINT_NAMES = [
|
||||
'FL_H_ABD','FL_H_PIT','FL_KNEE',
|
||||
'FR_H_ABD','FR_H_PIT','FR_KNEE',
|
||||
'RL_H_ABD','RL_H_PIT','RL_KNEE',
|
||||
'RR_H_ABD','RR_H_PIT','RR_KNEE',
|
||||
'FL_WHEEL','FR_WHEEL','RL_WHEEL','RR_WHEEL',
|
||||
];
|
||||
|
||||
const $ = id => document.getElementById(id);
|
||||
const cmd = { vx: 0, vy: 0, yaw: 0 };
|
||||
let cmdSendTimer = null;
|
||||
let currentMode = 'UNKNOWN';
|
||||
|
||||
// ── API ──────────────────────────────────────────────────────────────────────
|
||||
async function post(payload) {
|
||||
try {
|
||||
await fetch('/api/control', {
|
||||
method: 'POST',
|
||||
headers: { 'Content-Type': 'application/json' },
|
||||
body: JSON.stringify(payload),
|
||||
});
|
||||
} catch (e) {
|
||||
appendEvent('API_ERROR', e.message, 'bad');
|
||||
}
|
||||
}
|
||||
|
||||
function sendCmd() {
|
||||
post({
|
||||
type: 'cmd_vel',
|
||||
linear: { x: cmd.vx, y: cmd.vy, z: 0 },
|
||||
angular: { x: 0, y: 0, z: cmd.yaw },
|
||||
});
|
||||
$('cmd-display').textContent =
|
||||
`vx=${cmd.vx.toFixed(2)} vy=${cmd.vy.toFixed(2)} yaw=${cmd.yaw.toFixed(2)}`;
|
||||
}
|
||||
|
||||
function zeroAll() {
|
||||
cmd.vx = 0; cmd.vy = 0; cmd.yaw = 0;
|
||||
$('cmd-vx').value = 0;
|
||||
$('cmd-vy').value = 0;
|
||||
$('cmd-yaw').value = 0;
|
||||
$('cmd-vx-v').textContent = '0.00';
|
||||
$('cmd-vy-v').textContent = '0.00';
|
||||
$('cmd-yaw-v').textContent = '0.00';
|
||||
$('cmd-display').textContent = 'vx=0.00 vy=0.00 yaw=0.00';
|
||||
$('stick').style.transform = 'translate(-50%, -50%)';
|
||||
post({ type: 'zero' });
|
||||
}
|
||||
|
||||
// ── Buttons ──────────────────────────────────────────────────────────────────
|
||||
function setMode(mode) {
|
||||
if (mode === 'WEB' && !confirm('确认切换到 WEB 控制？\n请确认机器人安全且速度为 0。')) return;
|
||||
post({ type: 'mode', mode });
|
||||
appendEvent('MODE_SET', `→ ${mode}`, 'ok');
|
||||
}
|
||||
|
||||
function highlightMode(mode) {
|
||||
for (const m of ['DISABLED', 'REMOTE', 'WEB', 'NAV']) {
|
||||
const btn = $('btn-' + m.toLowerCase());
|
||||
if (btn) btn.classList.toggle('active-mode', m === mode);
|
||||
}
|
||||
const el = $('stage');
|
||||
if (el) {
|
||||
el.textContent = mode;
|
||||
el.className = 'stage ' + mode;
|
||||
}
|
||||
currentMode = mode;
|
||||
}
|
||||
|
||||
$('btn-disabled').onclick = () => { zeroAll(); setMode('DISABLED'); };
|
||||
$('btn-remote').onclick = () => setMode('REMOTE');
|
||||
$('btn-web').onclick = () => setMode('WEB');
|
||||
$('btn-nav').onclick = () => setMode('NAV');
|
||||
$('btn-zero').onclick = zeroAll;
|
||||
$('btn-estop').onclick = () => {
|
||||
if (confirm('确认触发软急停？')) {
|
||||
post({ type: 'estop', data: true });
|
||||
zeroAll();
|
||||
appendEvent('ESTOP', '软急停已触发', 'bad');
|
||||
}
|
||||
};
|
||||
|
||||
// ── Sliders ──────────────────────────────────────────────────────────────────
|
||||
for (const [id, key] of [['cmd-vx','vx'],['cmd-vy','vy'],['cmd-yaw','yaw']]) {
|
||||
$(id).addEventListener('input', e => {
|
||||
cmd[key] = parseFloat(e.target.value);
|
||||
$(id + '-v').textContent = cmd[key].toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
});
|
||||
}
|
||||
|
||||
// ── Joystick ─────────────────────────────────────────────────────────────────
|
||||
const joystick = $('joystick');
|
||||
const stick = $('stick');
|
||||
let dragging = false;
|
||||
|
||||
function updateJoystick(clientX, clientY) {
|
||||
const rect = joystick.getBoundingClientRect();
|
||||
const cx = rect.left + rect.width / 2;
|
||||
const cy = rect.top + rect.height / 2;
|
||||
const maxR = rect.width * 0.42;
|
||||
let dx = clientX - cx;
|
||||
let dy = clientY - cy;
|
||||
const dist = Math.hypot(dx, dy);
|
||||
if (dist > maxR) { dx = dx / dist * maxR; dy = dy / dist * maxR; }
|
||||
stick.style.transform = `translate(calc(-50% + ${dx}px), calc(-50% + ${dy}px))`;
|
||||
cmd.vx = parseFloat((-(dy / maxR) * 0.8).toFixed(3));
|
||||
cmd.vy = parseFloat(( (dx / maxR) * 0.3).toFixed(3));
|
||||
$('cmd-vx').value = cmd.vx;
|
||||
$('cmd-vy').value = cmd.vy;
|
||||
$('cmd-vx-v').textContent = cmd.vx.toFixed(2);
|
||||
$('cmd-vy-v').textContent = cmd.vy.toFixed(2);
|
||||
if (currentMode === 'WEB') sendCmd();
|
||||
}
|
||||
|
||||
joystick.addEventListener('pointerdown', e => {
|
||||
dragging = true;
|
||||
joystick.setPointerCapture(e.pointerId);
|
||||
updateJoystick(e.clientX, e.clientY);
|
||||
});
|
||||
joystick.addEventListener('pointermove', e => { if (dragging) updateJoystick(e.clientX, e.clientY); });
|
||||
joystick.addEventListener('pointerup', () => { dragging = false; zeroAll(); });
|
||||
joystick.addEventListener('pointercancel', () => { dragging = false; zeroAll(); });
|
||||
|
||||
// ── Joints grid init ─────────────────────────────────────────────────────────
|
||||
function initJointsGrid() {
|
||||
const grid = $('joints-grid');
|
||||
if (!grid) return;
|
||||
grid.innerHTML = JOINT_NAMES.map((name, i) => `
|
||||
<div class="motor-row" id="mi-${i}">
|
||||
<span class="stale" id="ms-${i}" style="color:#ef4444">●</span>
|
||||
<span class="name">${name}</span>
|
||||
<span class="val pos" id="mp-${i}">0.00</span>
|
||||
<span class="val vel" id="mv-${i}">0.00</span>
|
||||
<span class="val tau" id="mt-${i}">0.00</span>
|
||||
</div>`).join('');
|
||||
}
|
||||
|
||||
function updateJointsGrid(robot) {
|
||||
if (!robot) return;
|
||||
const pos = robot.joint_pos || [];
|
||||
const vel = robot.joint_vel || [];
|
||||
const tau = robot.joint_torque || [];
|
||||
const upd = robot.update_counts || [];
|
||||
for (let i = 0; i < 16; i++) {
|
||||
const dot = $('ms-' + i);
|
||||
const cnt = upd[i] ?? 0;
|
||||
if (dot) dot.style.color = cnt > 0 ? '#30d158' : '#ef4444';
|
||||
const p = $('mp-' + i); if (p) p.textContent = (pos[i] || 0).toFixed(2);
|
||||
const v = $('mv-' + i); if (v) v.textContent = (vel[i] || 0).toFixed(2);
|
||||
const t = $('mt-' + i);
|
||||
if (t) {
|
||||
t.textContent = (tau[i] || 0).toFixed(2);
|
||||
t.style.color = Math.abs(tau[i] || 0) > 16 ? '#ff453a' : '#ff9f0a';
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ── State polling ─────────────────────────────────────────────────────────────
|
||||
function setText(id, text, cls) {
|
||||
const el = $(id);
|
||||
if (!el) return;
|
||||
el.textContent = text;
|
||||
if (cls !== undefined) el.className = 'diag-value ' + cls;
|
||||
}
|
||||
|
||||
function applyState(data) {
|
||||
const connected = data.connected &&
|
||||
(!data.local_receive_time || Date.now() / 1000 - data.local_receive_time < 2.5);
|
||||
|
||||
const stage = $('stage');
|
||||
if (stage) {
|
||||
if (!connected) {
|
||||
stage.textContent = 'DISCONNECTED';
|
||||
stage.className = 'stage DISCONNECTED';
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const mode = data.mode || 'UNKNOWN';
|
||||
if (mode !== currentMode) highlightMode(mode);
|
||||
|
||||
const rt = data.runtime || {};
|
||||
const src = rt.target_source || '--';
|
||||
const srcCls = src === 'safety_brake' ? 'bad'
|
||||
: src === 'runtime_policy' ? 'ok'
|
||||
: src === 'runtime_zero_hold' ? 'active' : '';
|
||||
setText('d-source', src, srcCls);
|
||||
setText('d-released', String(rt.runtime_released ?? '--'),
|
||||
rt.runtime_released ? 'ok' : '');
|
||||
setText('d-alpha', (rt.release_alpha ?? '--') !== '--'
|
||||
? Number(rt.release_alpha).toFixed(2) : '--');
|
||||
setText('d-zero', String(rt.zero_command ?? '--'));
|
||||
setText('d-estop', String(data.estop ?? '--'),
|
||||
data.estop ? 'bad' : 'ok');
|
||||
setText('d-mux', data.mux_status || '--');
|
||||
|
||||
const robot = data.robot || {};
|
||||
const imuAge = robot.imu_age_ms ?? null;
|
||||
setText('d-imu-fresh', String(robot.imu_fresh ?? '--'),
|
||||
robot.imu_fresh ? 'ok' : 'bad');
|
||||
setText('d-imu-age', imuAge !== null ? imuAge.toFixed(1) : '--',
|
||||
imuAge !== null ? (imuAge > 200 ? 'bad' : imuAge > 60 ? 'warn' : 'ok') : '');
|
||||
|
||||
const grav = robot.projected_gravity;
|
||||
setText('d-gravity', grav ? grav.map(v => Number(v).toFixed(2)).join(', ') : '--',
|
||||
grav && grav[2] < -0.5 ? 'ok' : 'warn');
|
||||
|
||||
setText('d-holdover', String(robot.holdover_count ?? '--'),
|
||||
(robot.holdover_count || 0) > 10 ? 'warn' : '');
|
||||
|
||||
const odomAge = robot.odom_age_ms ?? null;
|
||||
setText('d-odom-age', odomAge !== null ? odomAge.toFixed(1) : '--',
|
||||
odomAge !== null ? (odomAge > 500 ? 'bad' : odomAge > 200 ? 'warn' : 'ok') : '');
|
||||
|
||||
const lp = robot.odom_local_pos;
|
||||
setText('d-odom-pos', lp ? `x=${Number(lp[0]).toFixed(2)} y=${Number(lp[1]).toFixed(2)}` : '--');
|
||||
|
||||
const cv = data.cmd_vel || {};
|
||||
const lin = cv.linear || {};
|
||||
const ang = cv.angular || {};
|
||||
setText('cv-vx', (lin.x ?? 0).toFixed(3));
|
||||
setText('cv-vy', (lin.y ?? 0).toFixed(3));
|
||||
setText('cv-yaw', (ang.z ?? 0).toFixed(3));
|
||||
|
||||
updateJointsGrid(robot);
|
||||
}
|
||||
|
||||
async function poll() {
|
||||
try {
|
||||
const res = await fetch('/api/state');
|
||||
const data = await res.json();
|
||||
applyState(data);
|
||||
} catch (_) {}
|
||||
}
|
||||
|
||||
// ── WEB mode heartbeat ────────────────────────────────────────────────────────
|
||||
setInterval(() => {
|
||||
if (currentMode === 'WEB' && !dragging) sendCmd();
|
||||
}, 50);
|
||||
|
||||
// ── Event log ─────────────────────────────────────────────────────────────────
|
||||
function appendEvent(kind, detail, cls) {
|
||||
const el = $('events-log');
|
||||
if (!el) return;
|
||||
const div = document.createElement('div');
|
||||
const t = new Date().toLocaleTimeString();
|
||||
div.innerHTML = `<span class="ev-t">${t}</span> <span class="ev-${cls || 'ok'}">${kind}</span> <span style="color:#8e8e93">${detail || ''}</span>`;
|
||||
el.appendChild(div);
|
||||
while (el.children.length > 200) el.removeChild(el.firstChild);
|
||||
el.scrollTop = el.scrollHeight;
|
||||
}
|
||||
|
||||
// ── Init ──────────────────────────────────────────────────────────────────────
|
||||
initJointsGrid();
|
||||
setInterval(poll, 100);
|
||||
appendEvent('READY', '页面已加载，等待 Nano 连接', 'ok');
|
||||
@@ -0,0 +1,98 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN" data-theme="dark">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<title>sim2real ROS2 控制台</title>
|
||||
<link rel="stylesheet" href="style.css">
|
||||
</head>
|
||||
<body>
|
||||
<header class="glass-panel top-bar">
|
||||
<div class="top-bar-left">
|
||||
<h1>sim2real ROS2</h1>
|
||||
<span class="stage" id="stage">DISCONNECTED</span>
|
||||
</div>
|
||||
<div class="top-bar-center">
|
||||
<span class="label">控制模式</span>
|
||||
<button class="btn" id="btn-disabled">DISABLED</button>
|
||||
<button class="btn btn-remote" id="btn-remote">REMOTE</button>
|
||||
<button class="btn btn-web" id="btn-web">WEB</button>
|
||||
<button class="btn btn-nav" id="btn-nav">NAV</button>
|
||||
<div class="divider"></div>
|
||||
<button class="btn" id="btn-zero">速度归零</button>
|
||||
</div>
|
||||
<div class="top-bar-right">
|
||||
<button class="btn btn-danger" id="btn-estop">软急停</button>
|
||||
</div>
|
||||
</header>
|
||||
|
||||
<div class="glass-panel side-panel left-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">运行状态</h2>
|
||||
<div class="diag-row"><span class="diag-label">target_source</span><span class="diag-value" id="d-source">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">runtime_released</span><span class="diag-value" id="d-released">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">release_alpha</span><span class="diag-value" id="d-alpha">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">zero_command</span><span class="diag-value" id="d-zero">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">estop</span><span class="diag-value" id="d-estop">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">mux</span><span class="diag-value" id="d-mux">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">IMU & 里程计</h2>
|
||||
<div class="diag-row"><span class="diag-label">IMU fresh</span><span class="diag-value" id="d-imu-fresh">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">IMU age ms</span><span class="diag-value" id="d-imu-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">projected_gravity</span><span class="diag-value" id="d-gravity">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">holdover</span><span class="diag-value" id="d-holdover">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom age ms</span><span class="diag-value" id="d-odom-age">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">odom local pos</span><span class="diag-value" id="d-odom-pos">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section flex-1">
|
||||
<h2 class="panel-title">关节状态 (16轴)</h2>
|
||||
<div id="joints-grid" class="motors-grid-list"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="glass-panel side-panel right-panel">
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">Web 手动控制</h2>
|
||||
<div class="joystick-area">
|
||||
<div class="joystick" id="joystick"><div id="stick"></div></div>
|
||||
<p class="hint">拖动控制前后(vx)和横移(vy)，松开归零</p>
|
||||
</div>
|
||||
<div class="slider-group">
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vx</span>
|
||||
<input type="range" id="cmd-vx" class="glass-slider" min="-0.8" max="0.8" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vx-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">vy</span>
|
||||
<input type="range" id="cmd-vy" class="glass-slider" min="-0.3" max="0.3" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-vy-v">0.00</span>
|
||||
</div>
|
||||
<div class="slider-row">
|
||||
<span class="slider-label">yaw</span>
|
||||
<input type="range" id="cmd-yaw" class="glass-slider" min="-0.5" max="0.5" step="0.01" value="0">
|
||||
<span class="slider-val" id="cmd-yaw-v">0.00</span>
|
||||
</div>
|
||||
</div>
|
||||
<div class="cmd-display" id="cmd-display">vx=0.00 vy=0.00 yaw=0.00</div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section">
|
||||
<h2 class="panel-title">当前输出 /cmd_vel</h2>
|
||||
<div class="diag-row"><span class="diag-label">linear.x</span><span class="diag-value" id="cv-vx">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">linear.y</span><span class="diag-value" id="cv-vy">--</span></div>
|
||||
<div class="diag-row"><span class="diag-label">angular.z</span><span class="diag-value" id="cv-yaw">--</span></div>
|
||||
</div>
|
||||
|
||||
<div class="panel-section log-section flex-1">
|
||||
<h2 class="panel-title">事件流</h2>
|
||||
<div id="events-log" class="log"></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script src="app.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,263 @@
|
||||
/* sim2real ROS2 Web Debug — Apple Glass Design */
|
||||
:root {
|
||||
--bg-primary: #000000;
|
||||
--glass-bg: rgba(20, 20, 22, 0.65);
|
||||
--glass-border: rgba(255, 255, 255, 0.12);
|
||||
--glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
|
||||
--text-primary: #ffffff;
|
||||
--text-secondary: #ebebf5;
|
||||
--text-tertiary: #8e8e93;
|
||||
--accent: #0a84ff;
|
||||
--accent-hover: #409cff;
|
||||
--success: #30d158;
|
||||
--warning: #ffd60a;
|
||||
--danger: #ff453a;
|
||||
--blur-amount: 24px;
|
||||
--saturation: 180%;
|
||||
--spring: cubic-bezier(0.4, 0, 0.2, 1);
|
||||
--panel-radius: 16px;
|
||||
--font-family: -apple-system, BlinkMacSystemFont, "SF Pro Display", "PingFang SC", sans-serif;
|
||||
}
|
||||
|
||||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
body {
|
||||
font-family: var(--font-family);
|
||||
background: radial-gradient(circle at top left, #1a1a24 0%, #000000 100%);
|
||||
color: var(--text-primary);
|
||||
-webkit-font-smoothing: antialiased;
|
||||
min-height: 100vh;
|
||||
overflow-x: hidden;
|
||||
}
|
||||
|
||||
.glass-panel {
|
||||
background: var(--glass-bg);
|
||||
backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
-webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
|
||||
border: 0.5px solid var(--glass-border);
|
||||
box-shadow: var(--glass-shadow);
|
||||
z-index: 50;
|
||||
}
|
||||
|
||||
/* Top Bar */
|
||||
.top-bar {
|
||||
position: fixed;
|
||||
top: 16px;
|
||||
left: 50%;
|
||||
transform: translateX(-50%);
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
padding: 8px 16px;
|
||||
border-radius: 24px;
|
||||
width: 96%;
|
||||
max-width: 1400px;
|
||||
gap: 16px;
|
||||
}
|
||||
.top-bar-left, .top-bar-center, .top-bar-right {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
.top-bar-center { flex: 1; justify-content: center; }
|
||||
.top-bar h1 {
|
||||
font-size: 16px;
|
||||
font-weight: 600;
|
||||
background: linear-gradient(45deg, #fff, #8e8e93);
|
||||
-webkit-background-clip: text;
|
||||
-webkit-text-fill-color: transparent;
|
||||
}
|
||||
.divider { width: 1px; height: 24px; background: var(--glass-border); margin: 0 4px; }
|
||||
|
||||
/* Stage badge */
|
||||
.stage {
|
||||
padding: 4px 10px;
|
||||
border-radius: 12px;
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
background: rgba(255,255,255,0.1);
|
||||
color: var(--text-secondary);
|
||||
}
|
||||
.stage.DISCONNECTED { background: rgba(142,142,147,0.3); color: #aaa; }
|
||||
.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
|
||||
.stage.REMOTE { background: rgba(48,209,88,0.3); color: #8deda7; }
|
||||
.stage.WEB { background: rgba(0,122,255,0.3); color: #82c4ff; }
|
||||
.stage.NAV { background: rgba(255,214,10,0.3); color: #ffe680; }
|
||||
.stage.DISABLED { background: rgba(142,142,147,0.25); color: #aaa; }
|
||||
.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
|
||||
|
||||
/* Buttons */
|
||||
.btn {
|
||||
background: rgba(255,255,255,0.08);
|
||||
border: 1px solid rgba(255,255,255,0.1);
|
||||
border-radius: 8px;
|
||||
color: var(--text-primary);
|
||||
font-size: 12px;
|
||||
font-weight: 500;
|
||||
padding: 6px 12px;
|
||||
cursor: pointer;
|
||||
transition: all 0.2s var(--spring);
|
||||
font-family: inherit;
|
||||
}
|
||||
.btn:hover:not(:disabled) { background: rgba(255,255,255,0.15); transform: translateY(-1px); }
|
||||
.btn:active:not(:disabled) { transform: translateY(1px); }
|
||||
.btn:disabled { opacity: 0.5; cursor: not-allowed; }
|
||||
.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
|
||||
.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; }
|
||||
.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; }
|
||||
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
|
||||
.btn.active-mode { box-shadow: 0 0 0 2px white; }
|
||||
|
||||
.label { font-size: 11px; color: var(--text-tertiary); }
|
||||
|
||||
/* Side panels */
|
||||
.side-panel {
|
||||
position: fixed;
|
||||
top: 80px;
|
||||
bottom: 20px;
|
||||
width: 340px;
|
||||
border-radius: var(--panel-radius);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
.left-panel { left: 2%; }
|
||||
.right-panel { right: 2%; }
|
||||
.flex-1 { flex: 1; min-height: 0; overflow: hidden; display: flex; flex-direction: column; }
|
||||
|
||||
.panel-section {
|
||||
padding: 14px 16px;
|
||||
border-bottom: 0.5px solid var(--glass-border);
|
||||
}
|
||||
.panel-section:last-child { border-bottom: none; }
|
||||
|
||||
.panel-title {
|
||||
font-size: 11px;
|
||||
font-weight: 700;
|
||||
color: var(--text-tertiary);
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 0.5px;
|
||||
margin-bottom: 10px;
|
||||
}
|
||||
|
||||
/* Diag rows */
|
||||
.diag-row {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
padding: 4px 6px;
|
||||
background: rgba(0,0,0,0.2);
|
||||
border-radius: 4px;
|
||||
margin-bottom: 3px;
|
||||
}
|
||||
.diag-label { font-size: 11px; color: var(--text-tertiary); }
|
||||
.diag-value { font-size: 11px; font-family: monospace; color: var(--text-primary); font-weight: 600; }
|
||||
.diag-value.ok { color: var(--success); }
|
||||
.diag-value.warn { color: var(--warning); }
|
||||
.diag-value.bad { color: var(--danger); }
|
||||
.diag-value.active { color: #82c4ff; }
|
||||
|
||||
/* Joints grid */
|
||||
.motors-grid-list {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 2px;
|
||||
overflow-y: auto;
|
||||
flex: 1;
|
||||
}
|
||||
.motor-row {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 4px;
|
||||
padding: 3px 6px;
|
||||
background: rgba(0,0,0,0.25);
|
||||
border-radius: 5px;
|
||||
}
|
||||
.motor-row .name { font-size: 10px; color: var(--text-secondary); width: 72px; font-family: monospace; flex-shrink: 0; }
|
||||
.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; flex: 1; }
|
||||
.motor-row .val.pos { color: #0a84ff; }
|
||||
.motor-row .val.vel { color: #30d158; }
|
||||
.motor-row .val.tau { color: #ff9f0a; }
|
||||
.motor-row .stale { font-size: 9px; width: 8px; flex-shrink: 0; }
|
||||
|
||||
/* Joystick */
|
||||
.joystick-area { display: flex; flex-direction: column; align-items: center; gap: 8px; margin-bottom: 12px; }
|
||||
.joystick {
|
||||
position: relative;
|
||||
width: 180px;
|
||||
height: 180px;
|
||||
border-radius: 50%;
|
||||
background: radial-gradient(circle, rgba(10,132,255,0.2), rgba(10,132,255,0.05));
|
||||
border: 1px solid rgba(10,132,255,0.3);
|
||||
touch-action: none;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
#stick {
|
||||
position: absolute;
|
||||
left: 50%; top: 50%;
|
||||
width: 56px; height: 56px;
|
||||
border-radius: 50%;
|
||||
background: linear-gradient(135deg, #30d158, #0a84ff);
|
||||
transform: translate(-50%, -50%);
|
||||
box-shadow: 0 8px 24px rgba(0,0,0,0.4);
|
||||
}
|
||||
|
||||
/* Sliders */
|
||||
.slider-group { display: flex; flex-direction: column; gap: 8px; }
|
||||
.slider-row { display: flex; align-items: center; gap: 8px; }
|
||||
.slider-label { font-size: 11px; color: var(--text-tertiary); width: 28px; font-family: monospace; }
|
||||
.slider-val { font-size: 11px; color: var(--accent); font-family: monospace; width: 38px; text-align: right; }
|
||||
.glass-slider {
|
||||
flex: 1;
|
||||
-webkit-appearance: none;
|
||||
height: 4px;
|
||||
border-radius: 2px;
|
||||
background: rgba(255,255,255,0.2);
|
||||
outline: none;
|
||||
}
|
||||
.glass-slider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
width: 14px; height: 14px;
|
||||
border-radius: 50%;
|
||||
background: white;
|
||||
cursor: pointer;
|
||||
box-shadow: 0 2px 4px rgba(0,0,0,0.5);
|
||||
}
|
||||
.cmd-display {
|
||||
margin-top: 8px;
|
||||
padding: 6px 10px;
|
||||
background: rgba(0,0,0,0.3);
|
||||
border-radius: 6px;
|
||||
font-family: monospace;
|
||||
font-size: 12px;
|
||||
color: var(--accent);
|
||||
text-align: center;
|
||||
}
|
||||
.hint { font-size: 11px; color: var(--text-tertiary); text-align: center; }
|
||||
|
||||
/* Log */
|
||||
.log-section { flex: 1; overflow: hidden; display: flex; flex-direction: column; }
|
||||
.log {
|
||||
flex: 1;
|
||||
background: rgba(0,0,0,0.4);
|
||||
border: 1px solid rgba(255,255,255,0.05);
|
||||
border-radius: 6px;
|
||||
padding: 8px;
|
||||
font-family: monospace;
|
||||
font-size: 11px;
|
||||
color: var(--text-secondary);
|
||||
overflow-y: auto;
|
||||
}
|
||||
.log div { margin-bottom: 2px; line-height: 1.4; }
|
||||
.ev-t { color: var(--text-tertiary); margin-right: 4px; }
|
||||
.ev-ok { color: var(--success); }
|
||||
.ev-warn { color: var(--warning); }
|
||||
.ev-bad { color: var(--danger); }
|
||||
|
||||
@media (max-width: 960px) {
|
||||
.side-panel { position: relative; top: auto; width: 100%; left: 0; right: 0; border-radius: 0; height: auto; }
|
||||
.top-bar { width: 100%; border-radius: 0; top: 0; }
|
||||
body { padding-top: 56px; }
|
||||
}
|
||||
@@ -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,80 @@
|
||||
# 使用 ROS2 官方 Humble 基础镜像
|
||||
FROM ros:humble-ros-base-jammy
|
||||
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
|
||||
# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
|
||||
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
can-utils \
|
||||
libyaml-cpp-dev \
|
||||
libeigen3-dev \
|
||||
libusb-1.0-0-dev \
|
||||
libpcl-dev \
|
||||
libopencv-dev \
|
||||
ros-humble-navigation2 \
|
||||
ros-humble-nav2-bringup \
|
||||
ros-humble-pointcloud-to-laserscan \
|
||||
ros-humble-cv-bridge \
|
||||
ros-humble-pcl-conversions \
|
||||
wget \
|
||||
tar \
|
||||
python3-pip \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# ============================================================================
|
||||
# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
|
||||
# ============================================================================
|
||||
# - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
|
||||
# - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
|
||||
WORKDIR /opt
|
||||
RUN ARCH=$(uname -m) && \
|
||||
if [ "$ARCH" = "aarch64" ]; then \
|
||||
echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
|
||||
pip3 install --no-cache-dir onnxruntime-gpu && \
|
||||
SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
|
||||
mkdir -p onnxruntime/include onnxruntime/lib && \
|
||||
cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
|
||||
cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
|
||||
echo "[ONNX] CUDA ONNX Runtime installed."; \
|
||||
else \
|
||||
echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
|
||||
wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
|
||||
mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
|
||||
rm onnxruntime-linux-x64-1.16.3.tgz; \
|
||||
fi
|
||||
|
||||
ENV ONNXRUNTIME_DIR=/opt/onnxruntime
|
||||
|
||||
# 创建工作空间，将所有 C++ 源码包拷入
|
||||
WORKDIR /sim2real_ws/src
|
||||
COPY src/sim2real_bringup sim2real_bringup
|
||||
COPY src/sim2real_common sim2real_common
|
||||
COPY src/sim2real_hw sim2real_hw
|
||||
COPY src/sim2real_interfaces sim2real_interfaces
|
||||
COPY src/sim2real_runtime sim2real_runtime
|
||||
COPY src/odin_ros_driver odin_ros_driver
|
||||
COPY src/sim2real_nav2 sim2real_nav2
|
||||
|
||||
# 拷贝策略文件与运行脚本
|
||||
WORKDIR /sim2real_ws
|
||||
COPY policies policies
|
||||
COPY map map
|
||||
COPY tools tools
|
||||
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,76 @@
|
||||
# ROS 2 Sim2Real v2：里程计导航联调
|
||||
|
||||
本目录归档 `real/sim2real_ros2_v2(odom)`，对应重排主线的 `v0.12.0`。该阶段在 `v0.11.1` 的 Odin/TensorRT 与站姿调参基础上，固定纯里程计模式，增加 odom fallback 的 TF 冲突保护、A_min 路线和地图工具。
|
||||
|
||||
本工程保留当前 `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 仅作为编辑环境使用。
|
||||
- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
|
||||
除非训练与部署同步更新。
|
||||
- 默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- Odin `custom_map_mode` 固定为 `0`（纯里程计），避免没有 `map_a.bin` 时产生 map/odom TF 冲突。
|
||||
- `A_min.pcd`、`C.pcd` 和 `map_b.pcd` 均为确定性抽样预览，点数和哈希见 [`map/README.md`](map/README.md)。
|
||||
- TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关，其他机器应从 ONNX 重新生成。
|
||||
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,9 @@
|
||||
# `v0.12.0` 里程计导航点云
|
||||
|
||||
本阶段使用三份 ASCII PCD 进行路线和里程计导航联调。原始文件均使用 `tools/nav_tools/downsample_ascii_pcd.py` 确定性等步长抽样，抽样文件仅用于可视化、路线编辑和软件回归，不等价于原始高密度地图。
|
||||
|
||||
| 文件 | 原始点数 | 步长 | 抽样点数 | 抽样大小 | 原始 SHA-256 | 抽样 SHA-256 |
|
||||
| --- | ---: | ---: | ---: | ---: | --- | --- |
|
||||
| `map_b.pcd` | 1,080,047 | 4 | 270,012 | 8,680,283 B | `F17FC7E188C772D48E8D3FE20E3A934AFCDF59357C27BCCDCAC3C7E9AD1D1CB7` | `F3052431F217DE8FFFE87726F8E547A59C65048B0285F02AF4803C5F4E4C1B42` |
|
||||
| `A_min.pcd` | 683,908 | 3 | 227,969 | 7,300,173 B | `77C884B17D78C4D157F84093BFEA0729FA25382C70E706C4BA12913A3C90ED20` | `CC4556D472BE6E5B232B9BFEE43F5F337D710F88E88D9B90994368C580327DC1` |
|
||||
| `C.pcd` | 3,186,653 | 11 | 289,696 | 9,040,786 B | `C5C7E60B06A0FFEA15E55C14B8CFAFBC8525371C8C3AC9C72821AB736AED8A6C` | `1FC4124642D934460811DBF62EBE9C5C9CF30668BC42E4782B18F3C002ED1A25` |
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,276 @@
|
||||
{
|
||||
"name": "A_min_route",
|
||||
"map": "A_min",
|
||||
"frame_id": "map",
|
||||
"createdAt": "2026-07-04T00:40:56",
|
||||
"segments": [
|
||||
{
|
||||
"name": "segment_1",
|
||||
"obstacle": "slalom",
|
||||
"waypoints": [
|
||||
{
|
||||
"id": 1,
|
||||
"x": 9.3549,
|
||||
"y": 0.9079,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 2,
|
||||
"x": 11.1136,
|
||||
"y": 0.8621,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 3,
|
||||
"x": 13.0,
|
||||
"y": 0.75,
|
||||
"yawDeg": 88.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 4,
|
||||
"x": 12.9102,
|
||||
"y": 2.611,
|
||||
"yawDeg": 90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 5,
|
||||
"x": 12.9102,
|
||||
"y": 4.88,
|
||||
"yawDeg": 190.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 6,
|
||||
"x": 11.94,
|
||||
"y": 4.6203,
|
||||
"yawDeg": 178.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 7,
|
||||
"x": 10.65,
|
||||
"y": 4.65,
|
||||
"yawDeg": 178.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 8,
|
||||
"x": 9.3997,
|
||||
"y": 4.649,
|
||||
"yawDeg": -95.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 9,
|
||||
"x": 9.3092,
|
||||
"y": 3.8864,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 10,
|
||||
"x": 7.9263,
|
||||
"y": 4.0028,
|
||||
"yawDeg": 150.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 11,
|
||||
"x": 6.8148,
|
||||
"y": 4.6619,
|
||||
"yawDeg": 170.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 12,
|
||||
"x": 5.0054,
|
||||
"y": 5.0,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 13,
|
||||
"x": 3.2994,
|
||||
"y": 5.04,
|
||||
"yawDeg": 185.0,
|
||||
"speed": 0.7,
|
||||
"policy": "crawl",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 14,
|
||||
"x": 1.8838,
|
||||
"y": 4.8455,
|
||||
"yawDeg": 190.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 15,
|
||||
"x": 1.06,
|
||||
"y": 4.45,
|
||||
"yawDeg": 270.0,
|
||||
"speed": 0.35,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 16,
|
||||
"x": 1.0561,
|
||||
"y": 3.9,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 17,
|
||||
"x": 1.04,
|
||||
"y": 2.79,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 18,
|
||||
"x": 1.552,
|
||||
"y": 2.77,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 19,
|
||||
"x": 1.519,
|
||||
"y": 1.6677,
|
||||
"yawDeg": 180.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 20,
|
||||
"x": 0.4708,
|
||||
"y": 1.6677,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 21,
|
||||
"x": 0.4552,
|
||||
"y": 0.5,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 22,
|
||||
"x": 1.2783,
|
||||
"y": 0.5,
|
||||
"yawDeg": 90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 23,
|
||||
"x": 1.2628,
|
||||
"y": 1.6211,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 24,
|
||||
"x": 2.3576,
|
||||
"y": 1.5978,
|
||||
"yawDeg": -90.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 25,
|
||||
"x": 2.3265,
|
||||
"y": 0.5,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 26,
|
||||
"x": 3.12,
|
||||
"y": 0.5,
|
||||
"yawDeg": 45.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 27,
|
||||
"x": 3.8639,
|
||||
"y": 0.9844,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 28,
|
||||
"x": 6.8222,
|
||||
"y": 0.9145,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
},
|
||||
{
|
||||
"id": 29,
|
||||
"x": 9.5321,
|
||||
"y": 0.9223,
|
||||
"yawDeg": 0.0,
|
||||
"speed": 0.7,
|
||||
"policy": "rough",
|
||||
"tolerance": 0.15
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"yawToleranceDegDefault": 30.0
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
# `v0.12.0` 策略文件
|
||||
|
||||
本阶段运行配置使用 `model_9600` 作为 Rough 策略，并保留 Crawl ONNX 和部署契约参考模型。
|
||||
|
||||
| 文件 | 用途 |
|
||||
| --- | --- |
|
||||
| `model_9600.onnx` | 里程计版本 Rough ONNX |
|
||||
| `model_9600_fp16.engine` | 里程计版本 Rough TensorRT |
|
||||
| `model_crawl.onnx` | Crawl 候选；当前配置使用 IK 后端 |
|
||||
| `model_rough.onnx` | 原部署契约参考基线 |
|
||||
|
||||
其余 `model_6800`、`model_8400`、`model_10200` 等候选策略属于相邻实验或后续比赛版本，不在本 Tag 重复归档。
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
recorddata/
|
||||
/config/calib.yaml
|
||||
!/config/calib.yaml
|
||||
/log
|
||||
/map
|
||||
@@ -0,0 +1,8 @@
|
||||
v0.10.5 2026_0525
|
||||
1. 修改 recorddata 数据格式，新增 device_id、algorithm_version 字段
|
||||
2. 修复 SLAM 模式下下载地图失败的问题(USB2.0)
|
||||
3. 修复重定位模式下上传地图失败的问题(USB2.0)
|
||||
|
||||
v0.10.4 2026_0522
|
||||
1. 修复 USB2.0 心跳超时导致软断开的问题
|
||||
2. control_command.yaml 新增 custom_init_pose_search_radius 和 custom_init_pose_max_rot_deg 参数
|
||||
@@ -0,0 +1,447 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(odin_ros_driver)
|
||||
|
||||
|
||||
if(DEFINED BUILD_SYSTEM)
|
||||
set(ROS_VERSION ${BUILD_SYSTEM})
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
elseif(DEFINED ENV{ROS_DISTRO})
|
||||
if("$ENV{ROS_DISTRO}" MATCHES "foxy|galactic|humble|iron|rolling")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
elseif(DEFINED ENV{ROS_VERSION})
|
||||
if("$ENV{ROS_VERSION}" EQUAL "2")
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
set(ROS_VERSION "ROS1")
|
||||
endif()
|
||||
else()
|
||||
# Attempt automatic detection
|
||||
if(COMMAND catkin_package)
|
||||
set(ROS_VERSION "ROS1")
|
||||
elseif(COMMAND ament_package)
|
||||
set(ROS_VERSION "ROS2")
|
||||
else()
|
||||
# Default to ROS2
|
||||
set(ROS_VERSION "ROS2")
|
||||
message(WARNING "Unable to determine ROS version, defaulting to ROS2")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Add compile definitions after detecting ROS version
|
||||
if(ROS_VERSION STREQUAL "ROS2")
|
||||
add_definitions(-DROS2)
|
||||
message(STATUS "Defining ROS2")
|
||||
else()
|
||||
add_definitions(-DROS1)
|
||||
message(STATUS "Defining ROS1")
|
||||
endif()
|
||||
|
||||
message(STATUS "Build system: ${ROS_VERSION}")
|
||||
|
||||
# Platform detection
|
||||
execute_process(
|
||||
COMMAND uname -m
|
||||
OUTPUT_VARIABLE ARCH
|
||||
OUTPUT_STRIP_TRAILING_WHITESPACE
|
||||
)
|
||||
|
||||
if(ARCH STREQUAL "x86_64")
|
||||
set(TARGET_PLATFORM "x86")
|
||||
message(STATUS "Detected x86_64 architecture")
|
||||
elseif(ARCH MATCHES "arm|aarch64")
|
||||
set(TARGET_PLATFORM "arm")
|
||||
message(STATUS "Detected ARM architecture: ${ARCH}")
|
||||
else()
|
||||
message(WARNING "Unsupported architecture: ${ARCH}. Using default settings")
|
||||
set(TARGET_PLATFORM "unknown")
|
||||
endif()
|
||||
|
||||
# Set library path
|
||||
set(LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib")
|
||||
message(STATUS "Library directory: ${LIB_DIR}")
|
||||
|
||||
# Set library name based on platform
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_arm")
|
||||
else()
|
||||
set(LYD_HOST_API_LIB_NAME "lydHostApi_amd")
|
||||
endif()
|
||||
|
||||
# Find precompiled lydHostApi library
|
||||
find_library(LYD_HOST_API_LIB
|
||||
NAMES
|
||||
${LYD_HOST_API_LIB_NAME}
|
||||
lib${LYD_HOST_API_LIB_NAME}.a
|
||||
lib${LYD_HOST_API_LIB_NAME}.so
|
||||
PATHS ${LIB_DIR}
|
||||
NO_DEFAULT_PATH
|
||||
)
|
||||
|
||||
if(LYD_HOST_API_LIB)
|
||||
message(STATUS "Found lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
else()
|
||||
file(GLOB LIB_FILES "${LIB_DIR}/lib${LYD_HOST_API_LIB_NAME}.*")
|
||||
if(LIB_FILES)
|
||||
message(STATUS "Found library files: ${LIB_FILES}")
|
||||
set(LYD_HOST_API_LIB ${LIB_FILES})
|
||||
else()
|
||||
message(FATAL_ERROR "Could not find precompiled lydHostApi library in ${LIB_DIR}")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Set common compile options
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# Set optimization flags
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2")
|
||||
|
||||
# Find common dependencies
|
||||
find_package(PkgConfig REQUIRED)
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(PCL REQUIRED)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(OpenSSL REQUIRED)
|
||||
pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
|
||||
|
||||
# Shared include directories
|
||||
include_directories(
|
||||
include
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${PCL_INCLUDE_DIRS}
|
||||
${yaml-cpp_INCLUDE_DIR}
|
||||
${LIBUSB_INCLUDE_DIRS}
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/include
|
||||
)
|
||||
|
||||
# Shared library list
|
||||
set(COMMON_LIBS
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
${yaml-cpp_LIBRARIES}
|
||||
${OPENSSL_LIBRARIES}
|
||||
${LIBUSB_LIBRARIES}
|
||||
pthread
|
||||
rt
|
||||
${CMAKE_DL_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
)
|
||||
|
||||
# ===== ROS1 Configuration =====
|
||||
if(ROS_VERSION STREQUAL "ROS1")
|
||||
message(STATUS "Configuring for ROS1 build")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
tf
|
||||
image_transport
|
||||
)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport
|
||||
INCLUDE_DIRS include
|
||||
)
|
||||
|
||||
# Set output directories
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/${PROJECT_NAME})
|
||||
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib)
|
||||
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
target_link_libraries(host_sdk_sample
|
||||
${catkin_LIBRARIES}
|
||||
${COMMON_LIBS}
|
||||
${LYD_HOST_API_LIB}
|
||||
${LIBUSB_LIBRARIES}
|
||||
yaml-cpp
|
||||
${OpenCV_LIBS}
|
||||
pthread
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_library(depth_image_ros_node src/depth_image_ros_node.cpp)
|
||||
target_link_libraries(depth_image_ros_node
|
||||
pointcloud_depth_converter
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
add_executable(pcd2depth_node src/pcd2depth_ros.cpp)
|
||||
target_link_libraries(pcd2depth_node
|
||||
depth_image_ros_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector src/cloud_reprojector.cpp)
|
||||
target_link_libraries(cloud_reprojector
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_node src/cloud_reprojection_ros.cpp)
|
||||
target_link_libraries(cloud_reprojection_node
|
||||
cloud_reprojector
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_link_libraries(image_overlay_node
|
||||
${catkin_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
|
||||
# Installation rules
|
||||
install(TARGETS host_sdk_sample
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# ===== ROS2 Configuration =====
|
||||
elseif(ROS_VERSION STREQUAL "ROS2")
|
||||
message(STATUS "Configuring for ROS2 build")
|
||||
|
||||
# Find all necessary ROS2 packages
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_transport REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(message_filters REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
|
||||
# Create executable
|
||||
add_executable(host_sdk_sample
|
||||
src/host_sdk_sample.cpp
|
||||
src/yaml_parser.cpp
|
||||
src/rawCloudRender.cpp
|
||||
src/camera_pose_visualization.cpp
|
||||
)
|
||||
|
||||
# Link libraries
|
||||
target_link_libraries(host_sdk_sample
|
||||
${COMMON_LIBS}
|
||||
yaml-cpp
|
||||
usb-1.0
|
||||
)
|
||||
|
||||
# Add ROS2 dependencies
|
||||
ament_target_dependencies(host_sdk_sample
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
add_library(pointcloud_depth_converter_ros2 src/pointcloud_depth_converter.cpp)
|
||||
target_link_libraries(pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(depth_image_ros2_node_lib src/depth_image_ros2_node.cpp)
|
||||
target_link_libraries(depth_image_ros2_node_lib
|
||||
pointcloud_depth_converter_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
ament_target_dependencies(depth_image_ros2_node_lib
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(pcd2depth_ros2_node src/pcd2depth_ros2.cpp)
|
||||
target_link_libraries(pcd2depth_ros2_node
|
||||
depth_image_ros2_node_lib
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(pcd2depth_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
std_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_library(cloud_reprojector_ros2 src/cloud_reprojector.cpp)
|
||||
target_compile_definitions(cloud_reprojector_ros2 PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(cloud_reprojection_ros2_node src/cloud_reprojection_ros.cpp)
|
||||
target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
|
||||
target_link_libraries(cloud_reprojection_ros2_node
|
||||
cloud_reprojector_ros2
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
ament_target_dependencies(cloud_reprojection_ros2_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
)
|
||||
|
||||
add_executable(image_overlay_node src/image_overlay_node.cpp)
|
||||
target_compile_definitions(image_overlay_node PRIVATE ROS2)
|
||||
target_link_libraries(image_overlay_node
|
||||
${OpenCV_LIBS}
|
||||
)
|
||||
ament_target_dependencies(image_overlay_node
|
||||
rclcpp
|
||||
sensor_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
message_filters
|
||||
)
|
||||
|
||||
# Installation rules - ensure all install targets are defined before ament_package()
|
||||
# Install executable
|
||||
install(TARGETS
|
||||
host_sdk_sample
|
||||
pcd2depth_ros2_node
|
||||
cloud_reprojection_ros2_node
|
||||
image_overlay_node
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install package.xml
|
||||
install(FILES package.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install headers
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
# Install launch_ROS2 directory
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch_ROS2")
|
||||
install(DIRECTORY launch_ROS2/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
message(STATUS "Installing launch_ROS2 directory to share/${PROJECT_NAME}/launch")
|
||||
else()
|
||||
message(WARNING "launch_ROS2 directory not found")
|
||||
endif()
|
||||
# Install config files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/config")
|
||||
install(DIRECTORY config/
|
||||
DESTINATION share/${PROJECT_NAME}/config
|
||||
)
|
||||
endif()
|
||||
|
||||
# Install launch files
|
||||
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION share/${PROJECT_NAME}/launch
|
||||
)
|
||||
endif()
|
||||
ament_export_targets(export_${PROJECT_NAME})
|
||||
# Declare dependencies
|
||||
ament_export_dependencies(
|
||||
rclcpp
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
nav_msgs
|
||||
visualization_msgs
|
||||
cv_bridge
|
||||
image_transport
|
||||
pcl_conversions
|
||||
message_filters
|
||||
tf2
|
||||
tf2_ros
|
||||
geometry_msgs
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
ament_package()
|
||||
message(STATUS "Install targets added")
|
||||
|
||||
else()
|
||||
message(FATAL_ERROR "Invalid ROS_VERSION: ${ROS_VERSION}")
|
||||
endif()
|
||||
|
||||
# ARM platform specific link options
|
||||
if(TARGET_PLATFORM STREQUAL "arm")
|
||||
set_target_properties(host_sdk_sample PROPERTIES
|
||||
LINK_FLAGS "-Wl,--no-as-needed -Wl,--rpath=${LIB_DIR}"
|
||||
)
|
||||
message(STATUS "Adding ARM-specific link options and RPATH")
|
||||
endif()
|
||||
|
||||
# Add debug information
|
||||
message(STATUS "=======================================")
|
||||
message(STATUS "Project: ${PROJECT_NAME}")
|
||||
message(STATUS "ROS_VERSION: ${ROS_VERSION}")
|
||||
message(STATUS "Target platform: ${TARGET_PLATFORM}")
|
||||
message(STATUS "lydHostApi library: ${LYD_HOST_API_LIB}")
|
||||
message(STATUS "libusb library: ${LIBUSB_LIBRARIES}")
|
||||
message(STATUS "=======================================")
|
||||
|
||||
@@ -0,0 +1,201 @@
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright 2025 Manifold Tech Ltd.
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@@ -0,0 +1,567 @@
|
||||
# Odin_ROS_Driver 说明文档
|
||||
|
||||
Odin 传感器模块 ROS 驱动套件（Manifold Tech Ltd.）
|
||||
|
||||
Odin1 Wiki：https://manifoldtechltd.github.io/wiki/Odin1/Cover.html
|
||||
|
||||
## Odin_ROS_Driver
|
||||
|
||||
兼容性：
|
||||
|
||||
● ROS 1（推荐 LTS 版本：Noetic）
|
||||
|
||||
● ROS 2（推荐 LTS 版本：Humble）
|
||||
|
||||
## 重要提示：
|
||||
|
||||
本驱动包提供点云 SLAM 应用的核心功能，面向特定使用场景。仅供专业技术人员进行二次开发使用。最终用户需根据实际部署环境进行场景优化和定制开发，以满足运行需求。
|
||||
|
||||
## 1. 版本
|
||||
|
||||
当前版本：v0.10.2
|
||||
|
||||
所需设备固件版本：v0.10.0
|
||||
|
||||
## 2. 准备工作
|
||||
|
||||
### 2.1 操作系统要求
|
||||
|
||||
● ROS Noetic 和 ROS2 Foxy 需 Ubuntu 20.04；
|
||||
|
||||
● ROS2 Humble 需 Ubuntu 22.04；
|
||||
|
||||
● 当前不支持 Ubuntu 18.04；
|
||||
|
||||
● Ubuntu 24.04 尚未官方支持，但可能经过一定修改后运行。
|
||||
|
||||
### 2.2 依赖项
|
||||
|
||||
● OpenCV >= 4.2.0（推荐 4.5.5/4.8.0，请确保仅安装一个 OpenCV 版本）
|
||||
|
||||
● yaml-cpp
|
||||
|
||||
● thread
|
||||
|
||||
● OpenSSL
|
||||
|
||||
● Eigen3
|
||||
|
||||
### 2.3 依赖安装
|
||||
|
||||
#### 2.3.1 系统基础
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt-get install build-essential cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
|
||||
```
|
||||
|
||||
#### 2.3.2 yaml-cpp
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt install -y libyaml-cpp-dev
|
||||
```
|
||||
|
||||
#### 2.3.3 libusb
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt install -y libusb-1.0-0-dev
|
||||
```
|
||||
|
||||
#### 2.3.4 OpenCV
|
||||
```shell
|
||||
sudo apt update
|
||||
sudo apt-get install libopencv-dev
|
||||
```
|
||||
|
||||
#### 2.3.5 ROS 安装
|
||||
|
||||
ROS Noetic 安装请参考：
|
||||
[ROS Noetic 安装指南](https://wiki.ros.org/noetic/Installation)
|
||||
|
||||
ROS2 Foxy 安装请参考：
|
||||
[ROS Foxy 安装指南](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
ROS2 Humble 安装请参考：
|
||||
[ROS Humble 安装指南](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
## 3. 准备
|
||||
|
||||
### 3.1 创建 Udev 规则
|
||||
```shell
|
||||
sudo vim /etc/udev/rules.d/99-odin-usb.rules
|
||||
```
|
||||
在 99-odin-usb.rules 文件中添加以下内容：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
重新加载规则并重新插拔设备：
|
||||
```shell
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
### 3.2 克隆源码
|
||||
```shell
|
||||
git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
|
||||
```
|
||||
注意：
|
||||
请将源码克隆到 "[ros_workspace]/src/" 目录下，否则会导致编译错误。
|
||||
|
||||
### 3.3 编译
|
||||
|
||||
#### 3.3.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/noetic/setup.bash
|
||||
./script/build_ros.sh
|
||||
```
|
||||
|
||||
#### 3.3.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source /opt/ros/foxy/setup.bash
|
||||
./script/build_ros2.sh
|
||||
```
|
||||
|
||||
### 3.4 运行：
|
||||
|
||||
#### 3.4.1 ROS1（以 Noetic 为例）：
|
||||
|
||||
```shell
|
||||
source [ros_workspace]/devel/setup.bash
|
||||
roslaunch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros_workspace：用户的 ROS 环境工作区；
|
||||
```shell
|
||||
roslaunch odin_ros_driver odin1_ros1.launch
|
||||
```
|
||||
|
||||
#### 3.4.2 ROS2（以 Foxy 为例）：
|
||||
|
||||
```shell
|
||||
source [ros2_workspace]/install/setup.bash
|
||||
ros2 launch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver：包名；
|
||||
|
||||
● launch file：启动文件名；
|
||||
|
||||
● ros2_workspace：用户的 ROS2 环境工作区；
|
||||
|
||||
ROS2 Demo 启动命令：
|
||||
```shell
|
||||
ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
```
|
||||
|
||||
### 3.5 运行模式：
|
||||
|
||||
可通过 `config/control_command.yaml` 中的 `custom_map_mode` 参数配置运行模式。
|
||||
|
||||
#### 里程计模式
|
||||
|
||||
设置 `custom_map_mode = 0` 启用里程计模式。此模式下，map 坐标系与 odom 坐标系共享同一位姿。
|
||||
|
||||
若发现里程计数据漂移，可使用脚本命令 `./set_param.sh algo_reset 1` 动态复位算法。
|
||||
|
||||
#### SLAM 建图模式
|
||||
|
||||
设置 `custom_map_mode = 1` 启用 SLAM 模式。此模式在里程计模式基础上，提供**回环检测**和**地图保存**功能。
|
||||
|
||||
启动驱动后，odin1 将自动进行建图并缓存地图数据。场景采集完成后，需在驱动源码目录下执行 `./set_param.sh save_map 1` 以保存自程序启动以来采集的所有地图数据。地图将保存至 `config/control_command.yaml` 中 `mapping_result_dest_dir` 和 `mapping_result_file_name` 参数指定的路径。若未指定这些参数，将使用默认值。
|
||||
|
||||
首次保存后，可再次执行该命令保存新地图。每次保存操作都会生成一个新的地图文件。（连续保存操作之间请间隔至少 5 秒）
|
||||
|
||||
地图原点对应程序启动时 odom 坐标系的起点。
|
||||
|
||||
##### 重定位模式
|
||||
|
||||
要启用重定位，设置 `custom_map_mode = 2`，并通过 `config/control_command.yaml` 中的 `relocalization_map_abs_path` 参数指定预建地图的绝对路径。
|
||||
|
||||
启动后，odin1 将基于当前视点和指定地图启动重定位过程。为保障高成功率，建议在距 SLAM 轨迹原始位置 1 米、±10° 范围内启动。
|
||||
|
||||
注意，重定位性能高度依赖环境条件。在特征丰富的场景中，成功匹配可能发生在 1m/10° 范围之外，而其他环境可能需要更严格的条件。建议在实际部署环境中测试，以确定实际容忍范围。
|
||||
|
||||
若初始重定位失败，系统将临时以降级 SLAM 模式运行（此状态下地图保存功能禁用）。在此过程中可自由移动 odin1，它将在后台持续尝试重定位。一旦成功，将发布 map 与 odom 坐标系之间的 TF。（提示：初始化后轻轻晃动或移动设备有助于提高重定位准确率。）
|
||||
|
||||
以下话题在 odom 坐标系下发布：`/odin1/cloud_slam`、`/odin1/odom`、`/odin1/highodom` 和 `/odin1/path`。若需在 map 坐标系下获取这些数据，请应用从 odom 坐标系到 map 坐标系的 TF 变换。
|
||||
|
||||
## 4. 文件结构与数据格式
|
||||
### 4.1 文件结构
|
||||
```shell
|
||||
Odin_ROS_Driver/ // ROS1/ROS2 驱动包
|
||||
3rdparty/ // 第三方库
|
||||
src/
|
||||
host_sdk_sample.cpp // 示例源码
|
||||
yaml_parser.cpp // YAML 参数读取源码
|
||||
rawCloudRender.cpp // RenderCloud 渲染源码
|
||||
depth_image_ros_node.cpp // depth_image_ros_node 节点
|
||||
depth_image_ros2_node.cpp // depth_image_ros2_node 节点
|
||||
pcd2depth_ros.cpp // pcd2depth_ros 源码
|
||||
pcd2depth_ros2.cpp // pcd2depth_ros2 源码
|
||||
pointcloud_depth_converter.cpp // pointcloud_depth_converter 源码
|
||||
cloud_reprojection_ros.cpp // 云重投影节点源码 (ROS1/ROS2)
|
||||
cloud_reprojector.cpp // 云重投影核心逻辑
|
||||
lib/
|
||||
liblydHostApi_amd.a // AMD 平台静态库
|
||||
liblydHostApi_arm.a // ARM 平台静态库
|
||||
include/
|
||||
host_sdk_sample.h // 示例头文件
|
||||
lidar_api_type.h // API 数据结构头文件
|
||||
lidar_api.h // API 函数声明
|
||||
yaml_parser.h // 参数文件读取头文件
|
||||
rawCloudRender.h // RenderCloud 相关 API
|
||||
data_logger.h // 数据保存日志
|
||||
depth_image_ros_node.hpp // depth_image_ros_node 头文件
|
||||
depth_image_ros2_node.hpp // depth_image_ros2_node 头文件
|
||||
pointcloud_depth_converter.hpp // pointcloud_depth_convert 头文件
|
||||
cloud_reprojection_ros_node.hpp // cloud_reprojection_ros_node 头文件 (ROS1/ROS2)
|
||||
cloud_reprojector.hpp // 云重投影核心类
|
||||
config/
|
||||
control_command.yaml // 驱动控制参数文件
|
||||
calib.yaml // 设备标定参数 yaml，每个设备独一无二。每次连接 ROS 驱动时从设备读取
|
||||
launch_ROS1/
|
||||
odin1_ros1.launch // ROS1 启动文件
|
||||
launch_ROS2/
|
||||
odin1_ros2.launch.py // ROS2 启动文件
|
||||
script/
|
||||
build_ros1.sh // ROS1 安装脚本
|
||||
build_ros2.sh // ROS2 安装脚本
|
||||
recorddata/ // 存放可导入 MindCloud 的录制数据
|
||||
log/ // 存放日志文件
|
||||
Driver_{timestamp}/ // 每次启动驱动时生成的日志文件夹
|
||||
Conn_{timestamp}/ // 每次 odin1 设备连接时生成的日志文件
|
||||
dev_status.csv // 设备状态日志
|
||||
README.md // 使用说明
|
||||
CMakeLists.txt // CMake 构建文件
|
||||
License // 许可证文件
|
||||
```
|
||||
### 4.2 启动文件
|
||||
| 启动文件名 | 说明 |
|
||||
|--------------------------|-------------|
|
||||
| odin1_ros1.launch | ROS1 启动文件 - Odin1 基础操作演示 |
|
||||
| odin1_ros2.launch.py | ROS2 启动文件 - Odin1 基础操作演示 |
|
||||
|
||||
|
||||
### 4.3 ROS 话题
|
||||
Odin ROS 驱动的内部参数定义在 config/control_command.yaml 中。以下是常用参数说明：
|
||||
|
||||
| 话题 | control_command.yaml | 详细说明 |
|
||||
|---------------------------|----------------------|----------------------|
|
||||
| odin1/imu | sendimu | IMU 话题 |
|
||||
| odin1/image | sendrgb | RGB 相机话题，由设备原始 JPEG 数据解码，bgr8 格式 |
|
||||
| odin1/image_undistort | sendrgbundistort | 去畸变 RGB 相机话题，经设备 calib.yaml 标定参数处理 |
|
||||
| odin1/image/compressed | sendrgbcompressed | RGB 相机压缩话题，设备原始 JPEG 数据 |
|
||||
| odin1/cloud_raw | senddtof | 原始点云话题 |
|
||||
| odin1/cloud_render | sendcloudrender | 渲染点云话题，经原始点云、RGB 图像及设备 calib.yaml 处理 |
|
||||
| odin1/cloud_slam | sendcloudslam | SLAM 点云话题 |
|
||||
| odin1/odometry | sendodom | 里程计话题 |
|
||||
| odin1/odometry_high | sendodom | 高频里程计话题 |
|
||||
| odin1/path | showpath | 里程计路径话题 |
|
||||
| tf | sendodom | TF 树话题 |
|
||||
| odin1/depth_img_competetion | senddepth | 稠密深度图话题。需较高算力，仅作演示。与 odin1/image_undistort 一一对应。使用时请直接订阅本话题而非 echo。原始值即为深度数据，无需额外转换。 |
|
||||
| odin1/depth_img_competetion_cloud | senddepth | 稠密深度点云话题。需较高算力，仅作演示 |
|
||||
| odin1/reprojected_image | sendreprojection | 重投影像素话题。利用里程计将 cloud_slam 投影至相机图像。在主机端处理。 |
|
||||
|
||||
### 4.4 数据格式
|
||||
|
||||
1. 原始点云（cloud_raw）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
uint8 intensity // 反射率，范围 0–255
|
||||
uint16 confidence // 点置信度，典型场景下取值范围约 0–1300，数值越高可靠性越强。推荐过滤阈值 30-35，应结合实际环境调整。
|
||||
float32 offset_time // 相对基准时间戳的时间偏移量，单位：秒
|
||||
```
|
||||
|
||||
要在 PCL 中使用此自定义格式，首先定义点类型：
|
||||
```cpp
|
||||
/*** LS ***/
|
||||
namespace ls_ros {
|
||||
struct EIGEN_ALIGN16 Point {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
uint8_t intensity;
|
||||
uint16_t confidence;
|
||||
float offset_time;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
} // namespace ls_ros
|
||||
|
||||
POINT_CLOUD_REGISTER_POINT_STRUCT(ls_ros::Point,
|
||||
(float, x, x)
|
||||
(float, y, y)
|
||||
(float, z, z)
|
||||
(uint8_t, intensity, intensity)
|
||||
(uint16_t, confidence, confidence)
|
||||
(float offset_time , offset_time)
|
||||
)
|
||||
```
|
||||
然后即可轻松将 ROS sensor_msgs::PointCloud2 消息转换为 PCL 点云：
|
||||
```
|
||||
pcl::PointCloud<ls_ros::Point> ls_cloud;
|
||||
pcl::fromROSMsg(*msg, ls_cloud);
|
||||
```
|
||||
|
||||
2. SLAM 点云（cloud_slam）与直接渲染点云（cloud_render）包含以下字段：
|
||||
```
|
||||
float32 x // X 轴，单位：米
|
||||
float32 y // Y 轴，单位：米
|
||||
float32 z // Z 轴，单位：米
|
||||
float32 rgb // RGB 颜色值
|
||||
```
|
||||
|
||||
### 4.5 其他功能
|
||||
|
||||
| control_command.yaml 参数 | 详细说明 |
|
||||
|----------------------------|----------------------|
|
||||
| use_host_ros_time | 时间同步模式：0 - 使用 odin 内部系统时间作为数据时间戳（典型用法，推荐）；1 - 接收时使用主机 ROS 时间（不推荐大多数用户使用）；2 - 通过类 NTP 同步将 odin1 时间对齐至主机时间，时间戳为传感器数据在主机时间轴上的接收时间。 |
|
||||
| strict_usb3.0_check | 严格 USB3.0 检查，关闭后即使 USB 连接低于 3.0 标准也允许连接 |
|
||||
| recorddata | 以特定格式记录数据，可导入 MindCloud(TM) 进行后处理。请注意这将消耗大量存储空间，测试显示 10 分钟数据约占 9.5GB。 |
|
||||
| devstatuslog | 设备状态日志记录，当前将设备状态（SoC 温度、CPU 占用率、RAM 占用率、dToF 传感器温度等）及数据发送/接收速率保存至 log 目录下的 devstatus.csv。每次启动驱动时创建新文件。 |
|
||||
| showcamerapose | 显示相机位姿及视野范围。 |
|
||||
| custom_map_mode | 运行模式：模式 0 - 里程计模式：map 坐标系与 odom 坐标系共享同一位姿。模式 1 - 建图模式（带回环检测）：该模式支持地图保存。模式 2 - 重定位模式：需指定地图文件绝对路径，重定位成功后将输出 map 与 odom 坐标系之间的 TF 关系。|
|
||||
| custom_init_pos | 初始化位置（当前未启用）。 |
|
||||
| relocalization_map_abs_path | 地图文件绝对路径：用于重定位模式。 |
|
||||
| mapping_result_dest_dir 和 mapping_result_file_name | 建图模式下地图保存路径与文件名：若未指定，将使用默认值。 |
|
||||
|
||||
## 5. 常见问题
|
||||
### 5.1 重新启动宿主 SDK 时出现段错误
|
||||
**错误信息**
|
||||
60 秒内未连接任何设备
|
||||
|
||||
**解决方案**
|
||||
1. 请重新为 Odin 模块上电 # 断开并重新连接 odin 电源
|
||||
|
||||
2. 重新初始化 Odin SDK # 设备重启后执行 SDK
|
||||
|
||||
|
||||
### 5.2 编译时出现库链接失败
|
||||
|
||||
**错误信息**
|
||||
ld: cannot find -llydHostApi 或符号查找错误
|
||||
|
||||
**解决方案**
|
||||
|
||||
1. 清理之前的构建产物
|
||||
|
||||
ROS1
|
||||
```shell
|
||||
rm -rf devel/ build/
|
||||
```
|
||||
ROS2
|
||||
```shell
|
||||
rm -rf devel/ install/ log/
|
||||
```
|
||||
2. 重新运行脚本安装
|
||||
|
||||
### 5.3 Docker GUI 透传失败
|
||||
|
||||
**错误信息**
|
||||
Unable to open X display 或 No protocol specified
|
||||
|
||||
**解决方案**
|
||||
```shell
|
||||
xhost + # 此命令启用 Docker 容器的图形透传
|
||||
```
|
||||
|
||||
### 5.4 ROS 驱动以"获取版本失败"错误退出
|
||||
|
||||
**错误信息**
|
||||
```shell
|
||||
<ERROR><api.cpp:lidar_get_version:672>: get device version fail.
|
||||
get version failed.
|
||||
```
|
||||
|
||||
**解决方案**
|
||||
|
||||
设备固件版本过低，请升级至最新版本。
|
||||
|
||||
|
||||
### 5.5 RVIZ 长时间无响应
|
||||
|
||||
**错误信息**
|
||||
Rviz 无响应，稍后终端打印"Device disconnected, waiting for reconnection..."
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新为 Odin 模块上电
|
||||
|
||||
### 5.6 设备无响应
|
||||
|
||||
**错误信息**
|
||||
Missed ok response from device, probably wrong interaction procedure.
|
||||
|
||||
**解决方案**
|
||||
|
||||
请采用 5.1 所述的解决方案
|
||||
|
||||
### 5.7 设备无外部标定文件
|
||||
|
||||
**错误信息**
|
||||
ERROR：Missing camera node 'cam_0'
|
||||
|
||||
**解决方案**
|
||||
|
||||
请重新插拔 USB
|
||||
|
||||
### 5.8 ROS 驱动在数据流启动后立即报设备断开
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
Device detaching...
|
||||
Wating for device reconnection...
|
||||
Device disconnected, waiting for reconnection...
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
多见于 ROS2 环境且连接到复杂网络环境（如办公 WiFi 和以太网）的情况。ROS2 默认为广播模式，复杂网络环境可能导致 ROS2 发布阻塞，从而引发设备断开。
|
||||
|
||||
**解决方案**
|
||||
|
||||
若不需要跨设备通信，请将 ROS2 限制为仅本地通信：
|
||||
```shell
|
||||
export ROS_LOCALHOST_ONLY=1
|
||||
```
|
||||
|
||||
若需要跨设备通信，请尽量简化网络环境。建议使用仅包含必要设备的小型局域网。
|
||||
|
||||
### 5.9 ROS 驱动在数据流启动后立即崩溃
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
[host_sdk_sample-2] process has died ......
|
||||
```
|
||||
|
||||
**测试**
|
||||
|
||||
在 control_command.yaml 中设置 sendrgb = 0 禁用 odin1/image，然后重试。若驱动此时正常工作，则问题很可能与系统安装了多个 OpenCV 版本有关。
|
||||
|
||||
**解决方案**
|
||||
|
||||
卸载多余的 OpenCV 版本，仅保留单一完整版本，然后重新编译驱动并重试。
|
||||
|
||||
### 5.10 ROS 驱动打印"TF_OLD_DATA ignoring data"警告
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
[rviz2-3] Warning: TF_OLD_DATA ignoring data from the past for frame odin1_base_link at time 20.547632 according to authority Authority undetectable
|
||||
[rviz2-3] Possible reasons are listed at http://wiki.ros.org/tf/Errors%20explained
|
||||
[rviz2-3] at line 294 in ./src/buffer_core.cpp
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是 ROS 和 rviz 的一项功能，用于警告用户某些 TF 数据因时间戳冲突而被忽略。常见于用户保持 ROS 驱动运行的同时对 odin 设备断电重启，导致 odin 内部系统时间被重置，新数据时间戳与 rviz 上次运行期间接收的旧数据产生冲突。
|
||||
|
||||
**解决方案**
|
||||
|
||||
rviz GUI 底部有一个重置按钮。点击此按钮将重置 rviz 内部状态并停止警告。
|
||||
|
||||
### 5.11 ROS 驱动打印"unknown cmd code: xx"错误
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
<ERROR><api.cpp:cmd_data_deal:418>: unknow command code 21.
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
这是由于 ROS 驱动版本与设备固件版本不匹配，导致 ROS 驱动无法解码新版固件新增的数据。
|
||||
|
||||
**解决方案**
|
||||
|
||||
请确保使用最新版本的 ROS 驱动和设备固件。
|
||||
|
||||
### 5.12 USB 设备访问错误（LIBUSB_ERROR_BUSY 或 LIBUSB_ERROR_ACCESS）
|
||||
|
||||
**错误信息**
|
||||
|
||||
```shell
|
||||
libusb: error [udev_hotplug_event] ignoring udev action bind
|
||||
LIBUSB_ERROR_BUSY
|
||||
```
|
||||
|
||||
或
|
||||
|
||||
```shell
|
||||
libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13
|
||||
LIBUSB_ERROR_ACCESS
|
||||
```
|
||||
|
||||
**原因**
|
||||
|
||||
- **LIBUSB_ERROR_BUSY**：另一个进程正在使用该 USB 设备。常见于多个 ROS 驱动实例正在运行，或其他应用程序（如之前崩溃的实例）仍持有设备句柄。
|
||||
|
||||
- **LIBUSB_ERROR_ACCESS**：当前用户无权访问 USB 设备。通常因缺少 udev 规则或用户权限不足导致。
|
||||
|
||||
**解决方案**
|
||||
|
||||
针对 **LIBUSB_ERROR_BUSY**：
|
||||
|
||||
1. 检查是否有其他驱动实例正在运行：
|
||||
```shell
|
||||
ps aux | grep host_sdk_sample
|
||||
```
|
||||
|
||||
2. 终止所有现存实例：
|
||||
```shell
|
||||
killall host_sdk_sample
|
||||
```
|
||||
|
||||
3. 若问题仍然存在，请拔插 USB 设备以重置设备状态。
|
||||
|
||||
针对 **LIBUSB_ERROR_ACCESS**：
|
||||
|
||||
1. 添加设备 udev 规则。创建文件 `/etc/udev/rules.d/99-odin.rules`，内容如下：
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
|
||||
2. 重新加载 udev 规则：
|
||||
```shell
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
3. 或者，使用 sudo 运行驱动（不推荐用于生产环境）：
|
||||
```shell
|
||||
sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py
|
||||
```
|
||||
|
||||
4. 确保当前用户属于 `plugdev` 用户组：
|
||||
```shell
|
||||
sudo usermod -aG plugdev $USER
|
||||
```
|
||||
然后注销并重新登录，使组变更生效。
|
||||
|
||||
## 6. 联系方式
|
||||
|
||||
您可通过 support@manifoldtech.cn 联系我们的技术支持。
|
||||
|
||||
为帮助诊断问题，请向我们的 FAE 工程师提供以下信息：
|
||||
|
||||
1. 当前固件版本
|
||||
```shell
|
||||
[device_version_capture]: ros_driver_version: [版本号]
|
||||
```
|
||||
2. 正在使用的电源适配器和转换线缆照片。
|
||||
|
||||
3. 问题是偶发性还是持续性的？
|
||||
|
||||
4. 提供问题场景的图像。
|
||||
|
||||
5. **第 V 节**中的故障排除方法是否解决了问题？
|
||||
|
||||
6. 问题解决的预期时间线。
|
||||
@@ -0,0 +1,642 @@
|
||||
# Relocalization Guide / 重定位使用指南
|
||||
|
||||
This guide explains how to use the relocalization feature in Odin ROS Driver, including automatic relocalization and init position relocalization modes.
|
||||
|
||||
本指南介绍如何使用 Odin ROS Driver 的重定位功能，包括自动重定位和指定初始位置重定位两种模式。
|
||||
|
||||
---
|
||||
|
||||
## Table of Contents / 目录
|
||||
|
||||
1. [Overview / 概述](#overview--概述)
|
||||
2. [Prerequisites / 前提条件](#prerequisites--前提条件)
|
||||
3. [Mode 1: Auto Relocalization / 自动重定位](#mode-1-auto-relocalization--自动重定位)
|
||||
4. [Mode 2: Init Position Relocalization / 指定初始位置重定位](#mode-2-init-position-relocalization--指定初始位置重定位)
|
||||
5. [init_pos Format / init_pos 格式说明](#init_pos-format--init_pos-格式说明)
|
||||
6. [Configuration Examples / 配置示例](#configuration-examples--配置示例)
|
||||
7. [Programmatic API / 编程接口](#programmatic-api--编程接口)
|
||||
8. [Troubleshooting / 故障排除](#troubleshooting--故障排除)
|
||||
|
||||
---
|
||||
|
||||
## Overview / 概述
|
||||
|
||||
### English
|
||||
|
||||
Relocalization mode (`custom_map_mode: 2`) allows Odin to localize itself within a pre-built map. There are two approaches:
|
||||
|
||||
| Mode | Description | Use Case |
|
||||
|------|-------------|----------|
|
||||
| **Auto Relocalization** | Algorithm automatically searches for position in the map | Starting position is unknown or within recommended range |
|
||||
| **Init Position Relocalization** | User provides an initial pose estimate via `init_pos` | Starting position is known, faster convergence needed |
|
||||
|
||||
### 中文
|
||||
|
||||
重定位模式（`custom_map_mode: 2`）允许 Odin 在预先构建的地图中进行自我定位。有两种方式：
|
||||
|
||||
| 模式 | 描述 | 适用场景 |
|
||||
|------|------|----------|
|
||||
| **自动重定位** | 算法自动在地图中搜索位置 | 起始位置未知，或在推荐范围内 |
|
||||
| **指定初始位置重定位** | 用户通过 `init_pos` 提供初始位姿估计 | 起始位置已知，需要更快收敛 |
|
||||
|
||||
---
|
||||
|
||||
## Prerequisites / 前提条件
|
||||
|
||||
### English
|
||||
|
||||
1. **Pre-built map file**: A `.bin` map file created in SLAM mode (`custom_map_mode: 1`)
|
||||
2. **Map file path**: Know the absolute path to your map file
|
||||
3. **Starting position**: For init position mode, know the approximate starting pose in map coordinates
|
||||
|
||||
### 中文
|
||||
|
||||
1. **预构建的地图文件**：在 SLAM 模式（`custom_map_mode: 1`）下创建的 `.bin` 地图文件
|
||||
2. **地图文件路径**：知道地图文件的绝对路径
|
||||
3. **起始位置**：对于指定初始位置模式，需要知道在地图坐标系中的大致起始位姿
|
||||
|
||||
---
|
||||
|
||||
## Mode 1: Auto Relocalization / 自动重定位
|
||||
|
||||
### English
|
||||
|
||||
In auto relocalization mode, the algorithm automatically searches for the device's position within the map based on current sensor observations.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos is NOT set or uses default [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**Recommended Starting Conditions**:
|
||||
- Within **1 meter** of a position on the original SLAM trajectory
|
||||
- Within **±10 degrees** of the original orientation
|
||||
- In a visually distinctive area of the map
|
||||
|
||||
**Behavior**:
|
||||
1. On startup, Odin attempts to match current observations with the map
|
||||
2. If successful, TF between `map` and `odom` frames is published
|
||||
3. If unsuccessful, system operates in fallback SLAM mode (map saving disabled)
|
||||
4. Relocalization attempts continue in background until successful
|
||||
|
||||
**Tips**:
|
||||
- Gently shaking or moving the device after startup can improve relocalization accuracy
|
||||
- Highly distinctive scenes may allow successful matching beyond the 1m/10° range
|
||||
|
||||
### 中文
|
||||
|
||||
在自动重定位模式下，算法根据当前传感器观测自动在地图中搜索设备位置。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
# custom_init_pos 不设置或使用默认值 [0,0,0,0,0,0,1]
|
||||
```
|
||||
|
||||
**推荐起始条件**：
|
||||
- 距离原始 SLAM 轨迹上某点 **1 米**以内
|
||||
- 朝向与原始方向偏差在 **±10 度**以内
|
||||
- 位于地图中视觉特征明显的区域
|
||||
|
||||
**行为**：
|
||||
1. 启动时，Odin 尝试将当前观测与地图匹配
|
||||
2. 如果成功，发布 `map` 和 `odom` 坐标系之间的 TF
|
||||
3. 如果失败，系统进入后备 SLAM 模式（地图保存功能禁用）
|
||||
4. 后台持续尝试重定位直到成功
|
||||
|
||||
**提示**：
|
||||
- 启动后轻轻晃动或移动设备可以提高重定位精度
|
||||
- 在特征明显的场景中，可能在超出 1m/10° 范围时也能成功匹配
|
||||
|
||||
---
|
||||
|
||||
## Mode 2: Init Position Relocalization / 指定初始位置重定位
|
||||
|
||||
### English
|
||||
|
||||
In init position relocalization mode, you provide an initial pose estimate to help the algorithm converge faster.
|
||||
|
||||
**Configuration** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**When to Use**:
|
||||
- You know the approximate starting position (e.g., from external localization system)
|
||||
- Starting position is far from the recommended 1m/10° range
|
||||
- You need faster relocalization convergence
|
||||
- Deploying in a fixed docking station with known pose
|
||||
|
||||
**Behavior**:
|
||||
1. Algorithm uses provided `init_pos` as initial pose estimate
|
||||
2. Searches for matches in the vicinity of the provided position
|
||||
3. Faster convergence compared to auto mode when estimate is accurate
|
||||
|
||||
### 中文
|
||||
|
||||
在指定初始位置重定位模式下，您提供初始位姿估计以帮助算法更快收敛。
|
||||
|
||||
**配置** (`config/control_command.yaml`):
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/path/to/your/map.bin"
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
**适用场景**：
|
||||
- 您知道大致的起始位置（例如，来自外部定位系统）
|
||||
- 起始位置远离推荐的 1m/10° 范围
|
||||
- 需要更快的重定位收敛速度
|
||||
- 部署在已知位姿的固定充电桩
|
||||
|
||||
**行为**：
|
||||
1. 算法使用提供的 `init_pos` 作为初始位姿估计
|
||||
2. 在提供位置的附近搜索匹配
|
||||
3. 当估计准确时，比自动模式收敛更快
|
||||
|
||||
---
|
||||
|
||||
## init_pos Format / init_pos 格式说明
|
||||
|
||||
### English
|
||||
|
||||
`init_pos` is an array of **7 float values** representing position and orientation:
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| Index | Parameter | Description | Unit |
|
||||
|-------|-----------|-------------|------|
|
||||
| 0 | x | X position in map frame | meters |
|
||||
| 1 | y | Y position in map frame | meters |
|
||||
| 2 | z | Z position in map frame | meters |
|
||||
| 3 | qx | Quaternion X component | - |
|
||||
| 4 | qy | Quaternion Y component | - |
|
||||
| 5 | qz | Quaternion Z component | - |
|
||||
| 6 | qw | Quaternion W component | - |
|
||||
|
||||
**Important Notes**:
|
||||
- The quaternion must be normalized: `sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- Coordinates are relative to the **map frame** (world frame at SLAM start)
|
||||
- Default value `[0, 0, 0, 0, 0, 0, 1]` represents origin with no rotation
|
||||
|
||||
**Common Quaternion Values**:
|
||||
|
||||
| Orientation | qx | qy | qz | qw |
|
||||
|-------------|----|----|----|----|
|
||||
| No rotation (identity) | 0 | 0 | 0 | 1 |
|
||||
| 90° around Z-axis | 0 | 0 | 0.707 | 0.707 |
|
||||
| 180° around Z-axis | 0 | 0 | 1 | 0 |
|
||||
| -90° around Z-axis | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
### 中文
|
||||
|
||||
`init_pos` 是一个包含 **7 个 float 值**的数组，表示位置和朝向：
|
||||
|
||||
```yaml
|
||||
custom_init_pos: [x, y, z, qx, qy, qz, qw]
|
||||
```
|
||||
|
||||
| 索引 | 参数 | 描述 | 单位 |
|
||||
|------|------|------|------|
|
||||
| 0 | x | 地图坐标系中的 X 位置 | 米 |
|
||||
| 1 | y | 地图坐标系中的 Y 位置 | 米 |
|
||||
| 2 | z | 地图坐标系中的 Z 位置 | 米 |
|
||||
| 3 | qx | 四元数 X 分量 | - |
|
||||
| 4 | qy | 四元数 Y 分量 | - |
|
||||
| 5 | qz | 四元数 Z 分量 | - |
|
||||
| 6 | qw | 四元数 W 分量 | - |
|
||||
|
||||
**重要说明**：
|
||||
- 四元数必须归一化：`sqrt(qx² + qy² + qz² + qw²) ≈ 1.0`
|
||||
- 坐标相对于**地图坐标系**（SLAM 启动时的世界坐标系）
|
||||
- 默认值 `[0, 0, 0, 0, 0, 0, 1]` 表示原点且无旋转
|
||||
|
||||
**常用四元数值**：
|
||||
|
||||
| 朝向 | qx | qy | qz | qw |
|
||||
|------|----|----|----|----|
|
||||
| 无旋转（单位四元数） | 0 | 0 | 0 | 1 |
|
||||
| 绕 Z 轴旋转 90° | 0 | 0 | 0.707 | 0.707 |
|
||||
| 绕 Z 轴旋转 180° | 0 | 0 | 1 | 0 |
|
||||
| 绕 Z 轴旋转 -90° | 0 | 0 | -0.707 | 0.707 |
|
||||
|
||||
---
|
||||
|
||||
## Configuration Examples / 配置示例
|
||||
|
||||
### Example 1: Auto Relocalization / 自动重定位示例
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
```
|
||||
|
||||
### Example 2: Init Position at Origin / 在原点指定初始位置
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/office_map.bin"
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
```
|
||||
|
||||
### Example 3: Init Position with Offset / 带偏移的初始位置
|
||||
|
||||
Position at (5.2, -3.1, 0) with 90° rotation around Z-axis:
|
||||
|
||||
位置在 (5.2, -3.1, 0)，绕 Z 轴旋转 90°：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/warehouse_map.bin"
|
||||
custom_init_pos: [5.2, -3.1, 0.0, 0.0, 0.0, 0.707, 0.707]
|
||||
```
|
||||
|
||||
### Example 4: Docking Station Pose / 充电桩位置
|
||||
|
||||
Known docking station at (10.5, 2.3, 0) facing -X direction (180° rotation):
|
||||
|
||||
已知充电桩位置在 (10.5, 2.3, 0)，朝向 -X 方向（旋转 180°）：
|
||||
|
||||
```yaml
|
||||
register_keys:
|
||||
custom_map_mode: 2
|
||||
relocalization_map_abs_path: "/home/user/maps/factory_map.bin"
|
||||
custom_init_pos: [10.5, 2.3, 0.0, 0.0, 0.0, 1.0, 0.0]
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Programmatic API / 编程接口
|
||||
|
||||
### English
|
||||
|
||||
You can also set `init_pos` programmatically using the `lidar_set_custom_parameter` API. This is useful for:
|
||||
- Dynamic relocalization during runtime
|
||||
- Integration with external localization systems
|
||||
- Setting initial pose from robot's last known position
|
||||
|
||||
#### API Function
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief Set a custom parameter on the device
|
||||
* @param device Device handle obtained from lidar_open_device()
|
||||
* @param param_name Parameter name (e.g., "init_pos")
|
||||
* @param value_data Pointer to the parameter data
|
||||
* @param value_length Size of the data in bytes
|
||||
* @return 0 on success, -1 on error, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### Complete Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// Helper function to create quaternion from yaw angle (rotation around Z-axis)
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos format: [x, y, z, qx, qy, qz, qw] - 7 floats
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // Parameter name
|
||||
init_pos, // Data pointer
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 bytes
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("Successfully set init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("Failed to set init_pos, error code: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Usage examples:
|
||||
|
||||
// Example 1: Set position at origin with no rotation
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw (identity)
|
||||
);
|
||||
}
|
||||
|
||||
// Example 2: Set position with 90° yaw rotation
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // quaternion from yaw
|
||||
);
|
||||
}
|
||||
|
||||
// Example 3: Set position from external localization system
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS Integration Example
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// or
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// Callback for /initialpose topic (from RViz "2D Pose Estimate" tool)
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("Set init_pos from RViz: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("Failed to set init_pos: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### Important Notes
|
||||
|
||||
1. **Call timing**: Set `init_pos` **before** starting the stream with `lidar_start_stream()`
|
||||
2. **Map mode**: Ensure `custom_map_mode` is set to `2` (relocalization mode)
|
||||
3. **Map file**: The relocalization map must be set via `lidar_set_relocalization_map()` or YAML config
|
||||
4. **Thread safety**: `lidar_set_custom_parameter` is thread-safe but blocks until response received
|
||||
|
||||
### 中文
|
||||
|
||||
您也可以使用 `lidar_set_custom_parameter` API 以编程方式设置 `init_pos`。适用于：
|
||||
- 运行时动态重定位
|
||||
- 与外部定位系统集成
|
||||
- 从机器人上次已知位置设置初始位姿
|
||||
|
||||
#### API 函数
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
|
||||
/**
|
||||
* @brief 在设备上设置自定义参数
|
||||
* @param device 从 lidar_open_device() 获取的设备句柄
|
||||
* @param param_name 参数名称（如 "init_pos"）
|
||||
* @param value_data 指向参数数据的指针
|
||||
* @param value_length 数据大小（字节）
|
||||
* @return 成功返回 0，错误返回 -1，文件传输中返回 -2
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device,
|
||||
const char* param_name,
|
||||
const void* value_data,
|
||||
size_t value_length);
|
||||
```
|
||||
|
||||
#### 完整示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
|
||||
// 辅助函数：从偏航角（绕 Z 轴旋转）创建四元数
|
||||
void yaw_to_quaternion(float yaw_rad, float* qx, float* qy, float* qz, float* qw) {
|
||||
*qx = 0.0f;
|
||||
*qy = 0.0f;
|
||||
*qz = sinf(yaw_rad / 2.0f);
|
||||
*qw = cosf(yaw_rad / 2.0f);
|
||||
}
|
||||
|
||||
int set_init_position(device_handle device,
|
||||
float x, float y, float z,
|
||||
float qx, float qy, float qz, float qw) {
|
||||
// init_pos 格式: [x, y, z, qx, qy, qz, qw] - 7 个 float
|
||||
float init_pos[7] = {x, y, z, qx, qy, qz, qw};
|
||||
|
||||
int result = lidar_set_custom_parameter(
|
||||
device,
|
||||
"init_pos", // 参数名
|
||||
init_pos, // 数据指针
|
||||
sizeof(init_pos) // 7 * sizeof(float) = 28 字节
|
||||
);
|
||||
|
||||
if (result == 0) {
|
||||
printf("成功设置 init_pos: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]\n",
|
||||
x, y, z, qx, qy, qz, qw);
|
||||
} else {
|
||||
printf("设置 init_pos 失败，错误码: %d\n", result);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 使用示例：
|
||||
|
||||
// 示例 1：在原点设置位置，无旋转
|
||||
void example_origin(device_handle device) {
|
||||
set_init_position(device,
|
||||
0.0f, 0.0f, 0.0f, // x, y, z
|
||||
0.0f, 0.0f, 0.0f, 1.0f // qx, qy, qz, qw（单位四元数）
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 2：设置带 90° 偏航旋转的位置
|
||||
void example_with_rotation(device_handle device) {
|
||||
float qx, qy, qz, qw;
|
||||
float yaw_degrees = 90.0f;
|
||||
float yaw_rad = yaw_degrees * M_PI / 180.0f;
|
||||
|
||||
yaw_to_quaternion(yaw_rad, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
5.2f, -3.1f, 0.0f, // x, y, z
|
||||
qx, qy, qz, qw // 从偏航角计算的四元数
|
||||
);
|
||||
}
|
||||
|
||||
// 示例 3：从外部定位系统设置位置
|
||||
void example_from_external_localization(device_handle device,
|
||||
double ext_x, double ext_y, double ext_yaw) {
|
||||
float qx, qy, qz, qw;
|
||||
yaw_to_quaternion((float)ext_yaw, &qx, &qy, &qz, &qw);
|
||||
|
||||
set_init_position(device,
|
||||
(float)ext_x, (float)ext_y, 0.0f,
|
||||
qx, qy, qz, qw
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
#### ROS 集成示例
|
||||
|
||||
```cpp
|
||||
#include "lidar_api.h"
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h> // ROS1
|
||||
// 或
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp> // ROS2
|
||||
|
||||
// /initialpose 话题的回调函数（来自 RViz 的 "2D Pose Estimate" 工具）
|
||||
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg,
|
||||
device_handle device) {
|
||||
float init_pos[7] = {
|
||||
(float)msg->pose.pose.position.x,
|
||||
(float)msg->pose.pose.position.y,
|
||||
(float)msg->pose.pose.position.z,
|
||||
(float)msg->pose.pose.orientation.x,
|
||||
(float)msg->pose.pose.orientation.y,
|
||||
(float)msg->pose.pose.orientation.z,
|
||||
(float)msg->pose.pose.orientation.w
|
||||
};
|
||||
|
||||
int result = lidar_set_custom_parameter(device, "init_pos", init_pos, sizeof(init_pos));
|
||||
|
||||
if (result == 0) {
|
||||
ROS_INFO("从 RViz 设置 init_pos: [%.2f, %.2f, %.2f]",
|
||||
init_pos[0], init_pos[1], init_pos[2]);
|
||||
} else {
|
||||
ROS_ERROR("设置 init_pos 失败: %d", result);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
#### 重要说明
|
||||
|
||||
1. **调用时机**：在调用 `lidar_start_stream()` 启动数据流**之前**设置 `init_pos`
|
||||
2. **地图模式**：确保 `custom_map_mode` 设置为 `2`（重定位模式）
|
||||
3. **地图文件**：必须通过 `lidar_set_relocalization_map()` 或 YAML 配置设置重定位地图
|
||||
4. **线程安全**：`lidar_set_custom_parameter` 是线程安全的，但会阻塞直到收到响应
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting / 故障排除
|
||||
|
||||
### Relocalization Fails / 重定位失败
|
||||
|
||||
**English**:
|
||||
- Ensure starting position is within recommended range (1m/10°)
|
||||
- Check that the map file path is correct and file exists
|
||||
- Verify the environment hasn't changed significantly since mapping
|
||||
- Try gently moving the device to provide more observations
|
||||
|
||||
**中文**：
|
||||
- 确保起始位置在推荐范围内（1m/10°）
|
||||
- 检查地图文件路径是否正确且文件存在
|
||||
- 验证环境自建图以来没有显著变化
|
||||
- 尝试轻轻移动设备以提供更多观测
|
||||
|
||||
### init_pos Not Taking Effect / init_pos 未生效
|
||||
|
||||
**English**:
|
||||
- Verify `custom_map_mode` is set to `2`
|
||||
- Check that `custom_init_pos` has exactly 7 values
|
||||
- Ensure quaternion is normalized (sum of squares ≈ 1)
|
||||
- Restart the driver after modifying configuration
|
||||
|
||||
**中文**：
|
||||
- 验证 `custom_map_mode` 设置为 `2`
|
||||
- 检查 `custom_init_pos` 是否恰好有 7 个值
|
||||
- 确保四元数已归一化（平方和 ≈ 1）
|
||||
- 修改配置后重启驱动程序
|
||||
|
||||
### TF Not Published / TF 未发布
|
||||
|
||||
**English**:
|
||||
- Relocalization may still be in progress
|
||||
- Check ROS logs for relocalization status messages
|
||||
- System operates in fallback mode until relocalization succeeds
|
||||
|
||||
**中文**：
|
||||
- 重定位可能仍在进行中
|
||||
- 检查 ROS 日志中的重定位状态消息
|
||||
- 系统在重定位成功前以后备模式运行
|
||||
|
||||
### Map File Not Found / 地图文件未找到
|
||||
|
||||
**English**:
|
||||
- Use absolute path (starting with `/`)
|
||||
- Check file permissions
|
||||
- Verify file extension is `.bin`
|
||||
|
||||
**中文**：
|
||||
- 使用绝对路径（以 `/` 开头）
|
||||
- 检查文件权限
|
||||
- 验证文件扩展名为 `.bin`
|
||||
|
||||
---
|
||||
|
||||
## Related Topics / 相关话题
|
||||
|
||||
| Topic | Description |
|
||||
|-------|-------------|
|
||||
| `/odin1/odometry` | Odometry in odom frame |
|
||||
| `/odin1/odometry_highfreq` | High-frequency odometry |
|
||||
| `/odin1/cloud_slam` | SLAM point cloud in odom frame |
|
||||
| `/tf` | Transform tree (includes map→odom after successful relocalization) |
|
||||
|
||||
---
|
||||
|
||||
## See Also / 参见
|
||||
|
||||
- [README.md](README.md) - Main documentation
|
||||
- [config/control_command.yaml](config/control_command.yaml) - Configuration file
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
# O1-P040100042
|
||||
cam_num: 1
|
||||
img_topic_0: /camera/rgb
|
||||
Tcl_0: [
|
||||
-0.00916, -0.99996, 0.00032, 0.03215,
|
||||
0.00239, -0.00034, -1.00000, -0.01185,
|
||||
0.99996, -0.00916, 0.00239, 0.00536,
|
||||
0.00000, 0.00000, 0.00000, 1.00000
|
||||
]
|
||||
|
||||
cam_0:
|
||||
cam_model: FishPoly
|
||||
image_width: 1600
|
||||
image_height: 1296
|
||||
k2: 6.5597407531742908e-04
|
||||
k3: -2.8960653254664001e-02
|
||||
k4: 4.5390330144604515e-02
|
||||
k5: -6.4513398102889072e-02
|
||||
k6: 3.8735200610848508e-02
|
||||
k7: -9.9027101865577991e-03
|
||||
p1: 0.
|
||||
p2: 0.
|
||||
A11: 7.3689426240691989e+02
|
||||
A12: -1.6114977429994648e-01
|
||||
A22: 7.3661135398452438e+02
|
||||
u0: 8.0612553451014685e+02
|
||||
v0: 6.3965071026991302e+02
|
||||
isFast: 0
|
||||
numDiff: 3000
|
||||
maxIncidentAngle: 120
|
||||
@@ -0,0 +1,106 @@
|
||||
register_keys:
|
||||
|
||||
# if off, allow connection even if usb connection is below usb 3.0
|
||||
# ATTENTION: usb 3.0 is always recommended, as advance functionality like SLAM mode requires usb 3.0 for reliable map file transfer
|
||||
strict_usb3.0_check: 0 # 0: off: 1: on;
|
||||
|
||||
# 0: use odin internal system time as data time stamp, typical and recommended;
|
||||
# 1: use host ros time (upon receive) as data time stamp, only use if you specifically require this setup, not recommended for most users
|
||||
# 2: align odin1 time to host time, timestamp is the sensor data reception time on host time axis
|
||||
use_host_ros_time: 1
|
||||
|
||||
streamctrl: 1 # 0: off; 1: on
|
||||
|
||||
# original rgb data in jpeg format from device
|
||||
sendrgbcompressed: 1 # 0: off; 1: on
|
||||
|
||||
# RGB data, decoded from original jpeg data from device, bgr8 format
|
||||
# Processed on host device
|
||||
sendrgb: 1 # 0: off; 1: on
|
||||
|
||||
# undistort rgb image processed from decoded rgb data.
|
||||
# depends on sendrgb. related camera parameters can be found in ws/src/odin_ros_driver/config/calib.yaml
|
||||
# Processed on host device
|
||||
sendrgbundistort: 0 # 0: off; 1: on.
|
||||
|
||||
# IMU data
|
||||
sendimu: 1 # 0: off; 1: on
|
||||
|
||||
# SDK IMU smooth sending feature
|
||||
# When enabled, SDK will send IMU data at precise intervals (default 400Hz)
|
||||
# using a dedicated high-priority thread to reduce jitter and timing variance
|
||||
enable_imu_smooth: 1 # 0: disable SDK IMU smooth sending; 1: enable (default)
|
||||
|
||||
# SDK IMU smooth sending frequency in Hz (only effective when enable_imu_smooth = 1)
|
||||
imu_smooth_frequency: 400 # 1-1000 Hz, recommended 400 Hz
|
||||
|
||||
# Odometry data
|
||||
sendodom: 1 # 0: off; 1: on
|
||||
|
||||
# TF from odom to base_link. Leave it on unless you specifically need it off.
|
||||
# ATTENTION: critical for rviz to show cloud_raw.
|
||||
send_odom_baselink_tf: 0 # 0: off; 1: on.
|
||||
|
||||
# raw dtof data
|
||||
senddtof: 1 # 0: off; 1: on
|
||||
cloud_raw_confidence_threshold: 35 # please refer to readme for more details
|
||||
|
||||
# dtof sensor frame rate. Supported values: 100 (10fps) or 145 (14.5fps)
|
||||
# Higher frame rate provides smoother point cloud data but may increase data bandwidth
|
||||
# Note: value is multiplied by 10 (e.g., 145 means 14.5fps)
|
||||
dtof_fps: 100 # 100: 10fps; 145: 14.5fps
|
||||
|
||||
# slam cloud data
|
||||
sendcloudslam: 1 # 0: off; 1: on
|
||||
|
||||
# processed with raw point cloud, rgb image, and calib.yaml from device
|
||||
# Processed on host device
|
||||
sendcloudrender: 1 # 0: off; 1: on
|
||||
|
||||
# depth completion demo, high computing resource usage.
|
||||
# for more information please refer to the readme file.
|
||||
# Processed on host device
|
||||
senddepth: 0 # 0: off; 1: on
|
||||
|
||||
# cloud reprojection demo, projects cloud_slam to camera image using odometry
|
||||
# Processed on host device
|
||||
sendreprojection: 0 # 0: off; 1: on
|
||||
|
||||
# image overlay settings - overlays reprojected points on camera image
|
||||
# Processed on host device
|
||||
sendoverlay: 0 # 0: off; 1: on
|
||||
overlay_reprojected_topic: "/odin1/reprojected_image" # reprojected image topic
|
||||
overlay_camera_topic: "/odin1/image/undistorted" # camera image topic (undistorted)
|
||||
overlay_output_topic: "/odin1/overlay_image" # output overlay image topic
|
||||
overlay_alpha: 0.6 # blend alpha (0.0-1.0, higher = more reproj color)
|
||||
|
||||
# record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
|
||||
# save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
|
||||
# ATTENTION: please copy the full folder for post-processing.
|
||||
recorddata: 0 # 0: off; 1: on
|
||||
|
||||
# Save device runtime status info to ws/src/odin_ros_driver/log/Driver_{drvier_start_time}/Conn_{device_connection_time}/dev_status.csv
|
||||
devstatuslog: 1 # 0: off; 1: on.
|
||||
|
||||
save_log: 0 # 0: off; 1: on;
|
||||
|
||||
# raw dtof sensor intensity data in gray format, mostly for debug purpose.
|
||||
pubintensitygray: 0 # 0: off; 1: on
|
||||
|
||||
showpath: 0 # 0: off; 1: on
|
||||
showcamerapose: 0 # 0: off; 1: on
|
||||
|
||||
custom_map_mode: 0 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode. Keep 0 for pure odom fallback to avoid Odin map/odom TF conflicts.
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
relocalization_map_abs_path: "/absolute/path/to/map_a.bin" # unused in pure odometry mode; edit before Relocalization
|
||||
|
||||
# To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
|
||||
mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
|
||||
mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
|
||||
|
||||
# Image mask transfer settings
|
||||
sendimagemask: 0 # 0: off; 1: on - transfer image mask to device on startup
|
||||
image_mask_abs_path: "" # absolute path to the image mask file (e.g., /path/to/mask.png(1600x1296 resolution)
|
||||
|
||||
# Algorithm reset settings
|
||||
resetalgo: 0 # 0: off; 1: on - send algo_reset command to device on startup
|
||||
@@ -0,0 +1,438 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.4993045926094055
|
||||
Tree Height: 495
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/image/undistorted
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/Image
|
||||
Enabled: false
|
||||
Image Topic: /odin1/reprojected_image
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_raw
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8493094444274902
|
||||
Min Value: -0.13891705870628357
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_render
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 1
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic: /odin1/odometry
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 100
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Queue Size: 10
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 52; 101; 164
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic: /odin1/odometry_highfreq
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/camera_pose_visual
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 5
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: slam_decay
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/cloud_slam
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz/MarkerArray
|
||||
Enabled: false
|
||||
Marker Topic: /odin1/path
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Queue Size: 100
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /odin1/depth_img_competetion
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Min Color: 0; 0; 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic: /odin1/depth_img_competetion_cloud
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Filter (blacklist): ""
|
||||
Filter (whitelist): ""
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Theta std deviation: 0.2617993950843811
|
||||
Topic: /initialpose
|
||||
X std deviation: 0.5
|
||||
Y std deviation: 0.5
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 16.25591278076172
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.010398030281067
|
||||
Target Frame: odom
|
||||
Yaw: 0.8753980994224548
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001e70000033afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000022c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026f000001080000001600fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000047d000001730000001600fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f007200740000000568000000df0000001600fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a0065006300740065006400000002b2000000c50000001600ffffff000000010000015f0000033afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e60000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
@@ -0,0 +1,499 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Image1/Topic1
|
||||
- /cloudslam_reprojected1
|
||||
- /Odometry1
|
||||
- /slam1
|
||||
- /dense_depth_demo1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 593
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image_undistort
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/image/undistorted
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: false
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: cloudslam_reprojected
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/reprojected_image
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 239
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: raw
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_raw
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 1.8730175495147705
|
||||
Min Value: -0.12252448499202728
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: rgb
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 2.3509885615147286e-38
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: render
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_render
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Keep: 1
|
||||
Name: Odometry
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 255; 25; 0
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Axes
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry
|
||||
Value: true
|
||||
- Angle Tolerance: 0.10000000149011612
|
||||
Class: rviz_default_plugins/Odometry
|
||||
Covariance:
|
||||
Orientation:
|
||||
Alpha: 0.5
|
||||
Color: 255; 255; 127
|
||||
Color Style: Unique
|
||||
Frame: Local
|
||||
Offset: 1
|
||||
Scale: 1
|
||||
Value: true
|
||||
Position:
|
||||
Alpha: 0.30000001192092896
|
||||
Color: 204; 51; 204
|
||||
Scale: 1
|
||||
Value: true
|
||||
Value: true
|
||||
Enabled: false
|
||||
Keep: 10
|
||||
Name: Odometry_high
|
||||
Position Tolerance: 0.10000000149011612
|
||||
Shape:
|
||||
Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.10000000149011612
|
||||
Color: 98; 160; 234
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Shaft Length: 1
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Value: Arrow
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/odometry_highfreq
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: camera_view
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/camera_pose_visual
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: Odometry
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_current
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.10000000149011612
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 10
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: slam_deacy
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 1
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/cloud_slam
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: false
|
||||
Name: path
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/path
|
||||
Value: false
|
||||
Enabled: true
|
||||
Name: slam
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: dense_depth_image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: dense_depth_cloud
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odin1/depth_img_competetion_cloud
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: false
|
||||
Name: dense_depth_demo
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
odin1_base_link:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
odom:
|
||||
odin1_base_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: odom
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 10.9336576461792
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.2568470537662506
|
||||
Y: 2.1451337337493896
|
||||
Z: 0.3774382472038269
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.3953983187675476
|
||||
Target Frame: odom
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.230407953262329
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
Image_undistort:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1920
|
||||
X: 540
|
||||
Y: 124
|
||||
cloudslam_reprojected:
|
||||
collapsed: false
|
||||
dense_depth_image:
|
||||
collapsed: false
|
||||
+78
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/color_rgba.hpp>
|
||||
#include <std_msgs/msg/header.hpp>
|
||||
#include <visualization_msgs/msg/marker.hpp>
|
||||
#include <visualization_msgs/msg/marker_array.hpp>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/ColorRGBA.h>
|
||||
#include <std_msgs/Header.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#endif
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
class camera_pose_visualization {
|
||||
public:
|
||||
std::string m_marker_ns;
|
||||
|
||||
camera_pose_visualization(float r, float g, float b, float a);
|
||||
|
||||
void setImageBoundaryColor(float r, float g, float b, float a = 1.0);
|
||||
void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0);
|
||||
void setScale(double s);
|
||||
void setLineWidth(double width);
|
||||
|
||||
void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
|
||||
void reset();
|
||||
|
||||
#ifdef ROS2
|
||||
using ColorRGBA = std_msgs::msg::ColorRGBA;
|
||||
using Marker = visualization_msgs::msg::Marker;
|
||||
using MarkerArray = visualization_msgs::msg::MarkerArray;
|
||||
using Header = std_msgs::msg::Header;
|
||||
using Publisher = rclcpp::Publisher<MarkerArray>;
|
||||
#else
|
||||
using ColorRGBA = std_msgs::ColorRGBA;
|
||||
using Marker = visualization_msgs::Marker;
|
||||
using MarkerArray = visualization_msgs::MarkerArray;
|
||||
using Header = std_msgs::Header;
|
||||
using Publisher = ros::Publisher;
|
||||
#endif
|
||||
|
||||
void publish_by(Publisher& pub, const Header& header);
|
||||
void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
|
||||
private:
|
||||
std::vector<Marker> m_markers;
|
||||
ColorRGBA m_image_boundary_color;
|
||||
ColorRGBA m_optical_center_connector_color;
|
||||
double m_scale;
|
||||
double m_line_width;
|
||||
|
||||
static const Eigen::Vector3d imlt;
|
||||
static const Eigen::Vector3d imlb;
|
||||
static const Eigen::Vector3d imrt;
|
||||
static const Eigen::Vector3d imrb;
|
||||
static const Eigen::Vector3d oc ;
|
||||
static const Eigen::Vector3d lt0 ;
|
||||
static const Eigen::Vector3d lt1 ;
|
||||
static const Eigen::Vector3d lt2 ;
|
||||
};
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef ROS2
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#endif
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include "cloud_reprojector.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class CloudReprojectionRosNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using PointCloud2 = sensor_msgs::msg::PointCloud2;
|
||||
using Odometry = nav_msgs::msg::Odometry;
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<Odometry> odom_sub_;
|
||||
message_filters::Subscriber<Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<PointCloud2, Odometry, Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const PointCloud2::ConstSharedPtr& cloud_msg,
|
||||
const Odometry::ConstSharedPtr& odom_msg,
|
||||
const Odometry::ConstSharedPtr& wiwc_msg);
|
||||
};
|
||||
#else
|
||||
class CloudReprojectionRosNode
|
||||
{
|
||||
public:
|
||||
CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string cloud_slam_topic_;
|
||||
std::string odometry_topic_;
|
||||
std::string wiwc_topic_;
|
||||
std::string reprojected_image_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> wiwc_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, nav_msgs::Odometry> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
ros::Publisher reprojected_image_pub_;
|
||||
|
||||
std::unique_ptr<CloudReprojector> reprojector_;
|
||||
|
||||
void loadParameters();
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
||||
const nav_msgs::OdometryConstPtr& odom_msg,
|
||||
const nav_msgs::OdometryConstPtr& wiwc_msg);
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
|
||||
#include "polynomial_camera.hpp"
|
||||
|
||||
#include <memory>
|
||||
|
||||
class CloudReprojector
|
||||
{
|
||||
public:
|
||||
struct CameraParams
|
||||
{
|
||||
int image_width = 1600;
|
||||
int image_height = 1296;
|
||||
double A11 = 0.0, A12 = 0.0, A22 = 0.0;
|
||||
double u0 = 0.0, v0 = 0.0;
|
||||
double k2 = 0.0, k3 = 0.0, k4 = 0.0, k5 = 0.0, k6 = 0.0, k7 = 0.0;
|
||||
};
|
||||
|
||||
struct ExtrinsicParams
|
||||
{
|
||||
Eigen::Matrix4d Tcl = Eigen::Matrix4d::Identity(); // camera to lidar
|
||||
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity(); // lidar to imu (fixed)
|
||||
Eigen::Matrix4d Tic = Eigen::Matrix4d::Identity(); // camera to imu (calculated)
|
||||
};
|
||||
|
||||
struct OdomPose
|
||||
{
|
||||
Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity();
|
||||
Eigen::Vector3d position = Eigen::Vector3d::Zero();
|
||||
};
|
||||
|
||||
CloudReprojector();
|
||||
~CloudReprojector() = default;
|
||||
|
||||
bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params);
|
||||
|
||||
cv::Mat reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
|
||||
const OdomPose& odom_pose);
|
||||
|
||||
void setPointRadius(int radius) { point_radius_ = radius; }
|
||||
int getPointRadius() const { return point_radius_; }
|
||||
|
||||
const CameraParams& getCameraParams() const { return camera_params_; }
|
||||
const ExtrinsicParams& getExtrinsicParams() const { return extrinsic_params_; }
|
||||
|
||||
// Update extrinsic parameters at runtime with real-time values from module
|
||||
void updateExtrinsics(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til) {
|
||||
extrinsic_params_.Tcl = Tcl;
|
||||
extrinsic_params_.Til = Til;
|
||||
extrinsic_params_.Tic = calculateTic(Tcl, Til);
|
||||
}
|
||||
|
||||
static Eigen::Matrix4d calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til);
|
||||
|
||||
private:
|
||||
Eigen::Matrix4d odomPoseToMatrix(const OdomPose& odom) const;
|
||||
|
||||
cv::Mat projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const;
|
||||
|
||||
CameraParams camera_params_;
|
||||
ExtrinsicParams extrinsic_params_;
|
||||
std::unique_ptr<mini_vikit::PolynomialCamera> camera_model_;
|
||||
|
||||
int point_radius_ = 4;
|
||||
bool initialized_ = false;
|
||||
};
|
||||
@@ -0,0 +1,240 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
#include <atomic>
|
||||
#include <deque>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <cstdlib>
|
||||
#include <ctime>
|
||||
#include <sstream>
|
||||
|
||||
class BinaryDataLogger {
|
||||
public:
|
||||
struct Options {
|
||||
public:
|
||||
// Number of frames per batch before flushing to disk
|
||||
size_t batch_size = 20;
|
||||
// Base directory to place time-stamped run directory under
|
||||
// If empty, will use $ODIN_DATA_DIR or default to $HOME/OdinData
|
||||
std::filesystem::path base_dir;
|
||||
};
|
||||
|
||||
explicit BinaryDataLogger(const Options& opts) {
|
||||
namespace fs = std::filesystem;
|
||||
// Determine base dir
|
||||
fs::path base = opts.base_dir;
|
||||
if (base.empty()) {
|
||||
const char* env_dir = std::getenv("ODIN_DATA_DIR");
|
||||
if (env_dir && *env_dir) {
|
||||
base = fs::path(env_dir);
|
||||
} else {
|
||||
const char* home = std::getenv("HOME");
|
||||
base = home && *home ? fs::path(home) / "OdinData" : fs::current_path() / "OdinData";
|
||||
}
|
||||
}
|
||||
// Make timestamp folder name: YYYYMMDD_HHMMSS
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t t = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm tm{};
|
||||
#ifdef _WIN32
|
||||
localtime_s(&tm, &t);
|
||||
#else
|
||||
localtime_r(&t, &tm);
|
||||
#endif
|
||||
char buf[32];
|
||||
std::strftime(buf, sizeof(buf), "%Y%m%d_%H%M%S", &tm);
|
||||
root_dir_ = base / buf;
|
||||
fs::create_directories(root_dir_);
|
||||
fs::create_directories(root_dir_ / "image");
|
||||
// Convert to std::string
|
||||
std::string timestamp(buf);
|
||||
created_at_ = timestamp;
|
||||
// Compose file name
|
||||
std::filesystem::path pcFile = root_dir_ / ("MT" + timestamp + ".olx");
|
||||
// Create placeholder files (device_id / firmware / algorithm filled later via update_info_file)
|
||||
write_info_file_unlocked();
|
||||
write_text_file(root_dir_ / "image" / "cam_in_ex.txt", "# camera intrinsics/extrinsics TBD\n");
|
||||
|
||||
// Init writers
|
||||
pose_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPose.bin", opts.batch_size);
|
||||
cloud_writer_ = std::make_unique<Writer>(pcFile, opts.batch_size);
|
||||
//cloud_writer_ = std::make_unique<Writer>(root_dir_ / "OdinPointCloud.olx", opts.batch_size);
|
||||
image_writer_ = std::make_unique<Writer>(root_dir_ / "OdinImage.bin", opts.batch_size);
|
||||
roatation_writer_ = std::make_unique<Writer>(root_dir_ / "OdinRotate.bin", opts.batch_size);
|
||||
imu_writer_ = std::make_unique<Writer>(root_dir_ / "OdinIMU.bin", opts.batch_size);
|
||||
}
|
||||
|
||||
~BinaryDataLogger() {
|
||||
// Ensure all writers flush on destruction
|
||||
if (pose_writer_) pose_writer_->shutdown();
|
||||
if (cloud_writer_) cloud_writer_->shutdown();
|
||||
if (image_writer_) image_writer_->shutdown();
|
||||
if (roatation_writer_) roatation_writer_->shutdown();
|
||||
if (imu_writer_) imu_writer_->shutdown();
|
||||
}
|
||||
|
||||
const std::filesystem::path& root_dir() const { return root_dir_; }
|
||||
|
||||
// Update info.txt with device_id (SN) / firmware (SoC) / algorithm version.
|
||||
// Safe to call multiple times; latest values win.
|
||||
void update_info_file(const std::string& device_id,
|
||||
const std::string& firmware_version,
|
||||
const std::string& algorithm_version) {
|
||||
std::lock_guard<std::mutex> lk(info_mtx_);
|
||||
device_id_ = device_id;
|
||||
firmware_version_ = firmware_version;
|
||||
algorithm_version_ = algorithm_version;
|
||||
write_info_file_unlocked();
|
||||
}
|
||||
|
||||
// Enqueue ready-to-write frame blobs (already formatted as per spec)
|
||||
void enqueuePoseFrame(std::vector<uint8_t>&& blob) {
|
||||
if (pose_writer_) pose_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueuePointCloudFrame(std::vector<uint8_t>&& blob) {
|
||||
if (cloud_writer_) cloud_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueImageFrame(std::vector<uint8_t>&& blob) {
|
||||
if (image_writer_) image_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueRotateFrame(std::vector<uint8_t>&& blob) {
|
||||
if (roatation_writer_) roatation_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
void enqueueIMUFrame(std::vector<uint8_t>&& blob) {
|
||||
if (imu_writer_) imu_writer_->enqueue(std::move(blob));
|
||||
}
|
||||
|
||||
private:
|
||||
struct Writer {
|
||||
explicit Writer(const std::filesystem::path& filepath, size_t batch)
|
||||
: file_path(filepath), batch_size(batch), stop(false) {
|
||||
worker = std::thread([this]() { this->run(); });
|
||||
}
|
||||
~Writer() {
|
||||
shutdown();
|
||||
}
|
||||
void enqueue(std::vector<uint8_t>&& frame) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
pending.emplace_back(std::move(frame));
|
||||
if (pending.size() >= batch_size) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
}
|
||||
cv.notify_one();
|
||||
}
|
||||
void shutdown() {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx);
|
||||
if (stop) return;
|
||||
// Move leftovers to write buffer
|
||||
if (!pending.empty()) {
|
||||
swap_pending_unlocked();
|
||||
}
|
||||
stop = true;
|
||||
}
|
||||
cv.notify_one();
|
||||
if (worker.joinable()) worker.join();
|
||||
}
|
||||
private:
|
||||
void swap_pending_unlocked() {
|
||||
if (!pending.empty()) {
|
||||
write_queue.emplace_back(std::move(pending));
|
||||
pending.clear();
|
||||
}
|
||||
}
|
||||
void run() {
|
||||
std::ofstream out(file_path, std::ios::binary | std::ios::app);
|
||||
if (!out.is_open()) {
|
||||
// If file can't be opened, silently drop (or could add logging hook)
|
||||
return;
|
||||
}
|
||||
for (;;) {
|
||||
std::vector<std::vector<uint8_t>> batch;
|
||||
{
|
||||
std::unique_lock<std::mutex> lk(mtx);
|
||||
cv.wait(lk, [&]{ return stop || !write_queue.empty(); });
|
||||
if (!write_queue.empty()) {
|
||||
batch = std::move(write_queue.front());
|
||||
write_queue.pop_front();
|
||||
} else if (stop) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!batch.empty()) {
|
||||
for (auto& frame : batch) {
|
||||
if (!frame.empty()) {
|
||||
out.write(reinterpret_cast<const char*>(frame.data()), static_cast<std::streamsize>(frame.size()));
|
||||
}
|
||||
}
|
||||
out.flush();
|
||||
}
|
||||
}
|
||||
}
|
||||
std::filesystem::path file_path;
|
||||
size_t batch_size;
|
||||
std::mutex mtx;
|
||||
std::condition_variable cv;
|
||||
std::vector<std::vector<uint8_t>> pending; // accumulate frames
|
||||
std::deque<std::vector<std::vector<uint8_t>>> write_queue; // queued batches
|
||||
std::thread worker;
|
||||
bool stop;
|
||||
};
|
||||
|
||||
static void write_text_file(const std::filesystem::path& p, const std::string& content) {
|
||||
std::ofstream f(p, std::ios::out | std::ios::trunc);
|
||||
if (f.is_open()) {
|
||||
f << content;
|
||||
}
|
||||
}
|
||||
|
||||
// Render info.txt from current member fields. Caller must hold info_mtx_.
|
||||
void write_info_file_unlocked() {
|
||||
std::ostringstream oss;
|
||||
oss << "device=OdinOne\n"
|
||||
<< "pointcloud=xyzrgbi\n";
|
||||
if (!device_id_.empty()) {
|
||||
oss << "device_id=" << device_id_ << "\n";
|
||||
}
|
||||
if (!firmware_version_.empty()) {
|
||||
oss << "firmware_version=" << firmware_version_ << "\n";
|
||||
}
|
||||
if (!algorithm_version_.empty()) {
|
||||
oss << "algorithm_version=" << algorithm_version_ << "\n";
|
||||
}
|
||||
oss << "created_at=" << created_at_ << "\n";
|
||||
write_text_file(root_dir_ / "image" / "info.txt", oss.str());
|
||||
}
|
||||
|
||||
std::filesystem::path root_dir_;
|
||||
std::string created_at_;
|
||||
std::string device_id_;
|
||||
std::string firmware_version_;
|
||||
std::string algorithm_version_;
|
||||
std::mutex info_mtx_;
|
||||
std::unique_ptr<Writer> pose_writer_;
|
||||
std::unique_ptr<Writer> cloud_writer_;
|
||||
std::unique_ptr<Writer> image_writer_;
|
||||
std::unique_ptr<Writer> roatation_writer_;
|
||||
std::unique_ptr<Writer> imu_writer_;
|
||||
};
|
||||
+85
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <sensor_msgs/msg/compressed_image.hpp>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
class DepthImageRos2Node : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit DepthImageRos2Node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
void initialize();
|
||||
|
||||
private:
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::CompressedImage> color_compressed_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::msg::Image> color_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<
|
||||
sensor_msgs::msg::PointCloud2,
|
||||
// sensor_msgs::msg::CompressedImage,
|
||||
sensor_msgs::msg::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
std::shared_ptr<image_transport::ImageTransport> it_;
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
void syncCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud_msg,
|
||||
// const sensor_msgs::msg::CompressedImage::ConstSharedPtr image_msg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr color_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::msg::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::msg::Header &header);
|
||||
};
|
||||
+83
@@ -0,0 +1,83 @@
|
||||
/*
|
||||
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/CompressedImage.h>
|
||||
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
|
||||
class DepthImageRosNode
|
||||
{
|
||||
public:
|
||||
DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
image_transport::ImageTransport it_;
|
||||
|
||||
|
||||
std::string cloud_raw_topic_;
|
||||
std::string color_raw_topic_;
|
||||
std::string color_compressed_topic_;
|
||||
std::string depth_image_topic_;
|
||||
std::string depth_cloud_topic_;
|
||||
|
||||
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::Image> color_sub_;
|
||||
message_filters::Subscriber<sensor_msgs::CompressedImage> color_compressed_sub_;
|
||||
|
||||
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy;
|
||||
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
|
||||
std::shared_ptr<Sync> sync_;
|
||||
|
||||
image_transport::Publisher depth_image_pub_;
|
||||
ros::Publisher depth_cloud_pub_;
|
||||
|
||||
std::unique_ptr<PointCloudToDepthConverter> depth_converter_;
|
||||
|
||||
PointCloudToDepthConverter::CameraParams loadCameraParams();
|
||||
|
||||
|
||||
void syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
||||
const sensor_msgs::ImageConstPtr &image_msg);
|
||||
|
||||
|
||||
void publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::Header &header,
|
||||
const std::string &encoding = "32FC1");
|
||||
|
||||
|
||||
void publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::Header &header);
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user