nvblox_ws（ROS 2 Humble）

English README

本工作空间在 Jetson 等设备上集成 nvblox 深度融合与可选的 TSDF 点云 / 彩色网格输出，并附带 PLY 导出脚本 与 Launch 示例。包名：nvblox_ros2_humble。

数据集验证

本流程（深度 + CameraInfo + TF，world_frame:=odom，TSDF / 可选彩色网格及 PLY 导出）已在 RoboCup@Home 2024 Receptionist 公开 rosbag 上验证过。数据集见 Zenodo：Robocup@Home 2024: Receptionist Rosbags Dataset（ROS 2 Humble；含 rosbag2_2024_07_18-13_15_28_0.db3、rosbag2_2024_07_18-13_56_14_0.db3 等）。DOI：10.5281/zenodo.13902011。

---

1. 根目录辅助脚本

文件	作用
save_tsdf_ply.py	订阅 /nvblox/tsdf（PointCloud2，xyz + intensity），保存为 ASCII PLY；支持单次与每 N 秒带时间戳批量保存；订阅 QoS 默认 best_effort（与节点发布一致）。
save_mesh_ply.py	订阅 /nvblox/mesh（visualization_msgs/Marker，TRIANGLE_LIST），导出带 RGB 顶点色的 ASCII PLY；同样支持单次与周期保存；默认输出目录可放在 rgbd_test 下。

2. run_nvblox.launch.py

• 示例 Launch：参数 + 话题 remap，便于与 bag / 实机话题对齐。
• 注意：节点实际订阅的话题名由参数 depth_topic / camera_info_topic 决定（默认不是 /camera/depth/image）。若 remap 与参数不一致，请以 run_template.txt 里显式 -p depth_topic:=... 为准，或在 Launch 的 parameters 里写上相同话题名。

3. src/nvblox_ros2_humble/run_template.txt

• 常用 ros2 run 参数模板与字段说明（头前相机 + TSDF + 可选彩色网格）。

---

环境与依赖

依据本仓库内 src/nvblox_ros2_humble/package.xml、src/nvblox_ros2_humble/CMakeLists.txt，以及嵌套 nvblox 的上游 CI（src/nvblox_ros2_humble/nvblox/.github/workflows/premerge.yml）。

• Operating System: Ubuntu 22.04（上游 nvblox 在该 workflow 中对 x86 使用 ubuntu-version: '22'。）
• ROS 2 Distribution: Humble（package.xml 包说明与依赖面向 Humble。）
• CUDA Toolkit: 需要 NVIDIA GPU 与 CUDA Toolkit；CMakeLists.txt 为 project(... LANGUAGES CXX CUDA) 且 find_package(CUDAToolkit REQUIRED)。可使用 11.8 或与驱动、本仓库所嵌 nvblox 构建相容的版本；上游 nvblox CI 在 Ubuntu 22 的 x86 任务中还配置 CUDA 11 与 12（src/nvblox_ros2_humble/nvblox/.github/workflows/premerge.yml 中 cuda-version）。
• CMake: ≥ 3.22（cmake_minimum_required(VERSION 3.22)）。

ROS 2 Packages（package.xml）：

• ament_cmake（buildtool_depend）
• rclcpp
• std_msgs
• sensor_msgs
• geometry_msgs
• visualization_msgs
• tf2
• tf2_ros
• tf2_eigen
• eigen3_cmake_module

Additional libraries: Eigen3（package.xml 中 build_depend / exec_depend）；在 Ubuntu 上通常对应 libeigen3-dev。

运行输入（nvblox_node）： 深度与 CameraInfo 宽高须一致；支持编码 16UC1 或 32FC1（实现见 src/nvblox_ros2_humble/src/nvblox_node.cpp）。

---

编译

source /opt/ros/humble/setup.bash
cd /home/run/rgbd_test/nvblox_ws
colcon build --symlink-install
source install/setup.bash

---

使用流程（推荐）

1. 回放 bag（或实机发布相同话题）

source /opt/ros/humble/setup.bash
ros2 bag play /path/to/your_bag.db3 --clock

按需为 bag 增加 read-ahead（大 bag 可减轻延迟）：

ros2 bag play /path/to/your_bag.db3 --clock --read-ahead-queue-size 5000

2. 启动 nvblox（仅 TSDF 点云）

