SensorBridge::HandleLaserScan之后进入下面的流程：
trajectory_builder_->AddSensorData当中的trajectory_builder_其实是CollatedTrajectoryBuilder，它的根源在sensor_bridge的初始化：1
2
3
4
5
6
7sensor_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实际是:1
2
3
4
5
6void 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:1
2
3
4
5
6
7
8void 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_1
2
3(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_，它会形成这么一个组织结构:1
2key1(sensor_1): queue
key2(sensor_2): queuequeue里面是按时间的数据的组织：1
2
3
4
5struct Queue {
common::BlockingQueue<std::unique_ptr<Data>> queue;
Callback callback; //回调
bool finished = false;
};
这里发现了Queue有个成员是 callback 函数，在Dispatch函数中，如果找出来的数据, 那么就调用这个数据的callback函数。那么在哪儿引入了这个callback函数呢？ 结果发现是在OrderedMultiQueue::AddQueue，它又在Collator::AddTrajectory中调用，这样就有了另一条线。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20// 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
