Depth → Point Cloud — ROS2 예제

소스: rbq_sdk/ros2/src/rbq_examples/src/depth_to_pcl.cpp

각 카메라의 깊이 이미지를 카메라 내부 파라미터로 PointCloud2로 변환하는 ROS 2 노드입니다.

동작 방식

번호형 센서 sensor_0–sensor_5 각각에 대해:

• /rbq/vision/<sensor>/depth/compressed(sensor_msgs/CompressedImage, PNG 인코딩 16UC1)와 /rbq/vision/<sensor>/depth/camera_info를 구독하고,
• PNG depth 프레임을 디코드(cv::imdecode, IMREAD_UNCHANGED)한 뒤 camera info의 fx, fy, cx, cy로 핀홀 모델을 사용해 각 픽셀을 역투영(deproject)합니다 — depth는 밀리미터이며(z = depth × 0.001), depth가 0인 픽셀은 NaN이 됩니다,
• /rbq/vision/<sensor>/pointcloud(sensor_msgs/PointCloud2, 비정렬 XYZ)를 발행합니다.

sensor_0–sensor_3은 몸체 depth 카메라, sensor_4는 전방, sensor_5는 후방 카메라입니다. PNG depth 디코드에 OpenCV를 사용합니다.

사전 요구사항

• ROS 2 Humble + RBQ ROS2 SDK 빌드 — ROS2 SDK 개요 참조.
• 압축 depth 스트림을 발행하는 측 — 로봇 비전 스택 또는 비전 포함 MuJoCo 시뮬레이터(bash scripts/sim.bash --vision). 둘 다 /rbq/vision/sensor_N/depth/compressed와 매칭되는 camera_info를 발행합니다.

빌드 & 실행

cd RBQ/rbq_sdk/ros2
colcon build --packages-up-to rbq_examples
source install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 run rbq_examples depth_to_pcl

생성된 클라우드를 확인하려면(예: 전방 카메라):

ros2 topic echo --once /rbq/vision/sensor_4/pointcloud