(02)Cartographer源码无死角解析-(24) Collator类与顺序多队列类

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

在正式讲解之前，回顾一下之前的的博客：
(02)Cartographer源码无死角解析-(22) 传感器数据分发→CollatedTrajectoryBuilder
总的来说，初始注册的回调函数，整理数据之后，最终都通过 CollatedTrajectoryBuilder::AddSensorData 调用CollatedTrajectoryBuilder::AddData() 最后再执行 sensor::Collator::AddTrajectory()，把数据传送给了 GlobalTrajectoryBuilder，但是并没有做具体分析。

在 src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h 文件中，可以看到看到 CollatedTrajectoryBuilder::AddSensorData() 为重载函数，该重载函数会调用 sensor::MakeDispatchabl() 函数实现模板参数的自动推导。然后把 Dispatchable 类型参数传递给 CollatedTrajectoryBuilder::AddData 函数，其代码如下：

// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
1
2
3
4

虽然上一篇博客，对 sensor::Dispatchable 以及其父类 sensor::Data 进行了分析，但是并没有讲述 sensor_collator_->AddSensorData 函数是如何使用这些数据的，也就是说没说明 CollatedTrajectoryBuilder::sensor_collator_的来源与作用

首先根据 CollatedTrajectoryBuilder 构造函数的初始化列表，可知其在创建实例时，已经对 sensor_collator_ 进行了赋值，根据前面的分析，可知 CollatedTrajectoryBuilder 在 src/cartographer/cartographer/mapping/map_builder.cc 文件中的 MapBuilder::AddTrajectoryBuilder() 函数中构建，查看该函数可以找到如下代码：

    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        ....
        
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
	....
1
2
3
4
5
6
7

可以看到 sensor_collator_，易知 sensor_collator_ 为一个智能指针，且是 MapBuilder 的一个成员变量。最中可以在 MapBuilder 的构造函数中找到答案，位于 src/cartographer/cartographer/mapping/map_builder.cc 之中：

  // 在 cartographer/configuration_files/map_builder.lua 中设置
  // param: MAP_BUILDER.collate_by_trajectory 默认为false
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    // sensor_collator_初始化, 实际使用这个
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
1
2
3
4
5
6
7
8

本人源码运行使用的是 absl::make_unique()，也就是collate_by_trajectory 使用默认值 false。
 

二、CollatorInterface

sensor::Collator 位于 src/cartographer/cartographer/sensor/internal/collator.h 中实现，其继承于 CollatorInterface。在 CollatorInterface.h 文件中，可以看到类 CollatorInterface 这个接口类与前面的的一些接口类都差不多，主要声明了一些纯虚函数，然后禁用了拷贝构造函数与赋值运算符重载函数。另外可以看到如下比较特别的代码：

  // note: CollatorInterface::Callback 2个参数
  using Callback =
      std::function<void(const std::string&, std::unique_ptr<Data>)>;

  // Adds a trajectory to produce sorted sensor output for. Calls 'callback'
  // for each collated sensor data.
  virtual void AddTrajectory(
      int trajectory_id,
      const absl::flat_hash_set<std::string>& expected_sensor_ids,
      const Callback& callback) = 0;
1
2
3
4
5
6
7
8
9
10

总的来说，要表的的意思就是 CollatorInterface 的派生类，需要重写 AddTrajectory 函数，该函数无返回值，且需要传递三个参数，第三个参数为用于回调的函数指针，该函数指针无返回值，需要传递两个参数：onst std::string& 与 std::unique_ptr。
 

三、Collator

现在回过头来看 Collator，在 Collator.h 文件中，可以明显的看到其依旧禁用了拷贝构造函数以及赋值运算符重载函数。然后对父类函数进行了重写。值得注意的是，其有两个私有成员变量：

 private:
  // Queue keys are a pair of trajectory ID and sensor identifier.
  OrderedMultiQueue queue_; //暂时理解为按时间排序之后的队列即可

  // Map of trajectory ID to all associated QueueKeys.
  //key为 trajectory ID，value是与该trajectory ID相关的所有QueueKeys
  absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
1
2
3
4
5
6
7

不是很明白也没有关系，后续会进行讲解。
 

1、AddTrajectory()

先来看到看其中的 Collator::AddTrajectory 函数，该函数是在 CollatedTrajectoryBuilder 构造函数中被调用的的：

  // sensor::Collator的初始化
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        HandleCollatedSensorData(sensor_id, std::move(data));
      });
1
2
3
4
5
6

从这里可以看出，CollatedTrajectoryBuilder 在构造函数中，把其成员函数作 HandleCollatedSensorData 作为回调函数传递给了 sensor_collator_->AddTrajectory()。

在前面的接口 CollatorInterface 中提到，其重写 sensor_collator_->AddTrajectory() 时，其第三个参数为函数指针，该函数指针的参数为 std::string& 与 std::unique_ptr 类型，那么可以知道其上的 lambda 表达式刚好满足需求。先来看看代码注释：

