2D-Scan Merger (ROS2)

A ROS2 composable node that merges any number of 2D LiDAR scans into a single unified sensor_msgs/LaserScan, with approximate-time synchronization and parallel ray projection.

Overview

Mobile robots equipped with multiple 2D LiDARs — for safety, navigation, or wider field of view — typically need a single merged scan for downstream algorithms like obstacle avoidance or SLAM. scan_2d_merger solves this by:

1. Subscribing to any number of LaserScan topics simultaneously.
2. Synchronising scans across topics using a timestamp-based approximate-time algorithm.
3. Transforming each scan into a common reference frame via TF2.
4. Projecting all rays directly into a shared angular bin array and publishing the result.

The node is implemented as a composable node (rclcpp_components), so it can run inside a shared component container with zero-copy intra-process communication, or as a standalone process depending on your deployment needs.

Features

• Unlimited input LiDARs — the custom approximate-time synchronizer imposes no cap on the number of input topics. Previous implementations based on message_filters::ApproximateTime were limited to a fixed number of inputs due to compile-time template constraints; that limitation is gone.

• Parallel ray projection — for N > 1 LiDARs, a persistent thread pool (one thread per LiDAR) projects all scans simultaneously using std::barrier (C++20) for cycle synchronization. Threads are created once at startup, eliminating per-callback overhead.

• No intermediate point cloud — rays are projected directly from polar coordinates in each scanner frame to angular bins in the merged frame. There is no conversion to PointCloud2 and no PCL dependency.

• Per-topic QoS — each input topic can independently use reliable or best_effort delivery policy, making it straightforward to mix LiDARs with different publishers.

• Static and dynamic scanner mounts — when moving_frames is false (the default), TF transforms are looked up once and cached for the lifetime of the node, which avoids repeated TF lookups on every callback. Set moving_frames: true for manipulator-mounted or otherwise moving scanners.

• Height filtering — after transforming each ray into the merged frame, points outside a configurable height band [min_height, max_height] are discarded. This is useful for filtering ground returns or ceiling reflections when LiDARs are tilted.

• Approximate-time synchronization with two-pass safety — the synchronizer first finds the best candidate scan per topic, verifies that all topics are within the configured sync_slop window, and only then dequeues any scans. If any topic fails the check, no scans are consumed, preventing silent data loss on partial match failures.

• Composable node — runs inside component_container for efficient multi-node deployments. A MutuallyExclusiveCallbackGroup ensures serial callback execution regardless of whether the container uses a single-threaded or multi-threaded executor.

How It Works

Synchronization

Each input topic has its own FIFO deque buffer. When a new scan arrives on any topic, the synchronizer:

1. Computes a reference timestamp as the newest stamp across all buffer heads.
2. Finds the scan closest in time to the reference for each topic (pass 1 — read-only).
3. If every topic has a candidate within sync_slop seconds, it dequeues them all (pass 2).
4. If any topic fails, no scans are consumed and the attempt is retried on the next arrival.

Parallel Projection

For N > 1 inputs, the main thread sets up N work items, then arrives at a start_barrier to release all worker threads simultaneously. Each worker independently projects its assigned scan’s rays into a private std::vector<float> range array. When all workers arrive at the done_barrier, the main thread folds the N arrays into the output by taking the minimum range at each angular bin. The worker threads persist across callbacks, avoiding the cost of thread creation at LiDAR rates.

For a single input, the projection runs directly on the main thread with no barriers or workers.

Ray Projection

For each valid ray (finite range, within [range_min, range_max]):

1. Convert polar (r, θ) to Cartesian (x, y, 0) in the scanner’s frame.
2. Apply the TF transform to get (X, Y, Z) in the merged frame.
3. Discard if Z falls outside [min_height, max_height].

File truncated at 100 lines see the full file