cartographer/sensor/internal/ordered_multi_queue.h

Overview

OrderedMultiQueue manages multiple sensor data queues, providing time-ordered dispatch across queues. Used by Collator to synchronize multi-sensor data.

Class: cartographer::sensor::OrderedMultiQueue

Construction

OrderedMultiQueue();

Queue Management

// Add a new queue for a sensor
void AddQueue(const std::string& sensor_id, 
              const std::function<void(std::unique_ptr<Data>)>& callback);

// Mark queue as finished (no more data expected)
void MarkQueueAsFinished(const std::string& sensor_id);

// Flush all queues (dispatch remaining data)
void Flush();

Dispatch

// Add data to a queue
void Add(const std::string& sensor_id, std::unique_ptr<Data> data);

Data is dispatched to the queue’s callback in timestamp order across all queues.

Algorithm

  1. Each sensor has a FIFO queue

  2. Add() pushes to sensor’s queue

  3. When all queues have data, finds earliest timestamp across queue fronts

  4. Dispatches that data via its callback

  5. Repeats until some queue is empty

  6. Flush() dispatches all remaining data

Time Synchronization

Requires all sensors to use a common time base. Data is ordered by Data::GetTime().

Usage

OrderedMultiQueue queue;

// Add queues for each sensor
queue.AddQueue("laser", [&](std::unique_ptr<Data> data) {
  trajectory_builder->AddSensorData("laser", std::move(data));
});
queue.AddQueue("imu", [&](std::unique_ptr<Data> data) {
  trajectory_builder->AddSensorData("imu", std::move(data));
});

// Add sensor data as it arrives
queue.Add("laser", MakeDispatchable("laser", time, range_data));
queue.Add("imu", MakeDispatchable("imu", time, imu_data));

// On shutdown
queue.MarkQueueAsFinished("laser");
queue.MarkQueueAsFinished("imu");
queue.Flush();

Configuration

Used by Collator which is configured in MapBuilder:

MAP_BUILDER = {
  collate_by_trajectory = false,  -- Single collator for all trajectories
}

When collate_by_trajectory = true, each trajectory gets its own OrderedMultiQueue.

See Also