/**
 * @brief 添加轨迹以生成排序的传感器输出, 每个topic设置一个回调函数
 * 
 * @param[in] trajectory_id 新生成的轨迹的id
 * @param[in] expected_sensor_ids 需要排序的topic名字的集合
 * @param[in] callback 2个参数的回调函数, 实际是CollatedTrajectoryBuilder::HandleCollatedSensorData()函数
 */
void Collator::AddTrajectory(
    const int trajectory_id, //轨迹id
    const absl::flat_hash_set<std::string>& expected_sensor_ids, //订阅的话题名字集合
    const Callback& callback) { //回调函数
  for (const auto& sensor_id : expected_sensor_ids) { //循环遍历所有话题的名字
    //为每个话题构建构建一个QueueKey，其包含了trajectory_id与订阅该话题名字
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    //为每条轨迹的话题都添加一一对应的数据队列
    queue_.AddQueue(queue_key,//把trajectory_id与题名字共同看作key
                    // void(std::unique_ptr data) 带了个默认参数sensor_id
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    //把同一trajectory_id的queue_key都存放在一个vector中
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

该函数其实也比较简单，就是为当前 轨迹(trajectory_id) 订阅的每个话题，都构建一个键值对添加到成员变量 OrderedMultiQueue queue_ 中，键值如下：

queue_key = QueueKey{trajectory_id, sensor_id}  //键
[callback, sensor_id](std::unique_ptr<Data> data) {callback(sensor_id, std::move(data));} //值
1
2

从前面已经知道传入的参数 callback 就是一个 lambda 的类型参数，但是这里又使用了一个 lambda 表达式？这时因为 OrderedMultiQueue queue_ 中存入的 valua 只能虽然时函数指针，但是其只能传入一个参数。最后，键 queue_key 还会被存储在如下变量之中

absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
1

其是一个字典，key 就是 trajectory_id，value 为 queue_key = QueueKey{trajectory_id, sensor_id}。后面大家就会知道其目的：

$目的：$ OrderedMultiQueue queue_ 保存了所有轨迹，及轨迹对应所有订阅话题的数据队列，后续详解。
 

2、FinishTrajectory()

// 将 trajectory_id 标记为完成
void Collator::FinishTrajectory(const int trajectory_id) {
  //传入一个trajectory_id，然后对该id下的td::vector进行遍历
  for (const auto& queue_key : queue_keys_[trajectory_id]) {
    //调用MarkQueueAsFinished函数进行处理,把queue_key
    //对应的队列的变量.finished标志位true,表示该队列以及完成
    queue_.MarkQueueAsFinished(queue_key);
  }
}
1
2
3
4
5
6
7
8
9

把一个轨迹中的所有数据队列.finished都标记为true。
 

3、AddSensorData

// 将 trajectory_id 标记为完成
void Collator::FinishTrajectory(const int trajectory_id) {
  //传入一个trajectory_id，然后对该id下的td::vector进行遍历
  for (const auto& queue_key : queue_keys_[trajectory_id]) {
    //调用MarkQueueAsFinished函数进行处理,把queue_key
    //对应的队列的变量.finished标志位true,表示该队列已经完成
    queue_.MarkQueueAsFinished(queue_key);
  }
}
1
2
3
4
5
6
7
8
9

暂时并不知道其具体作用，猜测大概是轨迹完成的时候，可能会调用吧，

4、AddSensorData

该函数就比较熟悉了，实际上就是被 CollatedTrajectoryBuilder::AddData() 调用的 sensor_collator_->AddSensorData() 函数：

// 向数据队列中添加 传感器数据 
void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data) {
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  queue_.Add(std::move(queue_key), std::move(data));
}
1
2
3
4
5
6

根据 trajectory_id 以及数据对应的话题，然后往对应的队列添加数据。

5、剩余函数：

// 将所有数据队列标记为已完成,分派所有剩下的传感器数据
// 只能调用一次, 在 Flush 之后不能再调用 AddSensorData()
void Collator::Flush() { queue_.Flush(); }

// 返回在 CollatorInterface 解锁之前需要更多数据的轨迹的 ID
// 对于不等待特定轨迹的实现, 返回 'nullopt'
absl::optional<int> Collator::GetBlockingTrajectoryId() const {
  return absl::optional<int>(queue_.GetBlocker().trajectory_id);
}

1
2
3
4
5
6
7
8
9
10

暂时不明白给来干啥的。
 

四、QueueKey

在 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h 文件中，可以看到如下代码

struct QueueKey {
  int trajectory_id;      // 轨迹id
  std::string sensor_id;  // topic名字

  // 重载小于运算符, map根据这个规则对QueueKey进行排序
  // 以tuple规则比较2者, tuple定义了<运算符, 逐个元素进行比较
  bool operator<(const QueueKey& other) const {
    return std::forward_as_tuple(trajectory_id, sensor_id) <
           std::forward_as_tuple(other.trajectory_id, other.sensor_id);
  }
};
1
2
3
4
5
6
7
8
9
10
11

这样就可以看出前面的代码 const auto queue_key = QueueKey{trajectory_id, sensor_id}; 其主要包含了 trajectory_id, sensor_id 两个成员变量。另外，其还定义
重载运算大于符号函数，功能是两个 QueueKey 实例逐个元素进行比较大小。
 

五、OrderedMultiQueue头文件

现在来看 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h 中 的 class OrderedMultiQueue。其中可以看到一段特别的代码：

  // c++11: 移动构造函数, 只在使用的时候编译器才会自动生成
  // 这里是显示指定让编译器生成一个默认的移动构造函数
  // && 标志着其为拷贝构造函数，default表示才表示生成
  OrderedMultiQueue(OrderedMultiQueue&& queue) = default;
1
2
3
4

该为移动构造函数，只有通过 = default 时才会生成，有兴趣的朋友可以深入了解一下。剩余函数后面进行讲解，来看看其私有成员变量中的一个结构体：

 private:
  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;   // 存储数据的队列
    Callback callback;                                    // 本数据队列对应的回调函数
    bool finished = false;                                // 这个queue是否finished
  };
1
2
3
4
5
6

其把存储数据的队列的队列，进一步进行了封装，该队列存储的 sensor::Data 或其派生类的实例对象。
 

六、OrderedMultiQueue函数定义

下面来 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc 文件中分析其成员函数的定义。由于其成员函数太多，且比较复杂，这里先调几个简单的进行讲解。

1、混合讲解

其首先重载了<<操作符，然后定义的构造函数没有做任何事情，析构函数检查了所有队列的 .finished 是否都标记为 true。

//重载输出操作符，用于输出，
inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) {
  return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
}

