[cartographer] 7. 传感器数据从ROS流到slam

我们知道了，GlobalTrajectoryBuilder 是 slam系统的顶层设计，该类中包含了两个成员变量：localTrajectoryBuilder 负责前端localslam ； PoseGraph 负责后端 (是从MapBuilder类中的posegraph传递过来的)。那么，传感器数据是如何流到GlobalTrajectoryBuilder从而被处理的呢？

一、基础类 OrderedMultiQueue

下面介绍 sensor::TrajectoryCollator 类

absl::flat_hash_map<int, OrderedMultiQueue> trajectory_to_queue_;

 第一个元素是轨迹ID，第二个元素 OrderedMultiQueue 是该轨迹的所有传感器数据

介绍 OrderedMultiQueue 类

首先声明：cartographer有轨迹的概念，即会同时存在多条轨迹。

struct QueueKey {
  int trajectory_id;// 轨迹id
  std::string sensor_id;// sensor id

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


// 不同轨迹不同sensor,有不同的data队列，有不同的回调函数
// 这里的回调函数就是 HandleCollatedSensorData() 位于collated_trajectory_builder.cc
  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;
    bool finished = false;
  };


class OrderedMultiQueue {

  std::map<QueueKey, Queue> queues_;// 不同的轨迹不同的sensor，有自己的数据队列
  
  //新增轨迹时，向queues_增加元素，并明确对应的回调函数
  void AddQueue(const QueueKey& queue_key, Callback callback);

  // 向对应的数据队列中添加数据，并且调用dispatch()函数，分发数据
  void Add(const QueueKey& queue_key, std::unique_ptr<Data> data);

  
  // 遍历 所有轨迹所有sensor 的 所有数据，并调用其对应的回调函数
  void Dispatch();

}

二、主要流程

1.  新增加一条轨迹

从ROS开始：

当新增加一条轨迹时：

HandleStartTrajectory() ---> AddTrajectory() ---->map_builder_bridge_.AddTrajectory()

--->map_builder_->AddTrajectoryBuilder() ----> CollatedTrajectoryBuilder的构造函数

----> sensor::TrajectoryCollator sensor_collator_->AddTrajectory()  ; HandleCollatedSensorData() 是真正的入口，作为AddTrajectory()的回调函数。什么时候回调呢？

----> OrderedMultiQueue->AddQueue()

所以，当新增一条轨迹时，从ROS开始，最终调用的是 OrderedMultiQueue->AddQueue()

2. 当接收传感器数据时

在新增这条轨迹时，就已经通过LaunchSubscribers()配置了ROS subscriber 来接收各个传感器数据，并且设置了回调函数。下面我们以lidar数据为例，梳理一下回调函数是怎么层层传递的：

HandleLaserScanMessage() ---->  map_builder_bridge_.sensor_bridge(trajectory_id).HandleLaserScanMessage(sensor_id, msg)

----> CollatedTrajectoryBuilder->AddSensorData() //根据不同的传感器数据，调用不同函数

----->sensor::TrajectoryCollator sensor_collator_->AddSensorData()

------>OrderedMultiQueue->Add() --->OrderedMultiQueue->Dispatch()

------> 调用回调函数 CollatedTrajectoryBuilder::HandleCollatedSensorData()

------>Dispatchable::AddToTrajectoryBuilder()

------> GlobalTrajectoryBuilder::AddSensorData()// 不同的传感器有不同的实现

------> 调用localTrajectoryBuilder 和 posegraph的 AddImuData() AddRangeData()，实现前端和后端

当接收到传感器数据时，从ROS的HandleLaserScanMessage()，最终调用OrderedMultiQueue->Add() ，将传感器数据输入到slam进行处理。