source /opt/ros/humble/setup.bash
source /home/run/rgbd_test/nvblox_ws/install/setup.bash

ros2 run nvblox_ros2_humble nvblox_node --ros-args \
  -p world_frame:=odom \
  -p depth_topic:=/head_front_camera/depth/image_raw \
  -p camera_info_topic:=/head_front_camera/rgb/camera_info \
  -p depth_scale_for_uint16:=0.001 \
  -p tsdf_pointcloud_enable:=true \
  -p tsdf_pointcloud_topic:=/nvblox/tsdf \
  -p tsdf_pointcloud_period_sec:=1.0 \
  -p voxel_size_m:=0.05 \
  -p log_integration_progress:=true \
  -p integration_log_every_n_frames:=60

• world_frame 必须是 TF 中存在的父坐标系（常见为 odom）；节点通过 tf2 查询 world_frame → depth.header.frame_id，不需要单独的 /odom 话题。
• 启动后若日志先出现大量 Creating GPUHashImpl / Adding Layer，属初始化过程；随后应出现 Integrated N depth frames 与约 1 Hz 的 /nvblox/tsdf。

3. 加上颜色与网格（RViz / 导出彩色 PLY）

在上一段基础上增加：

  -p color_integration_enable:=true \
  -p color_topic:=/head_front_camera/rgb/image_raw \
  -p mesh_compute_enable:=true \
  -p mesh_output_enable:=true \
  -p mesh_marker_topic:=/nvblox/mesh \
  -p mesh_update_period_sec:=1.0

说明：/nvblox/tsdf 仍为 强度点云，不含 RGB；颜色在 /nvblox/mesh 的 Marker 中。

4. Launch 启动

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch /home/run/rgbd_test/nvblox_ws/run_nvblox.launch.py

若话题与模板不一致，请修改 Launch 里的 parameters 或 remappings，与 depth_topic / camera_info_topic 保持一致。

---

导出 PLY

TSDF 点云（无颜色）

单次：

python3 /home/run/rgbd_test/nvblox_ws/save_tsdf_ply.py \
  --topic /nvblox/tsdf \
  --out /home/run/rgbd_test/tsdf_snapshot.ply

每 5 秒一份、带时间戳（默认订阅 best_effort）：

python3 /home/run/rgbd_test/nvblox_ws/save_tsdf_ply.py \
  --topic /nvblox/tsdf \
  --out_dir /home/run/rgbd_test/tsdf_snaps \
  --every 5 \
  --timeout 180

彩色网格（需开启 mesh 输出）

单次：

python3 /home/run/rgbd_test/nvblox_ws/save_mesh_ply.py \
  --topic /nvblox/mesh \
  --out /home/run/rgbd_test/nvblox_mesh_color.ply

周期保存（默认目录 ~/rgbd_test/mesh_snaps，可用 --out_dir 修改）：

python3 /home/run/rgbd_test/nvblox_ws/save_mesh_ply.py \
  --topic /nvblox/mesh \
  --out_dir /home/run/rgbd_test/mesh_snaps \
  --every 5 \
  --timeout 180

---

## 验收与常见问题

```bash
ros2 topic hz /nvblox/tsdf
ros2 topic echo --once /head_front_camera/depth/image_raw --field encoding

• 若导出 TSDF 时出现 QoS RELIABILITY 不兼容：save_tsdf_ply.py 已默认 --reliability best_effort，一般无需改节点。
• 偶发 TF fail: extrapolation into the future：常见于 bag 回放时钟与 TF 时间略有偏差，多数帧仍可融合；可尝试 --clock 或放慢播放倍率。
• 日志 Logging before InitGoogleLogging()：初始化顺序提示，一般可忽略。

---

目录结构（与本 README 相关）

nvblox_ws/
├── README.md                 # 本文件（中文）
├── README_EN.md              # 英文版
├── run_nvblox.launch.py      # Launch 示例
├── save_tsdf_ply.py          # TSDF PointCloud2 → PLY
├── save_mesh_ply.py          # 彩色 Mesh Marker → PLY
└── src/nvblox_ros2_humble/   # ROS 2 包与 nvblox 封装
    ├── run_template.txt      # 参数模板与说明
    └── src/nvblox_node.cpp   # 含 main() 与参数解析修改

更细的参数含义见 src/nvblox_ros2_humble/run_template.txt。