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].
4. Compute 2D range and angle in the merged frame.
5. Round to the nearest angular bin and take the minimum with the current bin value.

File truncated at 100 lines see the full file