ROS 2 Voxel Grid Downsampler Node

Overview

This script implements a ROS 2 node, voxel_downsampler, that subscribes to a sensor_msgs/msg/PointCloud2 topic, applies a voxel-grid downsampling filter, and publishes the resulting, smaller point cloud.

The node is designed for robustness and simplicity, using only NumPy for its computations. It can handle arbitrary input point cloud field layouts and ensures the output is always a sanitized, consistent PointCloud2 message with the fields [x, y, z, intensity] (all FLOAT32).

Core Functionality

The node’s processing pipeline, triggered on each incoming message, is as follows:

1. Robust Parsing: The input sensor_msgs/msg/PointCloud2 message is converted into an (N, 4) NumPy array.

   • It uses the sensor_msgs_py.point_cloud2.read_points utility to correctly handle different field names, data types, and memory offsets in the input cloud.

   • It explicitly looks for x, y, z, and intensity.

   • If an intensity field is not found, it is synthesized with a default value of 1.0 to maintain a consistent [x, y, z, intensity] structure.

2. Sanitization: All points (rows) containing NaN or Inf values are removed.

3. Ground Filtering (Optional): If the filter_ground parameter is set to True, any point with a z coordinate less than or equal to the ground_threshold parameter is discarded.

4. Voxel Downsampling: The core filtering logic is applied.

   • Points are discretized into integer voxel indices based on the voxel_size parameter.

   • The node computes the centroid (mean) of all point attributes [x, y, z, intensity] within each unique voxel.

   • Optionally discards voxels that contain fewer than min_points_per_voxel points.

5. Publishing: The downsampled NumPy array (now shape (M, 4), where M <= N) is converted back into a new sensor_msgs/msg/PointCloud2 message.

   • This output message always has the fixed, 16-byte layout: [x(float32), y(float32), z(float32), intensity(float32)].

   • The header.frame_id and header.stamp are copied from the input message to maintain spatial and temporal consistency.

   • If the resulting cloud is empty after filtering, nothing is published.

ROS 2 Interface

Parameters:

Voxel Downsampler Parameters

Parameter	Value (from params.yaml)	Meaning
voxel_size	0.2	The edge length (in meters) of a single voxel cube.
min_points_per_voxel	1	The minimum number of points a voxel must contain to be included in the output.
filter_ground	false	If True, enables the simple z-based ground removal filter.
ground_threshold	-1.5	The z height (in meters) to use for ground filtering. Points at or below this height are removed.
input_topic	"/kitti/pointcloud_raw"	The ROS 2 topic to subscribe to for incoming PointCloud2 messages.
output_topic	"preprocessing/downsampled_cloud"	The ROS 2 topic to publish the filtered and downsampled PointCloud2 messages on.
output_qos_reliability	"reliable" (Script Default)	The QoS reliability policy for the publisher. This parameter was not specified in params.yaml, so it uses the script’s default.

Subscriptions:

• <input_topic> (sensor_msgs/msg/PointCloud2):

  • Subscribes using qos_profile_sensor_data (typically BEST_EFFORT).

Publications:

• <output_topic> (sensor_msgs/msg/PointCloud2):

  • Publishes using a configurable QoS (defaults to RELIABLE for easier debugging with tools like ros2 topic echo).

  • The output format is always 16-byte points: [x, y, z, intensity], all FLOAT32.

Launch Files:

preprocessing.launch.py

File: src/lidar_preprocessing/launch/preprocessing.launch.py

This launch file starts the voxel downsampling node.

lidar_preprocessing/launch/preprocessing.launch.py

"""
Launch file for the LiDAR preprocessing pipeline and KITTI data loader.

This launch file starts:
  - `kitti_publisher`: Publishes raw KITTI LiDAR and pose data.
  - `voxel_downsampler`: Subscribes to raw point clouds, performs voxel-grid
    downsampling, and publishes the result.

Parameters Loaded:
  - `preprocessing_params.yaml`: Settings for voxel size, topics, and filtering.
  - `kitti_data_loader/config/params.yaml`: KITTI dataset and topic settings.
"""
from launch import LaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    Generate the launch description for preprocessing and KITTI data loading.

    - Launches the KITTI publisher node (from `kitti_data_loader.launch.py`)
    - Launches the voxel downsampler node (from `lidar_preprocessing`)
    """
    # Paths to configuration files
    preprocessing_config = PathJoinSubstitution(
        [FindPackageShare("lidar_preprocessing"), "config", "params.yaml"]
    )

    # Include the KITTI data loader launch file
    kitti_loader_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("kitti_data_loader"), "launch", "kitti_data_loader.launch.py"]
            )
        )
    )

    # Define the voxel downsampler node
    downsampler_node = Node(
        package="lidar_preprocessing",
        executable="voxel_downsampler",
        name="voxel_downsampler",
        parameters=[preprocessing_config],
        output="screen",
    )

    # Launch both the KITTI loader and the preprocessing node
    return LaunchDescription([kitti_loader_launch, downsampler_node])

Compile and Run

1. Build the Workspace

Build the workspace (if not built already):

colcon build --symlink-install
source install/setup.bash

2. Run and Visualize

You will need two sourced ROS 2 terminals and the Foxglove Studio application.

Terminal 1: Start Foxglove Bridge Launch the websocket bridge that Foxglove Studio uses to communicate with ROS 2.

ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765

Foxglove Studio GUI

1. Open Foxglove Studio.

2. Navigate to Open Connection -> Foxglove WebSocket.

3. Ensure the URL is ws://localhost:8765 and click Open.

4. Load the pre-configured layout: Layouts -> Personal -> Add, and navigate to your workspace to select lidar_preprocessing/config/lidar_preprocessing_layout.json.

Terminal 2: Launch the KITTI Publisher Once the bridge and Foxglove are running and connected, launch both the kitti_data_loader` and the ``lidar_preprocessing nodes:

ros2 launch lidar_preprocessing preprocessing.launch.py