OrderedMultiQueue::OrderedMultiQueue() {}

OrderedMultiQueue::~OrderedMultiQueue() {
  for (auto& entry : queues_) {
    //检查所有数据队列.finished是否都标记为true
    CHECK(entry.second.finished); 
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13

2、OrderedMultiQueue::AddQueue()

该函数是在 函数是在 sensor::Collator 的构造函数中调用的，其目的就是为每条轨迹的话题都添加一一对应的数据队列，且保定好与之对应的构造函数：

/**
 * @brief 添加一个数据队列,并保存回调函数 CollatedTrajectoryBuilder::HandleCollatedSensorData
 * 
 * @param[in] queue_key 轨迹id与topic名字
 * @param[in] callback void(std::unique_ptr data) 型的函数
 * 这里的callback已经是对应sensor_id的callback了
 */
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0); //如果为0，则说明该queue_key还没有对应的value
  //往queues_添加一个数据队列，且绑定好回调函数
  queues_[queue_key].callback = std::move(callback);
}
1
2
3
4
5
6
7
8
9
10
11
12

3、OrderedMultiQueue::Add()

该函数在sensor::Collator::AddSensorData() 函数中被调用，实际就是根据 trajectory_id 与 topic name 往对应的队列中添加数据。

// 向数据队列中添加数据
void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
  auto it = queues_.find(queue_key);
  // 如果queue_key不在queues_中, 就忽略data
  if (it == queues_.end()) {
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }

  // 向数据队列中添加数据
  it->second.queue.Push(std::move(data));

  // 传感器数据的分发处理
  Dispatch();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

4、OrderedMultiQueue

该函数在sensor::Collator::AddSensorData() ::Flush() 被调用，代码如下：

// 将所有处于未完成状态的数据队列标记为完成状态
void OrderedMultiQueue::Flush() {
  // 找到所有unfinished的数据队列
  std::vector<QueueKey> unfinished_queues;
  for (auto& entry : queues_) {
    if (!entry.second.finished) {
      unfinished_queues.push_back(entry.first);
    }
  }
  // 将unfinished_queues标记为完成状态
  for (auto& unfinished_queue : unfinished_queues) {
    MarkQueueAsFinished(unfinished_queue);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

 

七、结语

关于OrderedMultiQueue 还有一些总要的成员函数没有讲解，不过没有关系，后续会继续分析。先来总结一下该篇博客的内容。

$(1)$ 每条轨迹都会创建一个 CollatedTrajectoryBuilder 实例对象，但其所有实例对象都是绑定同一个 sensor::Collator 实例。

$(2)$每个 CollatedTrajectoryBuilder 实例都会根据 topic name 往 sensor::Collator 中注册多个回调函数，(如果数据类型相同，那么注册的回调函数也相同)

$(3)$ sensor::Collator 实例会会为每个 topic name 都创建一个数据队列。一条轨迹可能订阅了多个 topic，则其会对应多个队列。

$(4)$ sensor::Collator 大部分操作，都是围绕着数据队列 OrderedMultiQueue进行操作，可以把 sensor::Collator 看作是对 OrderedMultiQueue 的进一步封装。

下一篇博客，会进行剩余部分的讲解，如 OrderedMultiQueue::GetBlocker() 、OrderedMultiQueue::Dispatch() 等函数。