cartographer_learn_5

续接前文

在具体的讨论前后端前，觉得有必要讨论一下数据的处理和传输，即 unique_ptr< sensor::CollatorInterface > MapBuilder::sensor_collator_。看起来它的类型是sensor::CollatorInterface，但是F12进去后发现它仅仅是接口，定义了一堆虚函数而已，它真正的类型是sensor::Collator，它在文件src/cartographer/cartographer/sensor/internal/collator.h中。打开它

class Collator : public CollatorInterface {
......//一堆类方法，先忽略
 private:
  OrderedMultiQueue queue_;
  absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
}

发现只有两个类成员，但是看不懂，所以接下来研究一下，这究竟是个啥，这一篇先看看第一个OrderedMultiQueue这个类。

OrderedMultiQueue相关的定义

打开它在src/cartographer/cartographer/sensor/internal/OrderedMultiQueue.h中。

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

  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);
  }
};

第一个看到的是一个结构，叫**key，还重载了<运算符，相比这种东西是要在map或unordered_map作为索引值的了。
再具体的看这个类

using Callback = std::function<void(std::unique_ptr<Data>)>;

一开始定义了一个较Callback的东西，想必它是某个东西的回调函数。再看参数是一个叫Data的对象，进去看Data发现没什么，只有一个string和一些纯虚函数，这又是老套路，基类指针指向派生类。
再来看一下OrderedMultiQueue中定义的一种结构

class OrderedMultiQueue {
......//一些类方法
private:
  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;
    bool finished = false;                              
  };
  ......//一些类成员
}

可以看到一来定义了一个结构Queue，里面有个叫BlockingQueue<***>的东西，进去看看发现它就是封装一下c++中的deque。但是它的push和Pop和普通的c++容器在一些细节上有点不一样，而且对容器中的数据的数量做了一些控制，同时也在各个接口函数上加了互斥锁的保护。目前就把它当成存储数据的队列，不过这个队列又有点智能，可以自己加互斥锁，控制队列中数据的数量，同时他的push和Pop由于使用了右值引用做所有权的转移估计也会更高效。
再看Queue中的第二和第三个元素，是上面提到的Callback和一个标记是否结束的bool变量。所以总结一下这个struct这很有可能（没办法，一边学习一边写，错了多包涵）把一种数据按顺序存储在存储queue中，且将处理这种数据的函数指针也一并打包放在这个结构中，最后记录一下该种数据是否传输结束了。

OrderedMultiQueue的类成员

class OrderedMultiQueue {
......//一些类方法
private:
......//一些定义
common::Time last_dispatched_time_ = common::Time::min();

std::map<int, common::Time> common_start_time_per_trajectory_;
std::map<QueueKey, Queue> queues_;
QueueKey blocker_;
}

第一个类成员是一个时间，望文生义（瞎猜），它记录了上次发送的时间，难道这个类是用来存储一些数据后再通过每种数据中不同的Callbak函数将数据处理发送出去吗？很有可能。
第二个元素后面再解释，看第三个一个map<QueueKey, Queue>。看来我们猜测很可能是正确的，QueueKey中记录了轨迹id和topic，在ros中订阅对应的topic获得数据后放入Queue::queue中，或者是通过轨迹id和topic找到对应的数据存储处Queue::queue再就近使用Queue::callback处理发送出去。
最后一个成员记录了一个QueueKey。

OrderedMultiQueue的类方法

void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

给定一个queue_key和对应这种数据的处理的函数的函数指针，在OrderedMultiQueue::queues_中生成一个存储这种数据的BlockingQueue<std::unique_ptr< Data > >.。

common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) {
  auto emplace_result = common_start_time_per_trajectory_.emplace(
      trajectory_id, common::Time::min());
  common::Time& common_start_time = emplace_result.first->second;

  if (emplace_result.second) {
    for (auto& entry : queues_) {
      if (entry.first.trajectory_id == trajectory_id) {
        common_start_time = std::max(
            common_start_time, entry.second.queue.Peek<Data>()->GetTime());
      }
    }
    LOG(INFO) << "All sensor data for trajectory " << trajectory_id
              << " is available starting at '" << common_start_time << "'.";
  }

  return common_start_time;
}

这个函数是遍历queues_中的对应轨迹id的数据队列，取出取出这些队列中的最早的一个数据的时间，并找出这些时间中最晚的那个时间节点。就是一个轨迹对应着多个传感器，每个传感器的数据都按时间顺序放在一个队列中。这些传感器获得第一个数据的时间不一样，这个函数就把最晚的那个时间节点找出，记住这个时间，我们先称之为t（这个就是该类的第二个类成员所记录的时间）。
下面一个函数叫Dispatch，那估计就是这个类的关键函数了，用对应的回调函数处理和发送数据

void OrderedMultiQueue::Dispatch() {
  while (true) {
  //声明一些量，用来存储后面查找结果使用
    const Data* next_data = nullptr;
    Queue* next_queue = nullptr;
    QueueKey next_queue_key;
 //遍历所有数据的队列，找出所有队列中最早的数据放入next_data中
    for (auto it = queues_.begin(); it != queues_.end();) {
      const auto* data = it->second.queue.Peek<Data>();
      if (data == nullptr) {
        if (it->second.finished) {
          queues_.erase(it++);
          continue;
        }
        CannotMakeProgress(it->first);
        return;
      }
      if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
        next_data = data;
        next_queue = &it->second;
        next_queue_key = it->first;
      }
      CHECK_LE(last_dispatched_time_, next_data->GetTime())
          << "Non-sorted data added to queue: '" << it->first << "'";
      
      ++it;
    } // end for
//对查找到的最早的数据做一系列的检查
    if (next_data == nullptr) {
      CHECK(queues_.empty());
      return;
    }

    // If we haven't dispatched any data for this trajectory yet, fast forward
    // all queues of this trajectory until a common start time has been reached.
    const common::Time common_start_time =
        GetCommonStartTime(next_queue_key.trajectory_id);
//该数据的时间和时间t做比较
    if (next_data->GetTime() >= common_start_time) {
    //使用回调函数处理发布（？？）数据
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
    } 
    //这里没看懂。。。。。。
    else if (next_queue->queue.Size() < 2) {
      if (!next_queue->finished) {
        // We cannot decide whether to drop or dispatch this yet.
        CannotMakeProgress(next_queue_key);
        return;
      } 
      last_dispatched_time_ = next_data->GetTime();
      next_queue->callback(next_queue->queue.Pop());
    } 
    //直到找到比时间t时间晚的数据我们才处理
    else {
      // We take a peek at the time after next data. If it also is not beyond
      // 'common_start_time' we drop 'next_data', otherwise we just found the
      // first packet to dispatch from this queue.

      std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
      if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
        last_dispatched_time_ = next_data->GetTime();
        next_queue->callback(std::move(next_data_owner));
      }
    }

  }
}

下面也是一个重要的函数，向对应的传感器数据队列中添加数据

void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
  auto it = queues_.find(queue_key);
  //检查是否存在这种队列
  if (it == queues_.end()) {
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }
  // 向队列中添加数据
  it->second.queue.Push(std::move(data));
  //上面讨论的函数
  Dispatch();
}

其他函数都是一些小函数，这里就不在赘述了，后面发现又需要再添加吧。