Compare commits
7 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
117b02bec7 | ||
|
zeitvex
|
196044b993 | ||
|
zeitvex
|
645819a65f | ||
|
zeitvex
|
1b551f974b | ||
|
zeitvex
|
765173676f | ||
|
zeitvex
|
dc7a2c2a67 | ||
|
zeitvex
|
c8aa490b37 |
@@ -30,6 +30,8 @@ log/
|
||||
!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/
|
||||
|
||||
@@ -16,8 +16,15 @@
|
||||
| `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` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排后已正式归入 `v1.0.0`。
|
||||
|
||||
## `v0.9.0` 的 Python Sim2Real v2
|
||||
|
||||
@@ -34,6 +41,57 @@
|
||||
- 原始快照中的 `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` 的模型变化
|
||||
|
||||
- 机械 CAD 不变。
|
||||
|
||||
@@ -10,9 +10,13 @@
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real_v2/ # Python Sim2Real v2
|
||||
└─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
├─ sim2real_ros2/ # ROS 2/C++ Sim2Real 初版
|
||||
├─ sim2real_ros2_v2/ # ROS 2 导航原型及里程计演进
|
||||
└─ sim2real_ros2_v3/ # 最终比赛 ROS 2/C++ 部署
|
||||
```
|
||||
|
||||
当前工作树按架构大版本同时保留三个 ROS 2 目录：无后缀目录是初版，`_v2` 是第二版演进的最终里程计快照，`_v3` 是 `last_not_slalom_1050` 最终比赛部署。各目录内部的小阶段仍可通过对应 Tag 恢复。
|
||||
|
||||
## 数据流
|
||||
|
||||
```text
|
||||
@@ -32,7 +36,7 @@ MJCF + mjlab task
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ Sim2Real 初版对应 `v0.10.0`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，Python Sim2Real v2 对应 `v0.9.0`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`，纯里程计导航联调对应 `v0.12.0`，1050 分比赛最终部署对应 `v1.0.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -26,11 +26,23 @@ Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新
|
||||
|
||||
部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_ros2`
|
||||
## ROS 2/C++ 版本线
|
||||
|
||||
ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。
|
||||
### `sim2real_ros2`（初版，`v0.10.0`）
|
||||
|
||||
原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
无后缀目录固定表示 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)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
|
||||
@@ -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
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
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/image.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <mutex>
|
||||
#else
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#endif
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#ifdef ROS2
|
||||
class ImageOverlayNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
using Image = sensor_msgs::msg::Image;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
rclcpp::Subscription<Image>::SharedPtr reproj_sub_;
|
||||
rclcpp::Subscription<Image>::SharedPtr camera_sub_;
|
||||
rclcpp::Publisher<Image>::SharedPtr overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::msg::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(Image::ConstSharedPtr msg);
|
||||
void cameraCallback(Image::ConstSharedPtr msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#else
|
||||
#include <mutex>
|
||||
class ImageOverlayNode
|
||||
{
|
||||
public:
|
||||
ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh);
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_, pnh_;
|
||||
|
||||
std::string reprojected_topic_;
|
||||
std::string camera_topic_;
|
||||
std::string overlay_topic_;
|
||||
double alpha_; // blend alpha for overlay
|
||||
|
||||
ros::Subscriber reproj_sub_;
|
||||
ros::Subscriber camera_sub_;
|
||||
ros::Publisher overlay_pub_;
|
||||
|
||||
// Cache latest images
|
||||
cv::Mat latest_reproj_img_;
|
||||
cv::Mat latest_camera_img_;
|
||||
std_msgs::Header latest_header_;
|
||||
std::mutex mutex_;
|
||||
|
||||
void reprojCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void cameraCallback(const sensor_msgs::ImageConstPtr& msg);
|
||||
void publishOverlay();
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,532 @@
|
||||
#ifndef LIDAR_API_H
|
||||
#define LIDAR_API_H
|
||||
|
||||
/**
|
||||
* @file lidar_api.h
|
||||
* @brief LiDAR device API for controlling and accessing LiDAR sensor data
|
||||
*
|
||||
* This header provides the public interface for interacting with LiDAR devices.
|
||||
* It includes functions for device management, data streaming control, and
|
||||
* device configuration.
|
||||
*
|
||||
* @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
|
||||
* @version 1.0
|
||||
*
|
||||
* ┌──────────────────────────────────────────────────────────────────────────────┐
|
||||
* │ QUICK START │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 1. API Call Sequence (typical usage): │
|
||||
* │ │
|
||||
* │ lidar_system_init(device_cb) // Initialize system, register device cb│
|
||||
* │ ↓ │
|
||||
* │ [Wait for device_cb with attach=true to get device info] │
|
||||
* │ ↓ │
|
||||
* │ lidar_create_device(&dev_info, &handle) // Create device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_register_stream_callback(handle, cb_info) // Register data callback│
|
||||
* │ ↓ │
|
||||
* │ lidar_open_device(handle) // Connect to device │
|
||||
* │ ↓ │
|
||||
* │ lidar_set_mode(handle, mode) // Set RAW or SLAM mode │
|
||||
* │ ↓ │
|
||||
* │ lidar_start_stream(handle, type, odr) // Start specific data stream │
|
||||
* │ ↓ │
|
||||
* │ [Data arrives via registered callback] │
|
||||
* │ ↓ │
|
||||
* │ lidar_stop_stream(handle, type) // Stop data stream │
|
||||
* │ ↓ │
|
||||
* │ lidar_close_device(handle) // Disconnect │
|
||||
* │ ↓ │
|
||||
* │ lidar_destory_device(handle) // Release device handle │
|
||||
* │ ↓ │
|
||||
* │ lidar_system_deinit() // Cleanup system resources │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 2. Operating Modes and Available Data Types: │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_RAW: │
|
||||
* │ - LIDAR_DT_RAW_RGB (RGB camera image, NV12 format) │
|
||||
* │ - LIDAR_DT_RAW_IMU (IMU data at 400Hz) │
|
||||
* │ - LIDAR_DT_RAW_DTOF (DTOF depth + point cloud + confidence) │
|
||||
* │ - LIDAR_DT_DEV_STATUS (Device status info) │
|
||||
* │ - LIDAR_DT_NTP (PTP/NTP sync data) │
|
||||
* │ │
|
||||
* │ LIDAR_MODE_SLAM: │
|
||||
* │ - All RAW mode types, plus: │
|
||||
* │ - LIDAR_DT_SLAM_CLOUD (SLAM point cloud, XYZRGBA) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY (SLAM odometry at ~10Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ (Odometry at IMU rate ~400Hz) │
|
||||
* │ - LIDAR_DT_SLAM_ODOMETRY_TF (Map-Odom transform) │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 3. activate_stream_type vs start_stream: │
|
||||
* │ │
|
||||
* │ lidar_activate_stream_type(): │
|
||||
* │ - Configure which stream types are enabled in device │
|
||||
* │ - Can enable multiple types before starting │
|
||||
* │ - Does NOT start data transmission │
|
||||
* │ │
|
||||
* │ lidar_start_stream(): │
|
||||
* │ - Actually starts data transmission for the specified type │
|
||||
* │ - Callback will begin receiving data after this call │
|
||||
* │ │
|
||||
* │ Typical flow: │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_IMU); // Enable IMU │
|
||||
* │ activate_stream_type(LIDAR_DT_RAW_DTOF); // Enable DTOF │
|
||||
* │ start_stream(LIDAR_DT_RAW_IMU, ...); // Start IMU stream │
|
||||
* │ start_stream(LIDAR_DT_RAW_DTOF, ...); // Start DTOF stream │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 4. Callback Notes (IMPORTANT): │
|
||||
* │ │
|
||||
* │ Thread Safety: │
|
||||
* │ - Callbacks are invoked from internal SDK threads │
|
||||
* │ - Different data types may use different threads │
|
||||
* │ - User callback code must be thread-safe │
|
||||
* │ │
|
||||
* │ Data Lifetime: │
|
||||
* │ - Data pointers (pAddr) are ONLY valid during callback execution │
|
||||
* │ - If you need to keep data, COPY it before callback returns │
|
||||
* │ - Do NOT store or dereference pAddr after callback returns │
|
||||
* │ │
|
||||
* │ Performance: │
|
||||
* │ - Avoid blocking or time-consuming operations in callback │
|
||||
* │ - Long callback execution may cause data loss or jitter │
|
||||
* │ - For heavy processing, copy data and process in separate thread │
|
||||
* │ │
|
||||
* ├──────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ 5. Error Codes: │
|
||||
* │ 0 : Success │
|
||||
* │ -1 : General failure │
|
||||
* │ -2 : Invalid parameter │
|
||||
* │ -3 : Device not found / not connected │
|
||||
* │ -4 : Operation timeout │
|
||||
* │ -5 : Resource allocation failed │
|
||||
* │ │
|
||||
* └──────────────────────────────────────────────────────────────────────────────┘
|
||||
*/
|
||||
|
||||
#include "lidar_api_type.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Initialize the LiDAR system
|
||||
*
|
||||
* Must be called before any other lidar function to set up the system resources.
|
||||
* This function starts device discovery and will invoke the callback when devices
|
||||
* are found or disconnected.
|
||||
*
|
||||
* @param cb Callback function for device events:
|
||||
* - Called with attach=true when a new device is discovered
|
||||
* - Called with attach=false when a device is disconnected
|
||||
* - The lidar_device_info_t contains serial number to identify the device
|
||||
*
|
||||
* Example callback:
|
||||
* void device_callback(const lidar_device_info_t* info, bool attach) {
|
||||
* if (attach) {
|
||||
* printf("Device connected: %s\n", info->serial);
|
||||
* // Save info for lidar_create_device()
|
||||
* } else {
|
||||
* printf("Device disconnected: %s\n", info->serial);
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_init(lidar_device_callback_t cb);
|
||||
|
||||
/**
|
||||
* @brief Deinitialize the LiDAR system
|
||||
*
|
||||
* Releases all resources allocated by the system. Should be called when
|
||||
* application is shutting down.
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_system_deinit(void);
|
||||
|
||||
/**
|
||||
* @brief Create a handle for a LiDAR device
|
||||
*
|
||||
* Creates a device handle using the device info received from the device callback.
|
||||
* The handle is used for all subsequent operations on the device.
|
||||
*
|
||||
* @param dev_info Information about the LiDAR device (from lidar_system_init callback)
|
||||
* - serial: Device serial number (required, used to identify device)
|
||||
* - model: Device model string
|
||||
* - online: Device connection status
|
||||
* @param device [OUTPUT] Pointer to receive the device handle upon success
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note The dev_info should be the same structure received from the device callback
|
||||
*/
|
||||
int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
|
||||
|
||||
/**
|
||||
* @brief Destroy a LiDAR device handle
|
||||
*
|
||||
* Releases resources associated with the device handle. Must be called
|
||||
* when the device is no longer needed.
|
||||
*
|
||||
* @param device Handle to the device to destroy
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_destory_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Register callback function for receiving LiDAR data streams
|
||||
*
|
||||
* Sets up a callback function that will be called when new data is available.
|
||||
* All data types use the same callback; use lidar_data_t.type to distinguish.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param cb Callback info structure:
|
||||
* - data_callback: Function pointer, signature: void(const lidar_data_t*, void*)
|
||||
* - user_data: User context pointer passed to callback (can be NULL)
|
||||
*
|
||||
* Example:
|
||||
* void data_callback(const lidar_data_t* data, void* user_data) {
|
||||
* switch (data->type) {
|
||||
* case LIDAR_DT_RAW_IMU:
|
||||
* imu_convert_data_t* imu = (imu_convert_data_t*)data->stream.imageList[0].pAddr;
|
||||
* // Process IMU data (COPY if needed, pointer invalid after return)
|
||||
* break;
|
||||
* case LIDAR_DT_RAW_DTOF:
|
||||
* // data->stream.imageList[0]: depth
|
||||
* // data->stream.imageList[1]: point cloud XYZ
|
||||
* // data->stream.imageList[2]: confidence
|
||||
* // data->stream.imageList[3]: intensity
|
||||
* break;
|
||||
* }
|
||||
* }
|
||||
*
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @warning Callback is invoked from SDK internal threads. Avoid blocking operations.
|
||||
* @warning Data pointers are only valid during callback execution. Copy if needed.
|
||||
*/
|
||||
int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
|
||||
|
||||
/**
|
||||
* @brief Unregister stream callback for a device
|
||||
*
|
||||
* Stops the device from calling back when new data is available.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_unregister_stream_callback(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Open a LiDAR device for communication
|
||||
*
|
||||
* Establishes a connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to open
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_open_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Close a LiDAR device
|
||||
*
|
||||
* Closes the connection to the physical device.
|
||||
*
|
||||
* @param device Handle to the device to close
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_close_device(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Set the operating mode of the LiDAR device
|
||||
*
|
||||
* Must be called after lidar_open_device() and before lidar_start_stream().
|
||||
* Mode determines which data types are available for streaming.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param mode Operating mode to set:
|
||||
* - LIDAR_MODE_RAW: Raw sensor data (RGB, IMU, DTOF)
|
||||
* - LIDAR_MODE_SLAM: SLAM processing enabled (adds odometry, point cloud)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_mode(device_handle device, int mode);
|
||||
|
||||
/**
|
||||
* @brief Start data streaming from the device
|
||||
*
|
||||
* Begins the flow of data from the device for the specified type.
|
||||
* After calling this function, registered callbacks will start receiving data.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to start (lidar_data_type_e):
|
||||
* - LIDAR_DT_RAW_RGB: RGB camera frames
|
||||
* - LIDAR_DT_RAW_IMU: IMU data at 400Hz
|
||||
* - LIDAR_DT_RAW_DTOF: Depth sensor data
|
||||
* - LIDAR_DT_SLAM_CLOUD: SLAM point cloud (requires SLAM mode)
|
||||
* - LIDAR_DT_SLAM_ODOMETRY: SLAM odometry (requires SLAM mode)
|
||||
* - etc. (see lidar_data_type_e in lidar_api_type.h)
|
||||
* @param dtof_subframe_odr [OUTPUT] Returns DTOF subframe interval in microseconds.
|
||||
* Only meaningful when type=LIDAR_DT_RAW_DTOF.
|
||||
* For other types, this value can be ignored.
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @note You can start multiple stream types simultaneously by calling this
|
||||
* function multiple times with different types.
|
||||
*/
|
||||
int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
|
||||
|
||||
/**
|
||||
* @brief Stop data streaming from the device
|
||||
*
|
||||
* Stops the flow of data from the device for the specified type.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to stop
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_stop_stream(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Activate a specific stream type on the device
|
||||
*
|
||||
* Enables a specific data stream type in the device configuration.
|
||||
* This configures the device to be ready for the stream type, but does NOT
|
||||
* start actual data transmission. Call lidar_start_stream() to begin streaming.
|
||||
*
|
||||
* Use this to pre-configure multiple stream types before starting them:
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_IMU);
|
||||
* activate_stream_type(handle, LIDAR_DT_RAW_DTOF);
|
||||
* start_stream(handle, LIDAR_DT_RAW_IMU, odr);
|
||||
* start_stream(handle, LIDAR_DT_RAW_DTOF, odr);
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to activate (lidar_data_type_e)
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*
|
||||
* @see lidar_start_stream() to actually begin data transmission
|
||||
* @see lidar_deactivate_stream_type() to disable a stream type
|
||||
*/
|
||||
int lidar_activate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Deactivate a specific stream type on the device
|
||||
*
|
||||
* Disables a specific data stream type in the device configuration.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param type Type of data stream to deactivate
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_deactivate_stream_type(device_handle device, int type);
|
||||
|
||||
/**
|
||||
* @brief Get the calibration file from the device
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calib_file(device_handle device, const char* path);
|
||||
|
||||
/**
|
||||
* @brief Get device calibration parameters
|
||||
*
|
||||
* Retrieves the current calibration parameters from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to receive the calibration parameters
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_calibration(device_handle device, lidar_calibration_t* param);
|
||||
|
||||
/**
|
||||
* @brief Set device calibration parameters
|
||||
*
|
||||
* Applies new calibration parameters to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param Pointer to the calibration parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_calibration(device_handle device, const lidar_calibration_t *param);
|
||||
|
||||
/**
|
||||
* @brief Set log verbosity level
|
||||
*
|
||||
* Controls the amount of log information generated by the LiDAR API.
|
||||
*
|
||||
* @param level Log level to set (see level definitions in lidar_api_type.h)
|
||||
*/
|
||||
void lidar_log_set_level(lidar_log_level_e level);
|
||||
|
||||
/**
|
||||
* @brief Get the version information of the LiDAR device
|
||||
*
|
||||
* Retrieves version information including firmware, system, and application versions.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param version struct Pointer to receive the version information
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
|
||||
|
||||
/**
|
||||
* @brief Set custom algorithm parameters for the device
|
||||
*
|
||||
* Sends custom parameter settings to the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to set
|
||||
* @param value_data Pointer to the value data to set for the parameter
|
||||
* @param value_length Length of the value data in bytes
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Get custom algorithm parameters for the device
|
||||
*
|
||||
* Get custom parameter settings from the device.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param param_name String name of the parameter to get
|
||||
* @param value Integer value to get for the parameter
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
|
||||
|
||||
/**
|
||||
* @brief Set the map file used for relocalization
|
||||
*
|
||||
* Read & send specified map file to device for relocalization
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the map file
|
||||
* @return int 0 on success, otherwise on failure
|
||||
*/
|
||||
int lidar_set_relocalization_map(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief Get the mapping result file from device
|
||||
*
|
||||
* Read & send specified map file from device to host
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the map file
|
||||
* @param file_name File name to save the map file
|
||||
* @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
|
||||
*/
|
||||
int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
|
||||
|
||||
/**
|
||||
* @brief Save the current map to a file on the host. Synchronous, all-in-one API.
|
||||
*
|
||||
* Internally drives the complete save-map state machine, so callers do not need
|
||||
* to coordinate the multi-step protocol themselves:
|
||||
* 1) Set custom parameter save_map = 1 on the device (kicks off generation).
|
||||
* 2) Poll save_map until the device resets it to 0 (map generation finished).
|
||||
* 3) Pull the resulting file via the standard mapping-result transfer (same as
|
||||
* lidar_get_mapping_result).
|
||||
* 4) Return the final status to the caller.
|
||||
*
|
||||
* This is the recommended entry point for saving a map. The lower-level pair
|
||||
* (lidar_set_custom_parameter("save_map") + lidar_get_mapping_result) remains
|
||||
* available for advanced use cases.
|
||||
*
|
||||
* 同步保存地图到主机上的文件。一站式 API，内部完成「触发生成 → 等待设备完成 →
|
||||
* 拉取文件」的完整流程，调用方无需自行轮询和处理异步状态。
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param dest_dir Host directory to save the file into (must already exist).
|
||||
* @param file_name File name (e.g. "map.bin").
|
||||
* @param gen_timeout_ms Maximum time (ms) to wait for the device to finish
|
||||
* generating the map. Pass 0 to use the default (120000 ms).
|
||||
* @return int
|
||||
* 0 success, file saved at dest_dir/file_name
|
||||
* -1 invalid arguments (null device/dir, SDK not initialized, etc.)
|
||||
* -2 device is busy with another file transfer
|
||||
* -3 timed out waiting for the device to finish map generation
|
||||
* -4 file transfer stalled or failed (see logs for details)
|
||||
* other negative values are propagated from the underlying transfer.
|
||||
*/
|
||||
int lidar_save_map(device_handle device,
|
||||
const char *dest_dir,
|
||||
const char *file_name,
|
||||
uint32_t gen_timeout_ms);
|
||||
|
||||
/**
|
||||
* @brief Set the image mask file for the device
|
||||
*
|
||||
* Read & send specified image mask file to device
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param abs_path Absolute path to the image mask file (e.g., mask.png)
|
||||
* @return int 0 on success, -1 on failure, -2 if file transfer in progress
|
||||
*/
|
||||
int lidar_set_image_mask(device_handle device, const char* abs_path);
|
||||
|
||||
/**
|
||||
* @brief enable encrypted device log
|
||||
*
|
||||
* enable encrypted device log, save to specified directory
|
||||
* Please send to our support when needed
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param dest_dir Destination directory to save the encrypted logs
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the depth parameters for the device
|
||||
*
|
||||
* This function must be called before starting data stream.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @param params Pointer to the depth parameters to set
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
|
||||
|
||||
/**
|
||||
* @brief Enable or disable IMU smooth sending feature
|
||||
*
|
||||
* When enabled, IMU data will be sent at precise intervals (default 400Hz)
|
||||
* using a dedicated high-priority thread to reduce jitter and timing variance.
|
||||
* When disabled, IMU data will be sent immediately upon reception.
|
||||
*
|
||||
* @param enable 1 to enable smooth sending, 0 to disable
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_enable_imu_smooth_sending(int enable);
|
||||
|
||||
/**
|
||||
* @brief Set IMU smooth sending frequency
|
||||
*
|
||||
* Set the target frequency for IMU smooth sending. Only effective when
|
||||
* smooth sending is enabled via lidar_enable_imu_smooth_sending().
|
||||
*
|
||||
* @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
|
||||
* @return int 0 on success, -1 on failure
|
||||
*/
|
||||
int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
#ifndef LIDAR_TYPES_H
|
||||
#define LIDAR_TYPES_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define LIDAR_SERIAL_MAX 64
|
||||
#define LIDAR_MODEL_MAX 64
|
||||
#define LIDAR_IP_MAX 64
|
||||
|
||||
typedef void * device_handle;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_LOG_ERROR = 0,
|
||||
LIDAR_LOG_WARN,
|
||||
LIDAR_LOG_INFO,
|
||||
LIDAR_LOG_DEBUG,
|
||||
} lidar_log_level_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_OTA_ALGORITHM,
|
||||
LIDAR_OTA_FIRMWARE,
|
||||
LIDAR_OTA_SCRIPT,
|
||||
LIDAR_OTA_CALIBRATION
|
||||
} lidar_ota_type_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_MODE_RAW,
|
||||
LIDAR_MODE_SLAM,
|
||||
} lidar_mode_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0,
|
||||
LIDAR_DT_RAW_RGB,
|
||||
LIDAR_DT_RAW_IMU,
|
||||
LIDAR_DT_RAW_DTOF,
|
||||
LIDAR_DT_SLAM_CLOUD,
|
||||
LIDAR_DT_SLAM_ODOMETRY,
|
||||
LIDAR_DT_DEV_STATUS,
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF,
|
||||
LIDAR_DT_SLAM_WIWC,
|
||||
LIDAR_DT_NTP
|
||||
} lidar_data_type_e;
|
||||
|
||||
typedef struct {
|
||||
int8_t serial[LIDAR_SERIAL_MAX];
|
||||
int8_t model[LIDAR_MODEL_MAX];
|
||||
bool online;
|
||||
uint32_t initial_state;
|
||||
} lidar_device_info_t;
|
||||
|
||||
typedef struct {
|
||||
float x, y, z;
|
||||
float intensity;
|
||||
} lidar_point_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
float intrinsics[9];
|
||||
float extrinsics[16];
|
||||
} lidar_calibration_t;
|
||||
|
||||
|
||||
#define DEVICE_MAX_CH_NUMBER 4
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
} ros2_odom_convert_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ns;
|
||||
int64_t pos[3];
|
||||
int64_t orient[4];
|
||||
int64_t linear_velocity[3];
|
||||
int64_t angular_velocity[3];
|
||||
double pose_cov[36];
|
||||
double twist_cov[36];
|
||||
} ros_odom_convert_complete_t;
|
||||
|
||||
typedef struct {
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
uint64_t stamp;
|
||||
uint64_t sequence;
|
||||
} imu_convert_data_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t length;
|
||||
uint64_t sequence;
|
||||
uint64_t timestamp;
|
||||
uint64_t interval;
|
||||
void* pAddr;
|
||||
uint32_t width;
|
||||
uint32_t height;
|
||||
} buffer_List_t;
|
||||
|
||||
typedef struct {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_t;
|
||||
|
||||
typedef struct capture_Image_List_t {
|
||||
uint32_t imageCount;
|
||||
buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
|
||||
} capture_Image_List_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t type;
|
||||
capture_Image_List_t stream;
|
||||
} lidar_data_t;
|
||||
|
||||
typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
|
||||
typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
|
||||
|
||||
typedef struct {
|
||||
lidar_data_callback_t data_callback;
|
||||
void *user_data;
|
||||
} lidar_data_callback_info_t;
|
||||
|
||||
typedef struct {
|
||||
int major;
|
||||
int minor;
|
||||
int patch;
|
||||
}lidar_version_t;
|
||||
|
||||
typedef struct {
|
||||
lidar_version_t kernel_version;
|
||||
lidar_version_t mcu_version;
|
||||
lidar_version_t soc_version;
|
||||
lidar_version_t Daemon_proc_version;
|
||||
lidar_version_t slam_version;
|
||||
} lidar_fireware_version_t;
|
||||
|
||||
/**
|
||||
* @brief RGB image sensor frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* rgb image sensor configured output data rate */
|
||||
int tx_odr; /* rgb image sensor tx output data rate */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* dtof lidar sensor configured output data rate */
|
||||
int tx_odr; /* dtof lidar sensor tx output data rate */
|
||||
int subframe_odr; /* dtof lidar sensor subframe output data rate */
|
||||
short tx_temp; /* dtof lidar tx module temp */
|
||||
short rx_temp; /* dtof lidar rx module temp */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr; /* imu sensor configured output data rate */
|
||||
int tx_odr; /* imu sensor tx output data rate */
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp; /* soc package temp */
|
||||
int cpu_temp; /* cpu temp */
|
||||
int center_temp; /* center temp */
|
||||
int gpu_temp; /* gpu temp */
|
||||
int npu_temp; /* npu temp */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal;
|
||||
|
||||
int cpu_use_rate[8]; /* cpu usage rate */
|
||||
int ram_use_rate; /* ram usage rate */
|
||||
|
||||
lidar_rgb_sensor_status_t rgb_sensor;
|
||||
lidar_dtof_sensor_status_t dtof_sensor;
|
||||
lidar_imu_sensor_status_t imu_sensor;
|
||||
|
||||
int slam_cloud_tx_odr; /* slam cloud tx output data rate */
|
||||
int slam_odom_tx_odr; /* slam odom tx output data rate */
|
||||
int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */
|
||||
|
||||
} lidar_device_status_t;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEVICE_NONE = 0,
|
||||
LIDAR_DEVICE_NOT_INITIALIZED,
|
||||
LIDAR_DEVICE_INITIALIZED,
|
||||
LIDAR_DEVICE_STREAMING,
|
||||
LIDAR_DEVICE_STREAM_STOPPED,
|
||||
} lidar_device_initial_state_e;
|
||||
|
||||
typedef enum {
|
||||
LIDAR_DEPTH_ODR_10HZ = 0,
|
||||
LIDAR_DEPTH_ODR_14_5HZ,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
+93
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
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 <pcl/point_types.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/common/transforms.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Eigen/Dense>
|
||||
#include <vector>
|
||||
|
||||
|
||||
class PointCloudToDepthConverter
|
||||
{
|
||||
public:
|
||||
|
||||
struct CameraParams
|
||||
{
|
||||
int image_width;
|
||||
int image_height;
|
||||
double A11, A12, A22;
|
||||
double u0, v0;
|
||||
double k2, k3, k4, k5, k6, k7;
|
||||
double scale;
|
||||
int point_sampling_rate;
|
||||
Eigen::Matrix4d Tcl;
|
||||
};
|
||||
|
||||
|
||||
struct ProcessResult
|
||||
{
|
||||
cv::Mat depth_image;
|
||||
pcl::PointCloud<pcl::PointXYZRGB> colored_cloud;
|
||||
bool success;
|
||||
std::string error_message;
|
||||
};
|
||||
|
||||
|
||||
explicit PointCloudToDepthConverter(const CameraParams ¶ms);
|
||||
|
||||
|
||||
ProcessResult processCloudAndImage(const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image);
|
||||
|
||||
cv::Mat customResize(const cv::Mat& src, const cv::Size& size);
|
||||
const CameraParams &getCameraParams() const { return params_; }
|
||||
|
||||
|
||||
void updateCameraParams(const CameraParams ¶ms);
|
||||
|
||||
private:
|
||||
CameraParams params_;
|
||||
|
||||
Eigen::Matrix3d K_;
|
||||
Eigen::Matrix3d Kl_;
|
||||
Eigen::Matrix4d K_4x4_;
|
||||
Eigen::Matrix4d Kcl_;
|
||||
|
||||
cv::Mat map_x_, map_y_;
|
||||
cv::Mat inv_map_x_, inv_map_y_;
|
||||
|
||||
int scaled_width_, scaled_height_;
|
||||
|
||||
|
||||
void initializeInternalParams();
|
||||
|
||||
|
||||
void createDistortionMaps();
|
||||
|
||||
cv::Mat projectCloudToDepth(const pcl::PointCloud<pcl::PointXYZ> &cloud_in_cam);
|
||||
|
||||
|
||||
cv::Mat postProcessDepthImage(const cv::Mat &depth_img);
|
||||
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> generateColoredCloud(const cv::Mat &depth_img,
|
||||
const cv::Mat &color_img);
|
||||
|
||||
|
||||
std::pair<bool, std::string> validateInputs(const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image);
|
||||
};
|
||||
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
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 <cmath>
|
||||
|
||||
namespace mini_vikit {
|
||||
|
||||
using namespace Eigen;
|
||||
|
||||
class PolynomialCamera {
|
||||
private:
|
||||
const double fx_, fy_;
|
||||
const double cx_, cy_;
|
||||
const double skew_;
|
||||
bool distortion_;
|
||||
double k2_, k3_, k4_, k5_, k6_, k7_;
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
PolynomialCamera(double width, double height,
|
||||
double fx, double fy, double cx, double cy, double skew,
|
||||
double k2=0.0, double k3=0.0, double k4=0.0,
|
||||
double k5=0.0, double k6=0.0, double k7=0.0)
|
||||
: fx_(fx), fy_(fy), cx_(cx), cy_(cy), skew_(skew),
|
||||
distortion_(std::abs(k2) > 1e-7) {
|
||||
k2_ = k2; k3_ = k3; k4_ = k4; k5_ = k5; k6_ = k6; k7_ = k7;
|
||||
}
|
||||
|
||||
Vector3d cam2world(const double& u, const double& v) const {
|
||||
Vector3d xyz;
|
||||
if (!distortion_) {
|
||||
double y = (v - cy_) / fy_;
|
||||
double x = (u - cx_ - y * skew_) / fx_;
|
||||
xyz << x, y, 1.0;
|
||||
} else {
|
||||
double y = (v - cy_) / fy_;
|
||||
double x = (u - cx_ - y * skew_) / fx_;
|
||||
|
||||
const double thetad = std::sqrt(x * x + y * y);
|
||||
double theta = thetad;
|
||||
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
const double theta2 = theta * theta;
|
||||
const double theta3 = theta2 * theta;
|
||||
const double theta4 = theta3 * theta;
|
||||
const double theta5 = theta4 * theta;
|
||||
const double theta6 = theta5 * theta;
|
||||
theta = thetad / (1.0 + k2_ * theta + k3_ * theta2 + k4_ * theta3 +
|
||||
k5_ * theta4 + k6_ * theta5 + k7_ * theta6);
|
||||
}
|
||||
|
||||
const double scaling = std::tan(theta) / thetad;
|
||||
x *= scaling;
|
||||
y *= scaling;
|
||||
xyz << x, y, 1.0;
|
||||
}
|
||||
return xyz.normalized();
|
||||
}
|
||||
|
||||
Vector3d cam2world(const Vector2d& px) const {
|
||||
return cam2world(px[0], px[1]);
|
||||
}
|
||||
|
||||
Vector2d world2cam(const Vector3d& xyz) const {
|
||||
Vector2d px;
|
||||
if (!distortion_) {
|
||||
px[0] = fx_ * xyz[0] + cx_;
|
||||
px[1] = fy_ * xyz[1] + cy_;
|
||||
} else {
|
||||
double xd, yd;
|
||||
const double r = std::sqrt(xyz(1) * xyz(1) + xyz(0) * xyz(0));
|
||||
const double theta = std::acos(xyz(2) / xyz.norm());
|
||||
const double thetad = thetad_from_theta(theta);
|
||||
const double scaling = thetad / r;
|
||||
xd = xyz[0] * scaling;
|
||||
yd = xyz[1] * scaling;
|
||||
px[0] = xd * fx_ + yd * skew_ + cx_;
|
||||
px[1] = yd * fy_ + cy_;
|
||||
}
|
||||
return px;
|
||||
}
|
||||
|
||||
Vector2d world2cam(const Vector2d& uv) const {
|
||||
Vector2d px;
|
||||
if (!distortion_) {
|
||||
px[0] = fx_ * uv[0] + cx_;
|
||||
px[1] = fy_ * uv[1] + cy_;
|
||||
} else {
|
||||
double xd, yd;
|
||||
const double r = uv.norm();
|
||||
if (r < 1e-8) {
|
||||
return uv;
|
||||
}
|
||||
const double theta = std::atan(r);
|
||||
const double thetad = thetad_from_theta(theta);
|
||||
const double scaling = thetad / r;
|
||||
xd = uv[0] * scaling;
|
||||
yd = uv[1] * scaling;
|
||||
px[0] = xd * fx_ + yd * skew_ + cx_;
|
||||
px[1] = yd * fy_ + cy_;
|
||||
}
|
||||
return px;
|
||||
}
|
||||
|
||||
inline double thetad_from_theta(const double theta) const {
|
||||
const double theta2 = theta * theta;
|
||||
const double theta3 = theta2 * theta;
|
||||
const double theta4 = theta3 * theta;
|
||||
const double theta5 = theta4 * theta;
|
||||
const double theta6 = theta5 * theta;
|
||||
const double theta7 = theta6 * theta;
|
||||
const double thetad = theta + k2_ * theta2 + k3_ * theta3 +
|
||||
k4_ * theta4 + k5_ * theta5 + k6_ * theta6 + k7_ * theta7;
|
||||
return thetad;
|
||||
}
|
||||
|
||||
double fx() const { return fx_; }
|
||||
double fy() const { return fy_; }
|
||||
double cx() const { return cx_; }
|
||||
double cy() const { return cy_; }
|
||||
double skew() const { return skew_; }
|
||||
bool has_distortion() const { return distortion_; }
|
||||
|
||||
double k2() const { return k2_; }
|
||||
double k3() const { return k3_; }
|
||||
double k4() const { return k4_; }
|
||||
double k5() const { return k5_; }
|
||||
double k6() const { return k6_; }
|
||||
double k7() const { return k7_; }
|
||||
};
|
||||
} // namespace mini_vikit
|
||||
@@ -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.
|
||||
*/
|
||||
#ifndef RGBCLOUD_H
|
||||
#define RGBCLOUD_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <Eigen/Dense>
|
||||
#include "lidar_api.h"
|
||||
|
||||
namespace GlobalCameraParams {
|
||||
extern float g_fx;
|
||||
extern float g_fy;
|
||||
extern float g_cx;
|
||||
extern float g_cy;
|
||||
extern float g_skew;
|
||||
extern float g_k2;
|
||||
extern float g_k3;
|
||||
extern float g_k4;
|
||||
extern float g_k5;
|
||||
extern float g_k6;
|
||||
extern float g_k7;
|
||||
extern Eigen::Matrix4f g_T_camera_lidar;
|
||||
}
|
||||
|
||||
class rawCloudRender {
|
||||
public:
|
||||
|
||||
|
||||
bool init(const std::string& yamlFilePath);
|
||||
|
||||
void nv12buffer_2_rgb(buffer_List_t &image, std::vector<std::vector<float>>& rgb_image);
|
||||
void render(std::vector<std::vector<float>>& rgb_image, capture_Image_List_t* pcdStream, int pcdIdx, std::vector<float>& rgbCloud_flat);
|
||||
|
||||
void print_camera_calib();
|
||||
|
||||
int getImageWidth() const { return image_width_; }
|
||||
int getImageHeight() const { return image_height_; }
|
||||
|
||||
|
||||
|
||||
private:
|
||||
std::string model_type_;
|
||||
std::string camera_name_;
|
||||
int image_width_;
|
||||
int image_height_;
|
||||
int frame_size_;
|
||||
bool opencv_available_;
|
||||
|
||||
// 4x4 transformation matrix (T_camera_lidar)
|
||||
Eigen::Matrix4f T_camera_lidar_;
|
||||
|
||||
float k2_;
|
||||
float k3_;
|
||||
float k4_;
|
||||
float k5_;
|
||||
float k6_;
|
||||
float k7_;
|
||||
float p1_;
|
||||
float p2_;
|
||||
float A11_fx_;
|
||||
float A12_skew_;
|
||||
float A22_fy_;
|
||||
float u0_cx_;
|
||||
float v0_cy_;
|
||||
bool isFast_;
|
||||
int numDiff_;
|
||||
float maxIncidentAngle_;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // RGBCLOUD_H
|
||||
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
#ifndef YAML_PARSER_H
|
||||
#define YAML_PARSER_H
|
||||
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <unordered_set>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include "lidar_api.h"
|
||||
|
||||
namespace odin_ros_driver {
|
||||
|
||||
// Data type enum for supporting different value types
|
||||
enum class DataType {
|
||||
INT_TYPE,
|
||||
FLOAT_ARRAY_TYPE,
|
||||
INT_ARRAY_TYPE,
|
||||
};
|
||||
|
||||
// Generic parameter value holder
|
||||
struct ParameterValue {
|
||||
DataType type;
|
||||
std::vector<uint8_t> data;
|
||||
|
||||
ParameterValue() : type(DataType::INT_TYPE) {}
|
||||
|
||||
template<typename T>
|
||||
void setData(const T& value) {
|
||||
const uint8_t* ptr = reinterpret_cast<const uint8_t*>(&value);
|
||||
data.assign(ptr, ptr + sizeof(T));
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void setArray(const std::vector<T>& arr) {
|
||||
const uint8_t* ptr = reinterpret_cast<const uint8_t*>(arr.data());
|
||||
data.assign(ptr, ptr + arr.size() * sizeof(T));
|
||||
}
|
||||
|
||||
size_t getSize() const {
|
||||
return data.size();
|
||||
}
|
||||
|
||||
const void* getData() const {
|
||||
return data.empty() ? nullptr : data.data();
|
||||
}
|
||||
};
|
||||
|
||||
class YamlParser {
|
||||
public:
|
||||
YamlParser(const std::string& config_file);
|
||||
|
||||
bool loadConfig();
|
||||
const std::map<std::string, int>& getRegisterKeys() const;
|
||||
const std::map<std::string, std::string>& getRegisterKeysStrVal() const;
|
||||
const std::map<std::string, ParameterValue>& getCustomParameters() const;
|
||||
void printConfig() const;
|
||||
bool applyCustomParameters(device_handle device);
|
||||
int getCustomParameterInt(const std::string& param_name, int default_value) const;
|
||||
|
||||
int getCustomMapMode(int default_value) const {
|
||||
auto it = custom_parameters_.find("map_mode");
|
||||
if (it != custom_parameters_.end() && it->second.type == DataType::INT_TYPE) {
|
||||
printf("custom_map_mode = %d\n", *(int*)it->second.getData());
|
||||
return *(int*)it->second.getData();
|
||||
} else {
|
||||
return default_value;
|
||||
}
|
||||
};
|
||||
|
||||
private:
|
||||
std::string config_file_;
|
||||
std::map<std::string, int> register_keys_;
|
||||
std::map<std::string, std::string> register_keys_str_val_;
|
||||
std::map<std::string, double> register_keys_float_val_;
|
||||
std::map<std::string, ParameterValue> custom_parameters_;
|
||||
|
||||
// Keys whose YAML value is a string (not int).
|
||||
std::unordered_set<std::string> allowed_key_w_str_val = {
|
||||
"relocalization_map_abs_path",
|
||||
"mapping_result_dest_dir",
|
||||
"mapping_result_file_name",
|
||||
"image_mask_abs_path",
|
||||
"overlay_reprojected_topic",
|
||||
"overlay_camera_topic",
|
||||
"overlay_output_topic"
|
||||
};
|
||||
|
||||
// Keys whose YAML value is a floating-point number (not int).
|
||||
std::unordered_set<std::string> allowed_key_w_float_val = {
|
||||
"overlay_alpha"
|
||||
};
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,37 @@
|
||||
<launch>
|
||||
<!--
|
||||
Usage: roslaunch odin_ros_driver odin1_ros1.launch
|
||||
-->
|
||||
|
||||
<!-- Set node name -->
|
||||
<arg name="node_name" default="host_sdk_sample"/>
|
||||
|
||||
<!-- Set parameter file path -->
|
||||
<arg name="config_file" default="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
|
||||
<!-- Set RViz configuration file path -->
|
||||
<arg name="rviz_config" default="$(find odin_ros_driver)/config/odin_ros.rviz"/>
|
||||
|
||||
<!-- Launch main node -->
|
||||
<node name="$(arg node_name)" pkg="odin_ros_driver" type="host_sdk_sample" output="screen">
|
||||
<param name="config_file" value="$(arg config_file)"/>
|
||||
</node>
|
||||
|
||||
<node pkg="odin_ros_driver" type="pcd2depth_node" name="pcd2depth_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
|
||||
</node>
|
||||
|
||||
<node pkg="odin_ros_driver" type="cloud_reprojection_node" name="cloud_reprojection_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
<param name="calib_file_path" value="$(find odin_ros_driver)/config/calib.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Image overlay node - overlays reprojected points on camera image -->
|
||||
<node pkg="odin_ros_driver" type="image_overlay_node" name="image_overlay_node" output="screen" >
|
||||
<rosparam command="load" file="$(find odin_ros_driver)/config/control_command.yaml"/>
|
||||
</node>
|
||||
|
||||
<!-- Launch RViz with configuration -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)" output="screen"/>
|
||||
</launch>
|
||||
+111
@@ -0,0 +1,111 @@
|
||||
|
||||
# USAGE: ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
import os
|
||||
import yaml
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
# Get package directory
|
||||
package_dir = get_package_share_directory('odin_ros_driver')
|
||||
|
||||
# Declare configuration parameter
|
||||
config_file_arg = DeclareLaunchArgument(
|
||||
'config_file',
|
||||
default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
|
||||
description='Path to the control config YAML file'
|
||||
)
|
||||
|
||||
# Add RViz2 configuration file parameter
|
||||
rviz_config_arg = DeclareLaunchArgument(
|
||||
'rviz_config',
|
||||
default_value=os.path.join(package_dir, 'config', 'odin_ros2.rviz'),
|
||||
description='Path to RViz2 config file'
|
||||
)
|
||||
|
||||
# Declare launch rviz parameter
|
||||
launch_rviz_arg = DeclareLaunchArgument(
|
||||
'launch_rviz',
|
||||
default_value='false',
|
||||
description='Whether to launch RViz2'
|
||||
)
|
||||
|
||||
|
||||
# Create main node
|
||||
host_sdk_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='host_sdk_sample',
|
||||
name='host_sdk_sample',
|
||||
output='screen',
|
||||
# arguments=['--ros-args', '--log-level', 'debug'],
|
||||
parameters=[{
|
||||
'config_file': LaunchConfiguration('config_file')
|
||||
}]
|
||||
)
|
||||
|
||||
pcd2depth_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(pcd2depth_config_path, 'r') as f:
|
||||
pcd2depth_params = yaml.safe_load(f)
|
||||
pcd2depth_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
pcd2depth_params['calib_file_path'] = pcd2depth_calib_path
|
||||
pcd2depth_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='pcd2depth_ros2_node',
|
||||
name='pcd2depth_ros2_node',
|
||||
output='screen',
|
||||
parameters=[pcd2depth_params]
|
||||
)
|
||||
|
||||
# Cloud reprojection node
|
||||
reprojection_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(reprojection_config_path, 'r') as f:
|
||||
reprojection_params = yaml.safe_load(f)
|
||||
reprojection_calib_path = os.path.join(package_dir, 'config', 'calib.yaml')
|
||||
reprojection_params['calib_file_path'] = reprojection_calib_path
|
||||
cloud_reprojection_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='cloud_reprojection_ros2_node',
|
||||
name='cloud_reprojection_ros2_node',
|
||||
output='screen',
|
||||
parameters=[reprojection_params]
|
||||
)
|
||||
|
||||
# Image overlay node - overlays reprojected points on camera image
|
||||
overlay_config_path = os.path.join(package_dir, 'config', 'control_command.yaml')
|
||||
with open(overlay_config_path, 'r') as f:
|
||||
overlay_params = yaml.safe_load(f)
|
||||
image_overlay_node = Node(
|
||||
package='odin_ros_driver',
|
||||
executable='image_overlay_node',
|
||||
name='image_overlay_node',
|
||||
output='screen',
|
||||
parameters=[overlay_params]
|
||||
)
|
||||
|
||||
# Create RViz2 node - loads specified configuration file
|
||||
rviz_node = Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', LaunchConfiguration('rviz_config')],
|
||||
condition=IfCondition(LaunchConfiguration('launch_rviz'))
|
||||
)
|
||||
|
||||
# Create launch description
|
||||
ld = LaunchDescription()
|
||||
ld.add_action(config_file_arg)
|
||||
ld.add_action(rviz_config_arg) # Add RViz configuration argument
|
||||
ld.add_action(launch_rviz_arg) # Add launch_rviz argument
|
||||
ld.add_action(host_sdk_node)
|
||||
ld.add_action(pcd2depth_node)
|
||||
ld.add_action(cloud_reprojection_node)
|
||||
ld.add_action(image_overlay_node)
|
||||
ld.add_action(rviz_node) # Add RViz node
|
||||
|
||||
return ld
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<!-- ROS1 uses catkin as the build tool -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- ROS1 dependencies -->
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
|
||||
<!-- System dependencies -->
|
||||
<depend>eigen</depend>
|
||||
<depend>opencv</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
|
||||
<!-- Specify build type as catkin -->
|
||||
<export>
|
||||
<build_type>catkin</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>odin_ros_driver</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ROS2 driver for Odin sensor</description>
|
||||
<maintainer email="oliveiratang@manifoldtech.cn">rlk</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<!-- ROS2 uses colcon as the build tool -->
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<!-- ROS2 dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<!-- System dependencies -->
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>cv_bridge</depend>
|
||||
<depend>image_transport</depend>
|
||||
<depend>pcl_conversions</depend>
|
||||
<depend>message_filters</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,128 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Get the directory where the script is located (Odin_ROS_Driver directory)
|
||||
PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)"
|
||||
# Calculate the workspace root directory (contains devel, build, src)
|
||||
WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")"
|
||||
# Workspace source directory (contains all packages)
|
||||
WORKSPACE_SRC="${WORKSPACE_ROOT}/src"
|
||||
PROJECT_NAME="odin_ros_driver"
|
||||
|
||||
# Define color codes
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
NC='\033[0m'
|
||||
|
||||
# Clean workspace function
|
||||
clean_workspace() {
|
||||
echo -e "${YELLOW}Cleaning build directories${NC}"
|
||||
|
||||
# Clean build artifacts in workspace
|
||||
rm -rf "${WORKSPACE_ROOT}/build"
|
||||
rm -rf "${WORKSPACE_ROOT}/install"
|
||||
rm -rf "${WORKSPACE_ROOT}/log"
|
||||
rm -rf "${WORKSPACE_ROOT}/devel"
|
||||
|
||||
echo -e "${GREEN}Cleanup complete${NC}"
|
||||
}
|
||||
|
||||
# Run node function
|
||||
run_node() {
|
||||
echo -e "${YELLOW}Running ROS1 node${NC}"
|
||||
|
||||
# Check if environment file exists
|
||||
if [ ! -f "${WORKSPACE_ROOT}/devel/setup.bash" ]; then
|
||||
echo -e "${RED}Could not find devel/setup.bash, please build the project with ./build_ros1.sh first${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Source environment and run node
|
||||
source "${WORKSPACE_ROOT}/devel/setup.bash"
|
||||
|
||||
}
|
||||
|
||||
# Build workspace function
|
||||
build_workspace() {
|
||||
echo -e "${YELLOW}Workspace structure:${NC}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " ROS version: ROS1"
|
||||
|
||||
echo -e "${YELLOW}Starting ROS1 project build...${NC}"
|
||||
|
||||
# Clean
|
||||
cd $WS_DIR
|
||||
rm -rf build devel install
|
||||
|
||||
# Ensure ROS1 environment is loaded
|
||||
if [ -f "/opt/ros/noetic/setup.bash" ]; then
|
||||
source "/opt/ros/noetic/setup.bash"
|
||||
elif [ -f "/opt/ros/melodic/setup.bash" ]; then
|
||||
source "/opt/ros/melodic/setup.bash"
|
||||
else
|
||||
echo -e "${RED}Could not find ROS1 setup.bash file. Please ensure ROS1 is installed.${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Create temporary package.xml
|
||||
if [ -f "${PKG_DIR}/package_ros1.xml" ]; then
|
||||
echo "Creating temporary package.xml (using package_ros1.xml)"
|
||||
cp "${PKG_DIR}/package_ros1.xml" "${PKG_DIR}/package.xml"
|
||||
TEMP_PACKAGE=true
|
||||
elif [ -f "${PKG_DIR}/package.xml" ]; then
|
||||
echo "Using existing package.xml"
|
||||
else
|
||||
echo -e "${RED}Could not find package.xml in package directory${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Set build system variable
|
||||
export BUILD_SYSTEM=ROS1
|
||||
|
||||
# Switch to workspace root and build
|
||||
cd "${WORKSPACE_ROOT}" || return 1
|
||||
catkin_make -DBUILD_SYSTEM=ROS1 -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j$(nproc)
|
||||
BUILD_RESULT=$?
|
||||
|
||||
# If build successful, source environment
|
||||
if [[ $BUILD_RESULT -eq 0 ]]; then
|
||||
echo -e "${GREEN}ROS1 build successful, loading environment: source devel/setup.bash${NC}"
|
||||
source "${WORKSPACE_ROOT}/devel/setup.bash"
|
||||
else
|
||||
echo -e "${RED}ROS1 build failed, please check error logs${NC}"
|
||||
fi
|
||||
|
||||
|
||||
}
|
||||
|
||||
# Help function
|
||||
show_help() {
|
||||
echo -e "${YELLOW}Usage:${NC}"
|
||||
echo " ./build_ros.sh # Build project"
|
||||
echo " ./build_ros.sh -c # Clean build artifacts"
|
||||
echo " ./build_ros.sh -h # Show help information"
|
||||
echo ""
|
||||
echo -e "${YELLOW}Current configuration:${NC}"
|
||||
echo " Project name: ${PROJECT_NAME}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
}
|
||||
|
||||
# Main
|
||||
case "$1" in
|
||||
-c|--clean)
|
||||
clean_workspace
|
||||
;;
|
||||
-r|--run)
|
||||
run_node
|
||||
;;
|
||||
-h|--help)
|
||||
show_help
|
||||
;;
|
||||
*)
|
||||
build_workspace
|
||||
;;
|
||||
esac
|
||||
@@ -0,0 +1,158 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Get the directory where the script is located (Odin_ROS_Driver directory)
|
||||
PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)"
|
||||
# Calculate the workspace root directory (contains devel, build, src)
|
||||
WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")"
|
||||
# Workspace source directory (contains all packages)
|
||||
WORKSPACE_SRC="${WORKSPACE_ROOT}/src"
|
||||
PROJECT_NAME="odin_ros_driver"
|
||||
PACKAGE_DIR_NAME=$(basename "$PKG_DIR")
|
||||
|
||||
# Define color codes
|
||||
RED='\033[0;31m'
|
||||
GREEN='\033[0;32m'
|
||||
YELLOW='\033[1;33m'
|
||||
NC='\033[0m'
|
||||
|
||||
# Extract package name from package.xml
|
||||
get_package_name() {
|
||||
local package_xml="$1"
|
||||
if [ -f "$package_xml" ]; then
|
||||
# Extract content of <name> tag
|
||||
grep -oP '<name>\K[^<]+' "$package_xml" | head -1
|
||||
else
|
||||
echo ""
|
||||
fi
|
||||
}
|
||||
|
||||
# Clean workspace function
|
||||
clean_workspace() {
|
||||
echo -e "${YELLOW}Cleaning build directories${NC}"
|
||||
|
||||
# Clean build artifacts in workspace
|
||||
rm -rf "${WORKSPACE_ROOT}/build"
|
||||
rm -rf "${WORKSPACE_ROOT}/install"
|
||||
rm -rf "${WORKSPACE_ROOT}/log"
|
||||
rm -rf "${WORKSPACE_ROOT}/devel"
|
||||
|
||||
echo -e "${GREEN}Cleanup complete${NC}"
|
||||
}
|
||||
|
||||
# Run node function
|
||||
run_node() {
|
||||
echo -e "${YELLOW}Running ROS2 node${NC}"
|
||||
|
||||
# Check if environment file exists
|
||||
if [ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]; then
|
||||
echo -e "${RED}Could not find install/setup.bash, please build the project with ./build_ros2.sh first${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Source environment and run node
|
||||
source "${WORKSPACE_ROOT}/install/setup.bash"
|
||||
|
||||
}
|
||||
|
||||
# Build workspace function
|
||||
build_workspace() {
|
||||
echo -e "${YELLOW}Workspace structure:${NC}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Directory name: ${PACKAGE_DIR_NAME}"
|
||||
echo " ROS version: ROS2"
|
||||
|
||||
echo -e "${YELLOW}Starting ROS2 project build...${NC}"
|
||||
|
||||
cd $WS_DIR
|
||||
rm -rf build install log
|
||||
# Ensure ROS2 environment is loaded
|
||||
if [ -f "/opt/ros/foxy/setup.bash" ]; then
|
||||
source "/opt/ros/foxy/setup.bash"
|
||||
elif [ -f "/opt/ros/galactic/setup.bash" ]; then
|
||||
source "/opt/ros/galactic/setup.bash"
|
||||
elif [ -f "/opt/ros/humble/setup.bash" ]; then
|
||||
source "/opt/ros/humble/setup.bash"
|
||||
else
|
||||
echo -e "${RED}Could not find ROS2 setup.bash file. Please ensure ROS2 is installed.${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Create temporary package.xml
|
||||
if [ -f "${PKG_DIR}/package_ros2.xml" ]; then
|
||||
echo "Creating temporary package.xml (using package_ros2.xml)"
|
||||
cp "${PKG_DIR}/package_ros2.xml" "${PKG_DIR}/package.xml"
|
||||
TEMP_PACKAGE=true
|
||||
elif [ -f "${PKG_DIR}/package.xml" ]; then
|
||||
echo "Using existing package.xml"
|
||||
TEMP_PACKAGE=false
|
||||
else
|
||||
echo -e "${RED}Could not find package.xml in package directory${NC}"
|
||||
return 1
|
||||
fi
|
||||
|
||||
# Extract package name from package.xml
|
||||
PACKAGE_NAME=$(get_package_name "${PKG_DIR}/package.xml")
|
||||
if [ -z "$PACKAGE_NAME" ]; then
|
||||
echo -e "${RED}Failed to extract package name from package.xml${NC}"
|
||||
return 1
|
||||
fi
|
||||
echo " Package name: ${PACKAGE_NAME}"
|
||||
|
||||
# Set build system variable
|
||||
export BUILD_SYSTEM=ROS2
|
||||
|
||||
# Switch to workspace root and build
|
||||
cd "${WORKSPACE_ROOT}" || return 1
|
||||
|
||||
# Build with correct package name
|
||||
colcon build \
|
||||
--packages-select "${PACKAGE_NAME}" \
|
||||
--parallel-workers $(nproc) \
|
||||
--cmake-args \
|
||||
-DBUILD_SYSTEM=ROS2 \
|
||||
-DCMAKE_EXPORT_COMPILE_COMMANDS=ON
|
||||
|
||||
BUILD_RESULT=$?
|
||||
|
||||
# If build successful, source environment
|
||||
if [[ $BUILD_RESULT -eq 0 ]]; then
|
||||
echo -e "${GREEN}ROS2 build successful, loading environment: source install/setup.bash${NC}"
|
||||
source "${WORKSPACE_ROOT}/install/setup.bash"
|
||||
|
||||
else
|
||||
echo -e "${RED}ROS2 build failed, please check error logs${NC}"
|
||||
fi
|
||||
|
||||
}
|
||||
|
||||
# Help function
|
||||
show_help() {
|
||||
echo -e "${YELLOW}Usage:${NC}"
|
||||
echo " ./build_ros2.sh # Build project"
|
||||
echo " ./build_ros2.sh -c # Clean build artifacts"
|
||||
echo " ./build_ros2.sh -h # Show help information"
|
||||
echo ""
|
||||
echo -e "${YELLOW}Current configuration:${NC}"
|
||||
echo " Project name: ${PROJECT_NAME}"
|
||||
echo " Package directory: ${PKG_DIR}"
|
||||
echo " Workspace root: ${WORKSPACE_ROOT}"
|
||||
echo " Source directory: ${WORKSPACE_SRC}"
|
||||
}
|
||||
|
||||
# Main program
|
||||
case "$1" in
|
||||
-c|--clean)
|
||||
clean_workspace
|
||||
;;
|
||||
-r|--run)
|
||||
run_node
|
||||
;;
|
||||
-h|--help)
|
||||
show_help
|
||||
;;
|
||||
*)
|
||||
build_workspace
|
||||
;;
|
||||
esac
|
||||
@@ -0,0 +1,21 @@
|
||||
#!/bin/bash
|
||||
|
||||
# Usage: ./set_param.sh <parameter_name> <value>
|
||||
# Example: ./set_param.sh save_map 1
|
||||
|
||||
if [ $# -ne 2 ]; then
|
||||
echo "Usage: $0 <parameter_name> <value>"
|
||||
echo "Example: $0 save_map 1"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
PARAM_NAME=$1
|
||||
VALUE=$2
|
||||
|
||||
COMMAND_FILE="/tmp/odin_command.txt"
|
||||
|
||||
# Create the command file with the parameter
|
||||
echo "set $PARAM_NAME $VALUE" > "$COMMAND_FILE"
|
||||
|
||||
echo "Command sent: set $PARAM_NAME $VALUE"
|
||||
echo "Command file: $COMMAND_FILE"
|
||||
+232
@@ -0,0 +1,232 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "camera_pose_visualization.h"
|
||||
|
||||
const Eigen::Vector3d camera_pose_visualization::imlt = Eigen::Vector3d(-1.0, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imrt = Eigen::Vector3d( 1.0, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imlb = Eigen::Vector3d(-1.0, 0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::imrb = Eigen::Vector3d( 1.0, 0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt0 = Eigen::Vector3d(-0.7, -0.5, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt1 = Eigen::Vector3d(-0.7, -0.2, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::lt2 = Eigen::Vector3d(-1.0, -0.2, 1.0);
|
||||
const Eigen::Vector3d camera_pose_visualization::oc = Eigen::Vector3d(0.0, 0.0, 0.0);
|
||||
|
||||
#ifdef ROS2
|
||||
using GeometryPoint = geometry_msgs::msg::Point;
|
||||
#else
|
||||
using GeometryPoint = geometry_msgs::Point;
|
||||
#endif
|
||||
|
||||
void Eigen2Point(const Eigen::Vector3d& v, GeometryPoint& p) {
|
||||
p.x = v.x();
|
||||
p.y = v.y();
|
||||
p.z = v.z();
|
||||
}
|
||||
|
||||
camera_pose_visualization::camera_pose_visualization(float r, float g, float b, float a)
|
||||
: m_marker_ns("camera_pose_visualization"), m_scale(0.3), m_line_width(0.03) {
|
||||
m_image_boundary_color.r = r;
|
||||
m_image_boundary_color.g = g;
|
||||
m_image_boundary_color.b = b;
|
||||
m_image_boundary_color.a = a;
|
||||
m_optical_center_connector_color.r = r;
|
||||
m_optical_center_connector_color.g = g;
|
||||
m_optical_center_connector_color.b = b;
|
||||
m_optical_center_connector_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setImageBoundaryColor(float r, float g, float b, float a) {
|
||||
m_image_boundary_color.r = r;
|
||||
m_image_boundary_color.g = g;
|
||||
m_image_boundary_color.b = b;
|
||||
m_image_boundary_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setOpticalCenterConnectorColor(float r, float g, float b, float a) {
|
||||
m_optical_center_connector_color.r = r;
|
||||
m_optical_center_connector_color.g = g;
|
||||
m_optical_center_connector_color.b = b;
|
||||
m_optical_center_connector_color.a = a;
|
||||
}
|
||||
|
||||
void camera_pose_visualization::setScale(double s) {
|
||||
m_scale = s;
|
||||
}
|
||||
void camera_pose_visualization::setLineWidth(double width) {
|
||||
m_line_width = width;
|
||||
}
|
||||
void camera_pose_visualization::add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_LIST;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = 0.005;
|
||||
|
||||
marker.color.g = 1.0f;
|
||||
marker.color.a = 1.0;
|
||||
|
||||
GeometryPoint point0, point1;
|
||||
|
||||
Eigen2Point(p0, point0);
|
||||
Eigen2Point(p1, point1);
|
||||
|
||||
marker.points.push_back(point0);
|
||||
marker.points.push_back(point1);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
void camera_pose_visualization::add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_LIST;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_LIST;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = 0.04;
|
||||
//marker.scale.x = 0.3;
|
||||
|
||||
marker.color.r = 1.0f;
|
||||
marker.color.b = 1.0f;
|
||||
marker.color.a = 1.0;
|
||||
|
||||
GeometryPoint point0, point1;
|
||||
|
||||
Eigen2Point(p0, point0);
|
||||
Eigen2Point(p1, point1);
|
||||
|
||||
marker.points.push_back(point0);
|
||||
marker.points.push_back(point1);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
|
||||
void camera_pose_visualization::add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q) {
|
||||
Marker marker;
|
||||
|
||||
marker.ns = m_marker_ns;
|
||||
marker.id = m_markers.size() + 1;
|
||||
#ifdef ROS2
|
||||
marker.type = Marker::LINE_STRIP;
|
||||
marker.action = Marker::ADD;
|
||||
#else
|
||||
marker.type = visualization_msgs::Marker::LINE_STRIP;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
#endif
|
||||
marker.scale.x = m_line_width;
|
||||
|
||||
marker.pose.position.x = 0.0;
|
||||
marker.pose.position.y = 0.0;
|
||||
marker.pose.position.z = 0.0;
|
||||
marker.pose.orientation.w = 1.0;
|
||||
marker.pose.orientation.x = 0.0;
|
||||
marker.pose.orientation.y = 0.0;
|
||||
marker.pose.orientation.z = 0.0;
|
||||
|
||||
|
||||
GeometryPoint pt_lt, pt_lb, pt_rt, pt_rb, pt_oc, pt_lt0, pt_lt1, pt_lt2;
|
||||
|
||||
Eigen2Point(q * (m_scale * imlt) + p, pt_lt);
|
||||
Eigen2Point(q * (m_scale * imlb) + p, pt_lb);
|
||||
Eigen2Point(q * (m_scale * imrt) + p, pt_rt);
|
||||
Eigen2Point(q * (m_scale * imrb) + p, pt_rb);
|
||||
Eigen2Point(q * (m_scale * lt0 ) + p, pt_lt0);
|
||||
Eigen2Point(q * (m_scale * lt1 ) + p, pt_lt1);
|
||||
Eigen2Point(q * (m_scale * lt2 ) + p, pt_lt2);
|
||||
Eigen2Point(q * (m_scale * oc ) + p, pt_oc);
|
||||
|
||||
// image boundaries
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
// top-left indicator
|
||||
marker.points.push_back(pt_lt0);
|
||||
marker.points.push_back(pt_lt1);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
marker.points.push_back(pt_lt1);
|
||||
marker.points.push_back(pt_lt2);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
marker.colors.push_back(m_image_boundary_color);
|
||||
|
||||
// optical center connector
|
||||
marker.points.push_back(pt_lt);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
|
||||
marker.points.push_back(pt_lb);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
marker.points.push_back(pt_rt);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
marker.points.push_back(pt_rb);
|
||||
marker.points.push_back(pt_oc);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
marker.colors.push_back(m_optical_center_connector_color);
|
||||
|
||||
m_markers.push_back(marker);
|
||||
}
|
||||
|
||||
void camera_pose_visualization::reset() {
|
||||
m_markers.clear();
|
||||
}
|
||||
|
||||
void camera_pose_visualization::publish_by( Publisher& pub, const Header& header ) {
|
||||
MarkerArray markerArray_msg;
|
||||
|
||||
for (auto& marker : m_markers) {
|
||||
marker.header = header;
|
||||
markerArray_msg.markers.push_back(marker);
|
||||
}
|
||||
|
||||
pub.publish(markerArray_msg);
|
||||
}
|
||||
@@ -0,0 +1,503 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "cloud_reprojection_ros_node.hpp"
|
||||
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
|
||||
#ifdef ROS2
|
||||
#include <functional>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <rcpputils/filesystem_helper.hpp>
|
||||
#else
|
||||
#include <boost/bind.hpp>
|
||||
#endif
|
||||
|
||||
// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu
|
||||
// TODO: Fill in the actual Til values for your sensor setup
|
||||
static Eigen::Matrix4d getFixedTil()
|
||||
{
|
||||
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity();
|
||||
Til(0, 3) = -0.02663;
|
||||
Til(1, 3) = 0.03447;
|
||||
Til(2, 3) = 0.02174;
|
||||
return Til;
|
||||
}
|
||||
|
||||
static bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
#ifdef ROS2
|
||||
// Helper function to get package source directory for ROS2
|
||||
static std::string get_package_source_directory() {
|
||||
std::string current_file = __FILE__;
|
||||
size_t pos = current_file.find("/src/cloud_reprojection_ros.cpp");
|
||||
if (pos != std::string::npos) {
|
||||
return current_file.substr(0, pos);
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
// ==================== ROS2 Implementation ====================
|
||||
|
||||
CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options)
|
||||
: Node("cloud_reprojection_node", options)
|
||||
{
|
||||
loadParameters();
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(),
|
||||
"\n cloud_slam_topic: " << cloud_slam_topic_
|
||||
<< "\n odometry_topic: " << odometry_topic_
|
||||
<< "\n wiwc_topic: " << wiwc_topic_
|
||||
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
|
||||
|
||||
cloud_sub_.subscribe(this, cloud_slam_topic_);
|
||||
odom_sub_.subscribe(this, odometry_topic_);
|
||||
wiwc_sub_.subscribe(this, wiwc_topic_);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
|
||||
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||
|
||||
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized successfully");
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::loadParameters()
|
||||
{
|
||||
// Declare and get parameters
|
||||
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
|
||||
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
|
||||
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
|
||||
this->declare_parameter<std::string>("reprojected_image_topic", "/odin1/reprojected_image");
|
||||
|
||||
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
|
||||
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
|
||||
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
|
||||
reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
|
||||
|
||||
// Load camera parameters from calib.yaml file directly
|
||||
std::string package_path = get_package_source_directory();
|
||||
std::string calib_file = package_path + "/config/calib.yaml";
|
||||
|
||||
YAML::Node calib_config;
|
||||
try {
|
||||
calib_config = YAML::LoadFile(calib_file);
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
CloudReprojector::CameraParams cam_params;
|
||||
try {
|
||||
cam_params.image_width = calib_config["cam_0"]["image_width"].as<int>();
|
||||
cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
|
||||
cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
|
||||
cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
|
||||
cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
|
||||
cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
|
||||
cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
|
||||
cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
|
||||
cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
|
||||
cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
|
||||
cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
|
||||
cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
|
||||
cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
// Load extrinsic parameters
|
||||
CloudReprojector::ExtrinsicParams ext_params;
|
||||
try {
|
||||
auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
|
||||
|
||||
if (Tcl_vec.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
for (int j = 0; j < 4; ++j)
|
||||
ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j];
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
ext_params.Til = getFixedTil();
|
||||
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
||||
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
||||
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
||||
|
||||
reprojector_ = std::make_unique<CloudReprojector>();
|
||||
if (!reprojector_->initialize(cam_params, ext_params))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::syncCallback(
|
||||
const PointCloud2::ConstSharedPtr& cloud_msg,
|
||||
const Odometry::ConstSharedPtr& odom_msg,
|
||||
const Odometry::ConstSharedPtr& wiwc_msg)
|
||||
{
|
||||
// Debug: print that syncCallback is called
|
||||
static int sync_count = 0;
|
||||
// RCLCPP_INFO(this->get_logger(), "=== syncCallback called, count: %d ===", ++sync_count);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
||||
|
||||
if (cloud_odom.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Empty cloud_slam received");
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract real-time extrinsics from WIWC message covariance fields
|
||||
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
|
||||
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
||||
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
||||
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
||||
}
|
||||
|
||||
// Update extrinsics if valid (not identity matrix)
|
||||
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
|
||||
// // Debug print to compare with host_sdk_sample values
|
||||
// static int print_count = 0;
|
||||
// if (print_count++) {
|
||||
// // Extract rotation (3x3) and translation (3x1) from T_CL
|
||||
// Eigen::Matrix3d RCL = T_CL.block<3,3>(0,0);
|
||||
// Eigen::Vector3d TCL = T_CL.block<3,1>(0,3);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RCL (3x3 rotation from T_CL) ===\n" << RCL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TCL (3x1 translation from T_CL) ===\n" << TCL.transpose());
|
||||
|
||||
// // Extract rotation (3x3) and translation (3x1) from T_IL
|
||||
// Eigen::Matrix3d RIL = T_IL.block<3,3>(0,0);
|
||||
// Eigen::Vector3d TIL = T_IL.block<3,1>(0,3);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection RIL (3x3 rotation from T_IL) ===\n" << RIL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection TIL (3x1 translation from T_IL) ===\n" << TIL.transpose());
|
||||
|
||||
// RCLCPP_INFO(this->get_logger(), "T_CL_valid: %d, T_IL_valid: %d", T_CL_valid, T_IL_valid);
|
||||
// if (T_CL_valid && T_IL_valid) {
|
||||
// Eigen::Matrix4d Tic = CloudReprojector::calculateTic(T_CL, T_IL);
|
||||
// RCLCPP_INFO_STREAM(this->get_logger(), "=== cloud_reprojection calculated Tic ===\n" << Tic);
|
||||
// }
|
||||
// }
|
||||
|
||||
if (T_CL_valid && T_IL_valid) {
|
||||
reprojector_->updateExtrinsics(T_CL, T_IL);
|
||||
}
|
||||
|
||||
CloudReprojector::OdomPose odom_pose;
|
||||
odom_pose.orientation = Eigen::Quaterniond(
|
||||
odom_msg->pose.pose.orientation.w,
|
||||
odom_msg->pose.pose.orientation.x,
|
||||
odom_msg->pose.pose.orientation.y,
|
||||
odom_msg->pose.pose.orientation.z
|
||||
);
|
||||
odom_pose.position = Eigen::Vector3d(
|
||||
odom_msg->pose.pose.position.x,
|
||||
odom_msg->pose.pose.position.y,
|
||||
odom_msg->pose.pose.position.z
|
||||
);
|
||||
|
||||
cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose);
|
||||
|
||||
auto img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg();
|
||||
reprojected_image_pub_.publish(*img_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS2 Main ====================
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto temp_node = std::make_shared<rclcpp::Node>("cloud_reprojection_check");
|
||||
|
||||
// Check if reprojection is enabled from control_command.yaml
|
||||
std::string package_path = get_package_source_directory();
|
||||
std::string config_file = package_path + "/config/control_command.yaml";
|
||||
|
||||
try {
|
||||
YAML::Node config = YAML::LoadFile(config_file);
|
||||
std::cout << "config: " << config_file << std::endl;
|
||||
if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled.");
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int sendreprojection = config["register_keys"]["sendreprojection"].as<int>();
|
||||
if (sendreprojection == 0) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published.");
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what());
|
||||
rclcpp::shutdown();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Wait for calib.yaml file to be generated by host_sdk_sample
|
||||
std::string calib_file = package_path + "/config/calib.yaml";
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str());
|
||||
|
||||
int wait_count = 0;
|
||||
while (rclcpp::ok() && !fileExists(calib_file)) {
|
||||
if (wait_count % 10 == 0) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file...");
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
||||
wait_count++;
|
||||
|
||||
// Timeout after 5 seconds
|
||||
if (wait_count > 10) {
|
||||
RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file");
|
||||
rclcpp::shutdown();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node...");
|
||||
|
||||
auto node = std::make_shared<CloudReprojectionRosNode>();
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started");
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
// ==================== ROS1 Implementation ====================
|
||||
|
||||
CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
||||
: nh_(nh), pnh_(pnh)
|
||||
{
|
||||
loadParameters();
|
||||
|
||||
ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_
|
||||
<< "\n odometry_topic: " << odometry_topic_
|
||||
<< "\n wiwc_topic: " << wiwc_topic_
|
||||
<< "\n reprojected_image_topic: " << reprojected_image_topic_);
|
||||
|
||||
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
|
||||
odom_sub_.subscribe(nh_, odometry_topic_, 1);
|
||||
wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_);
|
||||
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3));
|
||||
|
||||
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
|
||||
|
||||
ROS_INFO("CloudReprojectionRosNode initialized successfully");
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::loadParameters()
|
||||
{
|
||||
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
|
||||
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
|
||||
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
|
||||
pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
|
||||
|
||||
// Load camera parameters
|
||||
CloudReprojector::CameraParams cam_params;
|
||||
pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
|
||||
pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
|
||||
pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
|
||||
pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
|
||||
pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
|
||||
pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
|
||||
pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
|
||||
pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
|
||||
pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
|
||||
pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
|
||||
pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
|
||||
pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
|
||||
pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);
|
||||
|
||||
// Load extrinsic parameters
|
||||
CloudReprojector::ExtrinsicParams ext_params;
|
||||
std::vector<double> Tcl_vec_param;
|
||||
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
for (int j = 0; j < 4; ++j)
|
||||
ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Tcl_0 param missing or invalid.");
|
||||
ros::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
ext_params.Til = getFixedTil();
|
||||
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
||||
|
||||
ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
||||
ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til);
|
||||
ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic);
|
||||
|
||||
ROS_INFO("Camera intrinsics:");
|
||||
ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
||||
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
||||
|
||||
reprojector_ = std::make_unique<CloudReprojector>();
|
||||
if (!reprojector_->initialize(cam_params, ext_params))
|
||||
{
|
||||
ROS_ERROR("Failed to initialize CloudReprojector");
|
||||
ros::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void CloudReprojectionRosNode::syncCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
||||
const nav_msgs::OdometryConstPtr& odom_msg,
|
||||
const nav_msgs::OdometryConstPtr& wiwc_msg)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
||||
|
||||
if (cloud_odom.empty())
|
||||
{
|
||||
ROS_WARN("Empty cloud_slam received");
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract real-time extrinsics from WIWC message covariance fields
|
||||
// pose.covariance contains T_CL (first 16 values), twist.covariance contains T_IL (first 16 values)
|
||||
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
||||
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
||||
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
||||
}
|
||||
|
||||
// Update extrinsics if valid (not identity matrix)
|
||||
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
||||
if (T_CL_valid && T_IL_valid) {
|
||||
reprojector_->updateExtrinsics(T_CL, T_IL);
|
||||
}
|
||||
|
||||
CloudReprojector::OdomPose odom_pose;
|
||||
odom_pose.orientation = Eigen::Quaterniond(
|
||||
odom_msg->pose.pose.orientation.w,
|
||||
odom_msg->pose.pose.orientation.x,
|
||||
odom_msg->pose.pose.orientation.y,
|
||||
odom_msg->pose.pose.orientation.z
|
||||
);
|
||||
odom_pose.position = Eigen::Vector3d(
|
||||
odom_msg->pose.pose.position.x,
|
||||
odom_msg->pose.pose.position.y,
|
||||
odom_msg->pose.pose.position.z
|
||||
);
|
||||
|
||||
cv::Mat reprojected_img = reprojector_->reprojectCloud(cloud_odom, odom_pose);
|
||||
|
||||
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", reprojected_img).toImageMsg();
|
||||
reprojected_image_pub_.publish(img_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS1 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "cloud_reprojection");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
// Check if reprojection is enabled
|
||||
int sendreprojection = 0;
|
||||
pnh.param("register_keys/sendreprojection", sendreprojection, 0);
|
||||
if(sendreprojection == 0)
|
||||
{
|
||||
ROS_INFO("Cloud reprojection will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path;
|
||||
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
||||
|
||||
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(ros::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
||||
ros::Duration(0.5).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
if(!ros::ok())
|
||||
{
|
||||
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
||||
|
||||
std::string node_name = ros::this_node::getName();
|
||||
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
||||
int result = system(rosparam_command.c_str());
|
||||
|
||||
if(result == 0)
|
||||
{
|
||||
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Failed to load parameters from calib.yaml");
|
||||
return 1;
|
||||
}
|
||||
|
||||
CloudReprojectionRosNode reprojection_node(nh, pnh);
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "cloud_reprojector.hpp"
|
||||
#include <cmath>
|
||||
|
||||
CloudReprojector::CloudReprojector()
|
||||
{
|
||||
}
|
||||
|
||||
bool CloudReprojector::initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params)
|
||||
{
|
||||
camera_params_ = cam_params;
|
||||
extrinsic_params_ = ext_params;
|
||||
|
||||
if (camera_params_.A11 < 1e-6 || camera_params_.A22 < 1e-6 ||
|
||||
camera_params_.u0 < 1e-6 || camera_params_.v0 < 1e-6)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
camera_model_ = std::make_unique<mini_vikit::PolynomialCamera>(
|
||||
camera_params_.image_width, camera_params_.image_height,
|
||||
camera_params_.A11, camera_params_.A22,
|
||||
camera_params_.u0, camera_params_.v0,
|
||||
camera_params_.A12,
|
||||
camera_params_.k2, camera_params_.k3, camera_params_.k4,
|
||||
camera_params_.k5, camera_params_.k6, camera_params_.k7
|
||||
);
|
||||
|
||||
initialized_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d CloudReprojector::calculateTic(const Eigen::Matrix4d& Tcl, const Eigen::Matrix4d& Til)
|
||||
{
|
||||
// Tic = Til * Tlc = Til * Tcl.inverse()
|
||||
Eigen::Matrix4d Tlc = Tcl.inverse();
|
||||
return Til * Tlc;
|
||||
}
|
||||
|
||||
Eigen::Matrix4d CloudReprojector::odomPoseToMatrix(const OdomPose& odom) const
|
||||
{
|
||||
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||
T.block<3, 3>(0, 0) = odom.orientation.toRotationMatrix();
|
||||
T(0, 3) = odom.position.x();
|
||||
T(1, 3) = odom.position.y();
|
||||
T(2, 3) = odom.position.z();
|
||||
return T;
|
||||
}
|
||||
|
||||
cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
|
||||
const OdomPose& odom_pose)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
// T_odom_imu: imu pose in odom frame
|
||||
Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose);
|
||||
|
||||
// T_imu_odom: transforms points from odom frame to imu frame
|
||||
Eigen::Matrix4d T_imu_odom = T_odom_imu.inverse();
|
||||
|
||||
// T_cam_imu = Tic.inverse(): transforms from imu to camera
|
||||
Eigen::Matrix4d T_cam_imu = extrinsic_params_.Tic.inverse();
|
||||
|
||||
// T_cam_odom: transforms points from odom frame to camera frame
|
||||
Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam;
|
||||
pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom);
|
||||
|
||||
return projectCloudToImage(cloud_in_cam);
|
||||
}
|
||||
|
||||
cv::Mat CloudReprojector::projectCloudToImage(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam) const
|
||||
{
|
||||
cv::Mat img = cv::Mat::zeros(camera_params_.image_height, camera_params_.image_width, CV_8UC3);
|
||||
img.setTo(cv::Scalar(255, 255, 255));
|
||||
|
||||
const double fx = camera_model_->fx();
|
||||
const double fy = camera_model_->fy();
|
||||
const double cx = camera_model_->cx();
|
||||
const double cy = camera_model_->cy();
|
||||
|
||||
for (const auto& pt : cloud_in_cam)
|
||||
{
|
||||
if (pt.z <= 0.01)
|
||||
continue;
|
||||
|
||||
int u_int, v_int;
|
||||
if (1)
|
||||
{
|
||||
// Pinhole projection (undistorted image)
|
||||
u_int = static_cast<int>(std::round(fx * pt.x / pt.z + cx));
|
||||
v_int = static_cast<int>(std::round(fy * pt.y / pt.z + cy));
|
||||
}
|
||||
else
|
||||
{
|
||||
// Distorted projection (original image)
|
||||
Eigen::Vector3d pt_cam(pt.x, pt.y, pt.z);
|
||||
Eigen::Vector2d uv = camera_model_->world2cam(pt_cam);
|
||||
u_int = static_cast<int>(std::round(uv[0]));
|
||||
v_int = static_cast<int>(std::round(uv[1]));
|
||||
}
|
||||
|
||||
if (u_int >= 0 && u_int < camera_params_.image_width &&
|
||||
v_int >= 0 && v_int < camera_params_.image_height)
|
||||
{
|
||||
cv::circle(img, cv::Point(u_int, v_int), point_radius_,
|
||||
cv::Scalar(pt.b, pt.g, pt.r), -1);
|
||||
}
|
||||
}
|
||||
|
||||
return img;
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "depth_image_ros2_node.hpp"
|
||||
#include <functional>
|
||||
|
||||
DepthImageRos2Node::DepthImageRos2Node(const rclcpp::NodeOptions & options)
|
||||
: Node("depth_image_ros2_node", options)
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams camera_params = loadCameraParams();
|
||||
|
||||
depth_converter_ = std::make_unique<PointCloudToDepthConverter>(camera_params);
|
||||
|
||||
cloud_raw_topic_ = this->declare_parameter<std::string>("cloud_raw_topic", "/odin1/cloud_raw");
|
||||
color_compressed_topic_ = this->declare_parameter<std::string>("color_compressed_topic", "/odin1/image/compressed");
|
||||
color_raw_topic_ = this->declare_parameter<std::string>("color_raw_topic", "/odin1/image");
|
||||
depth_image_topic_ = this->declare_parameter<std::string>("depth_image_topic", "/odin1/depth_img_competetion");
|
||||
depth_cloud_topic_ = this->declare_parameter<std::string>("depth_cloud_topic", "/odin1/depth_img_competetion_cloud");
|
||||
|
||||
RCLCPP_INFO_STREAM(this->get_logger(),
|
||||
"\n cloud_raw_topic: " << cloud_raw_topic_
|
||||
<< "\n color_compressed_topic: " << color_compressed_topic_
|
||||
<< "\n color_raw_topic: " << color_raw_topic_
|
||||
<< "\n depth_image_topic: " << depth_image_topic_
|
||||
<< "\n depth_cloud_topic: " << depth_cloud_topic_);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::initialize()
|
||||
{
|
||||
cloud_sub_.subscribe(this, cloud_raw_topic_);
|
||||
color_compressed_sub_.subscribe(this, color_compressed_topic_);
|
||||
color_sub_.subscribe(this, color_raw_topic_);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
|
||||
sync_->registerCallback(std::bind(&DepthImageRos2Node::syncCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
it_ = std::make_shared<image_transport::ImageTransport>(shared_from_this());
|
||||
depth_image_pub_ = it_->advertise(depth_image_topic_, 1);
|
||||
depth_cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(depth_cloud_topic_, 1);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "DepthImageRos2Node initialized successfully");
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::CameraParams DepthImageRos2Node::loadCameraParams()
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams params;
|
||||
|
||||
params.image_width = this->declare_parameter<int>("cam_0.image_width", 1600);
|
||||
params.image_height = this->declare_parameter<int>("cam_0.image_height", 1296);
|
||||
params.A11 = this->declare_parameter<double>("cam_0.A11", 0.0);
|
||||
params.A12 = this->declare_parameter<double>("cam_0.A12", 0.0);
|
||||
params.A22 = this->declare_parameter<double>("cam_0.A22", 0.0);
|
||||
params.u0 = this->declare_parameter<double>("cam_0.u0", 0.0);
|
||||
params.v0 = this->declare_parameter<double>("cam_0.v0", 0.0);
|
||||
|
||||
params.k2 = this->declare_parameter<double>("cam_0.k2", 0.0);
|
||||
params.k3 = this->declare_parameter<double>("cam_0.k3", 0.0);
|
||||
params.k4 = this->declare_parameter<double>("cam_0.k4", 0.0);
|
||||
params.k5 = this->declare_parameter<double>("cam_0.k5", 0.0);
|
||||
params.k6 = this->declare_parameter<double>("cam_0.k6", 0.0);
|
||||
params.k7 = this->declare_parameter<double>("cam_0.k7", 0.0);
|
||||
|
||||
params.scale = this->declare_parameter<double>("scale", 7.0);
|
||||
params.point_sampling_rate = this->declare_parameter<int>("point_sampling_rate", 5);
|
||||
|
||||
std::vector<double> Tcl_vec_param = this->declare_parameter<std::vector<double>>("Tcl_0", std::vector<double>(16, 0.0));
|
||||
if (Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Tcl_0 param missing or invalid, colored reproject cloud disabled.");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
if (params.A11 < 1e-6 || params.A22 < 1e-6 || params.u0 < 1e-6 || params.v0 < 1e-6)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid camera intrinsics A11 or A22");
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
||||
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", params.image_width, params.image_height);
|
||||
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
params.A11, params.A12, params.A22, params.u0, params.v0);
|
||||
RCLCPP_INFO(this->get_logger(), "Distortions: k2=%f k3=%f k4=%f k5=%f k6=%f k7=%f",
|
||||
params.k2, params.k3, params.k4, params.k5, params.k6, params.k7);
|
||||
RCLCPP_INFO(this->get_logger(), "Scale: %f, Point sampling rate: %d", params.scale, params.point_sampling_rate);
|
||||
RCLCPP_INFO_STREAM(this->get_logger(), "Extrinsics (Tcl):\n" << params.Tcl);
|
||||
|
||||
return params;
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::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)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud);
|
||||
if (cloud.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Empty point cloud received");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat img_raw;
|
||||
try
|
||||
{
|
||||
// img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
|
||||
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(color_msg, "bgr8");
|
||||
img_raw = cv_ptr->image;
|
||||
if (img_raw.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to decode compressed image");
|
||||
return;
|
||||
}
|
||||
}
|
||||
catch (const cv_bridge::Exception &e)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = depth_converter_->processCloudAndImage(cloud, img_raw);
|
||||
|
||||
if (!result.success)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Data processing failed: %s", result.error_message.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
publishDepthImage(result.depth_image, cloud_msg->header);
|
||||
publishDepthCloud(result.colored_cloud, cloud_msg->header);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::msg::Header &header,
|
||||
const std::string &encoding)
|
||||
{
|
||||
sensor_msgs::msg::Image::SharedPtr depth_msg = cv_bridge::CvImage(header, encoding, img).toImageMsg();
|
||||
depth_image_pub_.publish(*depth_msg);
|
||||
}
|
||||
|
||||
void DepthImageRos2Node::publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::msg::Header &header)
|
||||
{
|
||||
if (!colored_cloud.points.empty())
|
||||
{
|
||||
sensor_msgs::msg::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(colored_cloud, cloud_msg);
|
||||
cloud_msg.header = header;
|
||||
depth_cloud_pub_->publish(cloud_msg);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "depth_image_ros_node.hpp"
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
DepthImageRosNode::DepthImageRosNode(ros::NodeHandle &nh, ros::NodeHandle &pnh)
|
||||
: nh_(nh), pnh_(pnh), it_(nh_)
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams camera_params = loadCameraParams();
|
||||
|
||||
depth_converter_ = std::make_unique<PointCloudToDepthConverter>(camera_params);
|
||||
pnh_.param<std::string>("cloud_raw_topic", cloud_raw_topic_, std::string("/odin1/cloud_raw"));
|
||||
pnh_.param<std::string>("color_raw_topic", color_raw_topic_, std::string("/odin1/image"));
|
||||
pnh_.param<std::string>("color_compressed_topic_", color_compressed_topic_, std::string("/odin1/image/compressed"));
|
||||
pnh_.param<std::string>("depth_image_topic", depth_image_topic_, std::string("/odin1/depth_img_competetion"));
|
||||
pnh_.param<std::string>("depth_cloud_topic", depth_cloud_topic_, std::string("/odin1/depth_img_competetion_cloud"));
|
||||
|
||||
ROS_INFO_STREAM("\n cloud_raw_topic: " << cloud_raw_topic_
|
||||
<< "\n color_raw_topic: " << color_raw_topic_
|
||||
<< "\n color_compressed_topic: " << color_compressed_topic_
|
||||
<< "\n depth_image_topic: " << depth_image_topic_
|
||||
<< "\n depth_cloud_topic: " << depth_cloud_topic_);
|
||||
|
||||
cloud_sub_.subscribe(nh_, cloud_raw_topic_, 1);
|
||||
color_sub_.subscribe(nh_, color_raw_topic_, 1);
|
||||
color_compressed_sub_.subscribe(nh_, color_compressed_topic_, 1);
|
||||
|
||||
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, color_sub_);
|
||||
sync_->registerCallback(boost::bind(&DepthImageRosNode::syncCallback, this, _1, _2));
|
||||
|
||||
depth_image_pub_ = it_.advertise(depth_image_topic_, 1);
|
||||
depth_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(depth_cloud_topic_, 1);
|
||||
|
||||
ROS_INFO("DepthImageRosNode initialized successfully");
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::CameraParams DepthImageRosNode::loadCameraParams()
|
||||
{
|
||||
PointCloudToDepthConverter::CameraParams params;
|
||||
|
||||
pnh_.param<int>("cam_0/image_width", params.image_width, 1600);
|
||||
pnh_.param<int>("cam_0/image_height", params.image_height, 1296);
|
||||
pnh_.param<double>("cam_0/A11", params.A11, 0.0);
|
||||
pnh_.param<double>("cam_0/A12", params.A12, 0.0);
|
||||
pnh_.param<double>("cam_0/A22", params.A22, 0.0);
|
||||
pnh_.param<double>("cam_0/u0", params.u0, 0.0);
|
||||
pnh_.param<double>("cam_0/v0", params.v0, 0.0);
|
||||
|
||||
pnh_.param<double>("cam_0/k2", params.k2, 0.0);
|
||||
pnh_.param<double>("cam_0/k3", params.k3, 0.0);
|
||||
pnh_.param<double>("cam_0/k4", params.k4, 0.0);
|
||||
pnh_.param<double>("cam_0/k5", params.k5, 0.0);
|
||||
pnh_.param<double>("cam_0/k6", params.k6, 0.0);
|
||||
pnh_.param<double>("cam_0/k7", params.k7, 0.0);
|
||||
|
||||
pnh_.param<double>("scale", params.scale, 7.0);
|
||||
pnh_.param<int>("point_sampling_rate", params.point_sampling_rate, 5);
|
||||
|
||||
std::vector<double> Tcl_vec_param;
|
||||
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
for (int j = 0; j < 4; ++j)
|
||||
{
|
||||
params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Tcl_0 param missing or invalid, colored reproject cloud disabled.");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
if (params.A11 < 1e-6 || params.A22 < 1e-6 || params.u0 < 1e-6 || params.v0 < 1e-6)
|
||||
{
|
||||
ROS_ERROR("Invalid camera intrinsics A11 or A22");
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
ROS_INFO("Camera intrinsics:");
|
||||
ROS_INFO("Image size: %dx%d", params.image_width, params.image_height);
|
||||
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
||||
params.A11, params.A12, params.A22, params.u0, params.v0);
|
||||
ROS_INFO("Distortions: k2=%f k3=%f k4=%f k5=%f k6=%f k7=%f",
|
||||
params.k2, params.k3, params.k4, params.k5, params.k6, params.k7);
|
||||
ROS_INFO("Scale: %f, Point sampling rate: %d", params.scale, params.point_sampling_rate);
|
||||
ROS_INFO_STREAM("Extrinsics (Tcl):\n"
|
||||
<< params.Tcl);
|
||||
|
||||
return params;
|
||||
}
|
||||
|
||||
void DepthImageRosNode::syncCallback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
|
||||
const sensor_msgs::ImageConstPtr &image_msg)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud;
|
||||
pcl::fromROSMsg(*cloud_msg, cloud);
|
||||
if (cloud.empty())
|
||||
{
|
||||
ROS_WARN("Empty point cloud received");
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat img_raw;
|
||||
try
|
||||
{
|
||||
// img_raw = cv::imdecode(cv::Mat(image_msg->data), cv::IMREAD_COLOR);
|
||||
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
||||
img_raw = cv_ptr->image;
|
||||
if (img_raw.empty())
|
||||
{
|
||||
ROS_WARN("Failed to decode compressed image");
|
||||
return;
|
||||
}
|
||||
}
|
||||
catch (const cv_bridge::Exception &e)
|
||||
{
|
||||
ROS_ERROR("cv_bridge: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
auto result = depth_converter_->processCloudAndImage(cloud, img_raw);
|
||||
|
||||
if (!result.success)
|
||||
{
|
||||
ROS_WARN("Data processing failed: %s", result.error_message.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
publishDepthImage(result.depth_image, cloud_msg->header);
|
||||
publishDepthCloud(result.colored_cloud, cloud_msg->header);
|
||||
}
|
||||
|
||||
void DepthImageRosNode::publishDepthImage(const cv::Mat &img,
|
||||
const std_msgs::Header &header,
|
||||
const std::string &encoding)
|
||||
{
|
||||
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(header, encoding, img).toImageMsg();
|
||||
depth_image_pub_.publish(depth_msg);
|
||||
}
|
||||
|
||||
void DepthImageRosNode::publishDepthCloud(const pcl::PointCloud<pcl::PointXYZRGB> &colored_cloud,
|
||||
const std_msgs::Header &header)
|
||||
{
|
||||
if (!colored_cloud.points.empty())
|
||||
{
|
||||
sensor_msgs::PointCloud2 cloud_msg;
|
||||
pcl::toROSMsg(colored_cloud, cloud_msg);
|
||||
cloud_msg.header = header;
|
||||
depth_cloud_pub_.publish(cloud_msg);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,254 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "image_overlay_node.hpp"
|
||||
|
||||
#ifdef ROS2
|
||||
// ==================== ROS2 Implementation ====================
|
||||
|
||||
ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options)
|
||||
: Node("image_overlay_node", options)
|
||||
{
|
||||
// Read from register_keys (same structure as control_command.yaml)
|
||||
this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
|
||||
this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
|
||||
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/overlay_image");
|
||||
this->declare_parameter<double>("register_keys.overlay_alpha", 0.6);
|
||||
|
||||
reprojected_topic_ = this->get_parameter("register_keys.overlay_reprojected_topic").as_string();
|
||||
camera_topic_ = this->get_parameter("register_keys.overlay_camera_topic").as_string();
|
||||
overlay_topic_ = this->get_parameter("register_keys.overlay_output_topic").as_string();
|
||||
alpha_ = this->get_parameter("register_keys.overlay_alpha").as_double();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Subscribing to: %s and %s",
|
||||
reprojected_topic_.c_str(), camera_topic_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
|
||||
|
||||
// Independent subscriptions - no synchronization needed
|
||||
reproj_sub_ = this->create_subscription<Image>(
|
||||
reprojected_topic_, 10,
|
||||
std::bind(&ImageOverlayNode::reprojCallback, this, std::placeholders::_1));
|
||||
|
||||
camera_sub_ = this->create_subscription<Image>(
|
||||
camera_topic_, 10,
|
||||
std::bind(&ImageOverlayNode::cameraCallback, this, std::placeholders::_1));
|
||||
|
||||
overlay_pub_ = this->create_publisher<sensor_msgs::msg::Image>(overlay_topic_, 10);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "ImageOverlayNode initialized (no-sync mode)");
|
||||
}
|
||||
|
||||
void ImageOverlayNode::reprojCallback(Image::ConstSharedPtr msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_reproj_img_ = cv_ptr->image.clone();
|
||||
latest_header_ = msg->header;
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (reproj): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::cameraCallback(Image::ConstSharedPtr msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_camera_img_ = cv_ptr->image.clone();
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception (camera): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::publishOverlay()
|
||||
{
|
||||
cv::Mat reproj_copy, camera_copy;
|
||||
std_msgs::msg::Header header_copy;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
|
||||
return;
|
||||
}
|
||||
reproj_copy = latest_reproj_img_.clone();
|
||||
camera_copy = latest_camera_img_.clone();
|
||||
header_copy = latest_header_;
|
||||
}
|
||||
|
||||
if (reproj_copy.size() != camera_copy.size()) {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
|
||||
reproj_copy.cols, reproj_copy.rows,
|
||||
camera_copy.cols, camera_copy.rows);
|
||||
return;
|
||||
}
|
||||
|
||||
// Create overlay using alpha blending
|
||||
// Replace white background in reproj with camera image, keep colored points
|
||||
cv::Mat overlay = camera_copy.clone();
|
||||
|
||||
// Blend: where reproj has color (non-white), show reproj color semi-transparently
|
||||
// where reproj is white (background), show camera image
|
||||
|
||||
for (int y = 0; y < reproj_copy.rows; ++y) {
|
||||
for (int x = 0; x < reproj_copy.cols; ++x) {
|
||||
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
|
||||
// Check if pixel is not white (has point cloud color)
|
||||
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
|
||||
// Blend reproj color with camera color
|
||||
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
|
||||
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
|
||||
);
|
||||
}
|
||||
// else: keep camera image (already in overlay)
|
||||
}
|
||||
}
|
||||
|
||||
auto overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
|
||||
overlay_pub_->publish(*overlay_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS2 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<ImageOverlayNode>();
|
||||
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
// ==================== ROS1 Implementation ====================
|
||||
|
||||
ImageOverlayNode::ImageOverlayNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
||||
: nh_(nh), pnh_(pnh)
|
||||
{
|
||||
// Read from register_keys (same structure as control_command.yaml)
|
||||
pnh_.param<std::string>("register_keys/overlay_reprojected_topic", reprojected_topic_, "/odin1/reprojected_image");
|
||||
pnh_.param<std::string>("register_keys/overlay_camera_topic", camera_topic_, "/odin1/image/undistorted");
|
||||
pnh_.param<std::string>("register_keys/overlay_output_topic", overlay_topic_, "/odin1/overlay_image");
|
||||
pnh_.param<double>("register_keys/overlay_alpha", alpha_, 0.6);
|
||||
|
||||
ROS_INFO("Subscribing to: %s and %s", reprojected_topic_.c_str(), camera_topic_.c_str());
|
||||
ROS_INFO("Publishing to: %s (alpha=%.2f)", overlay_topic_.c_str(), alpha_);
|
||||
|
||||
// Independent subscriptions - no synchronization needed
|
||||
reproj_sub_ = nh_.subscribe(reprojected_topic_, 10, &ImageOverlayNode::reprojCallback, this);
|
||||
camera_sub_ = nh_.subscribe(camera_topic_, 10, &ImageOverlayNode::cameraCallback, this);
|
||||
|
||||
overlay_pub_ = nh_.advertise<sensor_msgs::Image>(overlay_topic_, 10);
|
||||
|
||||
ROS_INFO("ImageOverlayNode initialized (no-sync mode)");
|
||||
}
|
||||
|
||||
void ImageOverlayNode::reprojCallback(const sensor_msgs::ImageConstPtr& msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_reproj_img_ = cv_ptr->image.clone();
|
||||
latest_header_ = msg->header;
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
ROS_ERROR("cv_bridge exception (reproj): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::cameraCallback(const sensor_msgs::ImageConstPtr& msg)
|
||||
{
|
||||
try {
|
||||
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_camera_img_ = cv_ptr->image.clone();
|
||||
}
|
||||
} catch (cv_bridge::Exception& e) {
|
||||
ROS_ERROR("cv_bridge exception (camera): %s", e.what());
|
||||
return;
|
||||
}
|
||||
publishOverlay();
|
||||
}
|
||||
|
||||
void ImageOverlayNode::publishOverlay()
|
||||
{
|
||||
cv::Mat reproj_copy, camera_copy;
|
||||
std_msgs::Header header_copy;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (latest_reproj_img_.empty() || latest_camera_img_.empty()) {
|
||||
return;
|
||||
}
|
||||
reproj_copy = latest_reproj_img_.clone();
|
||||
camera_copy = latest_camera_img_.clone();
|
||||
header_copy = latest_header_;
|
||||
}
|
||||
|
||||
if (reproj_copy.size() != camera_copy.size()) {
|
||||
ROS_WARN_THROTTLE(2, "Image sizes don't match: reproj(%dx%d) vs camera(%dx%d)",
|
||||
reproj_copy.cols, reproj_copy.rows, camera_copy.cols, camera_copy.rows);
|
||||
return;
|
||||
}
|
||||
|
||||
// Create overlay using alpha blending
|
||||
cv::Mat overlay = camera_copy.clone();
|
||||
|
||||
for (int y = 0; y < reproj_copy.rows; ++y) {
|
||||
for (int x = 0; x < reproj_copy.cols; ++x) {
|
||||
cv::Vec3b reproj_pixel = reproj_copy.at<cv::Vec3b>(y, x);
|
||||
if (reproj_pixel[0] < 250 || reproj_pixel[1] < 250 || reproj_pixel[2] < 250) {
|
||||
cv::Vec3b cam_pixel = camera_copy.at<cv::Vec3b>(y, x);
|
||||
overlay.at<cv::Vec3b>(y, x) = cv::Vec3b(
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[0] + (1 - alpha_) * cam_pixel[0]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[1] + (1 - alpha_) * cam_pixel[1]),
|
||||
static_cast<uchar>(alpha_ * reproj_pixel[2] + (1 - alpha_) * cam_pixel[2])
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::ImagePtr overlay_msg = cv_bridge::CvImage(header_copy, "bgr8", overlay).toImageMsg();
|
||||
overlay_pub_.publish(overlay_msg);
|
||||
}
|
||||
|
||||
// ==================== ROS1 Main ====================
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "image_overlay_node");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
ImageOverlayNode node(nh, pnh);
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include "depth_image_ros_node.hpp"
|
||||
|
||||
bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "depth_projector");
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
int senddepth=0;
|
||||
pnh.param("register_keys/senddepth", senddepth, 0);
|
||||
if(senddepth==0)
|
||||
{
|
||||
ROS_INFO("Depth image will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path;
|
||||
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
||||
|
||||
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(ros::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
||||
ros::Duration(0.5).sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
if(!ros::ok())
|
||||
{
|
||||
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
||||
|
||||
std::string node_name = ros::this_node::getName();
|
||||
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
||||
int result = system(rosparam_command.c_str());
|
||||
|
||||
if(result == 0)
|
||||
{
|
||||
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Failed to load parameters from calib.yaml");
|
||||
return 1;
|
||||
}
|
||||
|
||||
DepthImageRosNode projector(nh, pnh);
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <fstream>
|
||||
#include <sys/stat.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include "depth_image_ros2_node.hpp"
|
||||
#include <rcpputils/filesystem_helper.hpp>
|
||||
bool fileExists(const std::string& filename) {
|
||||
struct stat buffer;
|
||||
return (stat(filename.c_str(), &buffer) == 0);
|
||||
}
|
||||
|
||||
bool loadCalibParameters(std::shared_ptr<rclcpp::Node> node, const std::string& calib_file_path) {
|
||||
try {
|
||||
RCLCPP_INFO(node->get_logger(), "Loading parameters from calib.yaml file: %s", calib_file_path.c_str());
|
||||
|
||||
YAML::Node config = YAML::LoadFile(calib_file_path);
|
||||
|
||||
if (config["cam_num"]) {
|
||||
int cam_num = config["cam_num"].as<int>();
|
||||
node->declare_parameter("cam_num", cam_num);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_num = %d", cam_num);
|
||||
}
|
||||
|
||||
if (config["img_topic_0"]) {
|
||||
std::string img_topic = config["img_topic_0"].as<std::string>();
|
||||
node->declare_parameter("img_topic_0", img_topic);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: img_topic_0 = %s", img_topic.c_str());
|
||||
}
|
||||
|
||||
if (config["Tcl_0"]) {
|
||||
std::vector<double> tcl_matrix = config["Tcl_0"].as<std::vector<double>>();
|
||||
node->declare_parameter("Tcl_0", tcl_matrix);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: Tcl_0 (transform matrix with %zu elements)", tcl_matrix.size());
|
||||
}
|
||||
|
||||
if (config["cam_0"]) {
|
||||
YAML::Node cam_0 = config["cam_0"];
|
||||
|
||||
if (cam_0["cam_model"]) {
|
||||
std::string cam_model = cam_0["cam_model"].as<std::string>();
|
||||
node->declare_parameter("cam_0.cam_model", cam_model);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.cam_model = %s", cam_model.c_str());
|
||||
}
|
||||
|
||||
if (cam_0["image_width"]) {
|
||||
int image_width = cam_0["image_width"].as<int>();
|
||||
node->declare_parameter("cam_0.image_width", image_width);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.image_width = %d", image_width);
|
||||
}
|
||||
|
||||
if (cam_0["image_height"]) {
|
||||
int image_height = cam_0["image_height"].as<int>();
|
||||
node->declare_parameter("cam_0.image_height", image_height);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.image_height = %d", image_height);
|
||||
}
|
||||
|
||||
std::vector<std::string> distortion_params = {"k2", "k3", "k4", "k5", "k6", "k7", "p1", "p2"};
|
||||
for (const auto& param : distortion_params) {
|
||||
if (cam_0[param]) {
|
||||
double value = cam_0[param].as<double>();
|
||||
node->declare_parameter("cam_0." + param, value);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.%s = %f", param.c_str(), value);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> intrinsic_params = {"A11", "A12", "A22", "u0", "v0"};
|
||||
for (const auto& param : intrinsic_params) {
|
||||
if (cam_0[param]) {
|
||||
double value = cam_0[param].as<double>();
|
||||
node->declare_parameter("cam_0." + param, value);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.%s = %f", param.c_str(), value);
|
||||
}
|
||||
}
|
||||
|
||||
if (cam_0["isFast"]) {
|
||||
int is_fast = cam_0["isFast"].as<int>();
|
||||
node->declare_parameter("cam_0.isFast", is_fast);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.isFast = %d", is_fast);
|
||||
}
|
||||
|
||||
if (cam_0["numDiff"]) {
|
||||
int num_diff = cam_0["numDiff"].as<int>();
|
||||
node->declare_parameter("cam_0.numDiff", num_diff);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.numDiff = %d", num_diff);
|
||||
}
|
||||
|
||||
if (cam_0["maxIncidentAngle"]) {
|
||||
int max_incident_angle = cam_0["maxIncidentAngle"].as<int>();
|
||||
node->declare_parameter("cam_0.maxIncidentAngle", max_incident_angle);
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded parameter: cam_0.maxIncidentAngle = %d", max_incident_angle);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Successfully loaded all parameters from calib.yaml");
|
||||
return true;
|
||||
|
||||
} catch (const YAML::Exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "YAML parsing error: %s", e.what());
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Error loading calib parameters: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
std::string get_package_source_directory() {
|
||||
// 使用 rcpputils::fs::path 替代 std::filesystem::path
|
||||
rcpputils::fs::path current_file(__FILE__);
|
||||
|
||||
// 回溯到包根目录
|
||||
auto path = current_file.parent_path();
|
||||
|
||||
// 使用 rcpputils::fs::exists 替代 std::filesystem::exists
|
||||
while (!path.empty() && !rcpputils::fs::exists(path / "package.xml")) {
|
||||
path = path.parent_path();
|
||||
}
|
||||
|
||||
if (path.empty()) {
|
||||
throw std::runtime_error("Failed to locate package root directory");
|
||||
}
|
||||
|
||||
return path.string();
|
||||
}
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
|
||||
auto node = std::make_shared<rclcpp::Node>("pcd2depth_node");
|
||||
RCLCPP_INFO(node->get_logger(), "Node created");
|
||||
std::string package_path = get_package_source_directory();
|
||||
RCLCPP_INFO(node->get_logger(), "Package path: %s", package_path.c_str());
|
||||
|
||||
std::string config_file = package_path + "/config/control_command.yaml";
|
||||
RCLCPP_INFO(node->get_logger(), "Loading config from: %s", config_file.c_str());
|
||||
|
||||
YAML::Node config = YAML::LoadFile(config_file);
|
||||
|
||||
if (!config["register_keys"]) {
|
||||
throw std::runtime_error("Missing 'register_keys' section");
|
||||
}
|
||||
|
||||
if (!config["register_keys"]["senddepth"]) {
|
||||
throw std::runtime_error("Missing 'senddepth' parameter");
|
||||
}
|
||||
int senddepth = config["register_keys"]["senddepth"].as<int>();
|
||||
std::cout << "senddepth: " << senddepth << std::endl;
|
||||
if(senddepth == 0)
|
||||
{
|
||||
RCLCPP_INFO(node->get_logger(), "Depth image will not be published.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string calib_file_path = node->declare_parameter<std::string>("calib_file_path", "");
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
||||
while(rclcpp::ok() && !fileExists(calib_file_path))
|
||||
{
|
||||
RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Still waiting for calib.yaml file...");
|
||||
rclcpp::sleep_for(std::chrono::milliseconds(500)); // 等待0.5秒后再检查
|
||||
rclcpp::spin_some(node);
|
||||
}
|
||||
|
||||
if(!rclcpp::ok())
|
||||
{
|
||||
RCLCPP_INFO(node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
||||
return 0;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Found calib.yaml file! Loading parameters...");
|
||||
|
||||
if (!loadCalibParameters(node, calib_file_path)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to load parameters from calib.yaml file");
|
||||
return 1;
|
||||
}
|
||||
|
||||
rclcpp::NodeOptions depth_node_options;
|
||||
|
||||
std::vector<rclcpp::Parameter> params_override;
|
||||
|
||||
try {
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.image_width", node->get_parameter("cam_0.image_width").as_int()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.image_height", node->get_parameter("cam_0.image_height").as_int()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A11", node->get_parameter("cam_0.A11").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A12", node->get_parameter("cam_0.A12").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.A22", node->get_parameter("cam_0.A22").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.u0", node->get_parameter("cam_0.u0").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.v0", node->get_parameter("cam_0.v0").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k2", node->get_parameter("cam_0.k2").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k3", node->get_parameter("cam_0.k3").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k4", node->get_parameter("cam_0.k4").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k5", node->get_parameter("cam_0.k5").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k6", node->get_parameter("cam_0.k6").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("cam_0.k7", node->get_parameter("cam_0.k7").as_double()));
|
||||
params_override.push_back(rclcpp::Parameter("Tcl_0", node->get_parameter("Tcl_0").as_double_array()));
|
||||
|
||||
depth_node_options.parameter_overrides(params_override);
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Parameters successfully prepared for DepthImageRos2Node");
|
||||
}
|
||||
catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Error preparing parameters for DepthImageRos2Node: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
try
|
||||
{
|
||||
auto depth_node = std::make_shared<DepthImageRos2Node>(depth_node_options);
|
||||
|
||||
depth_node->initialize();
|
||||
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(depth_node);
|
||||
executor.spin();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(node->get_logger(), "Error creating depth node: %s", e.what());
|
||||
return 1;
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
+376
@@ -0,0 +1,376 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "pointcloud_depth_converter.hpp"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
|
||||
PointCloudToDepthConverter::PointCloudToDepthConverter(const CameraParams ¶ms)
|
||||
: params_(params)
|
||||
{
|
||||
initializeInternalParams();
|
||||
createDistortionMaps();
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::initializeInternalParams()
|
||||
{
|
||||
scaled_width_ = static_cast<int>(params_.image_width / params_.scale);
|
||||
scaled_height_ = static_cast<int>(params_.image_height / params_.scale);
|
||||
|
||||
K_ = Eigen::Matrix3d::Identity();
|
||||
K_(0, 0) = params_.A11;
|
||||
K_(0, 1) = params_.A12;
|
||||
K_(0, 2) = params_.u0;
|
||||
K_(1, 1) = params_.A22;
|
||||
K_(1, 2) = params_.v0;
|
||||
|
||||
Kl_ = Eigen::Matrix3d::Identity();
|
||||
Kl_(0, 0) = params_.A11 / params_.scale;
|
||||
Kl_(0, 1) = 0.0;
|
||||
Kl_(0, 2) = params_.u0 / params_.scale;
|
||||
Kl_(1, 1) = params_.A22 / params_.scale;
|
||||
Kl_(1, 2) = params_.v0 / params_.scale;
|
||||
|
||||
K_4x4_ = Eigen::Matrix4d::Identity();
|
||||
K_4x4_.block<3, 3>(0, 0) = Kl_;
|
||||
|
||||
Kcl_ = K_4x4_ * params_.Tcl;
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::createDistortionMaps()
|
||||
{
|
||||
map_x_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
map_y_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
|
||||
|
||||
for (int u = 0; u < params_.image_width; ++u)
|
||||
{
|
||||
for (int v = 0; v < params_.image_height; ++v)
|
||||
{
|
||||
double y = (v - params_.v0) / params_.A22;
|
||||
double x = (u - params_.u0 - params_.A12 * y) / params_.A11;
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double theta = atan(r);
|
||||
|
||||
double theta_d = theta + params_.k2 * pow(theta, 2) + params_.k3 * pow(theta, 3) +
|
||||
params_.k4 * pow(theta, 4) + params_.k5 * pow(theta, 5) +
|
||||
params_.k6 * pow(theta, 6) + params_.k7 * pow(theta, 7);
|
||||
|
||||
double x_distorted = x * (r / theta_d);
|
||||
double y_distorted = y * (r / theta_d);
|
||||
|
||||
map_x_.at<float>(v, u) = static_cast<float>(x_distorted * params_.A11 + params_.A12 * y_distorted + params_.u0);
|
||||
map_y_.at<float>(v, u) = static_cast<float>(y_distorted * params_.A22 + params_.v0);
|
||||
}
|
||||
}
|
||||
|
||||
inv_map_x_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
inv_map_y_ = cv::Mat::zeros(params_.image_height, params_.image_width, CV_32FC1);
|
||||
for (int u = 0; u < params_.image_width; ++u)
|
||||
{
|
||||
for (int v = 0; v < params_.image_height; ++v)
|
||||
{
|
||||
double y = (v - params_.v0) / params_.A22;
|
||||
double x = (u - params_.u0 - params_.A12 * y) / params_.A11;
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double theta = atan(r);
|
||||
|
||||
double theta_d = theta + params_.k2 * pow(theta, 2) + params_.k3 * pow(theta, 3) +
|
||||
params_.k4 * pow(theta, 4) + params_.k5 * pow(theta, 5) +
|
||||
params_.k6 * pow(theta, 6) + params_.k7 * pow(theta, 7);
|
||||
|
||||
double x_distorted = x * (theta_d / r);
|
||||
double y_distorted = y * (theta_d / r);
|
||||
|
||||
inv_map_x_.at<float>(v, u) = static_cast<float>(x_distorted * params_.A11 + params_.A12 * y_distorted + params_.u0);
|
||||
inv_map_y_.at<float>(v, u) = static_cast<float>(y_distorted * params_.A22 + params_.v0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PointCloudToDepthConverter::ProcessResult PointCloudToDepthConverter::processCloudAndImage(
|
||||
const pcl::PointCloud<pcl::PointXYZ> &cloud,
|
||||
const cv::Mat &image)
|
||||
{
|
||||
ProcessResult result;
|
||||
result.success = false;
|
||||
|
||||
auto validation_result = validateInputs(cloud, image);
|
||||
if (!validation_result.first)
|
||||
{
|
||||
result.error_message = validation_result.second;
|
||||
return result;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ> cloud_in_cam;
|
||||
pcl::transformPointCloud(cloud, cloud_in_cam, Kcl_);
|
||||
|
||||
cv::Mat depth_img = projectCloudToDepth(cloud_in_cam);
|
||||
|
||||
cv::Mat processed_depth = postProcessDepthImage(depth_img);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> colored_cloud = generateColoredCloud(processed_depth, image);
|
||||
|
||||
result.depth_image = processed_depth;
|
||||
result.colored_cloud = colored_cloud;
|
||||
result.success = true;
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
result.error_message = std::string("Processing error: ") + e.what();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::projectCloudToDepth(const pcl::PointCloud<pcl::PointXYZ> &cloud_in_cam)
|
||||
{
|
||||
cv::Mat depth_img = cv::Mat::zeros(scaled_height_, scaled_width_, CV_32FC1);
|
||||
|
||||
for (const auto &camera_point : cloud_in_cam)
|
||||
{
|
||||
if (camera_point.z <= 0)
|
||||
continue;
|
||||
|
||||
int u = static_cast<int>(std::round(camera_point.x / camera_point.z));
|
||||
int v = static_cast<int>(std::round(camera_point.y / camera_point.z));
|
||||
|
||||
if (u >= 0 && u < scaled_width_ && v >= 0 && v < scaled_height_)
|
||||
{
|
||||
depth_img.at<float>(v, u) = static_cast<float>(camera_point.z);
|
||||
|
||||
for (int du = -1; du <= 1; ++du)
|
||||
{
|
||||
for (int dv = -1; dv <= 1; ++dv)
|
||||
{
|
||||
int nu = u + du;
|
||||
int nv = v + dv;
|
||||
if (nu >= 0 && nu < scaled_width_ && nv >= 0 && nv < scaled_height_)
|
||||
{
|
||||
if (depth_img.at<float>(nv, nu) == 0.0f)
|
||||
{
|
||||
depth_img.at<float>(nv, nu) = static_cast<float>(camera_point.z);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return depth_img;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::postProcessDepthImage(const cv::Mat &depth_img) {
|
||||
if (depth_img.empty()) {
|
||||
std::cerr << "ERROR: Input depth image is empty!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img.data == nullptr) {
|
||||
std::cerr << "ERROR: Input depth image has null data pointer!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img.rows <= 0 || depth_img.cols <= 0) {
|
||||
std::cerr << "ERROR: Invalid input dimensions: "
|
||||
<< depth_img.rows << "x" << depth_img.cols << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (params_.image_width <= 0 || params_.image_height <= 0) {
|
||||
std::cerr << "ERROR: Invalid target size: "
|
||||
<< params_.image_width << "x" << params_.image_height << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
cv::Mat safe_input = depth_img.clone();
|
||||
if (safe_input.empty()) {
|
||||
std::cerr << "ERROR: Failed to create safe copy of input image!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat depth_img_upsampled;
|
||||
try {
|
||||
depth_img_upsampled = customResize(safe_input, cv::Size(1600, 1296));
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "ERROR: Custom resize failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img_upsampled.empty()) {
|
||||
std::cerr << "ERROR: Resized image is empty!" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (depth_img_upsampled.rows != 1296 || depth_img_upsampled.cols != 1600) {
|
||||
std::cerr << "ERROR: Resized image has wrong dimensions: "
|
||||
<< depth_img_upsampled.cols << "x" << depth_img_upsampled.rows
|
||||
<< " (expected " << 1600 << "x" << 1296 << ")" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat grad_x, grad_y, grad_magnitude;
|
||||
try {
|
||||
cv::Sobel(depth_img_upsampled, grad_x, CV_32F, 1, 0, 3);
|
||||
cv::Sobel(depth_img_upsampled, grad_y, CV_32F, 0, 1, 3);
|
||||
cv::magnitude(grad_x, grad_y, grad_magnitude);
|
||||
} catch (const cv::Exception& e) {
|
||||
std::cerr << "ERROR: Sobel/magnitude failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
if (grad_magnitude.type() != CV_32F) {
|
||||
std::cerr << "ERROR: grad_magnitude has wrong type: "
|
||||
<< grad_magnitude.type() << " (expected CV_32F)" << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
|
||||
cv::Mat threshold_mask;
|
||||
try {
|
||||
cv::threshold(grad_magnitude, threshold_mask, 0.75, 1, cv::THRESH_BINARY);
|
||||
threshold_mask.convertTo(threshold_mask, CV_8U);
|
||||
|
||||
|
||||
depth_img_upsampled.setTo(0, threshold_mask);
|
||||
} catch (const cv::Exception& e) {
|
||||
std::cerr << "ERROR: Threshold mask failed: " << e.what() << std::endl;
|
||||
return cv::Mat();
|
||||
}
|
||||
|
||||
return depth_img_upsampled;
|
||||
}
|
||||
|
||||
cv::Mat PointCloudToDepthConverter::customResize(const cv::Mat& src, const cv::Size& size) {
|
||||
if (src.empty()) {
|
||||
throw std::runtime_error("Source image is empty");
|
||||
}
|
||||
|
||||
if (size.width <= 0 || size.height <= 0) {
|
||||
throw std::runtime_error("Invalid target size");
|
||||
}
|
||||
|
||||
|
||||
cv::Mat dst(size.height, size.width, src.type());
|
||||
|
||||
float scale_x = src.cols / static_cast<float>(size.width);
|
||||
float scale_y = src.rows / static_cast<float>(size.height);
|
||||
|
||||
if (src.channels() != 1 || src.type() != CV_32F) {
|
||||
throw std::runtime_error("Unsupported image type - expected single channel float");
|
||||
}
|
||||
|
||||
|
||||
for (int y = 0; y < dst.rows; y++) {
|
||||
|
||||
int src_y = static_cast<int>(y * scale_y);
|
||||
src_y = std::min(src_y, src.rows - 1);
|
||||
|
||||
for (int x = 0; x < dst.cols; x++) {
|
||||
int src_x = static_cast<int>(x * scale_x);
|
||||
src_x = std::min(src_x, src.cols - 1);
|
||||
dst.at<float>(y, x) = src.at<float>(src_y, src_x);
|
||||
}
|
||||
}
|
||||
|
||||
return dst;
|
||||
}
|
||||
pcl::PointCloud<pcl::PointXYZRGB> PointCloudToDepthConverter::generateColoredCloud(
|
||||
const cv::Mat &depth_img, const cv::Mat &color_img)
|
||||
{
|
||||
cv::Mat depth_undistorted, color_undistorted;
|
||||
depth_undistorted = depth_img.clone();
|
||||
cv::remap(color_img, color_undistorted, inv_map_x_, inv_map_y_, cv::INTER_LINEAR);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB> cloud_colored;
|
||||
|
||||
Eigen::Matrix4d Tlc = params_.Tcl.inverse();
|
||||
|
||||
for (int v = 0; v < depth_undistorted.rows; v += params_.point_sampling_rate)
|
||||
{
|
||||
for (int u = 0; u < depth_undistorted.cols; u += params_.point_sampling_rate)
|
||||
{
|
||||
float depth = depth_undistorted.at<float>(v, u);
|
||||
if (depth > 0.1f && depth < 100.0f)
|
||||
{
|
||||
double y_cam = (v - params_.v0) * depth / params_.A22;
|
||||
double x_cam = ((u - params_.u0) * depth - params_.A12 * y_cam)/ params_.A11;
|
||||
|
||||
double z_cam = depth;
|
||||
|
||||
Eigen::Vector4d point_cam(x_cam, y_cam, z_cam, 1.0);
|
||||
|
||||
Eigen::Vector4d point_lidar = Tlc * point_cam;
|
||||
|
||||
pcl::PointXYZRGB point;
|
||||
point.x = static_cast<float>(point_lidar[0]);
|
||||
point.y = static_cast<float>(point_lidar[1]);
|
||||
point.z = static_cast<float>(point_lidar[2]);
|
||||
|
||||
if (u < color_undistorted.cols && v < color_undistorted.rows)
|
||||
{
|
||||
cv::Vec3b color = color_undistorted.at<cv::Vec3b>(v, u);
|
||||
point.b = color[0];
|
||||
point.g = color[1];
|
||||
point.r = color[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
point.r = point.g = point.b = 255;
|
||||
}
|
||||
|
||||
cloud_colored.points.push_back(point);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cloud_colored.width = cloud_colored.points.size();
|
||||
cloud_colored.height = 1;
|
||||
cloud_colored.is_dense = false;
|
||||
|
||||
return cloud_colored;
|
||||
}
|
||||
|
||||
std::pair<bool, std::string> PointCloudToDepthConverter::validateInputs(
|
||||
const pcl::PointCloud<pcl::PointXYZ> &cloud, const cv::Mat &image)
|
||||
{
|
||||
if (cloud.empty())
|
||||
{
|
||||
return {false, "Empty point cloud"};
|
||||
}
|
||||
|
||||
if (image.empty())
|
||||
{
|
||||
return {false, "Empty image"};
|
||||
}
|
||||
|
||||
if (params_.A11 < 1e-6 || params_.A22 < 1e-6)
|
||||
{
|
||||
return {false, "Invalid camera intrinsics"};
|
||||
}
|
||||
|
||||
return {true, ""};
|
||||
}
|
||||
|
||||
void PointCloudToDepthConverter::updateCameraParams(const CameraParams ¶ms)
|
||||
{
|
||||
params_ = params;
|
||||
initializeInternalParams();
|
||||
createDistortionMaps();
|
||||
}
|
||||
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "rawCloudRender.h"
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <array>
|
||||
#include <algorithm>
|
||||
|
||||
struct ValidPointInfo {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
int u;
|
||||
int v;
|
||||
};
|
||||
|
||||
|
||||
namespace GlobalCameraParams {
|
||||
float g_fx = 0.0f;
|
||||
float g_fy = 0.0f;
|
||||
float g_cx = 0.0f;
|
||||
float g_cy = 0.0f;
|
||||
float g_skew = 0.0f;
|
||||
float g_k2 = 0.0f;
|
||||
float g_k3 = 0.0f;
|
||||
float g_k4 = 0.0f;
|
||||
float g_k5 = 0.0f;
|
||||
float g_k6 = 0.0f;
|
||||
float g_k7 = 0.0f;
|
||||
Eigen::Matrix4f g_T_camera_lidar = Eigen::Matrix4f::Identity();
|
||||
}
|
||||
bool raw_debug=0;
|
||||
bool rawCloudRender::init(const std::string& yamlFilePath) {
|
||||
YAML::Node config;
|
||||
try {
|
||||
config = YAML::LoadFile(yamlFilePath);
|
||||
} catch (const YAML::BadFile& e) {
|
||||
std::cerr << "Error: Could not open file '" << yamlFilePath << "' - " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const YAML::ParserException& e) {
|
||||
std::cerr << "Error: YAML parsing failed - " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Unexpected error: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Intrinsic parameters node (cam_0)
|
||||
const std::string cam_node_name = "cam_0";
|
||||
if (!config[cam_node_name]) {
|
||||
std::cerr << "Error: Missing camera node '" << cam_node_name << "'" << std::endl;
|
||||
return false;
|
||||
}
|
||||
YAML::Node cam_node = config[cam_node_name];
|
||||
|
||||
// Directly access Tcl_0 node
|
||||
const std::string tcl_node_name = "Tcl_0";
|
||||
if (!config[tcl_node_name]) {
|
||||
std::cerr << "Error: Missing transformation matrix node '" << tcl_node_name << "'" << std::endl;
|
||||
return false;
|
||||
}
|
||||
YAML::Node tclNode = config[tcl_node_name];
|
||||
if (tclNode.size() != 16) {
|
||||
std::cerr << "Error: Transformation matrix must be 4x4 (16 elements)" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// === Read intrinsic parameters ===
|
||||
GlobalCameraParams::g_k2 = cam_node["k2"].as<float>();
|
||||
GlobalCameraParams::g_k3 = cam_node["k3"].as<float>();
|
||||
GlobalCameraParams::g_k4 = cam_node["k4"].as<float>();
|
||||
GlobalCameraParams::g_k5 = cam_node["k5"].as<float>();
|
||||
GlobalCameraParams::g_k6 = cam_node["k6"].as<float>();
|
||||
GlobalCameraParams::g_k7 = cam_node["k7"].as<float>();
|
||||
|
||||
GlobalCameraParams::g_fx = cam_node["A11"].as<float>();
|
||||
GlobalCameraParams::g_skew = cam_node["A12"].as<float>();
|
||||
GlobalCameraParams::g_fy = cam_node["A22"].as<float>();
|
||||
|
||||
GlobalCameraParams::g_cx = cam_node["u0"].as<float>();
|
||||
GlobalCameraParams::g_cy = cam_node["v0"].as<float>();
|
||||
|
||||
// === Read extrinsic transformation matrix ===
|
||||
GlobalCameraParams::g_T_camera_lidar <<
|
||||
tclNode[0].as<float>(), tclNode[1].as<float>(), tclNode[2].as<float>(), tclNode[3].as<float>(),
|
||||
tclNode[4].as<float>(), tclNode[5].as<float>(), tclNode[6].as<float>(), tclNode[7].as<float>(),
|
||||
tclNode[8].as<float>(), tclNode[9].as<float>(), tclNode[10].as<float>(), tclNode[11].as<float>(),
|
||||
tclNode[12].as<float>(), tclNode[13].as<float>(), tclNode[14].as<float>(), tclNode[15].as<float>();
|
||||
|
||||
// === Display key parameters concisely ===
|
||||
std::cout << "=== Camera Calibration Parameters ===" << std::endl;
|
||||
std::cout << "Intrinsics:" << std::endl;
|
||||
std::cout << " fx: " << GlobalCameraParams::g_fx
|
||||
<< ", fy: " << GlobalCameraParams::g_fy
|
||||
<< ", cx: " << GlobalCameraParams::g_cx
|
||||
<< ", cy: " << GlobalCameraParams::g_cy << std::endl;
|
||||
std::cout << "Distortion: k2=" << GlobalCameraParams::g_k2
|
||||
<< ", k3=" << GlobalCameraParams::g_k3 << std::endl;
|
||||
|
||||
Eigen::Vector3f translation = GlobalCameraParams::g_T_camera_lidar.block<3,1>(0,3);
|
||||
Eigen::Matrix3f rotation = GlobalCameraParams::g_T_camera_lidar.block<3,3>(0,0);
|
||||
std::cout << "Extrinsics:" << std::endl;
|
||||
std::cout << " Translation: [" << translation.x() << ", "
|
||||
<< translation.y() << ", " << translation.z() << "]" << std::endl;
|
||||
std::cout << " Rotation (euler angles): "
|
||||
<< rotation.eulerAngles(0,1,2).transpose() * 180/M_PI << "°" << std::endl;
|
||||
|
||||
return true;
|
||||
}
|
||||
void rawCloudRender::render(std::vector<std::vector<float>>& rgb_image,
|
||||
capture_Image_List_t* pcd_stream,
|
||||
int pcdIdx,
|
||||
std::vector<float>& rgbCloud_flat)
|
||||
{
|
||||
// Initialize constants
|
||||
constexpr float inv_1000 = 0.001f;
|
||||
const float fx = GlobalCameraParams::g_fx;
|
||||
const float fy = GlobalCameraParams::g_fy;
|
||||
const float cx = GlobalCameraParams::g_cx;
|
||||
const float cy = GlobalCameraParams::g_cy;
|
||||
const float skew = GlobalCameraParams::g_skew;
|
||||
const float k2 = GlobalCameraParams::g_k2;
|
||||
const float k3 = GlobalCameraParams::g_k3;
|
||||
const float k4 = GlobalCameraParams::g_k4;
|
||||
const float k5 = GlobalCameraParams::g_k5;
|
||||
const float k6 = GlobalCameraParams::g_k6;
|
||||
const float k7 = GlobalCameraParams::g_k7;
|
||||
const Eigen::Matrix4f& T = GlobalCameraParams::g_T_camera_lidar;
|
||||
|
||||
// Precompute matrix elements
|
||||
const float T00 = T(0,0), T01 = T(0,1), T02 = T(0,2), T03 = T(0,3);
|
||||
const float T10 = T(1,0), T11 = T(1,1), T12 = T(1,2), T13 = T(1,3);
|
||||
const float T20 = T(2,0), T21 = T(2,1), T22 = T(2,2), T23 = T(2,3);
|
||||
|
||||
// Initialize lookup table
|
||||
static std::array<float, 10000> dist_table;
|
||||
static bool table_init = [&](){
|
||||
for (size_t i=0; i<dist_table.size(); ++i) {
|
||||
float theta = i * (M_PI/2) / dist_table.size();
|
||||
dist_table[i] = theta*(1 + theta*(k2 + theta*(k3 + theta*(k4 + theta*(k5 + theta*(k6 + theta*k7))))));
|
||||
}
|
||||
return true;
|
||||
}();
|
||||
|
||||
// Get point cloud data (direct access)
|
||||
if (!pcd_stream || pcdIdx < 0 || pcdIdx >= 10) {
|
||||
std::cerr << "ERROR: Invalid pcd_stream or index in render function" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
buffer_List_t& pcd_buffer = pcd_stream->imageList[pcdIdx];
|
||||
const int total_points = pcd_buffer.height * pcd_buffer.width;
|
||||
|
||||
if (!pcd_buffer.pAddr) {
|
||||
std::cerr << "ERROR: Null point cloud data pointer in render function" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
float* data = static_cast<float*>(pcd_buffer.pAddr);
|
||||
|
||||
// Prepare output
|
||||
rgbCloud_flat.clear();
|
||||
rgbCloud_flat.resize(total_points * 4); // Preallocate maximum space
|
||||
float* output_ptr = rgbCloud_flat.data();
|
||||
|
||||
// Get image dimensions
|
||||
const int img_height = 1296;
|
||||
const int img_width = 1600;
|
||||
|
||||
// Process point cloud
|
||||
int valid_count = 0;
|
||||
for (int idx = 0; idx < total_points; ++idx)
|
||||
{
|
||||
float* pf = data + idx*4;
|
||||
|
||||
// Quick check for invalid points
|
||||
if (std::abs(pf[0]) < 1e-5f && std::abs(pf[1]) < 1e-5f && std::abs(pf[2]) < 1e-5f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Coordinate transformation
|
||||
const float x = pf[2] * inv_1000;
|
||||
const float y = -pf[0] * inv_1000;
|
||||
const float z = pf[1] * inv_1000;
|
||||
|
||||
// Manual matrix transformation
|
||||
const float x1 = T00*x + T01*y + T02*z + T03;
|
||||
const float y1 = T10*x + T11*y + T12*z + T13;
|
||||
const float z1 = T20*x + T21*y + T22*z + T23;
|
||||
|
||||
// Check for points behind camera
|
||||
if (z1 <= 0.0f) continue;
|
||||
|
||||
// Calculate projection
|
||||
const float x1_sq = x1*x1;
|
||||
const float y1_sq = y1*y1;
|
||||
const float z1_sq = z1*z1;
|
||||
|
||||
const float norm = std::sqrt(x1_sq + y1_sq + z1_sq);
|
||||
if (norm < 1e-7f) continue;
|
||||
|
||||
const float r = std::sqrt(x1_sq + y1_sq);
|
||||
if (r < 1e-7f) continue;
|
||||
|
||||
const float cost = z1 / norm;
|
||||
const float theta = std::acos(cost);
|
||||
|
||||
// Safe table lookup
|
||||
const size_t table_idx = static_cast<size_t>(theta * (2.0f/M_PI) * dist_table.size());
|
||||
const size_t safe_idx = std::min(table_idx, dist_table.size()-1);
|
||||
const float thetad = dist_table[safe_idx];
|
||||
|
||||
const float scaling = thetad / r;
|
||||
const float xd = x1 * scaling;
|
||||
const float yd = y1 * scaling;
|
||||
const float pd_2d_x = xd * fx + yd * skew + cx;
|
||||
const float pd_2d_y = yd * fy + cy;
|
||||
|
||||
// Quick boundary check
|
||||
const int u = static_cast<int>(pd_2d_x);
|
||||
const int v = static_cast<int>(pd_2d_y);
|
||||
|
||||
// Strict boundary check
|
||||
if (u < 0 || u >= img_width || v < 0 || v >= img_height) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Safe image data access
|
||||
if (v < static_cast<int>(rgb_image.size()) && u < static_cast<int>(rgb_image[v].size())) {
|
||||
*output_ptr++ = x;
|
||||
*output_ptr++ = y;
|
||||
*output_ptr++ = z;
|
||||
*output_ptr++ = rgb_image[v][u];
|
||||
valid_count++;
|
||||
} else {
|
||||
// Handle invalid coordinates
|
||||
static bool warned = false;
|
||||
if (!warned) {
|
||||
if(raw_debug)
|
||||
{
|
||||
std::cerr << "WARNING: Invalid image coordinates: u=" << u << ", v=" << v
|
||||
<< " (image size: " << rgb_image.size() << "x"
|
||||
<< (rgb_image.empty() ? 0 : rgb_image[0].size()) << ")" << std::endl;
|
||||
}
|
||||
|
||||
warned = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Resize output
|
||||
rgbCloud_flat.resize(output_ptr - rgbCloud_flat.data());
|
||||
if(raw_debug)
|
||||
{
|
||||
std::cout << "Render completed: " << total_points << " points processed, "
|
||||
<< valid_count << " valid points ("
|
||||
<< (100.0 * valid_count / total_points) << "%)" << std::endl;
|
||||
}
|
||||
}
|
||||
void rawCloudRender::print_camera_calib() {
|
||||
std::cout << model_type_ << std::endl;
|
||||
std::cout << image_width_ << std::endl;
|
||||
std::cout << image_height_ << std::endl;
|
||||
|
||||
std::cout << "T_camera_lidar" << std::endl;
|
||||
std::cout << T_camera_lidar_(0,0) << " " << T_camera_lidar_(0,1) << " " << T_camera_lidar_(0,2) << " " << T_camera_lidar_(0,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(1,0) << " " << T_camera_lidar_(1,1) << " " << T_camera_lidar_(1,2) << " " << T_camera_lidar_(1,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(2,0) << " " << T_camera_lidar_(2,1) << " " << T_camera_lidar_(2,2) << " " << T_camera_lidar_(2,3) << std::endl;
|
||||
std::cout << T_camera_lidar_(3,0) << " " << T_camera_lidar_(3,1) << " " << T_camera_lidar_(3,2) << " " << T_camera_lidar_(3,3) << std::endl;
|
||||
|
||||
std::cout << "cam" << std::endl;
|
||||
std::cout << k2_ << std::endl;
|
||||
std::cout << k3_ << std::endl;
|
||||
std::cout << k4_ << std::endl;
|
||||
std::cout << k5_ << std::endl;
|
||||
std::cout << k6_ << std::endl;
|
||||
std::cout << k7_ << std::endl;
|
||||
|
||||
std::cout << A11_fx_ << std::endl;
|
||||
std::cout << A12_skew_ << std::endl;
|
||||
std::cout << A22_fy_ << std::endl;
|
||||
std::cout << u0_cx_ << std::endl;
|
||||
std::cout << v0_cy_ << std::endl;
|
||||
}
|
||||
@@ -0,0 +1,246 @@
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
|
||||
#include "yaml_parser.h"
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
#include <iomanip>
|
||||
|
||||
#include <cstring>
|
||||
namespace odin_ros_driver {
|
||||
|
||||
YamlParser::YamlParser(const std::string& config_file)
|
||||
: config_file_(config_file) {}
|
||||
|
||||
bool YamlParser::loadConfig() {
|
||||
try {
|
||||
std::cerr << "Loading config file: " << config_file_ << std::endl;
|
||||
|
||||
// Check if file exists
|
||||
if (!std::filesystem::exists(config_file_)) {
|
||||
std::cerr << "Config file not found: " << config_file_ << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Print file contents
|
||||
std::ifstream file(config_file_);
|
||||
std::string content((std::istreambuf_iterator<char>(file)),
|
||||
std::istreambuf_iterator<char>());
|
||||
std::cerr << "Config file content:\n" << content << "\n--- End of file ---" << std::endl;
|
||||
|
||||
// Load YAML
|
||||
YAML::Node config = YAML::LoadFile(config_file_);
|
||||
|
||||
// Check if 'register_keys' node exists
|
||||
if (!config["register_keys"]) {
|
||||
std::cerr << "Missing 'register_keys' section in config file" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
YAML::Node register_keys = config["register_keys"];
|
||||
register_keys_.clear();
|
||||
register_keys_str_val_.clear();
|
||||
custom_parameters_.clear();
|
||||
|
||||
// Print number of key-value pairs found
|
||||
std::cerr << "Found " << register_keys.size() << " keys in config" << std::endl;
|
||||
|
||||
for (YAML::const_iterator it = register_keys.begin(); it != register_keys.end(); ++it) {
|
||||
std::string key = it->first.as<std::string>();
|
||||
const YAML::Node& value_node = it->second;
|
||||
|
||||
// Convert key to lowercase
|
||||
std::transform(key.begin(), key.end(), key.begin(),
|
||||
[](unsigned char c){ return std::tolower(c); });
|
||||
|
||||
// Check if this is a custom parameter
|
||||
if (key.substr(0, 7) == "custom_") {
|
||||
std::string param_name = key.substr(7);
|
||||
|
||||
// Handle different value types
|
||||
if (value_node.IsScalar()) {
|
||||
// Single scalar value (int or float)
|
||||
try {
|
||||
int int_value = value_node.as<int>();
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::INT_TYPE;
|
||||
param_value.setData(int_value);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
std::cerr << "Loaded custom parameter (int): " << param_name << " = " << int_value << std::endl;
|
||||
} catch (...) {
|
||||
try {
|
||||
double float_value = value_node.as<double>();
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::FLOAT_ARRAY_TYPE;
|
||||
std::vector<float> float_array = {static_cast<float>(float_value)};
|
||||
param_value.setArray(float_array);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
std::cerr << "Loaded custom parameter (float): " << param_name << " = " << float_value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse custom parameter " << param_name << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
} else if (value_node.IsSequence()) {
|
||||
// Array of values
|
||||
size_t array_size = value_node.size();
|
||||
if (array_size == 0) {
|
||||
std::cerr << "Empty array for custom parameter: " << param_name << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Try to detect if it's a float or int array based on first element
|
||||
try {
|
||||
// Try to parse as float array first
|
||||
std::vector<float> float_array;
|
||||
for (size_t i = 0; i < array_size; ++i) {
|
||||
float_array.push_back(value_node[i].as<float>());
|
||||
}
|
||||
ParameterValue param_value;
|
||||
param_value.type = DataType::FLOAT_ARRAY_TYPE;
|
||||
param_value.setArray(float_array);
|
||||
custom_parameters_[param_name] = param_value;
|
||||
|
||||
std::cerr << "Loaded custom parameter (float array): " << param_name << " = [";
|
||||
for (size_t i = 0; i < float_array.size(); ++i) {
|
||||
if (i > 0) std::cerr << ", ";
|
||||
std::cerr << std::fixed << std::setprecision(4) << float_array[i];
|
||||
}
|
||||
std::cerr << "]" << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse custom parameter array " << param_name << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
} else if (allowed_key_w_str_val.find(key) != allowed_key_w_str_val.end()) {
|
||||
try {
|
||||
std::string value = value_node.as<std::string>();
|
||||
register_keys_str_val_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
} else if (allowed_key_w_float_val.find(key) != allowed_key_w_float_val.end()) {
|
||||
try {
|
||||
double value = value_node.as<double>();
|
||||
register_keys_float_val_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
} else {
|
||||
// Regular (non-custom) integer parameter
|
||||
try {
|
||||
int value = value_node.as<int>();
|
||||
register_keys_[key] = value;
|
||||
std::cerr << "Loaded key: " << key << " = " << value << std::endl;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Failed to parse key " << key << ": " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
} catch (const YAML::Exception& e) {
|
||||
std::cerr << "YAML exception: " << e.what() << std::endl;
|
||||
return false;
|
||||
} catch (const std::exception& e) {
|
||||
std::cerr << "Exception: " << e.what() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const std::map<std::string, int>& YamlParser::getRegisterKeys() const {
|
||||
return register_keys_;
|
||||
}
|
||||
|
||||
const std::map<std::string, ParameterValue>& YamlParser::getCustomParameters() const {
|
||||
return custom_parameters_;
|
||||
}
|
||||
|
||||
const std::map<std::string, std::string>& YamlParser::getRegisterKeysStrVal() const {
|
||||
return register_keys_str_val_;
|
||||
}
|
||||
|
||||
void YamlParser::printConfig() const {
|
||||
std::cerr << "Configuration Keys:" << std::endl;
|
||||
if (register_keys_.empty()) {
|
||||
std::cerr << " (int val empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, value] : register_keys_) {
|
||||
std::cerr << " " << key << ": " << value << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if (register_keys_str_val_.empty()) {
|
||||
std::cerr << " (str_val empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, value] : register_keys_str_val_) {
|
||||
std::cerr << " " << key << ": " << value << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
std::cerr << "Custom Parameters:" << std::endl;
|
||||
if (custom_parameters_.empty()) {
|
||||
std::cerr << " (custom param empty)" << std::endl;
|
||||
} else {
|
||||
for (const auto& [key, param_val] : custom_parameters_) {
|
||||
std::cerr << " " << key << ": (size=" << param_val.getSize() << " bytes)";
|
||||
if (param_val.type == DataType::INT_TYPE && param_val.getSize() == sizeof(int)) {
|
||||
int int_val = *reinterpret_cast<const int*>(param_val.getData());
|
||||
std::cerr << " = " << int_val;
|
||||
} else if (param_val.type == DataType::FLOAT_ARRAY_TYPE && param_val.getSize() % sizeof(float) == 0) {
|
||||
size_t count = param_val.getSize() / sizeof(float);
|
||||
const float* float_arr = reinterpret_cast<const float*>(param_val.getData());
|
||||
std::cerr << " = [";
|
||||
for (size_t i = 0; i < count; ++i) {
|
||||
if (i > 0) std::cerr << ", ";
|
||||
std::cerr << std::fixed << std::setprecision(4) << float_arr[i];
|
||||
}
|
||||
std::cerr << "]";
|
||||
}
|
||||
std::cerr << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool YamlParser::applyCustomParameters(device_handle device) {
|
||||
bool success = true;
|
||||
|
||||
for (const auto& [param_name, param_value] : custom_parameters_) {
|
||||
std::cerr << "Setting custom parameter: " << param_name << " (size=" << param_value.getSize() << " bytes)" << std::endl;
|
||||
|
||||
int result = lidar_set_custom_parameter(device, param_name.c_str(), param_value.getData(), param_value.getSize());
|
||||
if (result != 0) {
|
||||
std::cerr << "Failed to set custom parameter " << param_name << ": error code " << result << std::endl;
|
||||
success = false;
|
||||
} else {
|
||||
std::cerr << "Successfully set custom parameter " << param_name << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int YamlParser::getCustomParameterInt(const std::string& param_name, int default_value) const {
|
||||
auto it = custom_parameters_.find(param_name);
|
||||
if (it != custom_parameters_.end()) {
|
||||
const ParameterValue& param_value = it->second;
|
||||
if (param_value.type == DataType::INT_TYPE && param_value.getSize() == sizeof(int)) {
|
||||
return *reinterpret_cast<const int*>(param_value.getData());
|
||||
}
|
||||
}
|
||||
return default_value;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
+103
@@ -0,0 +1,103 @@
|
||||
# =============================================================================
|
||||
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||
# =============================================================================
|
||||
#
|
||||
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||
#
|
||||
# =============================================================================
|
||||
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||
- {name: projected_gravity, dim: 3}
|
||||
- {name: command, dim: 3}
|
||||
- {name: joint_pos_rel, dim: 12}
|
||||
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||
- {name: last_actions, dim: 16}
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
motor_mapping:
|
||||
can_id_map:
|
||||
fl_hip_abduction: [1, 1]
|
||||
fl_hip_pitch: [1, 2]
|
||||
fl_knee: [1, 3]
|
||||
fl_wheel: [1, 4]
|
||||
fr_hip_abduction: [1, 5]
|
||||
fr_hip_pitch: [1, 6]
|
||||
fr_knee: [1, 7]
|
||||
fr_wheel: [1, 8]
|
||||
rl_hip_abduction: [2, 1]
|
||||
rl_hip_pitch: [2, 2]
|
||||
rl_knee: [2, 3]
|
||||
rl_wheel: [2, 4]
|
||||
rr_hip_abduction: [2, 5]
|
||||
rr_hip_pitch: [2, 6]
|
||||
rr_knee: [2, 7]
|
||||
rr_wheel: [2, 8]
|
||||
direction_map:
|
||||
fl_hip_abduction: -1
|
||||
fl_hip_pitch: -1
|
||||
fl_knee: -1
|
||||
fl_wheel: -1
|
||||
fr_hip_abduction: -1
|
||||
fr_hip_pitch: 1
|
||||
fr_knee: 1
|
||||
fr_wheel: 1
|
||||
rl_hip_abduction: 1
|
||||
rl_hip_pitch: -1
|
||||
rl_knee: -1
|
||||
rl_wheel: -1
|
||||
rr_hip_abduction: 1
|
||||
rr_hip_pitch: 1
|
||||
rr_knee: 1
|
||||
rr_wheel: 1
|
||||
zero_offset_map:
|
||||
fl_hip_abduction: 0.003
|
||||
fl_hip_pitch: 0.030
|
||||
fl_knee: 0.028
|
||||
fl_wheel: 0.0
|
||||
fr_hip_abduction: 0.004
|
||||
fr_hip_pitch: 0.038
|
||||
fr_knee: 0.011
|
||||
fr_wheel: 0.0
|
||||
rl_hip_abduction: 0.019
|
||||
rl_hip_pitch: -0.034
|
||||
rl_knee: 0.025
|
||||
rl_wheel: 0.0
|
||||
rr_hip_abduction: -0.001
|
||||
rr_hip_pitch: 0.039
|
||||
rr_knee: 0.018
|
||||
rr_wheel: 0.0
|
||||
@@ -0,0 +1,196 @@
|
||||
/**:
|
||||
ros__parameters:
|
||||
policy_hz: 50.0
|
||||
motor_hz: 200.0
|
||||
status_hz: 10.0
|
||||
target_timeout_ms: 150.0
|
||||
event_log_dir: "logs_v2_web"
|
||||
model_engine_path: policies/model_9600_fp16.engine
|
||||
prefer_tensorrt: true
|
||||
model_path: policies/model_9600.onnx
|
||||
rough_model_engine_path: policies/model_9600_fp16.engine
|
||||
crawl_model_path: policies/model_crawl.onnx
|
||||
crawl_model_engine_path: ""
|
||||
rough_default_dof_pos: [0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.550, -1.125, 0.0, 0.0, 0.0, 0.0]
|
||||
crawl_backend: "ik"
|
||||
crawl_default_dof_pos: [0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.2, 1.697, -2.650, -0.2, 1.697, -2.650, 0.0, 0.0, 0.0, 0.0]
|
||||
crawl_ik_wheel_linear_gain: 6.25
|
||||
crawl_ik_wheel_yaw_gain: 4.0
|
||||
crawl_ik_max_wheel_speed: 6.0
|
||||
crawl_ik_abduction_clip: 0.45
|
||||
model_switch_transition_s: 1.5
|
||||
model_switch_to_stand_transition_scale: 2.0
|
||||
model_switch_to_model_transition_scale: 2.5
|
||||
model_switch_stand_hold_s: 0.8
|
||||
model_switch_stand_max_err: 0.18
|
||||
model_switch_stand_max_vel: 0.8
|
||||
model_switch_release_scale: 2.0
|
||||
debug_trace_enabled: true
|
||||
debug_trace_decimation: 1
|
||||
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.85
|
||||
remote_max_vy: 0.3
|
||||
remote_max_yaw_rate: 0.5
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
remote_publish_inactive_zero: true
|
||||
remote_estop_latch: true
|
||||
remote_poll_hz: 50.0
|
||||
remote_model_switch_enabled: true
|
||||
remote_model_switch_channel: 10
|
||||
remote_model_switch_debounce_frames: 3
|
||||
remote_model_switch_rough_level: "low"
|
||||
remote_model_switch_ik_level: "high"
|
||||
|
||||
# Command mux parameters
|
||||
cmd_mux_default_mode: "NAV"
|
||||
cmd_mux_output_hz: 50.0
|
||||
cmd_mux_remote_timeout_ms: 250.0
|
||||
cmd_mux_web_timeout_ms: 300.0
|
||||
cmd_mux_nav_timeout_ms: 500.0
|
||||
cmd_mux_max_vx: 0.8
|
||||
cmd_mux_max_vy: 0.3
|
||||
cmd_mux_max_yaw_rate: 0.5
|
||||
cmd_mux_max_vx_acc: 1.0
|
||||
cmd_mux_max_vy_acc: 1.0
|
||||
cmd_mux_max_yaw_acc: 1.5
|
||||
|
||||
# Windows/Nano Web UDP bridge parameters
|
||||
web_bridge_enabled: true
|
||||
web_http_host: "0.0.0.0"
|
||||
web_http_port: 18080
|
||||
web_static_dir: "tools/win_web_debug/static"
|
||||
web_udp_listen_host: "0.0.0.0"
|
||||
web_udp_listen_port: 15000
|
||||
web_udp_remote_host: ""
|
||||
web_udp_remote_port: 15001
|
||||
web_udp_state_hz: 20.0
|
||||
web_udp_cmd_timeout_ms: 300.0
|
||||
web_udp_max_packet_bytes: 8192
|
||||
web_udp_max_vx: 0.8
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.5
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 0.6
|
||||
model_switch_max_target_offset: 1.8
|
||||
hard_target_offset: 2.0
|
||||
max_ang_vel: 24.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 200.0
|
||||
wheel_no_effect_command_threshold: 1.0
|
||||
wheel_no_effect_min_response_ratio: 0.20
|
||||
wheel_no_effect_velocity_epsilon: 0.25
|
||||
wheel_no_effect_max_temperature_c: 90.0
|
||||
wheel_no_effect_min_bus_voltage_v: 18.0
|
||||
wheel_no_effect_command_warmup_cycles: 12
|
||||
wheel_no_effect_trigger_cycles: 30
|
||||
wheel_no_effect_attempt_limit: 2
|
||||
wheel_no_effect_cooldown_ms: 1200
|
||||
wheel_recovery_verify_timeout_ms: 180
|
||||
wheel_no_effect_diag_freshness_ms: 350
|
||||
wheel_no_effect_diag_request_period_ms: 80
|
||||
leg_no_effect_position_error_threshold: 0.18
|
||||
leg_no_effect_velocity_epsilon: 0.12
|
||||
leg_no_effect_max_estimated_current_arms: 4.0
|
||||
leg_no_effect_max_abs_torque_nm: 5.0
|
||||
leg_no_effect_max_temperature_c: 100.0
|
||||
leg_no_effect_min_bus_voltage_v: 18.0
|
||||
leg_no_effect_command_warmup_cycles: 40
|
||||
leg_no_effect_trigger_cycles: 25
|
||||
leg_no_effect_attempt_limit: 2
|
||||
leg_no_effect_cooldown_ms: 1200
|
||||
leg_recovery_verify_timeout_ms: 220
|
||||
|
||||
# Policy alignment with the 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
|
||||
model_switch_release_scale: 2.0
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: true
|
||||
|
||||
# Simple navigation parameters
|
||||
nav_map_frame: "map"
|
||||
nav_odom_frame: "odom"
|
||||
nav_base_frame: "base_link"
|
||||
nav_control_hz: 20.0
|
||||
nav_goal_tolerance: 0.20
|
||||
nav_yaw_stop_threshold: 0.80
|
||||
nav_max_vx: 0.45
|
||||
nav_max_wz: 0.80
|
||||
nav_kp_dist: 0.80
|
||||
nav_kp_yaw: 1.80
|
||||
nav_goal_exit_tolerance_margin: 0.08
|
||||
nav_goal_complete_stable_cycles: 4
|
||||
nav_final_align_kp_yaw_scale: 0.60
|
||||
nav_final_align_max_wz: 0.45
|
||||
nav_final_align_creep_speed: 0.05
|
||||
nav_goal_yaw_tolerance_deg: 12.0
|
||||
nav_astar_enabled: true
|
||||
nav_astar_resolution: 0.10
|
||||
nav_astar_pcd_sample_step: 5
|
||||
nav_astar_allow_diagonal: true
|
||||
nav_astar_smooth_enabled: true
|
||||
nav_astar_corner_blend_dist: 0.20
|
||||
nav_astar_waypoint_reach_dist: 0.18
|
||||
nav_astar_lookahead_dist: 0.35
|
||||
nav_astar_snap_radius: 0.60
|
||||
nav_astar_max_expansions: 120000
|
||||
nav_turn_in_place_enabled: true
|
||||
nav_turn_in_place_enter_yaw_deg: 70.0
|
||||
nav_turn_in_place_exit_yaw_deg: 18.0
|
||||
nav_turn_in_place_max_wz: 0.80
|
||||
nav_pre_dock_enabled: true
|
||||
nav_pre_dock_distance: 0.35
|
||||
nav_pre_dock_tolerance: 0.18
|
||||
nav_pre_dock_skip_within_goal_dist: 0.45
|
||||
nav_goals_file: src/sim2real_nav2/config/maps/factory_a/goals.yaml
|
||||
nav_missions_file: src/sim2real_nav2/config/maps/factory_a/missions.yaml
|
||||
# Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below.
|
||||
nav_route_file: src/sim2real_nav2/config/maps/a_min/test_route.yaml
|
||||
# Task switch entry: change this path to another .json/.yaml route file, then relaunch or reload the nav nodes.
|
||||
# The web "odom" button uses the first waypoint of this route as the fixed odom fallback start pose.
|
||||
nav_route_task_file: map/routes/A_min/A_min_route.json
|
||||
nav_route_auto_align_enabled: false
|
||||
nav_route_rotation_offset_deg: 0.0
|
||||
nav_route_align_max_angle_deg: 6.0
|
||||
nav_route_align_angle_step_deg: 0.5
|
||||
nav_route_align_search_radius: 0.35
|
||||
odom_fallback_require_odom_fresh: true
|
||||
odom_fallback_max_odom_age_ms: 500.0
|
||||
odom_fallback_block_existing_map_odom_tf: true
|
||||
odom_fallback_tf_conflict_window_s: 1.0
|
||||
odom_fallback_tf_conflict_xy_tolerance: 0.05
|
||||
odom_fallback_tf_conflict_yaw_tolerance_deg: 2.0
|
||||
# Keep odom fallback running if Odin relocalizes mid-task; hand off after mission end or Exit odom.
|
||||
odom_fallback_stop_on_external_tf: false
|
||||
pcd_nav_file: map/A_min.pcd
|
||||
pcd_floor_z_min: -1.6
|
||||
pcd_floor_z_max: 0.4
|
||||
pcd_sample_step: 25
|
||||
pcd_robot_radius: 0.18
|
||||
+152
@@ -0,0 +1,152 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterFile
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
runtime_params = ParameterFile(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("sim2real_bringup"),
|
||||
"config",
|
||||
"runtime.yaml",
|
||||
]),
|
||||
allow_substs=True,
|
||||
)
|
||||
|
||||
# Declare launch configurations
|
||||
launch_driver_arg = DeclareLaunchArgument(
|
||||
'launch_driver',
|
||||
default_value='true',
|
||||
description='Whether to launch the odin_ros_driver sensor node'
|
||||
)
|
||||
|
||||
launch_nav2_arg = DeclareLaunchArgument(
|
||||
'launch_nav2',
|
||||
default_value='false',
|
||||
description='Whether to launch the Nav2 navigation stack'
|
||||
)
|
||||
|
||||
launch_remote_arg = DeclareLaunchArgument(
|
||||
'launch_remote',
|
||||
default_value='true',
|
||||
description='Whether to launch the SBUS UART remote control node'
|
||||
)
|
||||
|
||||
launch_web_bridge_arg = DeclareLaunchArgument(
|
||||
'launch_web_bridge',
|
||||
default_value='true',
|
||||
description='Whether to launch the Windows/Nano UDP web debug bridge'
|
||||
)
|
||||
|
||||
launch_simple_nav_arg = DeclareLaunchArgument(
|
||||
'launch_simple_nav',
|
||||
default_value='true',
|
||||
description='Whether to launch the simple waypoint navigation node'
|
||||
)
|
||||
|
||||
event_log_dir_arg = DeclareLaunchArgument(
|
||||
'event_log_dir',
|
||||
default_value=PythonExpression([
|
||||
"'logs_v2_web/run_' + __import__('datetime').datetime.now().strftime('%Y-%m-%d_%H-%M-%S_%f')[:-3]"
|
||||
]),
|
||||
description='Per-run event log directory'
|
||||
)
|
||||
|
||||
# Include odin_ros_driver launch
|
||||
driver_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('odin_ros_driver'),
|
||||
'launch',
|
||||
'odin1_ros2.launch.py'
|
||||
])
|
||||
),
|
||||
launch_arguments={'launch_rviz': 'false'}.items(),
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver'))
|
||||
)
|
||||
|
||||
# Include sim2real_nav2 launch
|
||||
nav2_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('sim2real_nav2'),
|
||||
'launch',
|
||||
'nav2.launch.py'
|
||||
])
|
||||
),
|
||||
condition=IfCondition(LaunchConfiguration('launch_nav2'))
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
launch_driver_arg,
|
||||
launch_nav2_arg,
|
||||
launch_remote_arg,
|
||||
launch_web_bridge_arg,
|
||||
launch_simple_nav_arg,
|
||||
event_log_dir_arg,
|
||||
Node(
|
||||
package="sim2real_hw",
|
||||
executable="sim2real_hw_node",
|
||||
name="sim2real_hw_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="sim2real_runtime_node",
|
||||
name="sim2real_runtime_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params, {"event_log_dir": LaunchConfiguration("event_log_dir")}],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="cmd_mux_node.py",
|
||||
name="sim2real_cmd_mux_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="web_udp_bridge_node.py",
|
||||
name="sim2real_web_udp_bridge_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_web_bridge')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="remote_uart_node.py",
|
||||
name="sim2real_remote_uart_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_remote')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="simple_nav_node.py",
|
||||
name="sim2real_simple_nav_node",
|
||||
output="screen",
|
||||
parameters=[runtime_params],
|
||||
condition=IfCondition(LaunchConfiguration('launch_simple_nav')),
|
||||
),
|
||||
Node(
|
||||
package="sim2real_runtime",
|
||||
executable="odom_relay_node",
|
||||
name="odom_relay_node",
|
||||
output="screen",
|
||||
parameters=[{
|
||||
"odom_input_topic": "/odin1/odometry",
|
||||
"odom_output_topic": "/odom",
|
||||
"base_frame": "base_link",
|
||||
"publish_tf": True,
|
||||
}],
|
||||
condition=IfCondition(LaunchConfiguration('launch_driver')),
|
||||
),
|
||||
driver_launch,
|
||||
nav2_launch,
|
||||
])
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>sim2real_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Launch and configuration package for sim2real_ros2.</description>
|
||||
<maintainer email="todo@example.com">todo</maintainer>
|
||||
<license>Proprietary</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>sim2real_common</exec_depend>
|
||||
<exec_depend>sim2real_hw</exec_depend>
|
||||
<exec_depend>sim2real_interfaces</exec_depend>
|
||||
<exec_depend>sim2real_runtime</exec_depend>
|
||||
<exec_depend>sim2real_nav2</exec_depend>
|
||||
<exec_depend>odin_ros_driver</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,24 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_common)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} INTERFACE)
|
||||
target_include_directories(${PROJECT_NAME} INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_package()
|
||||
+94
@@ -0,0 +1,94 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
struct DeploymentContract
|
||||
{
|
||||
static constexpr std::size_t kObsDim = 53;
|
||||
static constexpr std::size_t kActionDim = 16;
|
||||
static constexpr std::size_t kLegJointCount = 12;
|
||||
static constexpr std::size_t kWheelCount = 4;
|
||||
static constexpr double kPolicyHz = 50.0;
|
||||
static constexpr double kMotorHz = 200.0;
|
||||
static constexpr double kStatusHz = 10.0;
|
||||
|
||||
static constexpr std::array<int, 4> kWheelIndices = {12, 13, 14, 15};
|
||||
static constexpr float kLegKp = 50.0f;
|
||||
static constexpr float kLegKd = 1.5f;
|
||||
static constexpr float kLegHoldKp = 80.0f;
|
||||
static constexpr float kLegHoldKd = 4.0f;
|
||||
static constexpr float kWheelKd = 1.0f;
|
||||
|
||||
static constexpr std::array<int, 16> kCanBusMap = {
|
||||
1, 1, 1, // fl legs
|
||||
1, 1, 1, // fr legs
|
||||
2, 2, 2, // rl legs
|
||||
2, 2, 2, // rr legs
|
||||
1, 1, 2, 2 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<int, 16> kCanIdMap = {
|
||||
1, 2, 3, // fl legs
|
||||
5, 6, 7, // fr legs
|
||||
1, 2, 3, // rl legs
|
||||
5, 6, 7, // rr legs
|
||||
4, 8, 4, 8 // wheels: fl, fr, rl, rr
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDirectionMap = {
|
||||
-1.0f, -1.0f, -1.0f, // fl
|
||||
-1.0f, 1.0f, 1.0f, // fr
|
||||
1.0f, -1.0f, -1.0f, // rl
|
||||
1.0f, 1.0f, 1.0f, // rr
|
||||
-1.0f, 1.0f, -1.0f, 1.0f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kZeroOffsetMap = {
|
||||
0.003f, 0.030f, 0.028f, // fl
|
||||
0.004f, 0.038f, 0.011f, // fr
|
||||
0.019f, -0.034f, 0.025f, // rl
|
||||
-0.001f, 0.039f, 0.018f, // rr
|
||||
0.000f, 0.000f, 0.000f, 0.000f // wheels
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kActionScale = {
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
0.125f, 0.25f, 0.25f,
|
||||
5.0f, 5.0f, 5.0f, 5.0f
|
||||
};
|
||||
|
||||
static constexpr std::array<float, 16> kDefaultDofPos = {
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.550f, -1.125f,
|
||||
0.0f, 0.0f, 0.0f, 0.0f
|
||||
};
|
||||
};
|
||||
|
||||
static constexpr std::array<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
|
||||
+167
@@ -0,0 +1,167 @@
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <ctime>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <mutex>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <system_error>
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class EventLogger
|
||||
{
|
||||
public:
|
||||
static std::string makeRunId()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now();
|
||||
const auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now.time_since_epoch()).count() % 1000;
|
||||
const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
|
||||
const std::tm tm_value = toLocalTm(now_time);
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << "run_"
|
||||
<< std::put_time(&tm_value, "%Y-%m-%d_%H-%M-%S")
|
||||
<< "_"
|
||||
<< std::setw(3) << std::setfill('0') << millis;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
void configure(const std::string & log_dir, const std::string & file_stem)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
log_dir_ = log_dir;
|
||||
file_stem_ = file_stem;
|
||||
}
|
||||
|
||||
std::string componentLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return componentLogPathUnlocked();
|
||||
}
|
||||
|
||||
std::string timelineLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return timelineLogPathUnlocked();
|
||||
}
|
||||
|
||||
std::string summaryLogPath() const
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
return summaryLogPathUnlocked();
|
||||
}
|
||||
|
||||
void log(
|
||||
const std::string & level,
|
||||
const std::string & component,
|
||||
const std::string & event,
|
||||
const std::string & message)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!isConfiguredUnlocked()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string line = buildLogLine(level, component, event, message);
|
||||
appendLineUnlocked(componentLogPathUnlocked(), line);
|
||||
appendLineUnlocked(timelineLogPathUnlocked(), line);
|
||||
}
|
||||
|
||||
void logSummary(const std::string & component, const std::string & message)
|
||||
{
|
||||
std::scoped_lock<std::mutex> lock(mutex_);
|
||||
if (!isConfiguredUnlocked()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string line = buildLogLine("SUMMARY", component, "session_summary", message);
|
||||
appendLineUnlocked(summaryLogPathUnlocked(), line);
|
||||
appendLineUnlocked(timelineLogPathUnlocked(), line);
|
||||
}
|
||||
|
||||
private:
|
||||
static std::tm toLocalTm(std::time_t now_time)
|
||||
{
|
||||
std::tm tm_value{};
|
||||
#ifdef _WIN32
|
||||
localtime_s(&tm_value, &now_time);
|
||||
#else
|
||||
localtime_r(&now_time, &tm_value);
|
||||
#endif
|
||||
return tm_value;
|
||||
}
|
||||
|
||||
static std::string formatTimestamp()
|
||||
{
|
||||
const auto now = std::chrono::system_clock::now();
|
||||
const auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now.time_since_epoch()).count() % 1000;
|
||||
const std::time_t now_time = std::chrono::system_clock::to_time_t(now);
|
||||
const std::tm tm_value = toLocalTm(now_time);
|
||||
|
||||
std::ostringstream oss;
|
||||
oss << std::put_time(&tm_value, "%Y-%m-%d %H:%M:%S")
|
||||
<< '.'
|
||||
<< std::setw(3) << std::setfill('0') << millis;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
static std::string buildLogLine(
|
||||
const std::string & level,
|
||||
const std::string & component,
|
||||
const std::string & event,
|
||||
const std::string & message)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "[" << formatTimestamp() << "]"
|
||||
<< "[" << level << "]"
|
||||
<< "[" << component << "]"
|
||||
<< "[" << event << "] "
|
||||
<< message;
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
bool isConfiguredUnlocked() const
|
||||
{
|
||||
return !log_dir_.empty() && !file_stem_.empty();
|
||||
}
|
||||
|
||||
void appendLineUnlocked(const std::string & path, const std::string & line) const
|
||||
{
|
||||
std::error_code ec;
|
||||
std::filesystem::create_directories(log_dir_, ec);
|
||||
|
||||
std::ofstream stream(path, std::ios::app);
|
||||
if (!stream.is_open()) {
|
||||
return;
|
||||
}
|
||||
stream << line << '\n';
|
||||
}
|
||||
|
||||
std::string componentLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / (file_stem_ + ".log")).string();
|
||||
}
|
||||
|
||||
std::string timelineLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / "timeline.log").string();
|
||||
}
|
||||
|
||||
std::string summaryLogPathUnlocked() const
|
||||
{
|
||||
return (std::filesystem::path(log_dir_) / "summary.log").string();
|
||||
}
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
std::string log_dir_;
|
||||
std::string file_stem_;
|
||||
};
|
||||
|
||||
} // 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
|
||||
+107
@@ -0,0 +1,107 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
#include "sim2real_common/deployment_contract.hpp"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
namespace sim2real_common
|
||||
{
|
||||
|
||||
class StandBalanceController
|
||||
{
|
||||
public:
|
||||
StandBalanceController(double control_dt = 0.02)
|
||||
: control_dt_(control_dt)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void setNominalLegPose(float hip_pitch, float knee)
|
||||
{
|
||||
nominal_hip_pitch_ = hip_pitch;
|
||||
nominal_knee_ = knee;
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
stable_time_ = 0.0f;
|
||||
}
|
||||
|
||||
std::array<float, 16> computeTarget(
|
||||
const std::array<float, 3>& projected_gravity,
|
||||
const std::array<float, 3>& imu_gyro,
|
||||
const std::array<float, 3>& cmd)
|
||||
{
|
||||
const float hip_base = nominal_hip_pitch_;
|
||||
const float knee_base = nominal_knee_;
|
||||
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
estimateRollPitch(projected_gravity, roll, pitch);
|
||||
|
||||
float roll_rate = imu_gyro[0];
|
||||
float pitch_rate = imu_gyro[1];
|
||||
|
||||
float roll_corr = -kp_roll_ * roll - kd_roll_rate_ * roll_rate;
|
||||
|
||||
float lateral_lean = lateral_lean_gain_ * cmd[1];
|
||||
|
||||
std::array<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)));
|
||||
}
|
||||
|
||||
double control_dt_;
|
||||
float nominal_hip_pitch_{DeploymentContract::kDefaultDofPos[1]};
|
||||
float nominal_knee_{DeploymentContract::kDefaultDofPos[2]};
|
||||
float kp_roll_{0.85f};
|
||||
float kd_roll_rate_{0.03f};
|
||||
float lateral_lean_gain_{0.0f};
|
||||
float hip_abduction_clip_{0.45f};
|
||||
std::array<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};
|
||||
|
||||
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()
|
||||
+318
@@ -0,0 +1,318 @@
|
||||
#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 "std_msgs/msg/string.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_state.hpp"
|
||||
#include "sim2real_interfaces/msg/runtime_target.hpp"
|
||||
#include "sim2real_common/event_logger.hpp"
|
||||
#include "sim2real_common/low_pass_filter.hpp"
|
||||
#include "sim2real_common/mahony_filter.hpp"
|
||||
#include "sim2real_common/safety_monitor.hpp"
|
||||
#include "sim2real_common/runtime_guard.hpp"
|
||||
|
||||
struct can_frame;
|
||||
|
||||
namespace sim2real_hw
|
||||
{
|
||||
|
||||
enum class RecoveryKind
|
||||
{
|
||||
None,
|
||||
Stale,
|
||||
NoEffect
|
||||
};
|
||||
|
||||
enum class RecoveryStage
|
||||
{
|
||||
Idle,
|
||||
AwaitInitFeedback,
|
||||
AwaitEffectVerification
|
||||
};
|
||||
|
||||
enum class ActiveModelMode
|
||||
{
|
||||
Rough,
|
||||
Crawl
|
||||
};
|
||||
|
||||
struct MotorConfig
|
||||
{
|
||||
int bus; // 1 or 2
|
||||
int id; // motor CAN id
|
||||
float direction;
|
||||
float offset;
|
||||
};
|
||||
|
||||
struct MotorStateInternal
|
||||
{
|
||||
float position{0.0f};
|
||||
float velocity{0.0f};
|
||||
float torque{0.0f};
|
||||
float temperature{0.0f};
|
||||
float bus_voltage{0.0f};
|
||||
float estimated_current_arms{0.0f};
|
||||
float last_command_sim{0.0f};
|
||||
std::uint32_t update_count{0};
|
||||
std::uint32_t stale_count{0};
|
||||
std::uint32_t command_active_count{0};
|
||||
std::uint32_t no_effect_count{0};
|
||||
std::uint16_t fault_code{0};
|
||||
std::uint16_t fault_detail_1{0};
|
||||
std::uint16_t fault_detail_2{0};
|
||||
// Hold-over state
|
||||
float last_valid_pos{0.0f};
|
||||
float last_valid_vel{0.0f};
|
||||
float last_valid_torque{0.0f};
|
||||
std::uint32_t prev_update_count{0};
|
||||
bool has_valid_data{false};
|
||||
bool has_bus_voltage{false};
|
||||
bool has_fault_snapshot{false};
|
||||
bool stale_reported{false};
|
||||
bool recovered_reported{false};
|
||||
bool disable_reported{false};
|
||||
bool command_effect_monitoring_active{false};
|
||||
bool no_effect_reported{false};
|
||||
bool high_temp_reported{false};
|
||||
bool high_current_reported{false};
|
||||
bool high_voltage_reported{false};
|
||||
bool low_voltage_reported{false};
|
||||
bool fault_code_reported{false};
|
||||
bool init_confirmed{false};
|
||||
std::uint32_t init_attempt_count{0};
|
||||
std::uint32_t recovery_attempt_count{0};
|
||||
std::uint32_t no_effect_recovery_attempt_count{0};
|
||||
std::chrono::steady_clock::time_point last_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_no_effect_recovery_attempt_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_snapshot_time_{};
|
||||
std::chrono::steady_clock::time_point last_diag_request_time_{};
|
||||
std::chrono::steady_clock::time_point recovery_stage_deadline_{};
|
||||
std::uint32_t recovery_start_update_count{0};
|
||||
std::uint32_t recovery_active_attempt_number{0};
|
||||
RecoveryKind recovery_kind{RecoveryKind::None};
|
||||
RecoveryStage recovery_stage{RecoveryStage::Idle};
|
||||
std::string recovery_trigger;
|
||||
std::string last_power_event_reason;
|
||||
};
|
||||
|
||||
class HardwareBridgeNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
HardwareBridgeNode();
|
||||
~HardwareBridgeNode();
|
||||
|
||||
private:
|
||||
void onTarget(const sim2real_interfaces::msg::RuntimeTarget::SharedPtr msg);
|
||||
void onModelStatus(const std_msgs::msg::String::SharedPtr msg);
|
||||
void onReadLoop();
|
||||
void onWriteLoop();
|
||||
void onImu(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
|
||||
|
||||
bool initCan(const std::string& ifname, int& fd);
|
||||
bool sendCanFrame(int fd, std::uint32_t can_id, const std::uint8_t* data, std::uint8_t dlc);
|
||||
bool readCanFrame(int fd, void* frame, int timeout_us);
|
||||
|
||||
bool enableMotor(int fd, int motor_id);
|
||||
bool disableMotor(int fd, int motor_id, bool clear_fault = false);
|
||||
bool writeParameterInt(int fd, int motor_id, std::uint16_t param_id, std::uint32_t value);
|
||||
bool setModeRaw(int fd, int motor_id, std::int8_t mode);
|
||||
bool readParameter(int fd, int motor_id, std::uint16_t param_id);
|
||||
bool writeLimit(int fd, int motor_id, std::uint16_t param_id, float limit);
|
||||
bool writeOperationFrame(int fd, int motor_id, double pos, double vel, double kp, double kd, double torque);
|
||||
bool initializeMotor(std::size_t index, const std::string & reason, int max_attempts = 3);
|
||||
bool initializeMotorsOnBus(int bus_id, const std::string & reason);
|
||||
bool waitForMotorFeedback(std::size_t index, std::chrono::milliseconds timeout);
|
||||
void processCanFrame(const struct can_frame & frame, int bus_id);
|
||||
void drainCanFrames(int fd, int bus_id, int timeout_us);
|
||||
bool isLegMotor(std::size_t index) const;
|
||||
bool isWheelMotor(std::size_t index) const;
|
||||
bool motorHasBlockingFault(std::size_t index) const;
|
||||
bool isNoEffectConditionPresent(std::size_t index) const;
|
||||
std::uint32_t noEffectCommandWarmupCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectTriggerCycles(std::size_t index) const;
|
||||
std::uint32_t noEffectAttemptLimit(std::size_t index) const;
|
||||
std::uint32_t noEffectCooldownMs(std::size_t index) const;
|
||||
std::uint32_t noEffectVerifyTimeoutMs(std::size_t index) const;
|
||||
bool hasFreshNoEffectDiagnostics(std::size_t index) const;
|
||||
void requestMotorDiagnostics(std::size_t index);
|
||||
std::string classifyNoEffectSuspect(std::size_t index) const;
|
||||
std::string buildNoEffectSummary(std::size_t index) const;
|
||||
void updateMotorCommandTracking(std::size_t index, float sim_command, const std::string & target_source);
|
||||
void updateNoEffectDetection(std::size_t index);
|
||||
bool startMotorRecoverySequence(
|
||||
std::size_t index,
|
||||
const std::string & trigger,
|
||||
RecoveryKind kind,
|
||||
std::uint32_t attempt_number);
|
||||
void processMotorRecoverySequence(std::size_t index);
|
||||
void clearMotorRecoverySequence(std::size_t index);
|
||||
bool shouldAttemptMotorRecovery(std::size_t index) const;
|
||||
bool attemptMotorRecovery(std::size_t index, const std::string & trigger);
|
||||
bool shouldAttemptNoEffectRecovery(std::size_t index) const;
|
||||
bool attemptNoEffectRecovery(std::size_t index, const std::string & trigger);
|
||||
const char * jointName(std::size_t index) const;
|
||||
std::string motorTag(std::size_t index) const;
|
||||
float estimateCurrentArms(float torque_nm) const;
|
||||
std::string decodeFaultCode(std::uint16_t fault_code) const;
|
||||
std::string decodeFaultDetailRegister(std::uint16_t register_value, int register_index) const;
|
||||
std::string buildMotorFaultSummary(std::size_t index) const;
|
||||
std::string formatProtectionReason(const std::string & trigger, const std::string & reason) const;
|
||||
void logProtectionEvent(const std::string & trigger, const std::string & reason, const std::string & action);
|
||||
void logMotorPowerEvent(std::size_t index, const std::string & state, const std::string & reason);
|
||||
void logMotorDiagnosticEvent(std::size_t index, const std::string & event, const std::string & reason, const char * level = "WARN");
|
||||
void updateMotorTelemetry(std::size_t index, float pos_sim, float vel_sim, float torque_sim, float temperature_c);
|
||||
void handleParameterResponse(const struct can_frame & frame, int bus_id);
|
||||
void updateMotorDiagnostics(std::size_t index);
|
||||
void pollMotorDiagnostics();
|
||||
void finalizeRunSummary();
|
||||
|
||||
rclcpp::Publisher<sim2real_interfaces::msg::RuntimeState>::SharedPtr state_pub_;
|
||||
rclcpp::Subscription<sim2real_interfaces::msg::RuntimeTarget>::SharedPtr target_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr model_status_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::array<float, 16> rough_default_dof_pos_{};
|
||||
std::array<float, 16> crawl_default_dof_pos_{};
|
||||
std::array<float, 16> active_default_dof_pos_{};
|
||||
ActiveModelMode active_model_mode_{ActiveModelMode::Rough};
|
||||
bool model_switch_active_{false};
|
||||
std::uint32_t target_sequence_{0};
|
||||
std::uint32_t state_sequence_{0};
|
||||
double target_timeout_ms_{150.0};
|
||||
|
||||
// SocketCAN file descriptors
|
||||
int can0_fd_{-1};
|
||||
int can1_fd_{-1};
|
||||
std::string can0_name_{"can0"};
|
||||
std::string can1_name_{"can1"};
|
||||
|
||||
// CAN error recovery
|
||||
static constexpr int kCanErrorThreshold = 50; // consecutive errors before reinit
|
||||
int can0_error_count_{0};
|
||||
int can1_error_count_{0};
|
||||
bool reinitCan(const std::string& ifname, int& fd, int& error_count);
|
||||
|
||||
// Hold-over constants
|
||||
static constexpr std::uint32_t kHoldoverThreshold = 2;
|
||||
static constexpr std::uint32_t kMotorDropReportThreshold = 40;
|
||||
|
||||
// Motor configurations and states
|
||||
std::array<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::SafetyMonitor> model_switch_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};
|
||||
rclcpp::Time last_diag_poll_time_{0, 0, RCL_ROS_TIME};
|
||||
std::size_t diag_poll_motor_index_{0};
|
||||
|
||||
// Telemetry
|
||||
std::uint32_t fresh_count_{0};
|
||||
std::uint32_t holdover_count_{0};
|
||||
std::uint32_t stale_max_{0};
|
||||
std::uint32_t holdover_events_total_{0};
|
||||
std::uint32_t protection_trigger_count_{0};
|
||||
std::uint32_t motor_drop_event_count_{0};
|
||||
std::uint32_t motor_recover_event_count_{0};
|
||||
std::uint32_t motor_fault_event_count_{0};
|
||||
bool timeout_hold_logged_{false};
|
||||
bool clip_active_logged_{false};
|
||||
|
||||
bool dry_run_{false};
|
||||
std::atomic<bool> estop_triggered_{false};
|
||||
std::atomic<bool> safety_enabled_{true};
|
||||
std::atomic<bool> safety_triggered_{false};
|
||||
std::string safety_reason_{""};
|
||||
sim2real_common::EventLogger event_logger_;
|
||||
std::string run_log_dir_;
|
||||
float motor_temp_warn_c_{100.0f};
|
||||
float motor_temp_fault_c_{135.0f};
|
||||
float motor_bus_overvoltage_v_{60.0f};
|
||||
float motor_bus_undervoltage_v_{12.0f};
|
||||
float motor_current_warn_arms_{10.5f};
|
||||
float motor_current_peak_arms_{14.0f};
|
||||
float motor_torque_warn_nm_{13.0f};
|
||||
float motor_torque_peak_nm_{17.0f};
|
||||
double diag_poll_period_s_{0.10};
|
||||
float wheel_no_effect_command_threshold_{1.0f};
|
||||
float wheel_no_effect_min_response_ratio_{0.20f};
|
||||
float wheel_no_effect_velocity_epsilon_{0.25f};
|
||||
float wheel_no_effect_max_temperature_c_{90.0f};
|
||||
float wheel_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t wheel_no_effect_command_warmup_cycles_{12};
|
||||
std::uint32_t wheel_no_effect_trigger_cycles_{30};
|
||||
std::uint32_t wheel_no_effect_attempt_limit_{2};
|
||||
std::uint32_t wheel_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t wheel_recovery_verify_timeout_ms_{180};
|
||||
std::uint32_t wheel_no_effect_diag_freshness_ms_{350};
|
||||
std::uint32_t wheel_no_effect_diag_request_period_ms_{80};
|
||||
float leg_no_effect_position_error_threshold_{0.18f};
|
||||
float leg_no_effect_velocity_epsilon_{0.12f};
|
||||
float leg_no_effect_max_estimated_current_arms_{4.0f};
|
||||
float leg_no_effect_max_abs_torque_nm_{5.0f};
|
||||
float leg_no_effect_max_temperature_c_{100.0f};
|
||||
float leg_no_effect_min_bus_voltage_v_{18.0f};
|
||||
std::uint32_t leg_no_effect_command_warmup_cycles_{40};
|
||||
std::uint32_t leg_no_effect_trigger_cycles_{25};
|
||||
std::uint32_t leg_no_effect_attempt_limit_{2};
|
||||
std::uint32_t leg_no_effect_cooldown_ms_{1200};
|
||||
std::uint32_t leg_recovery_verify_timeout_ms_{220};
|
||||
|
||||
void onEstop(const std_msgs::msg::Bool::SharedPtr msg);
|
||||
void logEvent(
|
||||
const std::string & level,
|
||||
const std::string & event,
|
||||
const std::string & message);
|
||||
};
|
||||
|
||||
} // namespace sim2real_hw
|
||||
@@ -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>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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()
|
||||
+155
@@ -0,0 +1,155 @@
|
||||
name: test_route
|
||||
map: map_b
|
||||
frame_id: map
|
||||
yaw_tolerance_deg_default: 30.0
|
||||
segments:
|
||||
- name: segment_1
|
||||
obstacle: slalom
|
||||
waypoints:
|
||||
- id: 1
|
||||
x: 5.3590
|
||||
y: 2.4420
|
||||
yaw_deg: 0.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 2
|
||||
x: 6.9240
|
||||
y: 2.3880
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 3
|
||||
x: 6.8340
|
||||
y: 0.9200
|
||||
yaw_deg: -5.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 4
|
||||
x: 9.3680
|
||||
y: 0.7060
|
||||
yaw_deg: -3.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 5
|
||||
x: 11.3730
|
||||
y: 0.4300
|
||||
yaw_deg: 0.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 6
|
||||
x: 12.8770
|
||||
y: 0.4320
|
||||
yaw_deg: 90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 7
|
||||
x: 12.9350
|
||||
y: 2.2410
|
||||
yaw_deg: 88.2
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 8
|
||||
x: 12.9910
|
||||
y: 4.2770
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 9
|
||||
x: 12.0040
|
||||
y: 4.3390
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 10
|
||||
x: 11.8720
|
||||
y: 2.5830
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 11
|
||||
x: 9.5719
|
||||
y: 2.7972
|
||||
yaw_deg: 174.7
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 12
|
||||
x: 9.5080
|
||||
y: 3.5620
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 13
|
||||
x: 8.4140
|
||||
y: 3.5730
|
||||
yaw_deg: 125.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 14
|
||||
x: 7.8670
|
||||
y: 4.5280
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 15
|
||||
x: 5.8060
|
||||
y: 4.7430
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.15
|
||||
- id: 16
|
||||
x: 4.9260
|
||||
y: 4.7850
|
||||
yaw_deg: 180.0
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 17
|
||||
x: 3.2951
|
||||
y: 4.7963
|
||||
yaw_deg: 179.6
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 18
|
||||
x: 3.0920
|
||||
y: 4.7770
|
||||
yaw_deg: -180.0
|
||||
speed: 0.35
|
||||
policy: ik
|
||||
tolerance: 0.10
|
||||
- id: 19
|
||||
x: 1.6940
|
||||
y: 4.8860
|
||||
yaw_deg: 210.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 20
|
||||
x: 1.0550
|
||||
y: 4.5480
|
||||
yaw_deg: -90.0
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
- id: 21
|
||||
x: 1.0420
|
||||
y: 2.9200
|
||||
yaw_deg: -90.4
|
||||
speed: 0.35
|
||||
policy: rough
|
||||
tolerance: 0.10
|
||||
@@ -0,0 +1,16 @@
|
||||
frame_id: map
|
||||
goals:
|
||||
dock:
|
||||
position: [0.0, 0.0, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "dock"
|
||||
|
||||
test_p1:
|
||||
position: [4.73, 1.75, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "test point 1"
|
||||
|
||||
test_p2:
|
||||
position: [4.94, 0.40, 0.0]
|
||||
yaw_deg: 0.0
|
||||
description: "test point 2"
|
||||
+8
@@ -0,0 +1,8 @@
|
||||
missions:
|
||||
test_roundtrip:
|
||||
description: "test_p1 -> test_p2 -> test_p1"
|
||||
loop: false
|
||||
goals:
|
||||
- test_p1
|
||||
- test_p2
|
||||
- test_p1
|
||||
@@ -0,0 +1,232 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
odom_frame_id: "odom"
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: -1.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
transform_tolerance: 1.0
|
||||
update_min_d: 0.25
|
||||
update_min_a: 0.2
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: "scan"
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_frame: odom
|
||||
default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
|
||||
plugin_lib_names:
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_recompute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_goal_updater_node
|
||||
- nav2_recovery_node
|
||||
- nav2_pipeline_sequence_node
|
||||
- nav2_round_robin_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
controller_frequency: 10.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.001
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"]
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
|
||||
general_goal_checker:
|
||||
stateful: true
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
prune_plan: true
|
||||
prune_distance: 1.0
|
||||
debug_trajectory_details: false
|
||||
trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator"
|
||||
velocity_iterator_name: "dwb_plugins::LimitedVelocityIterator"
|
||||
critics: ["ObstacleFootprint", "PathAlign", "GoalAlign", "PathDist", "GoalDist"]
|
||||
|
||||
# DWB Velocity parameters matching Units
|
||||
min_vel_x: 0.0
|
||||
max_vel_x: 0.6
|
||||
min_vel_y: 0.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 2.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.6
|
||||
min_speed_theta: 0.0
|
||||
|
||||
# DWB Acceleration parameters matching Units
|
||||
acc_lim_x: 15.0
|
||||
acc_lim_y: 15.0
|
||||
acc_lim_theta: 12.0
|
||||
decel_lim_x: -15.0
|
||||
decel_lim_y: -15.0
|
||||
decel_lim_theta: -12.0
|
||||
|
||||
# Critics tuning
|
||||
ObstacleFootprint.scale: 0.2
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 1.0
|
||||
use_sim_time: false
|
||||
planner_plugins: ["GridTransition"]
|
||||
GridTransition:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: true
|
||||
allow_unknown: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
recovery_plugins: ["spin", "backup", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors::Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors::Backup"
|
||||
wait:
|
||||
plugin: "nav2_behaviors::Wait"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 1.0
|
||||
update_frequency: 1.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 30
|
||||
height: 30
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.6
|
||||
cost_scaling_factor: 4.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
use_sim_time: false
|
||||
robot_radius: 0.25
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
publish_frequency: 5.0
|
||||
update_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 4
|
||||
height: 4
|
||||
resolution: 0.05
|
||||
plugins: ["obstacle_layer", "inflation_layer"]
|
||||
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: true
|
||||
observation_sources: pointcloud
|
||||
pointcloud:
|
||||
topic: /odin1/cloud_slam
|
||||
sensor_frame: base_link
|
||||
data_type: "PointCloud2"
|
||||
clearing: true
|
||||
marking: true
|
||||
max_obstacle_height: 2.0
|
||||
min_obstacle_height: 0.05
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.1
|
||||
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
enabled: true
|
||||
inflation_radius: 0.4
|
||||
cost_scaling_factor: 4.0
|
||||
@@ -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>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user