SensorBridge::HandleLaserScan之后进入下面的流程：

雷达和imu数据队列按时间整理，没有对齐

trajectory_builder_->AddSensorData当中的trajectory_builder_其实是CollatedTrajectoryBuilder，它的根源在sensor_bridge的初始化：

sensor_bridges_[trajectory_id] = 
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          // 实际类型是 CollatedTrajectoryBuilder
          map_builder_->GetTrajectoryBuilder(trajectory_id)  );

GetTrajectoryBuilder的里面是trajectory_builders_.push_back( absl::make_unique<CollatedTrajectoryBuilder>() )，已经写死了，所以说 后面的TrajectoryBuilderInterface只能是CollatedTrajectoryBuilder

trajectory_builder_->AddSensorData实际是:

void AddSensorData(
  const std::string& sensor_id,
  const sensor::TimedPointCloudData& timed_point_cloud_data) override
{
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}

将一个sensor::TimedPointCloudData的数据，变换成了 Dispatchable 的格式。 Dispatchable是一个类模板，所有类型的数据, 都会被放在data_里面，这个的作用会在后面显现，靠这个去判断调用的是GlobalTrajectoryBuilder里面的哪个函数

AddData就一句sensor_collator_->AddSensorData(trajectory_id_, std::move(data));. 根据参数collate_by_trajectory（Collator或TrajectoryCollator）选择， 默认是sensor::Collator，所以是sensor::Collator::AddSensorData:

void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data)
{
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  // Queue keys are a pair of trajectory ID and sensor identifier
  // OrderedMultiQueue  queue_;
  queue_.Add(std::move(queue_key), std::move(data));
}

然后会调用OrderedMultiQueue::add()的函数，把数据存入队列里，形成queues_

(0, scan):  {      4,   }    带callback
(0, imu) :  {1,  3,  5  }    带callback
(0, odom):  {  2,      6}    带callback

最后再调用OrderedMultiQueue::Dispatch()

OrderedMultiQueue::Dispatch()

函数将队列中的数据根据时间依次传入回调函数 GlobalTrajectoryBuilder::AddSensorData。

这里的处理是生产者——消费者模式。 生产者 OrderedMultiQueue::Add     消费者 CollatedTrajectoryBuilder::HandleCollatedSensorData

这个分发函数就太复杂了，涉及到OrderedMultiQueue这个数据结构了，它的主要作用就是管理多个有序的传感器数据, 主要的体现就是成员变量std::map<QueueKey, Queue> queues_，它会形成这么一个组织结构:

key1(sensor_1): queue
key2(sensor_2): queue

queue里面是按时间的数据的组织：

struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;   //回调
    bool finished = false;
};

这里发现了Queue有个成员是 callback 函数，在Dispatch函数中，如果找出来的数据, 那么就调用这个数据的callback函数。那么在哪儿引入了这个callback函数呢？ 结果发现是在OrderedMultiQueue::AddQueue，它又在Collator::AddTrajectory中调用，这样就有了另一条线。

// CollatedTrajectoryBuilder 的构造函数调用该函数
// CollatedTrajectoryBuilder 的 HandleCollatedSensorData 作为回调函数callback传进来
// 添加轨迹以生成排序的传感器输出，每个topic设置一个回调函数
void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback)
{
  for (const auto& sensor_id : expected_sensor_ids) 
  {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    // 根据QueueKey，将对应的回调函数callback(CollatedTrajectoryBuilder::HandleCollatedSensorData)
    // 放入 queue_ （OrderedMultiQueue::AddQueue）里
    queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

CollatedTrajectoryBuilder处理传感器数据，使其按照时间排列，然后传入GlobalTrajectoryBuilder，最终的回调函数其实就是GlobalTrajectoryBuilder::AddSensorData