ROS 2 PointCloud2 & PCL Guide 2026

PointCloud2 processing pipeline

  LiDAR driver
      │ sensor_msgs/PointCloud2 on /scan_cloud
      ▼
  ┌─────────────────────────────────────────────┐
  │  PassThrough filter  (z: 0.0 → 2.0 m)      │ Remove floor/ceiling
  └────────────────────────┬────────────────────┘
                           │
  ┌────────────────────────▼────────────────────┐
  │  VoxelGrid filter  (leaf 0.05 m)            │ Downsample to 5 cm grid
  └────────────────────────┬────────────────────┘
                           │
  ┌────────────────────────▼────────────────────┐
  │  StatisticalOutlierRemoval                   │ Remove noise points
  └────────────────────────┬────────────────────┘
                           │
  ┌────────────────────────▼────────────────────┐
  │  SACSegmentation  (RANSAC plane)             │ Find & remove ground plane
  └────────────────────────┬────────────────────┘
                           │ non-ground points
  ┌────────────────────────▼────────────────────┐
  │  EuclideanClusterExtraction                  │ Cluster remaining objects
  └─────────────────────────────────────────────┘

Install:
  sudo apt install ros-jazzy-pcl-ros ros-jazzy-pcl-conversions
  sudo apt install libpcl-dev

Key types:
  sensor_msgs/PointCloud2  — ROS message (wire format)
  pcl::PointCloud<pcl::PointXYZ>  — PCL in-memory format
  pcl::PointCloud<pcl::PointXYZI> — with intensity
  pcl::PointCloud<pcl::PointXYZRGB> — with color

Conversion:
  pcl::fromROSMsg(ros_cloud, pcl_cloud)  — ROS → PCL
  pcl::toROSMsg(pcl_cloud, ros_cloud)    — PCL → ROS

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>

using sensor_msgs::msg::PointCloud2;
using PCLCloud = pcl::PointCloud<pcl::PointXYZ>;

class PointCloudFilterNode : public rclcpp::Node {
public:
  PointCloudFilterNode()
  : Node("pointcloud_filter")
  {
    sub_ = create_subscription<PointCloud2>(
      "/scan_cloud", rclcpp::SensorDataQoS(),
      [this](const PointCloud2::SharedPtr msg) { filter_callback(msg); }
    );
    pub_filtered_  = create_publisher<PointCloud2>("/cloud/filtered",  10);
    pub_downsampled_ = create_publisher<PointCloud2>("/cloud/downsampled", 10);
  }

private:
  void filter_callback(const PointCloud2::SharedPtr msg) {
    // ── 1. Convert ROS → PCL ─────────────────────────────────
    PCLCloud::Ptr input(new PCLCloud);
    pcl::fromROSMsg(*msg, *input);

    // ── 2. PassThrough: keep only z in [0.0, 2.0] ────────────
    PCLCloud::Ptr pass_result(new PCLCloud);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(input);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 2.0);
    pass.filter(*pass_result);

    // ── 3. StatisticalOutlierRemoval: remove noise ────────────
    PCLCloud::Ptr sor_result(new PCLCloud);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(pass_result);
    sor.setMeanK(50);           // analyze 50 neighbors
    sor.setStddevMulThresh(1.0); // remove points > 1 std from mean
    sor.filter(*sor_result);

    // Publish filtered cloud
    PointCloud2 filtered_msg;
    pcl::toROSMsg(*sor_result, filtered_msg);
    filtered_msg.header = msg->header;
    pub_filtered_->publish(filtered_msg);

    // ── 4. VoxelGrid: downsample to 5 cm voxels ─────────────
    PCLCloud::Ptr voxel_result(new PCLCloud);
    pcl::VoxelGrid<pcl::PointXYZ> voxel;
    voxel.setInputCloud(sor_result);
    voxel.setLeafSize(0.05f, 0.05f, 0.05f);  // 5 cm voxels
    voxel.filter(*voxel_result);

    RCLCPP_INFO(get_logger(),
      "Raw: %zu pts → filtered: %zu → voxel: %zu",
      input->size(), sor_result->size(), voxel_result->size());

    PointCloud2 downsampled_msg;
    pcl::toROSMsg(*voxel_result, downsampled_msg);
    downsampled_msg.header = msg->header;
    pub_downsampled_->publish(downsampled_msg);
  }

  rclcpp::Subscription<PointCloud2>::SharedPtr sub_;
  rclcpp::Publisher<PointCloud2>::SharedPtr pub_filtered_, pub_downsampled_;
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PointCloudFilterNode>());
  rclcpp::shutdown();
}

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>

