Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 zeitvex
|
b082046b15 |
@@ -30,6 +30,10 @@ log/
|
||||
!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
|
||||
!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
|
||||
|
||||
# Required vendored Odin SDK libraries in the final ROS 2 deployment
|
||||
!05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_arm.a
|
||||
!05_software/real/sim2real_ros2/src/odin_ros_driver/lib/liblydHostApi_amd.a
|
||||
|
||||
# Training outputs
|
||||
logs/
|
||||
checkpoints/
|
||||
|
||||
@@ -14,6 +14,7 @@
|
||||
| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
|
||||
| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
|
||||
| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
|
||||
| `v0.9.0` | 最终比赛部署 | ROS 2/C++ 真机闭环、Odin、CAN、导航与屏幕 UI |
|
||||
|
||||
## `v0.4.0` 的模型变化
|
||||
|
||||
@@ -71,3 +72,13 @@
|
||||
- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
|
||||
- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
|
||||
- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。
|
||||
|
||||
## `v0.9.0` 的最终比赛部署
|
||||
|
||||
- 归档比赛得分 1050 所对应的 `last_not_slalom_1050` ROS 2 工作区；1050 是成绩，不是策略编号。
|
||||
- 保留 53D→16D C++ 策略运行时、200 Hz CAN 硬件桥、命令仲裁、安全监控和统一启动包。
|
||||
- 保留 Rough `model_6800`、Wall `model_84` 的 ONNX 与比赛 TensorRT engine；Crawl 使用 IK 后端。
|
||||
- 保留 Odin ROS 驱动及 Apache-2.0 许可证、五份比赛路线、抽样 PCD 和 Orin 触控屏 UI。
|
||||
- 排除嵌套 Git、缓存、日志、备份、候选模型、开发草稿和重复地图工具。
|
||||
- 原始备份缺少配置所引用的 Odin `1hao.bin`，因此重定位模式仍需从比赛设备补回该外部资产；纯里程计模式不受此限制。
|
||||
- 自研 ROS 包仍保留原工程的 `Proprietary` 清单字段，公开到 GitHub 前必须由权利人统一选择开源许可证。
|
||||
|
||||
@@ -8,7 +8,8 @@
|
||||
│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
|
||||
└─ real/
|
||||
├─ ik_real/ # IK 轨迹与早期真机控制
|
||||
└─ sim2real/ # 第一代 Python 策略真机部署
|
||||
├─ sim2real/ # 第一代 Python 策略真机部署
|
||||
└─ sim2real_ros2/ # 最终比赛 ROS 2/C++ 真机部署
|
||||
```
|
||||
|
||||
## 数据流
|
||||
@@ -24,11 +25,13 @@ MJCF + mjlab task
|
||||
+----> Sim2Sim 策略验证
|
||||
|
|
||||
+----> Python Sim2Real ----> 电机 / IMU
|
||||
|
|
||||
+----> ROS 2/C++ Sim2Real -> CAN / Odin / 导航 / 屏幕
|
||||
|
||||
IK real --------------------------------> 电机
|
||||
```
|
||||
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
|
||||
`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`，最终比赛 ROS 2 部署见 `v0.9.0`。
|
||||
|
||||
详细说明见：
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# 第一代真机控制
|
||||
# 真机控制与部署
|
||||
|
||||
本目录保存 16DOF 轮足机器人的早期真机控制实现。
|
||||
本目录保存 16DOF 轮足机器人从早期接口验证到最终比赛 ROS 2 部署的演进。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
@@ -20,6 +20,18 @@
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
|
||||
## `sim2real_ros2`
|
||||
|
||||
`last_not_slalom_1050` 最终比赛工程的规范化归档，包含：
|
||||
|
||||
- ROS 2 Humble + C++ 运行时
|
||||
- 50 Hz 策略推理与 200 Hz CAN 电机热路径
|
||||
- Rough `model_6800`、Wall `model_84` 和 Crawl IK 模式
|
||||
- Odin IMU/里程计驱动、简单导航、命令仲裁和触控屏 UI
|
||||
- 比赛路线、抽样 PCD、Docker 与部署说明
|
||||
|
||||
`1050` 是比赛得分，不是模型编号。完整入口与缺失的 Odin 重定位地图边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
|
||||
|
||||
## 实机记录
|
||||
|
||||
[](../../06_assets/videos/early_sim2real.mp4)
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
logs_v2_web/
|
||||
map/load/
|
||||
src/odin_ros_driver/log/
|
||||
src/odin_ros_driver/recorddata/
|
||||
src/odin_ros_driver/image/
|
||||
*.bak_*
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
.colcon/
|
||||
.vscode/
|
||||
compile_commands.json
|
||||
@@ -0,0 +1,177 @@
|
||||
# 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`（默认：`false`）：按需启动 ROS2 Navigation2；比赛默认使用 `simple_nav_node.py` 的路线跟踪。
|
||||
|
||||
#### 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,79 @@
|
||||
# 使用 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 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,104 @@
|
||||
# ROS 2 最终比赛 Sim2Real
|
||||
|
||||
本目录归档 `last_not_slalom_1050` 真机工程，对应 RC_WheelLeg 在 RoboCon 仿生足式障碍赛使用的最终 ROS 2 部署栈。`1050` 是比赛得分，不是模型编号；比赛 Rough 策略为 `model_6800.onnx`。
|
||||
|
||||
该里程碑计划标记为 `v0.9.0`。训练架构和策略来源见 `v0.6.0`，比赛 Rough 模型首次归档见 `v0.8.0`，导航打点与路线演进见 `v0.8.1`。
|
||||
|
||||
## 系统闭环
|
||||
|
||||
```text
|
||||
Odin IMU / Odom ──> hardware bridge ──> RuntimeState
|
||||
|
|
||||
导航 / 遥控 / 屏幕 ──> cmd mux ──> policy runtime (50 Hz)
|
||||
|
|
||||
RuntimeTarget
|
||||
|
|
||||
hardware bridge / CAN (200 Hz)
|
||||
```
|
||||
|
||||
核心约束：
|
||||
|
||||
- 53 维策略观测、16 维动作输出。
|
||||
- Rough：`model_6800`，优先 TensorRT，失败时回退 ONNX Runtime。
|
||||
- Wall：`model_84`，同样保留 TensorRT 与 ONNX 两种文件。
|
||||
- Crawl：比赛配置使用解析 IK，不加载 Crawl RL 权重。
|
||||
- 默认站姿：髋俯仰 `0.550`、膝关节 `-1.125`。
|
||||
- 默认命令源：`NAV`；默认定位模式：`relocal`。
|
||||
|
||||
## 目录
|
||||
|
||||
```text
|
||||
sim2real_ros2/
|
||||
├─ src/
|
||||
│ ├─ sim2real_interfaces/ # RuntimeState / RuntimeTarget 消息
|
||||
│ ├─ sim2real_common/ # 部署契约、滤波、平衡和安全监控
|
||||
│ ├─ sim2real_hw/ # SocketCAN、IMU 和 200 Hz 电机热路径
|
||||
│ ├─ sim2real_runtime/ # 策略、命令仲裁、导航、Web API
|
||||
│ ├─ sim2real_nav2/ # Nav2 配置入口
|
||||
│ ├─ sim2real_bringup/ # 统一参数和启动文件
|
||||
│ └─ odin_ros_driver/ # Odin ROS 驱动（Apache-2.0）
|
||||
├─ policies/ # 比赛实际使用的 Rough / Wall 模型
|
||||
├─ map/ # 比赛路线和抽样 PCD
|
||||
├─ screen/ # Orin 800×600 触控面板
|
||||
├─ docs/ # 架构、遥控、Web 和迁移说明
|
||||
├─ Dockerfile
|
||||
└─ start_sim2real.sh
|
||||
```
|
||||
|
||||
## 构建与运行
|
||||
|
||||
目标环境是 Ubuntu 22.04、ROS 2 Humble 和 Jetson Orin。系统依赖和 Docker 流程见 [`DEPLOYMENT_GUIDE.md`](DEPLOYMENT_GUIDE.md)。
|
||||
|
||||
```bash
|
||||
cd 05_software/real/sim2real_ros2
|
||||
colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
./start_sim2real.sh
|
||||
```
|
||||
|
||||
运行参数和模型/路线均使用工作区根目录相对路径，因此应从本目录启动。常用启动覆盖：
|
||||
|
||||
```bash
|
||||
# 纯里程计模式，不等待 Odin 重定位地图
|
||||
./start_sim2real.sh localization_mode:=odom \
|
||||
odin_config_file:=src/odin_ros_driver/config/control_command_odom.yaml
|
||||
|
||||
# 禁止驱动，仅做软件链路检查
|
||||
./start_sim2real.sh launch_driver:=false launch_remote:=false
|
||||
```
|
||||
|
||||
## 必须补充的部署资产
|
||||
|
||||
最终源目录配置引用了 Odin `map/1hao.bin`，但工作区备份中不存在这个文件；全盘检索也未找到同名文件。为避免用来源不明的 `.bin` 冒充比赛地图，本仓库不伪造该资产。
|
||||
|
||||
使用 `relocal` 前必须：
|
||||
|
||||
1. 从比赛 Orin 或 Odin 建图备份取得真实 `1hao.bin`。
|
||||
2. 修改 `src/odin_ros_driver/config/control_command_relocal.yaml` 中的 `relocalization_map_abs_path` 为目标机绝对路径。
|
||||
3. 核对文件哈希并在发布说明中补充来源。
|
||||
|
||||
缺少该文件时请使用 `localization_mode:=odom`，不要宣称重定位闭环已复现。地图和路线边界见 [`map/README.md`](map/README.md)。
|
||||
|
||||
## 归档边界
|
||||
|
||||
已保留：
|
||||
|
||||
- 最终六个 ROS 2 包、Odin 驱动源码、比赛设备标定参数和预编译 SDK 静态库。
|
||||
- 最终 Rough/Wall ONNX 与比赛机 TensorRT engine。
|
||||
- 五份最终工程路线、1 号场地抽样 PCD、屏幕 UI 和启动脚本。
|
||||
- Odin 驱动 Apache-2.0 许可证。
|
||||
|
||||
未保留：
|
||||
|
||||
- 嵌套 `.git`、`__pycache__`、日志、备份、构建/安装目录。
|
||||
- 未被比赛配置引用的候选模型与候选 TensorRT engine。
|
||||
- 开发计划、任务草稿、重复地图工具和运行时轨迹。
|
||||
- 原备份中大小为 0 的浏览器静态页面；HTTP JSON API 和屏幕 UI 源码仍保留。
|
||||
|
||||
TensorRT engine 与 JetPack、TensorRT 版本及 GPU 架构有关；其他机器应从同名 ONNX 重新生成，不应默认复用比赛 engine。模型哈希见 [`policies/README.md`](policies/README.md)。
|
||||
|
||||
## 安全与开源状态
|
||||
|
||||
- 真机运行前必须架空轮组验证 CAN 映射、方向、零位、急停和限幅。
|
||||
- `deployment_contract.hpp` 是电机映射和动作缩放真值源；参考 YAML 不会自动修改 C++ 契约。
|
||||
- 自研 ROS 包的 `package.xml` 仍保留原工程的 `Proprietary` 字段。迁移到 GitHub 公共开源前，需要由项目负责人选择许可证并统一修改；本次整理不代替权利人作许可证决定。
|
||||
- 当前 Windows 环境只能做静态检查，不能证明 ROS 2、SocketCAN、Odin SDK 或 TensorRT 真机运行成功。
|
||||
@@ -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,499 @@
|
||||
# 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_estop_channel: 7
|
||||
remote_estop_level: "high"
|
||||
remote_estop_debounce_frames: 3
|
||||
remote_estop_require_remote_mode: true
|
||||
remote_poll_hz: 50.0
|
||||
```
|
||||
|
||||
如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
或改成实际设备路径。
|
||||
|
||||
## 4. 启动前检查
|
||||
|
||||
### 4.1 确认串口存在
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
如果使用默认配置，应能看到：
|
||||
|
||||
```bash
|
||||
/dev/ttyACM0
|
||||
```
|
||||
|
||||
### 4.2 确认串口权限
|
||||
|
||||
如果节点提示串口权限不足，可以临时执行：
|
||||
|
||||
```bash
|
||||
sudo chmod 666 /dev/ttyACM0
|
||||
```
|
||||
|
||||
更推荐的长期方式是把当前用户加入 `dialout` 组：
|
||||
|
||||
```bash
|
||||
sudo usermod -aG dialout $USER
|
||||
```
|
||||
|
||||
然后重新登录。
|
||||
|
||||
### 4.3 确认 Python serial 依赖
|
||||
|
||||
节点依赖 `pyserial`。如果系统没有安装：
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install -y python3-serial
|
||||
```
|
||||
|
||||
## 5. 构建
|
||||
|
||||
如果刚修改过代码或参数，建议重新构建相关包：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
|
||||
```
|
||||
|
||||
构建完成后 source 环境：
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
确认可执行节点存在：
|
||||
|
||||
```bash
|
||||
ros2 pkg executables sim2real_runtime
|
||||
```
|
||||
|
||||
应包含：
|
||||
|
||||
```text
|
||||
sim2real_runtime remote_uart_node.py
|
||||
```
|
||||
|
||||
## 6. 推荐启动方式
|
||||
|
||||
### 6.1 启动完整系统，不启动 Nav2
|
||||
|
||||
这是你当前常用方式：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
|
||||
|
||||
等价完整写法：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
|
||||
```
|
||||
|
||||
### 6.2 不启动遥控器
|
||||
|
||||
如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
|
||||
```
|
||||
|
||||
## 7. 单独启动遥控器节点
|
||||
|
||||
如果系统已经在运行，只想单独测试遥控器节点：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
如果要临时指定串口：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args \
|
||||
--params-file src/sim2real_bringup/config/runtime.yaml \
|
||||
-p remote_port:=/dev/ttyUSB0
|
||||
```
|
||||
|
||||
## 8. 执行后会产生什么效果
|
||||
|
||||
启动以下命令后：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
系统会产生以下效果。
|
||||
|
||||
### 8.1 启动硬件桥接节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开 `can0` 和 `can1`。
|
||||
2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
|
||||
3. 设置电机 MIT 模式。
|
||||
4. 设置电机速度限制和力矩限制。
|
||||
5. 以 `200Hz` 运行硬件读写循环。
|
||||
6. 发布 `/runtime/state`。
|
||||
7. 订阅 `/runtime/target` 执行策略目标。
|
||||
|
||||
### 8.2 启动策略运行节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_runtime_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 加载 ONNX 策略模型。
|
||||
2. 订阅 `/runtime/state`。
|
||||
3. 订阅 `/cmd_vel`。
|
||||
4. 订阅 `/safety/estop`。
|
||||
5. 执行启动站立流程：
|
||||
- `boot_hold`
|
||||
- `startup_soft_hold`
|
||||
- `startup_hold`
|
||||
- `runtime_zero_hold`
|
||||
- `runtime_policy`
|
||||
6. 以 `50Hz` 发布 `/runtime/target`。
|
||||
|
||||
### 8.3 启动遥控器节点
|
||||
|
||||
节点：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
```
|
||||
|
||||
效果：
|
||||
|
||||
1. 打开默认串口 `/dev/ttyACM0`。
|
||||
2. 以 `50Hz` 轮询 SBUS 数据。
|
||||
3. 遥控器摇杆居中时持续发布零速度：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = 0.0
|
||||
linear.y = 0.0
|
||||
angular.z = 0.0
|
||||
```
|
||||
|
||||
4. 推动遥控器时发布非零速度，例如：
|
||||
|
||||
```text
|
||||
/cmd_vel:
|
||||
linear.x = vx
|
||||
linear.y = vy
|
||||
angular.z = yaw
|
||||
```
|
||||
|
||||
5. 当 CH7 打到高位时发布：
|
||||
|
||||
```text
|
||||
/safety/estop: true
|
||||
```
|
||||
|
||||
由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
|
||||
|
||||
### 8.4 机器人行为效果
|
||||
|
||||
正常启动后，机器人不会立即按策略行走，而是按阶段执行：
|
||||
|
||||
1. 电机使能。
|
||||
2. 读取当前关节位置。
|
||||
3. 软保持当前姿态。
|
||||
4. 平滑过渡到默认站立姿态。
|
||||
5. 稳定后进入 runtime。
|
||||
6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
|
||||
7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
|
||||
|
||||
也就是说：
|
||||
|
||||
| 遥控器状态 | 机器人效果 |
|
||||
|---|---|
|
||||
| 摇杆居中 | 站立保持，不主动行走 |
|
||||
| CH2 前后推动 | 前进/后退 |
|
||||
| CH4 左右推动 | 横向移动 |
|
||||
| CH1 左右推动 | 原地转向 |
|
||||
| CH7 高位 | 软件急停，进入安全刹车 |
|
||||
|
||||
## 9. 如何确认遥控器已经生效
|
||||
|
||||
### 9.1 查看节点是否存在
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
```
|
||||
|
||||
应看到：
|
||||
|
||||
```text
|
||||
/sim2real_remote_uart_node
|
||||
/sim2real_runtime_node
|
||||
/sim2real_hw_node
|
||||
```
|
||||
|
||||
### 9.2 查看 `/cmd_vel`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
|
||||
|
||||
### 9.3 查看 `/safety/estop`
|
||||
|
||||
```bash
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
|
||||
CH7 高位时应看到：
|
||||
|
||||
```yaml
|
||||
data: true
|
||||
```
|
||||
|
||||
### 9.4 查看策略目标阶段
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
常见输出含义：
|
||||
|
||||
| `target_source` | 含义 |
|
||||
|---|---|
|
||||
| `boot_hold` | 刚启动，保持初始姿态 |
|
||||
| `startup_soft_hold` | 启动软保持 |
|
||||
| `startup_hold` | 正在站立或站立后保持 |
|
||||
| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
|
||||
| `runtime_policy` | 遥控器有输入，策略已经介入 |
|
||||
| `safety_brake` | 安全刹车 |
|
||||
| `timeout_hold` | 目标超时，硬件保持默认姿态 |
|
||||
|
||||
### 9.5 查看完整目标状态
|
||||
|
||||
```bash
|
||||
ros2 topic echo --once /runtime/target
|
||||
```
|
||||
|
||||
重点关注字段：
|
||||
|
||||
```yaml
|
||||
target_source:
|
||||
zero_command:
|
||||
runtime_released:
|
||||
release_alpha:
|
||||
command:
|
||||
raw_command:
|
||||
```
|
||||
|
||||
如果遥控器摇杆有输入，通常会看到：
|
||||
|
||||
```yaml
|
||||
target_source: runtime_policy
|
||||
zero_command: false
|
||||
runtime_released: true
|
||||
release_alpha: 1.0
|
||||
```
|
||||
|
||||
## 10. 常见问题
|
||||
|
||||
### 10.1 启动后提示无法打开串口
|
||||
|
||||
可能原因：
|
||||
|
||||
1. 串口路径不对。
|
||||
2. 权限不足。
|
||||
3. 设备没有插好。
|
||||
4. 设备被其他程序占用。
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ls /dev/ttyACM* /dev/ttyUSB*
|
||||
```
|
||||
|
||||
修改 `runtime.yaml`：
|
||||
|
||||
```yaml
|
||||
remote_port: "/dev/ttyUSB0"
|
||||
```
|
||||
|
||||
### 10.2 `/cmd_vel` 没有变化
|
||||
|
||||
检查：
|
||||
|
||||
```bash
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
如果节点存在但无变化，可能是：
|
||||
|
||||
1. 遥控器没有输出 SBUS。
|
||||
2. 串口波特率不对。
|
||||
3. SBUS 接线错误。
|
||||
4. 遥控器通道未校准。
|
||||
5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
|
||||
|
||||
### 10.3 摇杆方向反了
|
||||
|
||||
修改：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: true
|
||||
remote_invert_vy: false
|
||||
remote_invert_yaw: true
|
||||
```
|
||||
|
||||
例如前后方向反了，就切换：
|
||||
|
||||
```yaml
|
||||
remote_invert_vx: false
|
||||
```
|
||||
|
||||
### 10.4 急停后不恢复
|
||||
|
||||
当前配置：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: true
|
||||
```
|
||||
|
||||
这表示急停锁存。触发后建议：
|
||||
|
||||
1. 先确认机器人物理安全。
|
||||
2. 停止 launch。
|
||||
3. 将 CH7 打回安全位置。
|
||||
4. 重新启动系统。
|
||||
|
||||
如果需要非锁存模式，可以改为：
|
||||
|
||||
```yaml
|
||||
remote_estop_latch: false
|
||||
```
|
||||
|
||||
但实机调试时更建议使用锁存模式。
|
||||
|
||||
## 11. 快速验证命令清单
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
|
||||
ros2 node list
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
|
||||
如果只测遥控器，不启动电机系统：
|
||||
|
||||
```bash
|
||||
ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
|
||||
```
|
||||
|
||||
另开终端：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
ros2 topic echo /safety/estop
|
||||
```
|
||||
@@ -0,0 +1,246 @@
|
||||
# sim2real_ros2 Web UDP 调试说明
|
||||
|
||||
本文档说明本次新增的最小 Web 调试链路。
|
||||
|
||||
## 1. 架构
|
||||
|
||||
```text
|
||||
Windows 本地浏览器/HTTP 服务
|
||||
|
|
||||
| UDP JSON
|
||||
v
|
||||
Nano: sim2real_web_udp_bridge_node.py
|
||||
|
|
||||
| ROS 2 topics
|
||||
v
|
||||
sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
|
||||
```
|
||||
|
||||
Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
|
||||
|
||||
## 2. 新增 ROS2 节点
|
||||
|
||||
### `remote_uart_node.py`
|
||||
|
||||
遥控器节点现在发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
```
|
||||
|
||||
不再直接发布 `/cmd_vel`。
|
||||
|
||||
通道触发阈值改为：
|
||||
|
||||
```yaml
|
||||
remote_axis_deadzone: 40
|
||||
remote_active_threshold: 40
|
||||
```
|
||||
|
||||
只有通道归一化值绝对值大于 `40` 才认为是有效输入。
|
||||
|
||||
### `cmd_mux_node.py`
|
||||
|
||||
输入：
|
||||
|
||||
```text
|
||||
/cmd_vel_remote
|
||||
/cmd_vel_web
|
||||
/cmd_vel_nav
|
||||
/control/mode
|
||||
/remote/enabled
|
||||
/web/enabled
|
||||
/nav/enabled
|
||||
/safety/estop
|
||||
```
|
||||
|
||||
输出：
|
||||
|
||||
```text
|
||||
/cmd_vel
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
控制模式：
|
||||
|
||||
```text
|
||||
DISABLED
|
||||
REMOTE
|
||||
WEB
|
||||
NAV
|
||||
```
|
||||
|
||||
急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
|
||||
|
||||
### `web_udp_bridge_node.py`
|
||||
|
||||
Nano 端 UDP 监听：
|
||||
|
||||
```text
|
||||
0.0.0.0:15000
|
||||
```
|
||||
|
||||
发布：
|
||||
|
||||
```text
|
||||
/cmd_vel_web
|
||||
/safety/estop
|
||||
/control/mode
|
||||
/web/enabled
|
||||
/remote/enabled
|
||||
/nav/enabled
|
||||
```
|
||||
|
||||
订阅并回传状态：
|
||||
|
||||
```text
|
||||
/runtime/state
|
||||
/runtime/target
|
||||
/cmd_vel
|
||||
/safety/estop
|
||||
/control/mode_state
|
||||
/control/mux_status
|
||||
```
|
||||
|
||||
## 3. Nano 启动
|
||||
|
||||
```bash
|
||||
cd /path/to/sim2real_ros2
|
||||
source /opt/ros/humble/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
|
||||
```
|
||||
|
||||
默认会启动：
|
||||
|
||||
```text
|
||||
sim2real_remote_uart_node
|
||||
sim2real_cmd_mux_node
|
||||
sim2real_web_udp_bridge_node
|
||||
```
|
||||
|
||||
如果不想启动 Web UDP bridge：
|
||||
|
||||
```bash
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
|
||||
```
|
||||
|
||||
## 4. Windows 本地 Web 启动
|
||||
|
||||
把目录复制到 Windows 或通过共享目录访问：
|
||||
|
||||
```text
|
||||
tools/win_web_debug
|
||||
```
|
||||
|
||||
在 Windows 上安装 Python 3 后运行：
|
||||
|
||||
```bash
|
||||
python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
|
||||
```
|
||||
|
||||
浏览器打开：
|
||||
|
||||
```text
|
||||
http://127.0.0.1:8088
|
||||
```
|
||||
|
||||
## 5. UDP 命令格式
|
||||
|
||||
### 切换模式
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"REMOTE"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"WEB"}
|
||||
```
|
||||
|
||||
```json
|
||||
{"type":"mode","mode":"DISABLED"}
|
||||
```
|
||||
|
||||
### Web 速度控制
|
||||
|
||||
```json
|
||||
{
|
||||
"type": "cmd_vel",
|
||||
"linear": {"x": 0.2, "y": 0.0, "z": 0.0},
|
||||
"angular": {"x": 0.0, "y": 0.0, "z": 0.1}
|
||||
}
|
||||
```
|
||||
|
||||
Nano 端会再次限幅：
|
||||
|
||||
```text
|
||||
vx <= ±0.8 m/s
|
||||
vy <= ±0.3 m/s
|
||||
yaw <= ±0.5 rad/s
|
||||
```
|
||||
|
||||
### 零速度
|
||||
|
||||
```json
|
||||
{"type":"zero"}
|
||||
```
|
||||
|
||||
### 软急停
|
||||
|
||||
```json
|
||||
{"type":"estop","data":true}
|
||||
```
|
||||
|
||||
## 6. 安全保护
|
||||
|
||||
当前最小版本已经包含：
|
||||
|
||||
1. 遥控器误触发阈值：`40`。
|
||||
2. 遥控器/Web/Nav 互斥控制模式。
|
||||
3. `cmd_mux` 二次限幅。
|
||||
4. `cmd_mux` 加速度限制。
|
||||
5. Web UDP 超时自动发布零速度。
|
||||
6. 急停优先级最高。
|
||||
7. Web 页面切换到 `WEB` 模式需要确认。
|
||||
8. Web 松开虚拟摇杆会自动发送零速度。
|
||||
|
||||
建议实机调试流程：
|
||||
|
||||
1. 先点击 `DISABLED`。
|
||||
2. 确认 `/cmd_vel` 为零。
|
||||
3. 如果使用遥控器，点击 `REMOTE`。
|
||||
4. 如果使用 Web，点击 `WEB` 并确认周围安全。
|
||||
5. 一旦异常，立即点击 `软急停`。
|
||||
|
||||
## 7. 验证命令
|
||||
|
||||
查看最终输出速度：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel
|
||||
```
|
||||
|
||||
查看遥控器输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_remote
|
||||
```
|
||||
|
||||
查看 Web 输入：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /cmd_vel_web
|
||||
```
|
||||
|
||||
查看当前仲裁模式：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /control/mode_state
|
||||
```
|
||||
|
||||
查看策略状态：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /runtime/target --field target_source
|
||||
```
|
||||
@@ -0,0 +1,164 @@
|
||||
# Pure Pursuit Yaw检查优化 - 最终配置
|
||||
|
||||
## 🎯 核心问题
|
||||
|
||||
**现象**: 机器人没转到位就开始前进 → 斜着撞杆子
|
||||
|
||||
**根本原因**: Pure Pursuit的yaw检查不够严格
|
||||
|
||||
---
|
||||
|
||||
## ✅ 已修改的参数
|
||||
|
||||
### runtime.yaml
|
||||
|
||||
```yaml
|
||||
# 关键修改1: Script模式yaw门限
|
||||
nav_slalom_script_yaw_gate_deg: 8.0 # 从12度 → 8度
|
||||
|
||||
# 关键修改2: 全局yaw容差
|
||||
nav_goal_yaw_tolerance_deg: 8.0 # 从12度 → 8度
|
||||
```
|
||||
|
||||
**作用**:
|
||||
- yaw偏差 > 8度时：只转向，不前进
|
||||
- yaw偏差 ≤ 8度时：才允许前进
|
||||
|
||||
---
|
||||
|
||||
## 📊 完整配置总结
|
||||
|
||||
### 1. 禁用干扰的规划器
|
||||
```yaml
|
||||
nav_local_planner_enabled: false
|
||||
nav_astar_enabled: false
|
||||
```
|
||||
|
||||
### 2. 速度和精度
|
||||
```yaml
|
||||
nav_slalom_max_vx: 0.50
|
||||
nav_slalom_lookahead: 0.15
|
||||
nav_slalom_tolerance: 0.05
|
||||
```
|
||||
|
||||
### 3. Yaw控制（新增）
|
||||
```yaml
|
||||
nav_goal_yaw_tolerance_deg: 8.0
|
||||
nav_slalom_script_yaw_gate_deg: 8.0
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 🔍 工作原理
|
||||
|
||||
### Pure Pursuit算法流程
|
||||
```
|
||||
1. 看前方lookahead距离的目标点
|
||||
2. 计算到目标的方向和距离
|
||||
3. 检查yaw偏差
|
||||
- 如果 yaw偏差 > yaw_gate (8度):
|
||||
只发wz转向，vx=0
|
||||
- 如果 yaw偏差 ≤ yaw_gate (8度):
|
||||
发vx前进 + wz微调
|
||||
4. 到达目标点，推进下一个
|
||||
```
|
||||
|
||||
### 之前的问题
|
||||
```
|
||||
yaw_gate = 12度（太宽松）
|
||||
↓
|
||||
yaw偏差11度时就开始前进
|
||||
↓
|
||||
还没对准就冲出去
|
||||
↓
|
||||
斜着撞杆子
|
||||
```
|
||||
|
||||
### 修改后
|
||||
```
|
||||
yaw_gate = 8度（更严格）
|
||||
↓
|
||||
yaw偏差必须≤8度才前进
|
||||
↓
|
||||
基本对准后才移动
|
||||
↓
|
||||
不会斜着撞
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 📁 使用的文件
|
||||
|
||||
**路线**: `points_nav1007_optimized.json`
|
||||
- 总航点: 47
|
||||
- 绕杆航点: 21
|
||||
- 参数: speed=0.50, lookahead=0.15, tolerance=0.05
|
||||
|
||||
**地形**: `tools/nav_tools/xml/A.xml`
|
||||
|
||||
**配置**: `runtime.yaml` (已修改)
|
||||
|
||||
---
|
||||
|
||||
## 🚀 下一步
|
||||
|
||||
### 实机测试
|
||||
```bash
|
||||
# 1. 重启系统加载新配置
|
||||
ros2 launch sim2real_bringup sim2real_system.launch.py
|
||||
|
||||
# 2. 验证参数
|
||||
ros2 param get /sim2real_simple_nav_node nav_slalom_script_yaw_gate_deg
|
||||
# 应该显示: 8.0
|
||||
|
||||
ros2 param get /sim2real_simple_nav_node nav_goal_yaw_tolerance_deg
|
||||
# 应该显示: 8.0
|
||||
|
||||
# 3. 加载路线
|
||||
# 使用: sim2real_ros2_v2_ooo/map/routes/points_nav1007_optimized.json
|
||||
|
||||
# 4. 监控
|
||||
ros2 topic echo /cmd_vel_nav
|
||||
# 观察: 转向时vx应该接近0，对准后才有vx速度
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 💡 预期效果
|
||||
|
||||
**修改前**:
|
||||
- 机器人边转边进
|
||||
- yaw偏差大时仍有前进速度
|
||||
- 导致斜着撞杆子
|
||||
|
||||
**修改后**:
|
||||
- 转向时几乎不前进（vx≈0）
|
||||
- 对准后才快速前进（vx=0.5）
|
||||
- 动作分离：先转向，后前进
|
||||
|
||||
---
|
||||
|
||||
## ⚠️ 如果还有问题
|
||||
|
||||
### 场景A: 还是斜着撞
|
||||
可能需要进一步收紧:
|
||||
```yaml
|
||||
nav_slalom_script_yaw_gate_deg: 5.0 # 改为5度
|
||||
```
|
||||
|
||||
### 场景B: 太慢，一直在转
|
||||
说明yaw_gate太严格:
|
||||
```yaml
|
||||
nav_slalom_script_yaw_gate_deg: 10.0 # 放宽到10度
|
||||
```
|
||||
|
||||
### 场景C: 卡顿
|
||||
检查是否在等待yaw对准:
|
||||
```bash
|
||||
ros2 topic echo /simple_nav/status
|
||||
# 看是否一直在"aligning"状态
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
**配置已优化完成，ready for testing!** 🎯
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,11 @@
|
||||
# 比赛地图与路线
|
||||
|
||||
`routes/` 保存 `last_not_slalom_1050` 工程中的五份路线快照，保留原文件名以维持运行时配置和版本演进关系。默认运行路线是 `routes/1hao_reall.json`。
|
||||
|
||||
`1hao.pcd` 是 `v0.8.1` 导航工具中同一份抽样点云，包含 199,215 点、大小 9,876,010 字节，SHA-256 为 `48B231C52BECA51316F352300C8B2046133E92359E0855227D93DEB0D927AD34`。它用于本地规划和路线显示，不替代原始高密度点云。
|
||||
|
||||
## 缺失的 Odin 重定位地图
|
||||
|
||||
比赛配置需要 Odin 专用二进制地图 `1hao.bin`，但源备份没有该文件。源目录中另有两个名称和时间不同的 `.bin`，无法证明它们就是比赛使用地图，因此没有复制或重命名。
|
||||
|
||||
重定位部署时应从比赛设备取回真实文件，并把 `control_command_relocal.yaml` 中的占位绝对路径改为目标机实际位置。纯里程计模式不需要该文件。
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,14 @@
|
||||
# 比赛部署策略
|
||||
|
||||
这里只保留最终运行配置实际使用的 Rough 和 Wall 策略。Crawl 在比赛配置中使用 IK 后端，因此不归档未使用的 Crawl RL engine。
|
||||
|
||||
| 用途 | 文件 | SHA-256 |
|
||||
| --- | --- | --- |
|
||||
| Rough ONNX | `model_6800.onnx` | `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13` |
|
||||
| Rough TensorRT | `model_6800_fp16.engine` | `BDC6583AFD594E84D9A4AAA4BB2EEE279C7EBF207A57CF4802A2DF5309C5F663` |
|
||||
| Wall ONNX | `model_84.onnx` | `E3A447782DF6C6E11E66C3ACBE41697FF88EBF5E75DCAB65EFCF9A2EB1CA639F` |
|
||||
| Wall TensorRT | `model_84_fp16.engine` | `0FE2001113B7146B589D7A62A6E54BAD68574765EBA45782FB41A26F65A8721D` |
|
||||
|
||||
`model_6800.onnx` 与 `05_software/train/rc_mjlab/model_6800.onnx` 哈希一致。此处重复保留是为了让真机工作区可以独立部署。
|
||||
|
||||
TensorRT 文件只作为比赛机历史产物；更换 JetPack、TensorRT 或 GPU 后应从 ONNX 重新构建并重新核对数值误差。
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,22 @@
|
||||
# Orin 触控屏控制面板
|
||||
|
||||
`fullscreen_quit.py` 是比赛 Orin 外接 `800×600` 屏幕使用的控制面板，通过本机 `http://127.0.0.1:18080/api/*` 调用 ROS 2 Web bridge，不建立第二套控制协议。
|
||||
|
||||
```bash
|
||||
cd <sim2real_ros2工作区>
|
||||
DISPLAY=:0 python3 screen/fullscreen_quit.py
|
||||
```
|
||||
|
||||
程序默认从脚本父目录自动确定工作区，也可设置 `SIM2REAL_REPO_DIR` 覆盖。显示权限检查：
|
||||
|
||||
```bash
|
||||
python3 screen/check_display.py
|
||||
```
|
||||
|
||||
自启动脚本：
|
||||
|
||||
- `install_autostart.sh`：图形桌面登录后启动。
|
||||
- `install_boot_service.sh <用户名>`：安装 systemd 服务。
|
||||
- `uninstall_boot_service.sh`：卸载服务。
|
||||
|
||||
使用屏幕启动系统前，先完成工作区构建、Odin 配置、CAN 映射检查和急停验证。
|
||||
@@ -0,0 +1,114 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Print display-related diagnostics for launching the fullscreen quit screen.
|
||||
"""
|
||||
|
||||
import glob
|
||||
import getpass
|
||||
import os
|
||||
import shlex
|
||||
import subprocess
|
||||
from typing import List, Optional
|
||||
|
||||
|
||||
def run(command: List[str]) -> str:
|
||||
try:
|
||||
result = subprocess.run(
|
||||
command,
|
||||
check=False,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
except OSError as exc:
|
||||
return f"<failed: {exc}>"
|
||||
|
||||
output = result.stdout.strip()
|
||||
error = result.stderr.strip()
|
||||
if error:
|
||||
return f"{output}\n{error}".strip()
|
||||
return output
|
||||
|
||||
|
||||
def current_user() -> str:
|
||||
try:
|
||||
return getpass.getuser()
|
||||
except OSError:
|
||||
return os.environ.get("USER", "unknown")
|
||||
|
||||
|
||||
def xauth_from_x_processes() -> List[str]:
|
||||
output = run(["ps", "-eo", "args"])
|
||||
candidates = []
|
||||
for line in output.splitlines():
|
||||
if "Xorg" not in line and "Xwayland" not in line:
|
||||
continue
|
||||
try:
|
||||
parts = shlex.split(line)
|
||||
except ValueError:
|
||||
parts = line.split()
|
||||
for index, part in enumerate(parts[:-1]):
|
||||
if part == "-auth":
|
||||
candidates.append(parts[index + 1])
|
||||
return candidates
|
||||
|
||||
|
||||
def print_path_status(label: str, path: Optional[str]) -> None:
|
||||
if not path:
|
||||
print(f"{label}: <unset>")
|
||||
return
|
||||
exists = os.path.exists(path)
|
||||
readable = os.access(path, os.R_OK) if exists else False
|
||||
print(f"{label}: {path} exists={exists} readable={readable}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
uid = os.getuid()
|
||||
print(f"user: {run(['whoami'])}")
|
||||
print(f"uid: {uid}")
|
||||
print(f"DISPLAY: {os.environ.get('DISPLAY', '<unset>')}")
|
||||
print(f"XAUTHORITY: {os.environ.get('XAUTHORITY', '<unset>')}")
|
||||
print()
|
||||
|
||||
print("candidate XAUTHORITY files:")
|
||||
candidates = [
|
||||
os.environ.get("XAUTHORITY"),
|
||||
*xauth_from_x_processes(),
|
||||
os.path.expanduser("~/.Xauthority"),
|
||||
f"/run/user/{uid}/gdm/Xauthority",
|
||||
f"/run/user/{uid}/Xauthority",
|
||||
*glob.glob(f"/run/user/{uid}/*Xauthority*"),
|
||||
]
|
||||
|
||||
seen = set()
|
||||
for candidate in candidates:
|
||||
key = candidate or "<unset>"
|
||||
if key in seen:
|
||||
continue
|
||||
seen.add(key)
|
||||
print_path_status(" -", candidate)
|
||||
|
||||
print()
|
||||
print("X server processes:")
|
||||
x_lines = [
|
||||
line
|
||||
for line in run(["ps", "-eo", "user,args"]).splitlines()
|
||||
if "Xorg" in line or "Xwayland" in line
|
||||
]
|
||||
if x_lines:
|
||||
for line in x_lines:
|
||||
print(f" {line}")
|
||||
else:
|
||||
print(" <none found>")
|
||||
|
||||
print()
|
||||
print("recommended SSH launch:")
|
||||
print(" cd <workspace>/screen")
|
||||
print(" bash run_fullscreen_quit.sh")
|
||||
print()
|
||||
print("if authorization fails, run this on the Nano desktop terminal once:")
|
||||
print(f" DISPLAY=:0 xhost +SI:localuser:{current_user()}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,27 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
AUTOSTART_DIR="$HOME/.config/autostart"
|
||||
DESKTOP_FILE="$AUTOSTART_DIR/sim2real-screen.desktop"
|
||||
|
||||
mkdir -p "$AUTOSTART_DIR"
|
||||
chmod +x "$SCRIPT_DIR/run_fullscreen_quit.sh"
|
||||
|
||||
cat > "$DESKTOP_FILE" <<EOF
|
||||
[Desktop Entry]
|
||||
Type=Application
|
||||
Name=sim2real Screen
|
||||
Comment=Start the sim2real touch control panel
|
||||
Exec=$SCRIPT_DIR/run_fullscreen_quit.sh
|
||||
Path=$SCRIPT_DIR
|
||||
Terminal=false
|
||||
X-GNOME-Autostart-enabled=true
|
||||
StartupNotify=false
|
||||
EOF
|
||||
|
||||
echo "Installed desktop autostart:"
|
||||
echo " $DESKTOP_FILE"
|
||||
echo
|
||||
echo "It will start after this user logs into the graphical desktop."
|
||||
echo "For boot-time use, enable automatic login for this user on the Orin desktop."
|
||||
@@ -0,0 +1,73 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
if ! command -v systemctl >/dev/null 2>&1; then
|
||||
echo "systemctl not found; this installer is for systemd-based Linux." >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
SERVICE_NAME="sim2real-screen.service"
|
||||
SERVICE_PATH="/etc/systemd/system/$SERVICE_NAME"
|
||||
|
||||
if [ "${EUID:-$(id -u)}" -eq 0 ]; then
|
||||
RUN_USER="${1:-${SUDO_USER:-rc2}}"
|
||||
else
|
||||
RUN_USER="${1:-$(id -un)}"
|
||||
fi
|
||||
|
||||
if ! id "$RUN_USER" >/dev/null 2>&1; then
|
||||
echo "User '$RUN_USER' does not exist. Usage: bash install_boot_service.sh rc2" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
RUN_GROUP="$(id -gn "$RUN_USER")"
|
||||
RUN_HOME="$(getent passwd "$RUN_USER" | cut -d: -f6)"
|
||||
AUTOSTART_FILE="$RUN_HOME/.config/autostart/sim2real-screen.desktop"
|
||||
|
||||
chmod +x "$SCRIPT_DIR/run_fullscreen_quit.sh" "$SCRIPT_DIR/run_boot_screen_service.sh"
|
||||
if [ -f "$AUTOSTART_FILE" ]; then
|
||||
rm -f "$AUTOSTART_FILE"
|
||||
echo "Removed desktop autostart to avoid duplicate screen instances:"
|
||||
echo " $AUTOSTART_FILE"
|
||||
fi
|
||||
|
||||
SERVICE_CONTENT="[Unit]
|
||||
Description=sim2real touchscreen control panel
|
||||
Wants=display-manager.service
|
||||
After=systemd-user-sessions.service display-manager.service
|
||||
StartLimitIntervalSec=0
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=$RUN_USER
|
||||
Group=$RUN_GROUP
|
||||
WorkingDirectory=$SCRIPT_DIR
|
||||
Environment=HOME=$RUN_HOME
|
||||
Environment=DISPLAY=:0
|
||||
Environment=PYTHONUNBUFFERED=1
|
||||
ExecStart=$SCRIPT_DIR/run_boot_screen_service.sh
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
KillSignal=SIGINT
|
||||
TimeoutStopSec=20
|
||||
|
||||
[Install]
|
||||
WantedBy=graphical.target
|
||||
"
|
||||
|
||||
printf "%s" "$SERVICE_CONTENT" | sudo tee "$SERVICE_PATH" >/dev/null
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable "$SERVICE_NAME"
|
||||
|
||||
echo "Installed and enabled:"
|
||||
echo " $SERVICE_PATH"
|
||||
echo
|
||||
echo "Start now:"
|
||||
echo " sudo systemctl restart $SERVICE_NAME"
|
||||
echo
|
||||
echo "Check status/logs:"
|
||||
echo " systemctl status $SERVICE_NAME --no-pager"
|
||||
echo " journalctl -u $SERVICE_NAME -f"
|
||||
echo
|
||||
echo "Important: the graphical desktop for user '$RUN_USER' must auto-login, otherwise Tk cannot open DISPLAY=:0."
|
||||
@@ -0,0 +1,23 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
|
||||
export DISPLAY="${DISPLAY:-:0}"
|
||||
export PYTHONUNBUFFERED=1
|
||||
|
||||
echo "[sim2real-screen] service starting as $(id -un), DISPLAY=$DISPLAY"
|
||||
|
||||
for _ in $(seq 1 120); do
|
||||
if [ -S "/tmp/.X11-unix/X${DISPLAY#:}" ]; then
|
||||
break
|
||||
fi
|
||||
sleep 1
|
||||
done
|
||||
|
||||
if [ ! -S "/tmp/.X11-unix/X${DISPLAY#:}" ]; then
|
||||
echo "[sim2real-screen] X11 socket for DISPLAY=$DISPLAY not found" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
exec "$SCRIPT_DIR/run_fullscreen_quit.sh"
|
||||
@@ -0,0 +1,24 @@
|
||||
#!/usr/bin/env bash
|
||||
set -u
|
||||
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
cd "$SCRIPT_DIR"
|
||||
|
||||
export DISPLAY="${DISPLAY:-:0}"
|
||||
|
||||
if [ -z "${XAUTHORITY:-}" ]; then
|
||||
UID_VALUE="$(id -u)"
|
||||
|
||||
for candidate in \
|
||||
"$HOME/.Xauthority" \
|
||||
"/run/user/$UID_VALUE/gdm/Xauthority" \
|
||||
"/run/user/$UID_VALUE/Xauthority"
|
||||
do
|
||||
if [ -f "$candidate" ]; then
|
||||
export XAUTHORITY="$candidate"
|
||||
break
|
||||
fi
|
||||
done
|
||||
fi
|
||||
|
||||
python3 fullscreen_quit.py
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 34 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 35 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 35 KiB |
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
SERVICE_NAME="sim2real-screen.service"
|
||||
SERVICE_PATH="/etc/systemd/system/$SERVICE_NAME"
|
||||
|
||||
sudo systemctl disable --now "$SERVICE_NAME" 2>/dev/null || true
|
||||
sudo rm -f "$SERVICE_PATH"
|
||||
sudo systemctl daemon-reload
|
||||
|
||||
echo "Removed $SERVICE_NAME"
|
||||
@@ -0,0 +1,5 @@
|
||||
recorddata/
|
||||
/config/calib.yaml
|
||||
!/config/calib.yaml
|
||||
/log
|
||||
/map
|
||||
@@ -0,0 +1,13 @@
|
||||
v0.11.0 2026_0618
|
||||
Required Minimum Firmware Version：0.12.0
|
||||
1.Reduce cloud slam latency
|
||||
2.Improve get mapping result
|
||||
|
||||
v0.10.5 2026_0525
|
||||
1.modify recorddata format，add device_id、algorithm_version key
|
||||
2.fix download map fail when in slam mode(USB2.0)
|
||||
3.fix upload map fail when in relocalization mode(USB2.0)
|
||||
|
||||
v0.10.4 2026_0522
|
||||
1.fix usb2.0 heartBeat timeout to cause soft detaching
|
||||
2.add custom_init_pose_search_radius and custom_init_pose_max_rot_deg in control_command.yaml
|
||||
@@ -0,0 +1,488 @@
|
||||
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
|
||||
message_generation
|
||||
)
|
||||
|
||||
# ---- AE/AWB debug services ----
|
||||
# Declare custom .srv files; catkin generates ros1 message headers for them.
|
||||
add_service_files(
|
||||
FILES
|
||||
GetAe.srv
|
||||
GetAwb.srv
|
||||
SetAe.srv
|
||||
SetAwb.srv
|
||||
)
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs cv_bridge image_transport message_runtime
|
||||
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
|
||||
)
|
||||
# Make sure the AE/AWB srv headers are generated before host_sdk_sample
|
||||
# is compiled (catkin's generate_messages produces this target).
|
||||
add_dependencies(host_sdk_sample ${PROJECT_NAME}_generate_messages_cpp)
|
||||
|
||||
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)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
|
||||
# ---- AE/AWB debug services ----
|
||||
# Generate C++ bindings for the 4 .srv files in srv/. The generated
|
||||
# headers are linked into host_sdk_sample so it can host the services.
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/GetAe.srv"
|
||||
"srv/GetAwb.srv"
|
||||
"srv/SetAe.srv"
|
||||
"srv/SetAwb.srv"
|
||||
)
|
||||
|
||||
# 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
|
||||
)
|
||||
|
||||
# Link the generated AE/AWB srv typesupport into host_sdk_sample so it
|
||||
# can host the 4 debug services.
|
||||
rosidl_get_typesupport_target(cpp_typesupport_target
|
||||
${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
target_link_libraries(host_sdk_sample "${cpp_typesupport_target}")
|
||||
|
||||
# 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
|
||||
ament_index_cpp
|
||||
)
|
||||
|
||||
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
|
||||
depth_image_ros2_node_lib
|
||||
pointcloud_depth_converter_ros2
|
||||
cloud_reprojector_ros2
|
||||
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_index_cpp
|
||||
)
|
||||
|
||||
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,821 @@
|
||||
# Odin_ROS_Driver Readme
|
||||
|
||||
ROS driver suite for Odin sensor modules (Manifold Tech Ltd.)
|
||||
|
||||
Odin1 wiki: https://manifoldtechltd.github.io/wiki/Odin1/Cover.html
|
||||
|
||||
## Odin_ROS_Driver
|
||||
|
||||
Compatibility:
|
||||
|
||||
● ROS 1(LTS Release: Noetic recommended)
|
||||
|
||||
● ROS 2(LTS Release: Humble recommended)
|
||||
|
||||
## Important Notice:
|
||||
|
||||
This driver package provides core functionality for point cloud SLAM applications and targets specific use cases. It is intended exclusively for technical professionals conducting secondary development. End users must perform scenario-specific optimization and custom development to align with operational requirements in practical deployment environments.
|
||||
|
||||
## 1. Version
|
||||
|
||||
Current version: v0.12.0
|
||||
|
||||
Required device firmware version: v0.12.0
|
||||
|
||||
## 2. Preparation
|
||||
|
||||
### 2.1 OS Requirement
|
||||
|
||||
● Ubuntu 20.04 for ROS Noetic and ROS2 Foxy;
|
||||
|
||||
● Ubuntu 22.04 for ROS2 Humble;
|
||||
|
||||
● Ubuntu 18.04 is currently not supported;
|
||||
|
||||
● Ubuntu 24.04 is not officially supported but may work with some modifications.
|
||||
|
||||
### 2.2 Dependencies
|
||||
|
||||
● Opencv >= 4.2.0(recommand 4.5.5/4.8.0. Make sure only one version of opencv is installed)
|
||||
|
||||
● yaml-cpp
|
||||
|
||||
● thread
|
||||
|
||||
● OpenSSL
|
||||
|
||||
● Eigen3
|
||||
|
||||
### 2.3 Dependencies Install
|
||||
|
||||
#### 2.3.1 System
|
||||
```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.4 ROS install
|
||||
|
||||
For ROS Noetic installation, please refer to:
|
||||
[ROS Noetic installation instructions](https://wiki.ros.org/noetic/Installation)
|
||||
|
||||
For ROS2 Foxy installation, please refer to:
|
||||
[ROS Foxy installation instructions](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
For ROS2 Humble installation, please refer to:
|
||||
[ROS Humble installation instructions](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
|
||||
|
||||
## 3. Preparation
|
||||
|
||||
### 3.1 Create Udev rules
|
||||
```shell
|
||||
sudo vim /etc/udev/rules.d/99-odin-usb.rules
|
||||
```
|
||||
Add the following content to the 99-odin-usb.rules file
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
Reload rules and reinsert devices
|
||||
```shell
|
||||
sudo udevadm control --reload
|
||||
sudo udevadm trigger
|
||||
```
|
||||
### 3.2 OS Requirement
|
||||
```shell
|
||||
git clone https://github.com/manifoldsdk/odin_ros_driver.git catkin_ws/src/odin_ros_driver
|
||||
```
|
||||
Note:
|
||||
Please clone the source code into the "[ros_workspace]/src/" folder, otherwise compilation errors will occur.
|
||||
|
||||
### 3.3 make
|
||||
|
||||
#### 3.3.1 ROS1 (Noetic for example):
|
||||
|
||||
```shell
|
||||
source /opt/ros/noetic/setup.bash
|
||||
./script/build_ros.sh
|
||||
```
|
||||
|
||||
#### 3.3.2 ROS2 (Foxy for example):
|
||||
|
||||
```shell
|
||||
source /opt/ros/foxy/setup.bash
|
||||
./script/build_ros2.sh
|
||||
```
|
||||
|
||||
### 3.4 run:
|
||||
|
||||
#### 3.4.1 ROS1 (Noetic for example):
|
||||
|
||||
```shell
|
||||
source [ros_workspace]/devel/setup.bash
|
||||
roslaunch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver: package name;
|
||||
|
||||
● launch file: launch file;
|
||||
|
||||
● ros_workspace: User's ROS environment workspace;
|
||||
```shell
|
||||
roslaunch odin_ros_driver odin1_ros1.launch
|
||||
```
|
||||
#### 3.4.2 ROS2 (Foxy for example):
|
||||
|
||||
```shell
|
||||
source [ros2_workspace]/install/setup.bash
|
||||
ros2 launch odin_ros_driver [launch file]
|
||||
```
|
||||
● odin_ros_driver: package name;
|
||||
|
||||
● launch file: launch file;
|
||||
|
||||
● ros2_workspace: User's ROS2 environment workspace;
|
||||
|
||||
ROS2 Demo Launch Instructions:
|
||||
```shell
|
||||
ros2 launch odin_ros_driver odin1_ros2.launch.py
|
||||
```
|
||||
|
||||
### 3.5 Operation Mode:
|
||||
|
||||
The operation mode can be configured via the `custom_map_mode` parameter in config/control_command.yaml.
|
||||
|
||||
#### Odometry mode
|
||||
|
||||
Set `custom_map_mode = 0` to enable odometry mode. In this mode, the map frame and odom frame share the same pose.
|
||||
|
||||
If the odom data is found to drift, the script command "./set_param.sh algo_reset 1" can be used to dynamically reset the algorithm.
|
||||
|
||||
#### SLAM mode
|
||||
|
||||
Set `custom_map_mode = 1` to enable slam mode. This mode provides a complete SLAM system that builds upon the Odometry Mode by adding **loop closure detection** and **map saving** capabilities.
|
||||
|
||||
After launching the driver, odin1 will automatically perform mapping and cache map data. When the scene capture is complete, users need to execute `./set_param.sh save_map 1` in the driver's source directory to save all map data collected since the program started. The map will be saved to the location specified by the `mapping_result_dest_dir` and `mapping_result_file_name` parameters in config/control_command.yaml. If these parameters are not specified, default values will be used.
|
||||
|
||||
After the initial save, you can execute the command again to save a new map. Each save operation will generate a new map file. (Please allow at least 5 seconds between consecutive save operations)
|
||||
|
||||
The map origin corresponds to the odom coordinate system's origin at the program's startup.
|
||||
|
||||
##### Relocalization mode
|
||||
|
||||
To enable relocalization, set `custom_map_mode = 2` and specify the absolute path to the pre-built map using the `relocalization_map_abs_path` parameter in config/control_command.yaml.
|
||||
|
||||
Once launched, odin1 will initiate the relocalization process based on the current viewpoint and the specified map. To ensure a high success rate, it is recommended to starting within 1 meter ±10 degrees of the original position and orientation from the SLAM trajectory.
|
||||
|
||||
Note that relocalization performance is highly environment-dependent. In highly distinctive scenes, successful matching may occur even beyond the 1m/10° range, while other environments may require more stringent conditions. We advise testing in your target environment to determine practical tolerances.
|
||||
|
||||
If relocalization fails initially, the system will temporarily operate in a fallback SLAM mode (map saving is disabled in this state). During this time, you can freely move odin1. It will continue relocalization attempts in the background. Once successful, the TF between map and odom frames will be published. (Tip: Gently shaking or moving the device after initialization can help improve relocalization accuracy.)
|
||||
|
||||
The following topics are published in the odom frame: `/odin1/cloud_slam, /odin1/odom, /odin1/highodom and /odin1/path`. To obtain these in the map frame, apply the TF from odom frame to map frame.
|
||||
|
||||
## 4. File structure and data format
|
||||
### 4.1 File structure
|
||||
```shell
|
||||
Odin_ROS_Driver/ // ROS1/ROS2 driver package
|
||||
3rdparty/ // Third-party libraries
|
||||
src/
|
||||
host_sdk_sample.cpp // Example source code
|
||||
yaml_parser.cpp // Source code for reading yaml parameters
|
||||
rawCloudRender.cpp // Source code for RenderCloud
|
||||
depth_image_ros_node.cpp //depth_image_ros_node
|
||||
depth_image_ros2_node.cpp //depth_image_ros2_node
|
||||
pcd2depth_ros.cpp //Source code for pcd2depth_ros
|
||||
pcd2depth_ros2.cpp //Source code for pcd2depth_ros2
|
||||
pointcloud_depth_converter.cpp //Source code for pointcloud_depth_converter
|
||||
cloud_reprojection_ros.cpp //Source code for cloud reprojection node (ROS1/ROS2)
|
||||
cloud_reprojector.cpp //Core logic for cloud reprojection
|
||||
lib/
|
||||
liblydHostApi_amd.a // Static library for AMD platform
|
||||
liblydHostApi_arm.a // Static library for ARM platform
|
||||
include/
|
||||
host_sdk_sample.h // Example header file
|
||||
lidar_api_type.h // API data structure header file
|
||||
lidar_api.h // API function declarations
|
||||
yaml_parser.h // Parameter file reading header file
|
||||
rawCloudRender.h // API about RenderCloud
|
||||
data_logger.h // LOG about save_data
|
||||
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 // Core class for cloud reprojection
|
||||
config/
|
||||
control_command.yaml // Control parameter file for driver
|
||||
calib.yaml // Machine calibration yaml，differ for each individual device. Retrieved from the device everytime it connects to ROS driver
|
||||
launch_ROS1/
|
||||
odin1_ros1.launch // ROS1 launch file
|
||||
launch_ROS2/
|
||||
odin1_ros2.launch.py // ROS2 launch file
|
||||
script/
|
||||
build_ros1.sh // Installation script for ROS1
|
||||
build_ros2.sh // Installation script for ROS2
|
||||
recorddata/ // holds recorded data that can import into MindCloud
|
||||
log/ // holds log files
|
||||
Driver_{timestamp}/ // holds all log folders for each time driver started
|
||||
Conn_{timestamp}/ // holds all log files for each odin1 device connection
|
||||
dev_status.csv // device status log file
|
||||
README.md // Usage instructions
|
||||
CMakeLists.txt // CMake build file
|
||||
License // License file
|
||||
```
|
||||
### 4.2 File structure
|
||||
| Launch File Name | Description |
|
||||
|--------------------------|-------------|
|
||||
| odin1_ros1.launch | Launch file for ROS1 - Odin1 Basic Operations Demo |
|
||||
| odin1_ros2.launch.py | Launch file for ROS2 - Odin1 Basic Operations Demo |
|
||||
|
||||
|
||||
### 4.3 ROS topics
|
||||
Internal parameters of the Odin ROS driver are defined in config/control_command.yaml. Below are descriptions of the commonly used parameters:
|
||||
|
||||
| Topic |control_command.yaml | Detailed Description |
|
||||
|---------------------|----------------------|----------------------|
|
||||
| odin1/imu | sendimu | Imu Topic |
|
||||
| odin1/image | sendrgb | RGB Camera Topic, decoded from original jpeg data from device, bgr8 format |
|
||||
| odin1/image_undistort | sendrgbundistort | undistorted RGB Camera Topic, processed with calib.yaml from device |
|
||||
| odin1/image/compressed | sendrgbcompressed | RGB Camera compressed Topic, original jpeg data from device |
|
||||
| odin1/cloud_raw | senddtof | Raw_Cloud Topic |
|
||||
| odin1/cloud_render | sendcloudrender | Render_Cloud Topic, processed with raw point cloud, rgb image, and calib.yaml from device |
|
||||
| odin1/cloud_slam | sendcloudslam | Slam_PointCloud Topic |
|
||||
| odin1/odometry | sendodom | Odom Topic |
|
||||
| odin1/odometry_high | sendodom | high frequency Odom Topic |
|
||||
| odin1/path | showpath | Odom Path Topic |
|
||||
| tf | sendodom | tf tree Topic |
|
||||
| odin1/depth_img_competetion | senddepth | Dense depth image Topic. Demo, high computing power required. One-to-one with odin1/image_undistort. To utilize the data please directly subscribe to this topic instead of echoing it. Original value is already depth data, no need for further convert. |
|
||||
| odin1/depth_img_competetion_cloud | senddepth | Dense Depth_Cloud Topic. Demo, high computing power required |
|
||||
| odin1/reprojected_image | sendreprojection | Reprojected cloud to image Topic. Projects cloud_slam to camera image using odometry. Processed on host device. |
|
||||
|
||||
### 4.4 Data format
|
||||
|
||||
1. The raw point cloud (cloud_raw) has the following fields:
|
||||
```
|
||||
float32 x // X axis, in meters
|
||||
float32 y // Y axis, in meters
|
||||
float32 z // Z axis, in meters
|
||||
uint8 intensity // Reflectivity, range 0–255
|
||||
uint16 confidence // Point confidence, actual value range from 0 to around 1300 in typical scene, higher value means more reliable. Recommanded filtering threshold is 30-35, should be adjusted accordingly.
|
||||
float32 offset_time // Time offset relative to the base timestamp unit: s
|
||||
```
|
||||
|
||||
To work with this custom format in PCL, first define the point type:
|
||||
```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)
|
||||
)
|
||||
```
|
||||
Then, you can easily convert a ROS sensor_msgs::PointCloud2 message into a PCL point cloud:
|
||||
```
|
||||
pcl::PointCloud<ls_ros::Point> ls_cloud;
|
||||
pcl::fromROSMsg(*msg, ls_cloud);
|
||||
```
|
||||
|
||||
2. The slam point cloud (cloud_slam) and directly rendered point cloud (cloud_render) has the following fields:
|
||||
```
|
||||
float32 x // X axis, in meters
|
||||
float32 y // Y axis, in meters
|
||||
float32 z // Z axis, in meters
|
||||
float32 rgb // RGB value
|
||||
```
|
||||
|
||||
### 4.5 Other functionalities
|
||||
|
||||
|control_command.yaml | Detailed Description |
|
||||
|-----------------------|----------------------|
|
||||
| use_host_ros_time | Time synchronization mode: 0 - use odin internal system time as data timestamp (typical and recommended); 1 - use host ROS time upon receive (not recommended for most users); 2 - align odin1 time to host time via NTP-like synchronization, timestamp is the sensor data reception time on host time axis. |
|
||||
| strict_usb3.0_check | Strict USB3.0 check, if off, allow connection even if usb connection is below usb 3.0 |
|
||||
| recorddata | Record data in specific format that can be imported into MindCloud(TM) for post-processing. Please be aware that this will consume a lot of storage space. Testing shows 9.5G for 10mins of data. The per-frame timestamps written into the recorded files (IMU / image / point cloud / pose / rotate) follow the same alignment policy as `use_host_ros_time`, so under NTP mode (`use_host_ros_time=1` or `2`) the recorded timestamps are NTP-aligned host time instead of odin1 boot time. <br>录制文件 (IMU / 图像 / 点云 / Pose / Rotate) 中每帧的时间戳与 `use_host_ros_time` 采用相同对齐策略：在 NTP 模式 (`use_host_ros_time=1` 或 `2`) 下，录制时间戳为 NTP 对齐后的主机时间，而非 odin1 开机时间。 |
|
||||
| devstatuslog | Device status logging, currently save device status (soc temperature, cpu usage, ram usage, dtof sensor temp .etc) and data tx & rx rate to devstatus.csv under log folder. A new file will be created every time the driver is started. |
|
||||
| showcamerapose | Display Camera Pose and Field of View. |
|
||||
| custom_map_mode | Operation Modes: Mode 0 - Odometry mode: The map frame and odom frame share the same pose. Mode 1 - Mapping (with loop closure) mode: This mode supports map saving. Mode 2 - Relocalization mode: Requires specifying the absolute path to the map file. After successful relocalization, it will output the TF relationship between the map and odom frames.|
|
||||
| custom_init_pos | Initialization Position (currently unused). |
|
||||
| relocalization_map_abs_path | Absolute Path to Map File: Used for relocalization mode. |
|
||||
| mapping_result_dest_dir and mapping_result_file_name| Path and Name for Saving Maps in Mapping Mode: If not specified, default values will be used. |
|
||||
|
||||
### 4.6 Runtime AE/AWB Tuning via ROS Service / 通过 ROS Service 在线调节 AE/AWB
|
||||
|
||||
The driver hosts four ROS services that let a side terminal tune the
|
||||
camera's auto exposure (AE) and auto white balance (AWB) at runtime,
|
||||
while the main data streams keep flowing. The same SDK call is shared
|
||||
with the driver's main control path and serialised by an internal
|
||||
mutex, so it is safe to invoke these services concurrently with normal
|
||||
operation.
|
||||
|
||||
驱动启动后会注册 4 个 ROS Service，允许在不重启 driver 的前提下，从另一个终端动态调节
|
||||
相机的自动曝光（AE）和自动白平衡（AWB）。底层 SDK 调用与驱动主控制路径共享同一把
|
||||
互斥锁，因此可以与正常数据流并发调用。
|
||||
|
||||
**Service list / Service 一览**
|
||||
|
||||
| Service name | Type / 类型 | Purpose / 用途 |
|
||||
|---|---|---|
|
||||
| `/odin1/get_ae` | `odin_ros_driver/srv/GetAe` | Query current AE status / 查询当前 AE 状态 |
|
||||
| `/odin1/get_awb` | `odin_ros_driver/srv/GetAwb` | Query current AWB status / 查询当前 AWB 状态 |
|
||||
| `/odin1/set_ae` | `odin_ros_driver/srv/SetAe` | Set AE mode and (manual) exposure / gain / 设置 AE 模式和手动曝光/增益 |
|
||||
| `/odin1/set_awb` | `odin_ros_driver/srv/SetAwb` | Set AWB mode and (manual) R/B gain / 设置 AWB 模式和手动 R/B 增益 |
|
||||
|
||||
#### 4.6.1 Request fields, ranges, physical meaning / 请求字段、范围与物理含义
|
||||
|
||||
**`SetAe.Request`**
|
||||
|
||||
| Field | Range / 范围 | Meaning / 含义 |
|
||||
|---|---|---|
|
||||
| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AE loop, the two floats below are ignored / 设备自动调 AE，下方参数被忽略<br>`1` = device locks AE and applies the provided values / 设备锁 AE 并应用提供的值 |
|
||||
| `exposure_time` | `0.0001` ~ `0.033` s (manual only / 仅手动模式) | Sensor exposure time per frame. Longer = brighter but more motion blur / 每帧传感器曝光时间。越长越亮但运动模糊增大 |
|
||||
| `gain` | `1.0` ~ `64.0` (manual only / 仅手动模式) | Analog gain. Higher = brighter output but worse SNR / 模拟增益。越大越亮但信噪比越差 |
|
||||
|
||||
**`SetAwb.Request`**
|
||||
|
||||
| Field | Range / 范围 | Meaning / 含义 |
|
||||
|---|---|---|
|
||||
| `mode` | `0` (AUTO) or / 或 `1` (MANUAL) | `0` = device runs its own AWB loop / 设备自动 AWB<br>`1` = device locks AWB and applies provided gains / 设备锁定 AWB 并应用所给增益 |
|
||||
| `rgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | R channel gain. Higher `rgain` vs `bgain` shifts the image warm (yellow/red) / R 通道增益，相对 bgain 越大，画面越偏暖 |
|
||||
| `bgain` | `0.1` ~ `4.0` (manual only / 仅手动模式) | B channel gain. Higher `bgain` vs `rgain` shifts the image cool (blue) / B 通道增益，相对 rgain 越大，画面越偏冷 |
|
||||
|
||||
> Gr / Gb channels are fixed to 1.0 by the device and are not adjustable.
|
||||
> Gr / Gb 通道被设备固定为 1.0，不可调节。
|
||||
|
||||
#### 4.6.2 Response fields / 响应字段
|
||||
|
||||
All four services return a `success` (bool) and `rc` (int32). Get
|
||||
services additionally return the queried state.
|
||||
4 个 Service 都返回 `success` (bool) 与 `rc` (int32)。Get 类还会返回查询到的状态字段。
|
||||
|
||||
**`GetAe.Response`**
|
||||
|
||||
| Field | Typical range / 典型范围 | Meaning / 含义 |
|
||||
|---|---|---|
|
||||
| `exposure_time` | `0.0001`~`0.033` s | Current exposure / 当前曝光时间 |
|
||||
| `gain` | `1.0`~`64.0` | Current analog gain / 当前模拟增益 |
|
||||
| `iso` | `100`~`6400` | Equivalent ISO / 等效 ISO |
|
||||
| `brightness` | `0`~`255` | Average frame brightness / 平均帧亮度 |
|
||||
| `is_converged` | `0` or `1` | `1` = AE settled / AE 已收敛 |
|
||||
| `env_lv` | `0`~`15` | Ambient luminance index, higher = brighter / 环境光强度指数，越大越亮 |
|
||||
| `fps` | `~10` / `~14.5` / `~29` | Current frame rate / 当前帧率 |
|
||||
|
||||
**`GetAwb.Response`**
|
||||
|
||||
| Field | Typical range / 典型范围 | Meaning / 含义 |
|
||||
|---|---|---|
|
||||
| `rgain` / `bgain` | `0.1`~`4.0` | R / B channel gain / R / B 通道增益 |
|
||||
| `grgain` / `gbgain` | `1.0` (fixed / 固定) | Gr / Gb gain, device-fixed / Gr / Gb 增益，设备固定 |
|
||||
| `cct` | `2500`~`8000` K | Correlated color temperature / 相关色温 |
|
||||
| `ccri` | `-50`~`50` | Color temp deviation index, 0 = on Planckian locus / 色温偏离指数，0 表示在普朗克轨迹上 |
|
||||
| `is_converged` | `0` or `1` | `1` = AWB settled / AWB 已收敛 |
|
||||
|
||||
#### 4.6.3 `rc` return code / `rc` 返回码
|
||||
|
||||
| `rc` | Meaning / 含义 |
|
||||
|---|---|
|
||||
| `0` | Success / 成功 |
|
||||
| `400` | Device payload too short / 设备载荷过短 |
|
||||
| `401` | Device opcode not supported / 设备不支持该 opcode |
|
||||
| `402` | Device parameter length wrong / 参数长度错误 |
|
||||
| `403` | **Parameter out of range** / 参数越界 — most common when manual values exceed the table above / 手动值超出上表范围时最常见 |
|
||||
| `404` | Device-side socket error / 设备端 socket 错误 |
|
||||
| `405` | Device-side `ae_control` did not respond / 设备端 `ae_control` 无应答（确认 lydapp 已运行） |
|
||||
| `255` (`0xFF`) | Unknown opcode reported by ae_control / ae_control 报未知 opcode |
|
||||
| `-1` | SDK not initialised / SDK 未初始化 |
|
||||
| `-2` ~ `-5` | USB transfer / timeout / malformed reply / USB 传输异常、超时、应答畸形 |
|
||||
| `-100` | **Driver has not opened the device yet** / driver 还未打开设备，请等设备连接成功 |
|
||||
|
||||
#### 4.6.4 Usage examples / 调用示例
|
||||
|
||||
ROS2 (Humble) — start the driver in one terminal, then in a side terminal:
|
||||
ROS2（Humble）—— 在一个终端启动 driver，在另一个终端：
|
||||
|
||||
```bash
|
||||
source install/setup.bash
|
||||
|
||||
# Query current state / 查询当前状态
|
||||
ros2 service call /odin1/get_ae odin_ros_driver/srv/GetAe
|
||||
ros2 service call /odin1/get_awb odin_ros_driver/srv/GetAwb
|
||||
|
||||
# Set AE to AUTO / 设置 AE 为自动
|
||||
ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}"
|
||||
|
||||
# Set AE to MANUAL with 10 ms exposure and gain 4.0
|
||||
# 设置 AE 为手动，10 毫秒曝光，增益 4.0
|
||||
ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe \
|
||||
"{mode: 1, exposure_time: 0.010, gain: 4.0}"
|
||||
|
||||
# Set AWB to MANUAL with rgain=1.5, bgain=2.0
|
||||
# 设置 AWB 为手动，rgain=1.5、bgain=2.0
|
||||
ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb \
|
||||
"{mode: 1, rgain: 1.5, bgain: 2.0}"
|
||||
|
||||
# Restore AUTO / 一键回自动
|
||||
ros2 service call /odin1/set_ae odin_ros_driver/srv/SetAe "{mode: 0}"
|
||||
ros2 service call /odin1/set_awb odin_ros_driver/srv/SetAwb "{mode: 0}"
|
||||
|
||||
# Inspect srv definition / 查看 srv 完整定义
|
||||
ros2 interface show odin_ros_driver/srv/SetAe
|
||||
```
|
||||
|
||||
ROS1 (Noetic) — start the driver, then in a side terminal:
|
||||
ROS1（Noetic）—— 启动 driver 后，新开终端：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
|
||||
# Query / 查询
|
||||
rosservice call /odin1/get_ae
|
||||
rosservice call /odin1/get_awb
|
||||
|
||||
# Set AE manual / 设置 AE 手动
|
||||
rosservice call /odin1/set_ae "{mode: 1, exposure_time: 0.010, gain: 4.0}"
|
||||
|
||||
# Set AWB manual / 设置 AWB 手动
|
||||
rosservice call /odin1/set_awb "{mode: 1, rgain: 1.5, bgain: 2.0}"
|
||||
|
||||
# Restore AUTO (ROS1 requires all fields to be present)
|
||||
# 一键回自动（ROS1 要求填齐全部字段）
|
||||
rosservice call /odin1/set_ae "{mode: 0, exposure_time: 0.0, gain: 0.0}"
|
||||
rosservice call /odin1/set_awb "{mode: 0, rgain: 0.0, bgain: 0.0}"
|
||||
|
||||
# Inspect srv definition / 查看 srv 完整定义
|
||||
rossrv show odin_ros_driver/SetAe
|
||||
```
|
||||
|
||||
#### 4.6.5 Recommended starting points by scene / 不同场景推荐起步参数
|
||||
|
||||
**AE (`exposure_time`, `gain`)**
|
||||
|
||||
| Scene / 场景 | `exposure_time` | `gain` |
|
||||
|---|---|---|
|
||||
| Bright outdoor / 明亮室外 | `0.001` ~ `0.005` s | `1.0` ~ `2.0` |
|
||||
| Normal indoor / 普通室内 | `0.008` ~ `0.015` s | `2.0` ~ `8.0` |
|
||||
| Dim light / 暗光环境 | `0.020` ~ `0.030` s | `8.0` ~ `32.0` |
|
||||
| Very dark / 极暗 | `0.033` s | `32.0` ~ `64.0` |
|
||||
|
||||
**AWB (`rgain`, `bgain`)**
|
||||
|
||||
| Target tone / 目标色调 | `rgain` | `bgain` |
|
||||
|---|---|---|
|
||||
| Warm (tungsten, sunset) / 暖（钨丝灯、夕阳） | `2.0` ~ `2.5` | `1.0` ~ `1.2` |
|
||||
| Neutral (D65 daylight) / 中性（D65 日光） | `1.5` ~ `1.7` | `1.8` ~ `2.0` |
|
||||
| Cool (cloudy, fluorescent) / 冷（阴天、荧光） | `1.2` ~ `1.4` | `2.2` ~ `2.6` |
|
||||
| Very cool / 极冷 | `1.0` | `3.0` ~ `4.0` |
|
||||
|
||||
#### 4.6.6 Caveats / 注意事项
|
||||
|
||||
- The service blocks for up to ~10 s waiting for the device to reply;
|
||||
typical latency is tens of milliseconds.
|
||||
Service 最长阻塞约 10 秒等设备应答；正常几十毫秒返回。
|
||||
- Manual mode is **not** persisted across driver / device restart;
|
||||
it falls back to AUTO on each new connection.
|
||||
手动模式**不会**跨重启保留；每次重连默认回到 AUTO。
|
||||
- `rc = -100` means the driver has not yet opened the device.
|
||||
Wait until the driver logs `device connected` before calling.
|
||||
返回 `rc = -100` 表示 driver 还没打开设备，等到 driver 日志显示 `device connected` 再调用。
|
||||
- The effective maximum `exposure_time` is bounded by the frame
|
||||
period `1 / fps`. With `dtof_fps = 290` (29 Hz, period ~34 ms)
|
||||
the upper limit 0.033 s is already at the frame boundary.
|
||||
最大可用 `exposure_time` 受帧周期 `1/fps` 限制。在 `dtof_fps = 290`（29 Hz、周期 ~34 ms）下，上限 0.033 s 已经贴到帧边界。
|
||||
|
||||
## 5. FAQ
|
||||
### 5.1 Segmentation fault upon re-launching host SDK
|
||||
**Error Message**
|
||||
No device connected after 60 seconds
|
||||
|
||||
**Solution**
|
||||
1. Please power on Odin module again # Disconnect and reconnect odin power
|
||||
|
||||
2. Reinitialize Odin SDK # Execute SDK after device reboot
|
||||
|
||||
|
||||
### 5.2 Library binding failure during compilation
|
||||
|
||||
**Error Message**
|
||||
ld: cannot find -llydHostApi or symbol lookup errors
|
||||
|
||||
**Resolution**
|
||||
|
||||
1. Clean previous build artifacts
|
||||
|
||||
ROS1
|
||||
```shell
|
||||
rm -rf devel/ build/
|
||||
```
|
||||
ROS2
|
||||
```shell
|
||||
rm -rf devel/ install/ log/
|
||||
```
|
||||
2. Re-run script installation
|
||||
|
||||
### 5.3 Docker GUI passthrough failure
|
||||
|
||||
**Error Message**
|
||||
Unable to open X display or No protocol specified
|
||||
|
||||
**Resolution**
|
||||
```shell
|
||||
xhost + #This command enables graphical passthrough to Docker containers
|
||||
```
|
||||
|
||||
### 5.4 ROS driver exit with get version failed error
|
||||
|
||||
**Error Message**
|
||||
```shell
|
||||
<ERROR><api.cpp:lidar_get_version:672>: get device version fail.
|
||||
get version failed.
|
||||
```
|
||||
|
||||
**Resolution**
|
||||
|
||||
Device firmware version is too low, please update to latest version.
|
||||
|
||||
|
||||
### 5.5 RVIZ has not responded for a long time
|
||||
|
||||
**Error Message**
|
||||
Rviz does not respond, and after a while the terminal prints Device disconnected, waiting for reconnection...
|
||||
|
||||
**Resolution**
|
||||
|
||||
Please power on Odin module again
|
||||
|
||||
### 5.6 Device not responding
|
||||
|
||||
**Error Message**
|
||||
Missed ok response from device,probably wrong interaction procedure.
|
||||
|
||||
**Resolution**
|
||||
|
||||
Please adopt the solution mentioned in 5.1
|
||||
|
||||
### 5.7 Device has no external calibration file
|
||||
|
||||
**Error Message**
|
||||
ERROR：Missing camera node 'cam_0'
|
||||
|
||||
**Resolution**
|
||||
|
||||
Please plug and unplug the USB again
|
||||
|
||||
### 5.8 ROS Driver report device disconnected immediately after stream started
|
||||
|
||||
**Error Message**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
Device detaching...
|
||||
Wating for device reconnection...
|
||||
Device disconnected, waiting for reconnection...
|
||||
```
|
||||
|
||||
**Reason**
|
||||
|
||||
Mostly common on ros2 environment and connected to complex network environment, such as office wifi & ethernet. ROS2 default to broadcast, and complex network environment will cause ros2 publish to block, leading to device disconnection.
|
||||
|
||||
**Resolution**
|
||||
|
||||
If cross-device communication is not required, please restrict ros2 to localhost only with:
|
||||
```shell
|
||||
export ROS_LOCALHOST_ONLY=1
|
||||
```
|
||||
|
||||
If cross-device communication is required, please simplify the network environment as much as possible. Mini local network with only required devices is recommended.
|
||||
|
||||
### 5.9 ROS Driver died immediately after stream started
|
||||
|
||||
**Error Message**
|
||||
|
||||
```shell
|
||||
Device ready and streams activated
|
||||
[host_sdk_sample-2] process has died ......
|
||||
```
|
||||
|
||||
**Test**
|
||||
|
||||
Disable odin1/image with sendrgb = 0 in control_command.yaml and try again. If the driver now works, it is likely that the issue is related to multiple version of opencv is installed on the system.
|
||||
|
||||
**Resolution**
|
||||
|
||||
Purge the unused version of opencv and maintain a single complete version, then rebuild the driver and try again.
|
||||
|
||||
### 5.10 ROS Driver printing "TF_OLD_DATA ignoring data" warning
|
||||
|
||||
**Error Message**
|
||||
|
||||
```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
|
||||
```
|
||||
|
||||
**Reason**
|
||||
|
||||
This is a ros & rviz feature to warn user that some tf data is being ignored due to timestamp conflicts. It happens when user keeps ros driver running and power-cycles odin device, which cause odin's internal system time being reset and now data timestamps conflicts with old data recieved by rviz during last run.
|
||||
|
||||
**Resolution**
|
||||
|
||||
There's a reset button on bottom of rviz gui. Click on this button will reset rviz's internal state and stop the warning.
|
||||
|
||||
### 5.11 ROS Driver printing "unknown cmd code: xx" error
|
||||
|
||||
**Error Message**
|
||||
|
||||
```shell
|
||||
<ERROR><api.cpp:cmd_data_deal:418>: unknow command code 21.
|
||||
```
|
||||
|
||||
**Reason**
|
||||
|
||||
This is due to ros driver version mismatch with device firmware version, resulting in ros driver unable to decode new data added in newer firmware.
|
||||
|
||||
**Resolution**
|
||||
|
||||
Please make sure you are using most up-to-date ros driver and device firmware.
|
||||
|
||||
### 5.12 USB device access error (LIBUSB_ERROR_BUSY or LIBUSB_ERROR_ACCESS)
|
||||
|
||||
**Error Message**
|
||||
|
||||
```shell
|
||||
libusb: error [udev_hotplug_event] ignoring udev action bind
|
||||
LIBUSB_ERROR_BUSY
|
||||
```
|
||||
|
||||
or
|
||||
|
||||
```shell
|
||||
libusb: error [_get_usbfs_fd] libusb couldn't open USB device /dev/bus/usb/xxx/xxx, errno=13
|
||||
LIBUSB_ERROR_ACCESS
|
||||
```
|
||||
|
||||
**Reason**
|
||||
|
||||
- **LIBUSB_ERROR_BUSY**: Another process is already using the USB device. This commonly happens when multiple instances of the ROS driver are running, or another application (such as a previous crashed instance) still holds the device handle.
|
||||
|
||||
- **LIBUSB_ERROR_ACCESS**: The current user does not have permission to access the USB device. This is typically caused by missing udev rules or insufficient user privileges.
|
||||
|
||||
**Resolution**
|
||||
|
||||
For **LIBUSB_ERROR_BUSY**:
|
||||
|
||||
1. Check if another instance of the driver is running:
|
||||
```shell
|
||||
ps aux | grep host_sdk_sample
|
||||
```
|
||||
|
||||
2. Kill any existing instances:
|
||||
```shell
|
||||
killall host_sdk_sample
|
||||
```
|
||||
|
||||
3. If the issue persists, unplug and replug the USB device to reset the device state.
|
||||
|
||||
For **LIBUSB_ERROR_ACCESS**:
|
||||
|
||||
1. Add udev rules for the device. Create a file `/etc/udev/rules.d/99-odin.rules` with the following content:
|
||||
```shell
|
||||
SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"
|
||||
```
|
||||
|
||||
2. Reload udev rules:
|
||||
```shell
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
3. Alternatively, run the driver with sudo (not recommended for production):
|
||||
```shell
|
||||
sudo -E ros2 launch odin_ros_driver odin_ros_driver.launch.py
|
||||
```
|
||||
|
||||
4. Make sure your user is in the `plugdev` group:
|
||||
```shell
|
||||
sudo usermod -aG plugdev $USER
|
||||
```
|
||||
Then log out and log back in for the group change to take effect.
|
||||
|
||||
### 5.13 ros2 bag drops high-frequency topics (IMU / odometry_highfreq) / ros2 bag 录制丢失高频话题（IMU / odometry_highfreq）
|
||||
|
||||
**Symptom / 现象**
|
||||
|
||||
When recording with `ros2 bag record`, low-frequency topics (cloud, image, odometry, wiwc) are intact, but `/odin1/imu` (400 Hz) and `/odin1/odometry_highfreq` (400 Hz) show missing samples — analysis scripts report inter-message intervals that are 2× or more of the expected period, while no drop is reported on the SDK side or by an online subscriber such as `ros2 topic hz`.
|
||||
|
||||
使用 `ros2 bag record` 录制时，低频话题（cloud、image、odometry、wiwc）完整无丢，但 `/odin1/imu`（400 Hz）和 `/odin1/odometry_highfreq`（400 Hz）会出现丢帧——分析脚本上看到消息间隔达到正常周期的 2 倍以上，而 SDK 侧不报丢，独立的 `ros2 topic hz` 订阅者也看不到丢。
|
||||
|
||||
**Reason / 原因**
|
||||
|
||||
The driver publishes `/odin1/imu` and `/odin1/odometry_highfreq` with `RELIABLE` QoS. By default `ros2 bag record` subscribes with `history = keep_last`, `depth = 10`, which only buffers ~25 ms of samples at 400 Hz. Whenever the recorder is briefly delayed (disk flush, mcap/sqlite chunk write, scheduler jitter), its subscription queue overflows and DDS silently drops the oldest samples on the **subscriber side**. The SDK and publisher are unaffected, which is why no drop appears in the driver logs or in `ros2 topic hz`.
|
||||
|
||||
驱动以 `RELIABLE` QoS 发布 `/odin1/imu` 与 `/odin1/odometry_highfreq`。`ros2 bag record` 默认订阅使用 `history = keep_last`、`depth = 10`，在 400 Hz 下只能缓冲约 25 ms。一旦录制端有短暂阻塞（落盘 flush、mcap/sqlite chunk 写入、调度抖动），订阅队列就会溢出，DDS 在**订阅端**静默丢掉最旧的样本。SDK 与 publisher 不受影响，因此驱动日志和 `ros2 topic hz` 都看不到丢。
|
||||
|
||||
**Resolution / 解决方案**
|
||||
|
||||
Use the provided QoS override file `script/rosbag2_qos.yaml` to raise the subscriber-side queue depth on the recorder for the two high-rate topics:
|
||||
|
||||
使用本仓库提供的 QoS 配置 `script/rosbag2_qos.yaml`，把高频话题的录制订阅 depth 拉大：
|
||||
|
||||
```yaml
|
||||
# script/rosbag2_qos.yaml
|
||||
/odin1/imu:
|
||||
reliability: reliable
|
||||
history: keep_last
|
||||
depth: 4000
|
||||
|
||||
/odin1/odometry_highfreq:
|
||||
reliability: reliable
|
||||
history: keep_last
|
||||
depth: 4000
|
||||
```
|
||||
|
||||
Apply it when recording / 录制时通过 `--qos-profile-overrides-path` 应用：
|
||||
|
||||
```shell
|
||||
ros2 bag record -a \
|
||||
--qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
|
||||
-o my_bag
|
||||
```
|
||||
|
||||
Or only the high-rate topics / 也可以只录制高频话题：
|
||||
|
||||
```shell
|
||||
ros2 bag record \
|
||||
--qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
|
||||
-o my_bag \
|
||||
/odin1/imu /odin1/odometry_highfreq /odin1/odometry /odin1/wiwc /odin1/cloud_raw
|
||||
```
|
||||
|
||||
**Optional further tuning / 可选的进一步优化**
|
||||
|
||||
If drops still occur after applying the override (typically on slower disks), try the following in addition / 套用上述 override 后仍有丢包时（通常发生在慢盘上），可叠加以下措施：
|
||||
|
||||
```shell
|
||||
# Use mcap backend with a larger internal cache (faster than sqlite3).
|
||||
# 使用 mcap 后端 + 更大的内部缓存（比 sqlite3 快）。
|
||||
ros2 bag record -s mcap --max-cache-size 1073741824 \
|
||||
--qos-profile-overrides-path src/odin_ros_driver/script/rosbag2_qos.yaml \
|
||||
-o my_bag \
|
||||
/odin1/imu /odin1/odometry_highfreq ...
|
||||
|
||||
# Enlarge kernel UDP socket buffers (the most common hidden bottleneck for
|
||||
# 400 Hz RELIABLE traffic, default is only 208 KB).
|
||||
# 放大内核 UDP socket buffer（400 Hz RELIABLE 流量最常见的隐藏瓶颈，默认仅 208 KB）。
|
||||
sudo sysctl -w net.core.rmem_max=33554432
|
||||
sudo sysctl -w net.core.wmem_max=33554432
|
||||
```
|
||||
|
||||
**Does ROS1 have the same problem? / ROS1 是否存在同样的问题？**
|
||||
|
||||
No. ROS1 uses TCP-based publish/subscribe with a single `queue_size` parameter on each side, and has no QoS profile mismatch between publisher and subscriber. The ROS1 publisher path in this driver already sizes the IMU and `odometry_highfreq` publishers to `queue_size = 4000` (`include/host_sdk_sample.h`, see `initialize_publishers` ROS1 branch), and `rosbag record` uses TCP transport which is reliable by construction. As a result this specific drop pattern does not occur under ROS1; no additional configuration is required.
|
||||
|
||||
不存在。ROS1 使用基于 TCP 的发布/订阅，发布端与订阅端各自只有一个 `queue_size` 参数，不存在 ROS2 那种 QoS profile 不匹配的问题。本驱动 ROS1 路径已经把 IMU 与 `odometry_highfreq` 的发布队列设置为 `queue_size = 4000`（见 `include/host_sdk_sample.h` 中 `initialize_publishers` 的 ROS1 分支），并且 `rosbag record` 使用 TCP 传输本身即可靠传递。因此在 ROS1 下不会出现该丢帧现象，也不需要额外配置。
|
||||
|
||||
## 6. Contact Information
|
||||
|
||||
You can contact our support through support@manifoldtech.cn
|
||||
|
||||
To help diagnose the issue, please provide the following details to our FAE engineer:
|
||||
|
||||
1. Current firmware version
|
||||
```shell
|
||||
[device_version_capture]: ros_driver_version: [Version Number]
|
||||
```
|
||||
2. Photos of power adapter and converter cable in use.
|
||||
|
||||
3. Does the issue happen occasionally or consistently?
|
||||
|
||||
4. Provide images of the problem scenario.
|
||||
|
||||
5. Did the troubleshooting methods in Section V resolve the issue?
|
||||
|
||||
6. Expected timeline for issue resolution.
|
||||
@@ -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,108 @@
|
||||
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: 2 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
|
||||
custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
|
||||
relocalization_map_abs_path: "/absolute/path/to/1hao.bin" # required for Relocalization mode; edit on the target computer
|
||||
|
||||
# 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,110 @@
|
||||
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
|
||||
|
||||
# Screen LOC=ODOM profile. Keep Odin in odometry mode; sim2real_web_udp_bridge_node
|
||||
# may publish the pure-odom map->odom fallback anchor.
|
||||
custom_map_mode: 0 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
|
||||
custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
|
||||
relocalization_map_abs_path: "" # unused in odometry mode
|
||||
|
||||
# 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
|
||||
+110
@@ -0,0 +1,110 @@
|
||||
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
|
||||
|
||||
# Screen LOC=RELOC profile. Odin loads relocalization_map_abs_path and publishes
|
||||
# the map/odom TF after relocalization succeeds; pure-odom fallback is disabled.
|
||||
custom_map_mode: 2 # 0: Odometry mode 1: SLAM mode 2: Relocalization mode.
|
||||
custom_init_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
|
||||
custom_init_pose_search_radius: 4.0 # max position offset in meters, recommended <= 10
|
||||
custom_init_pose_max_rot_deg: 180.0 # max rotation offset in degrees, up to 180
|
||||
relocalization_map_abs_path: "/absolute/path/to/1hao.bin" # required for Relocalization mode; edit on the target computer
|
||||
|
||||
# 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 ;
|
||||
};
|
||||
+122
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
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>
|
||||
// cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
|
||||
// Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
|
||||
#if defined(__has_include)
|
||||
# if __has_include(<cv_bridge/cv_bridge.hpp>)
|
||||
# include <cv_bridge/cv_bridge.hpp>
|
||||
# else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
# endif
|
||||
#else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
#endif
|
||||
#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_;
|
||||
};
|
||||
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
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>
|
||||
// cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
|
||||
// Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
|
||||
#if defined(__has_include)
|
||||
# if __has_include(<cv_bridge/cv_bridge.hpp>)
|
||||
# include <cv_bridge/cv_bridge.hpp>
|
||||
# else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
# endif
|
||||
#else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
#endif
|
||||
#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);
|
||||
};
|
||||
@@ -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,101 @@
|
||||
/*
|
||||
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>
|
||||
// cv_bridge header: ROS2 humble+ provides cv_bridge.hpp; ROS2 jazzy removes the legacy .h.
|
||||
// Prefer .hpp when available, fall back to .h for older ROS distros (e.g. foxy/galactic).
|
||||
#if defined(__has_include)
|
||||
# if __has_include(<cv_bridge/cv_bridge.hpp>)
|
||||
# include <cv_bridge/cv_bridge.hpp>
|
||||
# else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
# endif
|
||||
#else
|
||||
# include <cv_bridge/cv_bridge.h>
|
||||
#endif
|
||||
#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,726 @@
|
||||
#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);
|
||||
|
||||
/**
|
||||
* @brief Reset the USB connection to the device
|
||||
*
|
||||
* Performs a USB port reset on the device (libusb_reset_device). This
|
||||
* re-enumerates the device on the host without requiring a physical
|
||||
* re-plug. The device handle should typically be reopened after the
|
||||
* device reconnects.
|
||||
*
|
||||
* @param device Handle to the target device
|
||||
* @return int 0 on success, negative error code on failure
|
||||
*/
|
||||
int lidar_reset_usb(device_handle device);
|
||||
|
||||
/**
|
||||
* @brief Query the current device initial/running state.
|
||||
*
|
||||
* Returns the latest state reported by the device through heartbeats. The
|
||||
* value corresponds to ::lidar_device_initial_state_e (NONE,
|
||||
* NOT_INITIALIZED, INITIALIZED, STREAMING, STREAM_STOPPED). Callers that
|
||||
* need to wait until the device is fully booted (for example before
|
||||
* uploading a relocalization map) should poll this until it becomes
|
||||
* LIDAR_DEVICE_STREAMING (or LIDAR_DEVICE_INITIALIZED at minimum).
|
||||
*
|
||||
*
|
||||
* @param state Output pointer that receives the current state. Must not be NULL.
|
||||
* @return int 0 on success, negative error code on failure.
|
||||
*/
|
||||
int lidar_get_device_state(lidar_device_initial_state_e *state);
|
||||
|
||||
/**
|
||||
* @brief Send a host-defined pass-through user-data blob to the device.
|
||||
*
|
||||
* The device forwards data[] as-is to SLAM via shared memory
|
||||
* ("user_data_shm"). No acknowledgment is returned from the device
|
||||
* (fire-and-forget). An SDK-internal monotonic counter is packed into
|
||||
* the on-wire frameId field so SLAM can still distinguish consecutive
|
||||
* frames without the caller having to manage an id.
|
||||
*
|
||||
* Preconditions:
|
||||
* - SDK initialized and the device opened (lidar_open_device).
|
||||
* - SLAM has been started on the device side; otherwise the device
|
||||
* silently drops the frame and logs a warning.
|
||||
*
|
||||
* Constraints:
|
||||
* - blob != NULL.
|
||||
* - 0 < blob_len <= 8 MiB (8 * 1024 * 1024).
|
||||
* - Sending faster than SLAM can consume causes device-side timeouts
|
||||
* and dropped frames. Pace according to SLAM throughput.
|
||||
*
|
||||
* @param device Handle returned by lidar_create_device / lidar_open_device.
|
||||
* @param blob Pointer to user payload.
|
||||
* @param blob_len Length in bytes.
|
||||
* @return int 0 on success, negative error code on failure.
|
||||
*/
|
||||
int lidar_send_user_data(device_handle device,
|
||||
const void *blob, uint32_t blob_len);
|
||||
|
||||
/* ---------------------------------------------------------------------
|
||||
* Camera AE / AWB control APIs.
|
||||
*
|
||||
* All four APIs are synchronous (send + wait response) and serialised by
|
||||
* an internal mutex (the same mutex protecting other control commands).
|
||||
* Return value convention:
|
||||
* == 0 success
|
||||
* > 0 device-side error, see lidar_ae_error_e (400..405 or 0xFF)
|
||||
* < 0 SDK-side error (not initialised, bad argument, USB failure,
|
||||
* response timeout, malformed reply, ...)
|
||||
*
|
||||
* Wire-level details: sdk/api/Host_USB_AE_Protocol.md.
|
||||
* ------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief Query current AE (auto exposure) state.
|
||||
*
|
||||
* Sends AE opcode 0x01 and decodes the 25-byte little-endian payload
|
||||
* returned by the device-side ae_control service.
|
||||
*
|
||||
* Output fields and their physical meaning:
|
||||
* exposure_time : current exposure time in seconds (manual range
|
||||
* 0.0001 .. 0.033; in auto mode it varies with
|
||||
* scene illumination).
|
||||
* gain : current analog gain (manual range 1.0 .. 64.0;
|
||||
* higher = brighter but noisier).
|
||||
* iso : equivalent ISO, typically 100 .. 6400.
|
||||
* brightness : average frame brightness (0 .. 255).
|
||||
* is_converged : 1 = AE has settled, 0 = still adjusting.
|
||||
* env_lv : ambient luminance index, typically 0 .. 15
|
||||
* (higher = brighter scene).
|
||||
* fps : actual frame rate, follows dtof_fps config
|
||||
* (~10 / 14.5 / 29 Hz).
|
||||
*
|
||||
* @param device Device handle returned by lidar_create_device / lidar_open_device.
|
||||
* @param out Output buffer, must not be NULL.
|
||||
* @return See return value convention above.
|
||||
*/
|
||||
int lidar_get_ae_info(device_handle device, lidar_ae_info_t *out);
|
||||
|
||||
/**
|
||||
* @brief Query current AWB (auto white balance) state.
|
||||
*
|
||||
* Sends AE opcode 0x30 and decodes the 25-byte little-endian payload.
|
||||
*
|
||||
* Output fields and their physical meaning:
|
||||
* rgain : R channel gain (manual range 0.1 .. 4.0).
|
||||
* grgain : Gr channel gain, always 1.0 (device-fixed).
|
||||
* gbgain : Gb channel gain, always 1.0 (device-fixed).
|
||||
* bgain : B channel gain (manual range 0.1 .. 4.0).
|
||||
* cct : correlated color temperature in Kelvin
|
||||
* (typically 2500 .. 8000 K).
|
||||
* ccri : color temperature deviation index (-50 .. 50,
|
||||
* signed; 0 means on the Planckian locus).
|
||||
* is_converged : 1 = AWB has settled, 0 = still adjusting.
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param out Output buffer, must not be NULL.
|
||||
* @return See return value convention above.
|
||||
*/
|
||||
int lidar_get_awb_info(device_handle device, lidar_awb_info_t *out);
|
||||
|
||||
/**
|
||||
* @brief Set AE mode and (in manual mode) exposure/gain.
|
||||
*
|
||||
* Behaviour:
|
||||
* - mode == LIDAR_CAM_MODE_AUTO : sends opcode 0x02 only;
|
||||
* exposure_time / gain are ignored.
|
||||
* - mode == LIDAR_CAM_MODE_MANUAL : sends opcode 0x03 to switch to
|
||||
* manual AE, then opcode 0x06 to
|
||||
* apply (exposure_time, gain).
|
||||
*
|
||||
* Parameter ranges and physical meaning
|
||||
* (manual mode only; out-of-range returns rc = 403):
|
||||
*
|
||||
* exposure_time : 0.0001 s .. 0.033 s
|
||||
* Sensor exposure time per frame. Longer = brighter but more
|
||||
* motion blur and lower effective fps if it exceeds the frame
|
||||
* period (1/fps). For dtof_fps = 290 (29 Hz, period ~34 ms) the
|
||||
* upper bound 0.033 s is already at the frame limit.
|
||||
*
|
||||
* gain : 1.0 .. 64.0
|
||||
* Analog gain applied to the raw sensor signal. Higher = brighter
|
||||
* output but worse SNR. Typical sweet spot: 1.0 .. 8.0 for daylight,
|
||||
* 8.0 .. 32.0 for indoor / dim light, 32.0 .. 64.0 only when image
|
||||
* must be visible at any cost.
|
||||
*
|
||||
* Recommended starting points by scene:
|
||||
* bright outdoor : exposure 0.001~0.005 s, gain 1.0~2.0
|
||||
* normal indoor : exposure 0.008~0.015 s, gain 2.0~8.0
|
||||
* dim light : exposure 0.020~0.030 s, gain 8.0~32.0
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param mode See lidar_cam_mode_e.
|
||||
* @param exposure_time Exposure time in seconds (manual mode only).
|
||||
* @param gain Analog gain (manual mode only).
|
||||
* @return See return value convention above.
|
||||
*/
|
||||
int lidar_set_ae_param(device_handle device, lidar_cam_mode_e mode,
|
||||
float exposure_time, float gain);
|
||||
|
||||
/**
|
||||
* @brief Set AWB mode and (in manual mode) R/B channel gains.
|
||||
*
|
||||
* Behaviour:
|
||||
* - mode == LIDAR_CAM_MODE_AUTO : sends opcode 0x31 only;
|
||||
* rgain / bgain are ignored.
|
||||
* - mode == LIDAR_CAM_MODE_MANUAL : sends opcode 0x32 to switch to
|
||||
* manual AWB, then opcode 0x33 to
|
||||
* apply (rgain, bgain). Gr/Gb are
|
||||
* fixed to 1.0 by the device.
|
||||
*
|
||||
* Parameter ranges and physical meaning
|
||||
* (manual mode only; out-of-range returns rc = 403):
|
||||
*
|
||||
* rgain : 0.1 .. 4.0
|
||||
* Multiplier on the R channel before color matrix. Higher rgain
|
||||
* relative to bgain shifts the image toward warm (yellow/red).
|
||||
*
|
||||
* bgain : 0.1 .. 4.0
|
||||
* Multiplier on the B channel. Higher bgain relative to rgain
|
||||
* shifts the image toward cool (blue).
|
||||
*
|
||||
* Color-temperature cookbook (approximate):
|
||||
* warm (tungsten, sunset) : rgain ~2.0..2.5, bgain ~1.0..1.2
|
||||
* neutral (daylight D65) : rgain ~1.5..1.7, bgain ~1.8..2.0
|
||||
* cool (cloudy, fluor.) : rgain ~1.2..1.4, bgain ~2.2..2.6
|
||||
*
|
||||
* @param device Device handle.
|
||||
* @param mode See lidar_cam_mode_e.
|
||||
* @param rgain R channel gain (manual mode only).
|
||||
* @param bgain B channel gain (manual mode only).
|
||||
* @return See return value convention above.
|
||||
*/
|
||||
int lidar_set_awb_param(device_handle device, lidar_cam_mode_e mode,
|
||||
float rgain, float bgain);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // LIDAR_API_H
|
||||
@@ -0,0 +1,443 @@
|
||||
#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;
|
||||
|
||||
/**
|
||||
* @brief Data types for lidar_data_callback_t
|
||||
*
|
||||
* Each type corresponds to a specific stream format in lidar_data_t.stream (capture_Image_List_t).
|
||||
*
|
||||
* ┌─────────────────────────────────────────────────────────────────────────────────────────────┐
|
||||
* │ LIDAR_DT_RAW_RGB │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: NV12 image data │
|
||||
* │ - pAddr: uint8_t* (Y plane followed by UV plane) │
|
||||
* │ - width: 1536, height: 1280 │
|
||||
* │ - length: width * height * 3 / 2 bytes │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_RAW_IMU │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: IMU data │
|
||||
* │ - pAddr: imu_convert_data_t* │
|
||||
* │ - accel[3]: float (m/s^2) │
|
||||
* │ - gyro[3]: float (rad/s) │
|
||||
* │ - stamp: uint64_t (ns) │
|
||||
* │ - sequence: uint64_t │
|
||||
* │ - length: sizeof(imu_convert_data_t) │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_RAW_DTOF │
|
||||
* │ imageCount: 4 │
|
||||
* │ Resolution: 256 x 192 │
|
||||
* │ imageList[0]: Depth image │
|
||||
* │ - pAddr: float* (depth in meters) │
|
||||
* │ - length: 256 * 192 * sizeof(float) │
|
||||
* │ imageList[1]: Point cloud XYZ │
|
||||
* │ - pAddr: float* (x,y,z interleaved) │
|
||||
* │ - length: 256 * 192 * 3 * sizeof(float) │
|
||||
* │ imageList[2]: Confidence │
|
||||
* │ - pAddr: uint8_t* │
|
||||
* │ - length: 256 * 192 * sizeof(uint8_t) │
|
||||
* │ imageList[3]: Intensity/Reflectivity │
|
||||
* │ - pAddr: uint16_t* │
|
||||
* │ - length: 256 * 192 * sizeof(uint16_t) │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_SLAM_CLOUD │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: SLAM point cloud (XYZRGBA, fixed-point on the wire) │
|
||||
* │ - pAddr: slam_cloud_point_t* (7 * int32_t per point, see struct below) │
|
||||
* │ - length: num_points * sizeof(slam_cloud_point_t) ( == num_points * 28 bytes ) │
|
||||
* │ xyz are stored in 0.1 mm units: meters = xyz * SLAM_CLOUD_XYZ_TO_M │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_SLAM_ODOMETRY / LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ / LIDAR_DT_SLAM_ODOMETRY_TF │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: Odometry data │
|
||||
* │ - pAddr: ros_odom_convert_complete_t* │
|
||||
* │ - timestamp_ns: uint64_t │
|
||||
* │ - pos[3]: int64_t (x,y,z in μm, divide by 1e6 for meters) │
|
||||
* │ - orient[4]: int64_t (quaternion x,y,z,w, divide by 1e6) │
|
||||
* │ - linear_velocity[3]: int64_t │
|
||||
* │ - angular_velocity[3]: int64_t │
|
||||
* │ - pose_cov[36]: double │
|
||||
* │ - twist_cov[36]: double │
|
||||
* │ - length: sizeof(ros_odom_convert_complete_t) │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_DEV_STATUS │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: Device status │
|
||||
* │ - pAddr: lidar_device_status_t* │
|
||||
* │ - length: sizeof(lidar_device_status_t) │
|
||||
* ├─────────────────────────────────────────────────────────────────────────────────────────────┤
|
||||
* │ LIDAR_DT_NTP │
|
||||
* │ imageCount: 1 │
|
||||
* │ imageList[0]: PTP/NTP sync data │
|
||||
* │ - pAddr: ptp_sync_data_t* │
|
||||
* │ - delay: double │
|
||||
* │ - offset: double │
|
||||
* │ - length: sizeof(ptp_sync_data_t) │
|
||||
* └─────────────────────────────────────────────────────────────────────────────────────────────┘
|
||||
*/
|
||||
typedef enum {
|
||||
LIDAR_DT_NONE = 0, /**< No data */
|
||||
LIDAR_DT_RAW_RGB, /**< RGB image (NV12 format, 1536x1280) */
|
||||
LIDAR_DT_RAW_IMU, /**< IMU data (imu_convert_data_t) */
|
||||
LIDAR_DT_RAW_DTOF, /**< DTOF raw data (depth + xyz + confidence + intensity, 256x192) */
|
||||
LIDAR_DT_SLAM_CLOUD, /**< SLAM point cloud (XYZRGBA) */
|
||||
LIDAR_DT_SLAM_ODOMETRY, /**< SLAM odometry (ros_odom_convert_complete_t) */
|
||||
LIDAR_DT_DEV_STATUS, /**< Device status (lidar_device_status_t) */
|
||||
LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,/**< High frequency odometry (ros_odom_convert_complete_t) */
|
||||
LIDAR_DT_SLAM_ODOMETRY_TF, /**< Map-Odom TF transform (ros_odom_convert_complete_t) */
|
||||
LIDAR_DT_SLAM_WIWC, /**< WIWC odometry */
|
||||
LIDAR_DT_NTP /**< PTP/NTP sync data (ptp_sync_data_t) */
|
||||
} 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]; // x, y, z in μm
|
||||
int64_t orient[4]; // quaternion x, y, z, w in 1e6 precision
|
||||
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 {
|
||||
double delay;
|
||||
double offset;
|
||||
} ptp_sync_data_t;
|
||||
|
||||
/* ---------------------------------------------------------------------
|
||||
* SLAM cloud wire format.
|
||||
*
|
||||
* One LIDAR_DT_SLAM_CLOUD point on the bus is a 7 * int32_t record:
|
||||
* xyz[0..2] : x, y, z in 0.1 mm fixed-point.
|
||||
* meters = xyz * SLAM_CLOUD_XYZ_TO_M.
|
||||
* rgba[0..3]: r, g, b, a; each stored in the low byte of an int32_t.
|
||||
*
|
||||
* Consumers (SDK hooks, ROS driver, etc.) should reference this struct
|
||||
* and the scale macro below as the single source of truth rather than
|
||||
* re-hardcoding the stride or the divisor.
|
||||
* ------------------------------------------------------------------- */
|
||||
#define SLAM_CLOUD_XYZ_TO_M (1.0e-4) /* device 0.1mm units -> meters */
|
||||
#define SLAM_CLOUD_XYZ_FROM_M (1.0e4) /* meters -> device 0.1mm units */
|
||||
|
||||
typedef struct {
|
||||
int32_t xyz[3]; /* x, y, z in 0.1 mm fixed-point */
|
||||
int32_t rgba[4]; /* r, g, b, a; only low byte of each is meaningful */
|
||||
} slam_cloud_point_t;
|
||||
|
||||
typedef struct icm_6aixs_data_t {
|
||||
int16_t aacx;
|
||||
int16_t aacy;
|
||||
int16_t aacz;
|
||||
int16_t gyrox;
|
||||
int16_t gyroy;
|
||||
int16_t gyroz;
|
||||
uint8_t valid;
|
||||
uint32_t nums;
|
||||
uint8_t fsync_pack;
|
||||
uint16_t interval;
|
||||
uint64_t stamp;
|
||||
} icm_6aixs_data_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 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 frame rate, offset: */
|
||||
int tx_odr;/*The actual rgb image sensor frame rate, offset: */
|
||||
|
||||
} lidar_rgb_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief DTOF Lidar frame rate
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr;/* dtof lidar sensor frame rate, offset: */
|
||||
int tx_odr;/*The actual dtof lidar sensor frame rate, offset: */
|
||||
int subframe_odr;/*DTOF 6行为一组 这个是组间隔时间*/
|
||||
short tx_temp;/* dtof lidar tx temp offset: */
|
||||
short rx_temp;/* dtof lidar rx temp offset: */
|
||||
|
||||
} lidar_dtof_sensor_status_t;
|
||||
|
||||
/**
|
||||
* @brief IMU Sensor
|
||||
*
|
||||
*/
|
||||
typedef struct{
|
||||
|
||||
int configured_odr;
|
||||
int tx_odr;
|
||||
|
||||
} lidar_imu_sensor_status_t;
|
||||
|
||||
typedef struct{
|
||||
|
||||
int package_temp;/*SOC整体温度*/
|
||||
// int bigcore_temp;/*大核集群温度：4*A76*/
|
||||
// int littlecore_temp;/*小核集群温度：4*A53*/
|
||||
int cpu_temp;
|
||||
int center_temp;/*SOC中心温度：4*A53*/
|
||||
int gpu_temp;/* GPU模块温度 */
|
||||
int npu_temp;/* NPU模块温度 */
|
||||
|
||||
} lidar_soc_thermal_t;
|
||||
typedef struct
|
||||
{
|
||||
double uptime_seconds;
|
||||
lidar_soc_thermal_t soc_thermal; /*offset: 0*/
|
||||
|
||||
int cpu_use_rate[8];/*cpu 使用率,offset: */
|
||||
int ram_use_rate;/*运行内存使用率 ,offset: */
|
||||
|
||||
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; /* Actual frame rate of slam cloud offset: */
|
||||
int slam_odom_tx_odr; /* Actual frame rate of slam odom offset: */
|
||||
int slam_odom_highfreq_tx_odr; /* Actual frame rate of slam odom offset: */
|
||||
|
||||
} 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 = 1,
|
||||
LIDAR_DEPTH_ODR_29HZ = 2,
|
||||
} lidar_depth_odr_e;
|
||||
|
||||
typedef struct {
|
||||
lidar_depth_odr_e odr;
|
||||
} lidar_depth_para_t;
|
||||
|
||||
/* ---------------------------------------------------------------------
|
||||
* Camera AE / AWB control types.
|
||||
*
|
||||
* The host SDK forwards AE/AWB requests through the USB control channel
|
||||
* (CMD_CODE_CONTROL_CMD + SYS_CONTROL_AE_UDP). The device-side lydapp
|
||||
* relays them via UDP loopback to its ISP service. See
|
||||
* sdk/api/Host_USB_AE_Protocol.md for the wire-level details.
|
||||
* ------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief AE/AWB control mode used by lidar_set_ae_param / lidar_set_awb_param.
|
||||
*
|
||||
* - AUTO : the device runs its own AE/AWB convergence loop. The two
|
||||
* float parameters of the corresponding Set call are ignored.
|
||||
* - MANUAL : the device locks AE/AWB and applies the user-supplied
|
||||
* (exposure_time, gain) or (rgain, bgain). Out-of-range
|
||||
* values are rejected with rc = 403
|
||||
* (LIDAR_AE_PARAM_OUT_OF_RANGE).
|
||||
*/
|
||||
typedef enum {
|
||||
LIDAR_CAM_MODE_AUTO = 0, /**< switch to auto AE / AWB */
|
||||
LIDAR_CAM_MODE_MANUAL = 1, /**< switch to manual AE / AWB and apply params */
|
||||
} lidar_cam_mode_e;
|
||||
|
||||
/**
|
||||
* @brief Current AE status returned by lidar_get_ae_info().
|
||||
*
|
||||
* Field-by-field meaning and typical range:
|
||||
*
|
||||
* exposure_time : current sensor exposure time, in seconds.
|
||||
* Manual-mode valid range: 0.0001 .. 0.033.
|
||||
* In auto mode varies with scene illumination.
|
||||
* gain : current analog gain (linear, not dB).
|
||||
* Manual-mode valid range: 1.0 .. 64.0.
|
||||
* Higher value = brighter output but worse SNR.
|
||||
* iso : equivalent ISO speed, typically 100 .. 6400.
|
||||
* Derived from gain; informational only.
|
||||
* brightness : average frame brightness in [0, 255]. AE target
|
||||
* converges toward a mid-range value.
|
||||
* is_converged : 1 = AE has settled, 0 = still adjusting.
|
||||
* env_lv : ambient luminance index, typically 0 .. 15
|
||||
* (higher = brighter scene).
|
||||
* fps : actual frame rate in Hz, follows dtof_fps config
|
||||
* (~10 / 14.5 / 29).
|
||||
*/
|
||||
typedef struct {
|
||||
float exposure_time; /**< current exposure time (s), 0.0001..0.033 */
|
||||
float gain; /**< current analog gain, 1.0..64.0 */
|
||||
int32_t iso; /**< equivalent ISO, ~100..6400 */
|
||||
float brightness; /**< average frame brightness, 0..255 */
|
||||
uint8_t is_converged; /**< 1 = AE converged, 0 = not converged */
|
||||
float env_lv; /**< ambient luminance level, ~0..15 */
|
||||
float fps; /**< current frame rate (Hz) */
|
||||
} lidar_ae_info_t;
|
||||
|
||||
/**
|
||||
* @brief Current AWB status returned by lidar_get_awb_info().
|
||||
*
|
||||
* Field-by-field meaning and typical range:
|
||||
*
|
||||
* rgain : R channel gain. Manual-mode valid range: 0.1 .. 4.0.
|
||||
* Raising rgain relative to bgain shifts the image
|
||||
* toward warm (yellow/red).
|
||||
* grgain : Gr channel gain. Device-fixed at 1.0, not adjustable.
|
||||
* gbgain : Gb channel gain. Device-fixed at 1.0, not adjustable.
|
||||
* bgain : B channel gain. Manual-mode valid range: 0.1 .. 4.0.
|
||||
* Raising bgain relative to rgain shifts the image
|
||||
* toward cool (blue).
|
||||
* cct : correlated color temperature in Kelvin, typically
|
||||
* 2500 .. 8000 K.
|
||||
* ccri : color temperature deviation index, signed value
|
||||
* roughly in -50 .. 50; 0 = on the Planckian locus.
|
||||
* is_converged : 1 = AWB has settled, 0 = still adjusting.
|
||||
*/
|
||||
typedef struct {
|
||||
float rgain; /**< R channel gain, 0.1..4.0 */
|
||||
float grgain; /**< Gr channel gain, device-fixed 1.0 */
|
||||
float gbgain; /**< Gb channel gain, device-fixed 1.0 */
|
||||
float bgain; /**< B channel gain, 0.1..4.0 */
|
||||
float cct; /**< color temperature (K), ~2500..8000 */
|
||||
float ccri; /**< color temperature deviation, ~-50..50 */
|
||||
uint8_t is_converged; /**< 1 = AWB converged, 0 = not converged */
|
||||
} lidar_awb_info_t;
|
||||
|
||||
/**
|
||||
* @brief AE/AWB device-side error codes.
|
||||
*
|
||||
* Mapped to positive return values of lidar_get_ae_info / lidar_set_ae_param /
|
||||
* lidar_get_awb_info / lidar_set_awb_param when the device replies with
|
||||
* CMD_CODE_FAIL. See Host_USB_AE_Protocol.md section 6.
|
||||
*/
|
||||
typedef enum {
|
||||
LIDAR_AE_OK = 0,
|
||||
LIDAR_AE_BAD_REQUEST = 400, /**< payload too short */
|
||||
LIDAR_AE_UNSUPPORTED_OPCODE = 401,
|
||||
LIDAR_AE_BAD_PARAM_LEN = 402,
|
||||
LIDAR_AE_PARAM_OUT_OF_RANGE = 403,
|
||||
LIDAR_AE_SOCKET_ERROR = 404,
|
||||
LIDAR_AE_NO_RESPONSE = 405, /**< ae_control UDP timeout */
|
||||
LIDAR_AE_UNKNOWN_OPCODE = 0xFF,/**< status byte from ae_control */
|
||||
} lidar_ae_error_e;
|
||||
|
||||
#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>
|
||||
@@ -0,0 +1,108 @@
|
||||
|
||||
# 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'
|
||||
)
|
||||
|
||||
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)
|
||||
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,33 @@
|
||||
<?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>
|
||||
<depend>ament_index_cpp</depend>
|
||||
<!-- AE/AWB debug services (custom .srv files in srv/) -->
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
<!-- Specify build type as ament -->
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,33 @@
|
||||
<?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>
|
||||
|
||||
<!-- AE/AWB debug services (custom .srv files in srv/) -->
|
||||
<build_depend>message_generation</build_depend>
|
||||
<exec_depend>message_runtime</exec_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,33 @@
|
||||
<?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>
|
||||
<depend>ament_index_cpp</depend>
|
||||
<!-- AE/AWB debug services (custom .srv files in srv/) -->
|
||||
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
<!-- 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,172 @@
|
||||
#!/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.
|
||||
# 1) Prefer the currently active distro (ROS_DISTRO env var) if its setup.bash exists.
|
||||
# 2) Otherwise probe a known list of ROS2 distros from newest to oldest.
|
||||
ROS2_DISTRO_CANDIDATES=("rolling" "jazzy" "iron" "humble" "galactic" "foxy")
|
||||
ROS2_SETUP_BASH=""
|
||||
|
||||
if [ -n "${ROS_DISTRO}" ] && [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then
|
||||
ROS2_SETUP_BASH="/opt/ros/${ROS_DISTRO}/setup.bash"
|
||||
else
|
||||
for distro in "${ROS2_DISTRO_CANDIDATES[@]}"; do
|
||||
if [ -f "/opt/ros/${distro}/setup.bash" ]; then
|
||||
ROS2_SETUP_BASH="/opt/ros/${distro}/setup.bash"
|
||||
break
|
||||
fi
|
||||
done
|
||||
fi
|
||||
|
||||
if [ -n "${ROS2_SETUP_BASH}" ]; then
|
||||
echo -e "${GREEN}Sourcing ROS2 environment: ${ROS2_SETUP_BASH}${NC}"
|
||||
# shellcheck disable=SC1090
|
||||
source "${ROS2_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,18 @@
|
||||
# rosbag2_qos.yaml
|
||||
# QoS overrides for rosbag2 recording
|
||||
# 用于 rosbag2 录制的 QoS 配置
|
||||
#
|
||||
# Usage / 使用方法:
|
||||
# ros2 bag record -a --qos-profile-overrides-path rosbag2_qos.yaml
|
||||
|
||||
# High frequency data (IMU 400Hz, Odom 100Hz)
|
||||
# 高频数据 (IMU 400Hz, 里程计 100Hz)
|
||||
/odin1/imu:
|
||||
reliability: reliable
|
||||
history: keep_last
|
||||
depth: 4000
|
||||
|
||||
/odin1/odometry_highfreq:
|
||||
reliability: reliable
|
||||
history: keep_last
|
||||
depth: 4000
|
||||
@@ -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"
|
||||
@@ -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,13 @@
|
||||
# Query current AE (auto exposure) state.
|
||||
# Request: empty.
|
||||
# Response: success + raw return code + decoded fields.
|
||||
---
|
||||
bool success # true if rc == 0
|
||||
int32 rc # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
|
||||
float32 exposure_time # seconds (manual range 0.0001 .. 0.033)
|
||||
float32 gain # analog gain
|
||||
int32 iso # equivalent ISO
|
||||
float32 brightness # average frame brightness
|
||||
uint8 is_converged # 1=converged, 0=not converged
|
||||
float32 env_lv # ambient luminance level
|
||||
float32 fps # current frame rate
|
||||
@@ -0,0 +1,13 @@
|
||||
# Query current AWB (auto white balance) state.
|
||||
# Request: empty.
|
||||
# Response: success + raw return code + decoded fields.
|
||||
---
|
||||
bool success # true if rc == 0
|
||||
int32 rc # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
|
||||
float32 rgain
|
||||
float32 grgain
|
||||
float32 gbgain
|
||||
float32 bgain
|
||||
float32 cct # color temperature in Kelvin
|
||||
float32 ccri # color temperature deviation
|
||||
uint8 is_converged # 1=converged, 0=not converged
|
||||
@@ -0,0 +1,14 @@
|
||||
# Set AE (auto exposure) mode and (in manual mode) exposure / gain.
|
||||
#
|
||||
# mode == 0 (AUTO) : sends opcode 0x02 only; exposure_time and gain ignored.
|
||||
# mode == 1 (MANUAL) : sends opcode 0x03 then 0x06 (exposure_time, gain).
|
||||
#
|
||||
# Valid manual ranges (device-enforced; out-of-range returns rc=403):
|
||||
# exposure_time : 0.0001 s .. 0.033 s
|
||||
# gain : 1.0 .. 64.0
|
||||
uint8 mode
|
||||
float32 exposure_time
|
||||
float32 gain
|
||||
---
|
||||
bool success
|
||||
int32 rc # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
|
||||
@@ -0,0 +1,15 @@
|
||||
# Set AWB (auto white balance) mode and (in manual mode) R/B gains.
|
||||
#
|
||||
# mode == 0 (AUTO) : sends opcode 0x31 only; rgain and bgain ignored.
|
||||
# mode == 1 (MANUAL) : sends opcode 0x32 then 0x33 (rgain, bgain).
|
||||
# Gr/Gb are fixed to 1.0 by the device.
|
||||
#
|
||||
# Valid manual ranges:
|
||||
# rgain : 0.1 .. 4.0
|
||||
# bgain : 0.1 .. 4.0
|
||||
uint8 mode
|
||||
float32 rgain
|
||||
float32 bgain
|
||||
---
|
||||
bool success
|
||||
int32 rc # 0=ok, >0=device error (400..405 or 0xFF), <0=SDK error
|
||||
@@ -0,0 +1,11 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(sim2real_bringup)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,103 @@
|
||||
# =============================================================================
|
||||
# 部署契约参考文件（仅供参考，C++ 代码不读取此文件）
|
||||
# =============================================================================
|
||||
#
|
||||
# ⚠️ 注意：所有部署参数（电机映射、动作缩放、默认姿态等）均硬编码在
|
||||
# sim2real_common/include/sim2real_common/deployment_contract.hpp 中。
|
||||
# 本 YAML 文件仅作为可读参考，修改此文件不会影响运行时行为！
|
||||
# 如需修改部署参数，请同步更新 .hpp 文件和本文件。
|
||||
#
|
||||
# =============================================================================
|
||||
|
||||
model:
|
||||
path: "policies/model_rough.onnx"
|
||||
source_pt: "policies/model_rough.pt"
|
||||
backend: "onnxruntime"
|
||||
obs_dim: 53
|
||||
action_dim: 16
|
||||
clip_obs: 100.0
|
||||
|
||||
observation:
|
||||
terms:
|
||||
- {name: base_ang_vel, dim: 3, scale: 0.25}
|
||||
- {name: projected_gravity, dim: 3}
|
||||
- {name: command, dim: 3}
|
||||
- {name: joint_pos_rel, dim: 12}
|
||||
- {name: joint_vel_rel, dim: 12, scale: 0.05}
|
||||
- {name: wheel_vel, dim: 4, scale: 0.05}
|
||||
- {name: last_actions, dim: 16}
|
||||
|
||||
action:
|
||||
joint_order:
|
||||
- fl_hip_abduction
|
||||
- fl_hip_pitch
|
||||
- fl_knee
|
||||
- fr_hip_abduction
|
||||
- fr_hip_pitch
|
||||
- fr_knee
|
||||
- rl_hip_abduction
|
||||
- rl_hip_pitch
|
||||
- rl_knee
|
||||
- rr_hip_abduction
|
||||
- rr_hip_pitch
|
||||
- rr_knee
|
||||
- fl_wheel
|
||||
- fr_wheel
|
||||
- rl_wheel
|
||||
- rr_wheel
|
||||
wheel_indices: [12, 13, 14, 15]
|
||||
scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
|
||||
default_dof_pos: [0.0, 0.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,304 @@
|
||||
/**:
|
||||
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_6800_fp16.engine
|
||||
prefer_tensorrt: true
|
||||
model_path: policies/model_6800.onnx
|
||||
rough_model_engine_path: policies/model_6800_fp16.engine
|
||||
crawl_model_path: policies/model_crawl.onnx # unused while crawl_backend is "ik"
|
||||
crawl_model_engine_path: ""
|
||||
wall_model_path: policies/model_84.onnx
|
||||
wall_model_engine_path: policies/model_84_fp16.engine
|
||||
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]
|
||||
wall_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: 12.5
|
||||
crawl_ik_wheel_yaw_gain: 8.0
|
||||
crawl_ik_max_wheel_speed: 12.0
|
||||
crawl_ik_abduction_clip: 0.45
|
||||
crawl_ik_yaw_rate_kp: 0.5
|
||||
crawl_ik_imu_posture: false
|
||||
crawl_ik_encoder_posture_kp: 0.0
|
||||
crawl_ik_encoder_posture_max: 0.03
|
||||
crawl_ik_encoder_guard: false
|
||||
crawl_ik_encoder_guard_start: 0.28
|
||||
crawl_ik_encoder_guard_stop: 0.65
|
||||
crawl_ik_imu_guard: true
|
||||
crawl_ik_imu_guard_start_deg: 12.0
|
||||
crawl_ik_imu_guard_stop_deg: 28.0
|
||||
model_switch_transition_s: 0.9
|
||||
model_switch_min_transition_s: 0.4
|
||||
model_switch_to_stand_transition_scale: 2.1
|
||||
model_switch_to_model_transition_scale: 2.4
|
||||
model_switch_stand_hold_s: 0.45
|
||||
model_switch_stand_max_err: 0.18
|
||||
model_switch_stand_max_vel: 0.8
|
||||
model_switch_release_scale: 1.0
|
||||
runtime_max_vx: 0.9
|
||||
runtime_max_vy: 0.5
|
||||
runtime_max_yaw_rate: 0.85
|
||||
debug_trace_enabled: true
|
||||
debug_trace_decimation: 1
|
||||
use_cuda: true # enable CUDA Execution Provider on 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 first-generation Python deployment
|
||||
remote_enabled: false # 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.9
|
||||
remote_max_vy: 0.5
|
||||
remote_max_yaw_rate: 0.85
|
||||
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.9
|
||||
cmd_mux_max_vy: 0.5
|
||||
cmd_mux_max_yaw_rate: 0.85
|
||||
cmd_mux_max_vx_acc: 1.0
|
||||
cmd_mux_max_vy_acc: 1.0
|
||||
cmd_mux_max_yaw_acc: 1.5
|
||||
cmd_mux_max_vx_decel: 2.0
|
||||
cmd_mux_max_vy_decel: 2.0
|
||||
cmd_mux_max_yaw_decel: 2.0
|
||||
# The rough locomotion policy has an approximately 0.2 m/s linear command dead zone.
|
||||
# Skip that ineffective band on start-up, but still allow exact zero for braking/estop.
|
||||
cmd_mux_linear_deadzone_epsilon: 0.05
|
||||
cmd_mux_yaw_deadzone_epsilon: 0.02
|
||||
cmd_mux_min_effective_vx: 0.22
|
||||
cmd_mux_min_effective_vy: 0.22
|
||||
cmd_mux_min_effective_yaw_rate: 0.0
|
||||
cmd_mux_deadzone_sources: "nav"
|
||||
|
||||
# Windows/Nano Web UDP bridge parameters
|
||||
web_bridge_enabled: true
|
||||
web_http_host: "0.0.0.0"
|
||||
web_http_port: 18080
|
||||
web_static_dir: ""
|
||||
# Odom task actual path export; files can be opened by nav_tools over the PCD.
|
||||
odom_trace_export_dir: "map/load"
|
||||
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.9
|
||||
web_udp_max_vy: 0.3
|
||||
web_udp_max_yaw_rate: 0.85
|
||||
web_udp_estop_on_timeout: false
|
||||
|
||||
# Safety parameters
|
||||
safety_enabled: true
|
||||
max_target_offset: 2.4
|
||||
model_switch_max_target_offset: 1.8
|
||||
hard_target_offset: 3.0
|
||||
max_ang_vel: 30.0
|
||||
max_tilt_z: -0.3
|
||||
clip_to_brake: 0
|
||||
imu_age_warn_ms: 60.0
|
||||
imu_age_stop_ms: 500.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 first-generation Python deployment
|
||||
command_release_s: 0.35
|
||||
release_command_hold_s: 0.12
|
||||
release_posture_max_err: 0.35
|
||||
release_target_blend_s: 0.30
|
||||
clip_obs: 100.0
|
||||
hold_zero_command_pose: true
|
||||
enable_zero_cmd_suppression: true
|
||||
require_active_command_to_release: true
|
||||
zero_cmd_use_yaw_rate: true
|
||||
|
||||
# Simple navigation parameters
|
||||
localization_mode: "relocal" # relocal: wait for Odin map/odom TF; odom: bridge map->odom fallback
|
||||
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.90
|
||||
nav_max_vy: 0.50
|
||||
nav_max_wz: 0.85
|
||||
nav_kp_dist: 0.80
|
||||
nav_kp_yaw: 1.80
|
||||
nav_goal_exit_tolerance_margin: 0.08
|
||||
nav_goal_complete_stable_cycles: 2
|
||||
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
|
||||
# Slalom is treated as a continuous path by simple_nav even if the route JSON
|
||||
# was saved without precisionFollow/stableCycles/lookahead metadata.
|
||||
nav_slalom_auto_precision_enabled: true
|
||||
nav_slalom_auto_precision_force: true
|
||||
nav_slalom_task_names: "slalom"
|
||||
nav_slalom_stable_cycles: 0
|
||||
nav_slalom_lookahead: 0.35
|
||||
nav_slalom_yaw_rate_limit: 0.45
|
||||
nav_slalom_tolerance: 0.15
|
||||
nav_slalom_max_vx: 0.58
|
||||
nav_slalom_min_vx: 0.22
|
||||
nav_slalom_curvature_slowdown_enabled: true
|
||||
nav_slalom_min_turn_speed_scale: 0.45
|
||||
# Execute waypoints marked slalomStraight as odometry-closed scripted moves.
|
||||
nav_slalom_script_enabled: true
|
||||
nav_slalom_script_start_tolerance: 0.22
|
||||
nav_slalom_script_pos_tolerance: 0.10
|
||||
nav_slalom_script_yaw_tolerance_deg: 5.0
|
||||
nav_slalom_script_drive_yaw_deadband_deg: 8.0
|
||||
nav_slalom_script_stable_cycles: 1
|
||||
nav_slalom_script_rotate_steps_enabled: false
|
||||
nav_slalom_script_final_rotate_enabled: false
|
||||
nav_slalom_script_require_yaw_at_step: false
|
||||
nav_slalom_script_kp_dist: 1.00
|
||||
nav_slalom_script_kp_yaw: 1.20
|
||||
nav_slalom_script_max_vx: 0.58
|
||||
nav_slalom_script_max_vy: 0.50
|
||||
nav_slalom_script_max_wz: 0.50
|
||||
nav_slalom_script_min_cmd_linear: 0.22
|
||||
nav_slalom_script_min_cmd_angular: 0.20
|
||||
nav_slalom_script_min_cmd_epsilon: 0.05
|
||||
nav_slalom_script_min_step_distance: 0.02
|
||||
nav_slalom_script_yaw_gate_deg: 8.0
|
||||
nav_slalom_script_lateral_gate: 0.07
|
||||
nav_slalom_script_lateral_slow_gate: 0.15
|
||||
nav_slalom_script_lateral_creep_vx: 0.22
|
||||
nav_slalom_script_drive_yaw_source: "segment"
|
||||
nav_slalom_script_segment_yaw_min_dist: 0.45
|
||||
nav_precision_lateral_control_enabled: true
|
||||
nav_precision_lateral_kp: 0.80
|
||||
nav_precision_lateral_max_vy: 0.22
|
||||
# Lightweight DWA-style local safety layer over the route/avoid polygons.
|
||||
nav_local_planner_enabled: true
|
||||
nav_local_planner_tasks: "slalom"
|
||||
nav_local_planner_precision_enabled: true
|
||||
nav_local_planner_sim_time: 0.9
|
||||
nav_local_planner_sim_dt: 0.1
|
||||
nav_local_planner_v_samples: 5
|
||||
nav_local_planner_w_samples: 7
|
||||
nav_local_planner_vy_samples: 3
|
||||
nav_local_planner_obstacle_margin: 0.08
|
||||
nav_local_planner_recovery_clearance_epsilon: 0.005
|
||||
# 0.0 means auto: use the nav_tools body+wheel lateral footprint.
|
||||
nav_local_planner_robot_radius: 0.0
|
||||
nav_local_planner_clearance_weight: 2.0
|
||||
nav_local_planner_path_weight: 2.0
|
||||
nav_local_planner_heading_weight: 0.7
|
||||
nav_local_planner_speed_weight: 0.3
|
||||
nav_local_planner_nominal_weight: 1.0
|
||||
nav_local_planner_min_vx: 0.22
|
||||
nav_slalom_script_safety_filter_enabled: true
|
||||
nav_local_planner_use_astar_grid: false
|
||||
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: ""
|
||||
nav_missions_file: ""
|
||||
# Keep the legacy YAML route for reference; active task is selected by nav_route_task_file below.
|
||||
nav_route_file: ""
|
||||
# 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/1hao_reall.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
|
||||
nav_avoid_regions_enabled: true
|
||||
nav_avoid_region_margin: 0.0
|
||||
# 0.0 means auto: use the nav_tools body+wheel lateral footprint for avoid-region inflation.
|
||||
nav_avoid_footprint_radius: 0.0
|
||||
nav_robot_body_length: 0.356
|
||||
nav_robot_body_width: 0.235
|
||||
nav_robot_body_center_x: 0.1518
|
||||
nav_robot_origin_from_front: 0.105
|
||||
nav_robot_pose_hip: 0.550
|
||||
nav_robot_pose_knee: -1.125
|
||||
nav_robot_wheel_vis_length: 0.16
|
||||
nav_robot_wheel_vis_width: 0.055
|
||||
nav_robot_footprint_padding: 0.02
|
||||
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/1hao.pcd
|
||||
pcd_floor_z_min: -1.6
|
||||
pcd_floor_z_max: 0.4
|
||||
pcd_sample_step: 25
|
||||
pcd_robot_radius: 0.18
|
||||
|
||||
|
||||
@@ -0,0 +1,173 @@
|
||||
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'
|
||||
)
|
||||
|
||||
localization_mode_arg = DeclareLaunchArgument(
|
||||
'localization_mode',
|
||||
default_value='relocal',
|
||||
description='Localization profile: odom uses bridge fallback; relocal waits for Odin map/odom TF'
|
||||
)
|
||||
|
||||
odin_config_file_arg = DeclareLaunchArgument(
|
||||
'odin_config_file',
|
||||
default_value=PathJoinSubstitution([
|
||||
FindPackageShare('odin_ros_driver'),
|
||||
'config',
|
||||
'control_command_relocal.yaml',
|
||||
]),
|
||||
description='Odin control config YAML for the selected localization profile'
|
||||
)
|
||||
|
||||
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',
|
||||
'config_file': LaunchConfiguration('odin_config_file'),
|
||||
}.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,
|
||||
localization_mode_arg,
|
||||
odin_config_file_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, {"localization_mode": LaunchConfiguration("localization_mode")}],
|
||||
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 = kLegKp;
|
||||
static constexpr float kLegHoldKd = kLegKd;
|
||||
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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user