Depth → Point Cloud — ROS2 Example

Source: rbq_sdk/ros2/src/rbq_examples/src/depth_to_pcl.cpp

A ROS 2 node that converts each camera's depth image into a PointCloud2 using the camera intrinsics.

How it works

For each numeric sensor sensor_0–sensor_5 it:

• subscribes to /rbq/vision/<sensor>/depth/compressed (sensor_msgs/CompressedImage, PNG-encoded 16UC1) and /rbq/vision/<sensor>/depth/camera_info,
• decodes the PNG depth frame (cv::imdecode with IMREAD_UNCHANGED) and deprojects each pixel with the pinhole model using fx, fy, cx, cy from the camera info — depth is in millimeters (z = depth × 0.001), and zero-depth pixels become NaN,
• publishes /rbq/vision/<sensor>/pointcloud (sensor_msgs/PointCloud2, unorganized XYZ).

sensor_0–sensor_3 are the body depth cameras, sensor_4 is the front camera, and sensor_5 the rear. Uses OpenCV for the PNG depth decode.

Prerequisites

• ROS 2 Humble + RBQ ROS2 SDK built — see ROS2 SDK Overview.
• A publisher of the compressed depth streams — the robot's vision stack, or the MuJoCo simulator with vision (bash scripts/sim.bash --vision). Both publish /rbq/vision/sensor_N/depth/compressed with matching camera_info.

Build & run

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

Inspect a resulting cloud, e.g. the front camera:

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