// After VoxelGrid downsampling, separate ground from objects
void segment_ground(PCLCloud::Ptr cloud,
                    PCLCloud::Ptr& ground,
                    PCLCloud::Ptr& obstacles)
{
  // ── RANSAC plane segmentation ─────────────────────────────
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(1000);
  seg.setDistanceThreshold(0.01);  // 1 cm tolerance from plane
  seg.setInputCloud(cloud);
  seg.segment(*inliers, *coefficients);

  if (inliers->indices.empty()) {
    RCLCPP_WARN(rclcpp::get_logger("seg"), "No plane found");
    return;
  }

  // ── Extract inliers (ground) and outliers (obstacles) ────
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud(cloud);
  extract.setIndices(inliers);

  ground = PCLCloud::Ptr(new PCLCloud);
  extract.setNegative(false);  // keep inliers = ground
  extract.filter(*ground);

  obstacles = PCLCloud::Ptr(new PCLCloud);
  extract.setNegative(true);   // keep outliers = obstacles
  extract.filter(*obstacles);
}

// ── Euclidean Cluster Extraction — find individual objects ──
std::vector<pcl::PointIndices> cluster_obstacles(PCLCloud::Ptr obstacles)
{
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
    new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(obstacles);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(0.15);  // 15 cm between clusters
  ec.setMinClusterSize(50);      // at least 50 points per object
  ec.setMaxClusterSize(25000);
  ec.setSearchMethod(tree);
  ec.setInputCloud(obstacles);
  ec.extract(cluster_indices);

  return cluster_indices;  // one entry per detected object
}

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
import numpy as np


class PointCloudPythonNode(Node):
    def __init__(self):
        super().__init__("pointcloud_python")
        from rclpy.qos import QoSProfile, ReliabilityPolicy
        qos = QoSProfile(depth=10,
                         reliability=ReliabilityPolicy.BEST_EFFORT)
        self.sub = self.create_subscription(
            PointCloud2, "/scan_cloud", self.callback, qos
        )

    def callback(self, msg: PointCloud2):
        # ── Convert PointCloud2 → numpy array ────────────────
        # Returns structured array with fields: x, y, z (and more)
        cloud_array = pc2.read_points_numpy(
            msg,
            field_names=("x", "y", "z"),
            skip_nans=True
        )
        # shape: (N, 3)  dtype: float32

        self.get_logger().info(f"Points: {cloud_array.shape[0]}")

        # ── PassThrough: keep z 0.0 → 2.0 m ─────────────────
        mask = (cloud_array[:, 2] >= 0.0) & (cloud_array[:, 2] <= 2.0)
        filtered = cloud_array[mask]

        # ── Distance filter: keep within 10 m range ──────────
        dist = np.linalg.norm(filtered[:, :2], axis=1)
        filtered = filtered[dist < 10.0]

        # ── Voxel downsampling (simple numpy approach) ────────
        voxel_size = 0.05  # 5 cm
        keys = np.floor(filtered / voxel_size).astype(int)
        # Unique voxel centers
        _, unique_idx = np.unique(keys, axis=0, return_index=True)
        downsampled = filtered[unique_idx]

        self.get_logger().info(
            f"Filtered: {filtered.shape[0]} → Downsampled: {downsampled.shape[0]}"
        )

        # ── Open3D processing (optional) ──────────────────────
        # import open3d as o3d
        # pcd = o3d.geometry.PointCloud()
        # pcd.points = o3d.utility.Vector3dVector(filtered)
        # pcd = pcd.voxel_down_sample(voxel_size=0.05)
        # plane_model, inliers = pcd.segment_plane(0.01, 3, 1000)


def main():
    rclpy.init()
    rclpy.spin(PointCloudPythonNode())
    rclpy.shutdown()

# Install: pip install open3d
# sudo apt install ros-jazzy-sensor-msgs-py

# ── Inspect a PointCloud2 topic ─────────────────────────────
# Field names, point step, width, height
ros2 topic echo /scan_cloud --once

# Check field names (x y z intensity ring time, etc.)
ros2 topic echo /scan_cloud --field fields

# Publication rate
ros2 topic hz /scan_cloud

# Bandwidth
ros2 topic bw /scan_cloud

# ── RViz2 visualization ──────────────────────────────────────
# 1. Open RViz2: ros2 run rviz2 rviz2
# 2. Set Fixed Frame to your LiDAR frame (e.g. velodyne, lidar_link)
# 3. Add → By Topic → /scan_cloud → PointCloud2
# 4. Style: FlatSquares, size 0.02 m
# 5. Color: Axis z (height) or Intensity

# ── pcl_ros nodelets / composable nodes ─────────────────────
# VoxelGrid node (no code needed)
ros2 run pcl_ros filter_voxel_grid_node \
  --ros-args \
  -r input:=/scan_cloud \
  -r output:=/cloud/voxel \
  -p leaf_size:=0.05

# PassThrough node
ros2 run pcl_ros filter_passthrough_node \
  --ros-args \
  -r input:=/scan_cloud \
  -r output:=/cloud/pass \
  -p filter_field_name:=z \
  -p filter_limit_min:=0.0 \
  -p filter_limit_max:=2.0

# CMakeLists.txt dependencies:
# find_package(PCL REQUIRED)
# find_package(pcl_conversions REQUIRED)
# target_link_libraries(node PCL::PCL)