梳理cartographer传感器数据流

前言

因为cartographer中的trajectory_builder、global_trajectory_builder、sensor_collator这些东西搞得苦不堪言。
因此在这里梳理一下cartographer传感器数据流。

说的很高大上，其实也就是冰山一角的梳理。希望读者在阅读map_builder.cc和collated_trajectory_builder.cc的时候能给予一定的帮助。

本文引用了很多cartographer的源代码。因为是自己注释过的版本，因此会有很多**//杂乱的注释**，大家不用过多关注那些代码引用里的注释。因为毕竟本文只是以管窥豹。

因为其中用到了许多文件，因此我在这里先列一个文件列表，以便读者寻找文件所在目录进行查阅。

cartographer_ros部分

• Node.cc：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc
• map_builder_bridge.cc：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc
• sensor_bridge.h：file:///home/mjy/dev/carto_ws/src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc

cartographer部分

• map_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/map_builder.cc
• collated_trajectory_builder.h：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h
• collated_trajectory_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.cc
• trajectory_collator.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/trajectory_collator.cc
• ordered_multi_queue.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc
• global_trajectory_builder.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc
• data.h：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/sensor/data.h
• local_slam_result_2d.cc：file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/internal/2d/local_slam_result_2d.cc

目录

0.从ros引入
1.先讲“壳”
2.再讲“壳中物”
3.再看回调
4.总结

0.从ros引入

毕竟先得看ros壳，才能捋清传感器信息流

Node.cc

首先看node.cc

首先看用户请求增加一条轨迹，那么服务端service_servers_会回调函数Node::HandleStartTrajectory

  service_servers_.push_back(node_handle_.advertiseService(
      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));

HandleStartTrajectory
这个函数的定义为：

bool Node::HandleStartTrajectory(
    ::cartographer_ros_msgs::StartTrajectory::Request& request,
    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
  carto::common::MutexLocker lock(&mutex_);
  TrajectoryOptions options;
  if (!FromRosMessage(request.options, &options) ||
      !ValidateTrajectoryOptions(options)) {
    const std::string error = "Invalid trajectory options.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else if (!ValidateTopicNames(request.topics, options)) {
    const std::string error = "Invalid topics.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else {
    response.trajectory_id = AddTrajectory(options, request.topics);
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
    response.status.message = "Success.";
  }
  return true;
}

上述函数除了先行判断以外，最重要的是调用的Node::AddTrajectory函数，用来增加一条轨迹。

Node::AddTrajectory 输入：options配置项、request.topics请求（即传感器topic）

然后我们查看Node::AddTrajectory的定义，输入Wie配置项和传感器信息的topics（不是指的传感器消息，而是以后用来建立订阅者的传感器话题）

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);  // 跟位姿估计有关
  AddSensorSamplers(trajectory_id, options);  // 传感器数据采集相关
  LaunchSubscribers(options, topics, trajectory_id);  // 重点关注这个
  is_active_trajectory_[trajectory_id] = true;
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

主要注意这一句——建壳操作：

const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

即使用MapBuilderBridge::AddTrajectory函数，以此增加了一条轨迹。

map_builder_bridge_在map_builder_bridge.h中的定义为：

std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

注意，由于是新建轨迹，因此并没有接受传感器数据。新建一个轨迹壳而已。

所以我们先看map_builder_bridge.cc中是如何实现它的

1.先讲“壳”

map_builder_bridge.cc

// 重要函数特殊对待
/**
  @brief 增加一条轨迹
  @param expected_sensor_ids 期望的传感器id？
  @param trajectory_options 轨迹的配置参数
  */
int MapBuilderBridge::AddTrajectory(  // 重要函数！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！增加一条轨迹
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(  // 这已经是cartographer项目中的代码了
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,  // OnLocalSlamResult函数
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  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_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);  // 返回一个新的哈希容器，哈希容器比原来多一个pair，这个pair包含了这条轨迹的id及其配置参数
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

其中让我们重点关注：map_builder_->AddTrajectoryBuilder

其输入参数有：
（1）expected_sensor_ids 不仅是能检测距离的传感器如laser、kinect，其他有用的传感器也会包含进来，如里程计、imu
（2）trajectory_options.trajectory_builder_options 配置选项，定义于
/home/mjy/dev/carto_ws/src/cartographer/configuration_files/trajectory_builder.lua
主要是决定了采用3d-slam还是2d-slam
（3）bind了一个函数
::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, // OnLocalSlamResult函数
::std::placeholders::_1, ::std::placeholders::_2,
::std::placeholders::_3, ::std::placeholders::_4,
::std::placeholders::_5));
用bind绑了一下。这个函数到后面我们会知道是一个回调函数。
暂时先不管

看完输入参数，我们来看具体实现：

map_builder.cc

这已经是cartographer文件夹中的东西了。
file:///home/mjy/dev/carto_ws/src/cartographer/cartographer/mapping/map_builder.cc

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback)

其函数定义中能看出传感器流端倪（注意，只是端倪，但并没有接受传感器数据）的是：

    // -----------------------------------3d情况---------------------------------------------------------------
  if (options_.use_trajectory_builder_3d()) {
    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;  // /mapping/internal/3d/local_trajectory_builder_3d.h
    if (trajectory_options.has_trajectory_builder_3d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder3D>(  // 新建一个local_trajectory_builder
          trajectory_options.trajectory_builder_3d_options(),
          SelectRangeSensorIds(expected_sensor_ids));  // 找到那些能检测距离的传感器，如laser，kinect
    }
    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));  // 将接口类型强转为3d位姿图类型
    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(  //真正地创建一个TrajectoryBuilder，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder3D( 
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph3D*>(pose_graph_.get()),
                local_slam_result_callback))); // 需要传入回调local_slam_result_callback
  } else {
    // -----------------------------------2d情况---------------------------------------------------------------
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;  // local slam 的builder，值得点进去看看这个类
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(  // 新建一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));  // 找到那些能检测距离的传感器，如laser，kinect
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // 传入的参数sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator，具体是什么类型是由mapbuilder的构造函数决定
    trajectory_builders_.push_back(    //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
        common::make_unique<CollatedTrajectoryBuilder>(  // CollatedTrajectoryBuilder这里面我写了回调的总体思路
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D( 
                std::move(local_trajectory_builder), trajectory_id,		// GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));  // 需要传入回调local_slam_result_callback
    // 上述回调先是由trajectory_builder_interface.h中定义了一个函数指针
//       using LocalSlamResultCallback =
//       std::function
//                          transform::Rigid3d /* local pose estimate */,
//                          sensor::RangeData /* in local frame */,
//                          std::unique_ptr)>; 
    // 上述回调的具体实现在：
    // 由于trajectory_builder是属于CollatedTrajectoryBuilder类型，所以在collated_trajectory_builder.cc文件中：

让我们只看2d情况吧：

    // -----------------------------------2d情况---------------------------------------------------------------
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;  // local slam 的builder，值得点进去看看这个类
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(  // 新建一个local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));  // 找到那些能检测距离的传感器，如laser，kinect
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // 传入的参数sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator，具体是什么类型是由mapbuilder的构造函数决定
    trajectory_builders_.push_back(    //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
        common::make_unique<CollatedTrajectoryBuilder>(  // CollatedTrajectoryBuilder这里面我写了回调的总体思路
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D( 
                std::move(local_trajectory_builder), trajectory_id,		// GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));  // 需要传入回调local_slam_result_callback
    // 上述回调先是由trajectory_builder_interface.h中定义了一个函数指针
//       using LocalSlamResultCallback =
//       std::function
//                          transform::Rigid3d /* local pose estimate */,
//                          sensor::RangeData /* in local frame */,
//                          std::unique_ptr)>; 
    // 上述回调的具体实现在：
    // 由于trajectory_builder是属于CollatedTrajectoryBuilder类型，所以在collated_trajectory_builder.cc文件中：

请着重关注这一部分：

    trajectory_builders_.push_back(    //真正地创建一个TrajectoryBuilder并推进容器，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
        common::make_unique<CollatedTrajectoryBuilder>(  // CollatedTrajectoryBuilder这里面我写了回调的总体思路
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D( 
                std::move(local_trajectory_builder), trajectory_id,		// GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));  // 需要传入回调local_slam_result_callback

以上代码真正建立了一个CollatedTrajectoryBuilder类型的trajectory_builder，并pushback进trajectory_builders_容器。

其中建立trajectory_builder的时候用到了CollatedTrajectoryBuilder的构造函数。

所以我们点进CollatedTrajectoryBuilder类型中，看一看端倪吧。

collated_trajectory_builder.cc

重点看构造函数，因为上面用到了它的构造函数：

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)  // 传入的这个wrapped（打包的）_trajectory_builder即GlobalTrajectoryBuilder类型
    : sensor_collator_(sensor_collator),  // 取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
      last_logging_time_(std::chrono::steady_clock::now()) {
  std::unordered_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) {
    expected_sensor_id_strings.insert(sensor_id.id);
  }
  
  // sensor_collator_可能为sensor::TrajectoryCollator，也可能为sensor::Collator
  // HandleCollatedSensorData为其回调函数
  // 这里很重要！
  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));
      });
}

重点关注最后一部分，这是建立轨迹的核心：

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

首先，sensor_collator_的类型取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator

现在我们假设sensor_collator_为TrajectoryCollator类型吧。

上面的sensor_collator_->AddTrajectory函数中，输入参数为：
（1）trajectory_id轨迹的id
（2）expected_sensor_id_strings 传感器id，也不仅仅指距离传感器
（3）一个回调函数壳HandleCollatedSensorData，注意这里的data并不是指现在构造的时候需要传入data，而只是一个回调函数壳而已。以后data进来以后才生效。（好好理解这句话，很重要）

现在我们点进trajectory_collator.cc，来看看sensor_collator_->AddTrajectory的具体实现

trajectory_collator.cc

// 在trajectory_builder被建立时，通过CollatedTrajectoryBuilder的构造函数中，sensor_collator_进行调用
void TrajectoryCollator::AddTrajectory(
    const int trajectory_id,
    const std::unordered_set<std::string>& expected_sensor_ids,
    const Callback& callback) {  // 这里有个回调
  CHECK_EQ(trajectory_to_queue_.count(trajectory_id), 0);
  for (const auto& sensor_id : expected_sensor_ids) {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    /*
     * struct QueueKey {
	int trajectory_id;
	std::string 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);
      }
    };
    */
    // trajectory_to_queue_为哈希容器，存放<轨迹索引， 顺序队列>
    // AddQueue为OrderedMultiQueue类的成员函数
    // 向队列（以为pair的map容器）中加入一个pair，其中queue_key为queue_key，queue的回调callback为输入的回调函数
    trajectory_to_queue_[trajectory_id].AddQueue(
        queue_key, [callback, sensor_id](std::unique_ptr<Data> data) {
          callback(sensor_id, std::move(data));
        });
    trajectory_to_queue_keys_[trajectory_id].push_back(queue_key);
  }
}

仔细看一下：
先对所有有用的，传进来的传感器进行循环。比如我先拿到了传感器A
对于A，先分配一个queue_key，queue_key是一个pair，{trajectory_id, sensor_id}，代表了这个传感器位于哪条轨迹，是有用传感器的第几个传感器

然后使用AddQueue函数，向trajectory_to_queue_容器中的第trajectory_id个元素（一个元素即一个多列（为什么叫多列之后你会知道））加入一个关于A的多列子，成为A*。

让我们仔细看一下AddQueue的实现吧：
AddQueue为OrderedMultiQueue类的成员函数

ordered_multi_queue.cc

多列子A*包含两部分，分别为queue_key和Queue

请着重关注Queue

Queue的结构定义于.h文件中：

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

可以发现其结构包括一个小写queue，和一个callback
（1）对于queue：
可以看出是一个数据队列。我们终于找到传感器数据的存放位置了!!!
（2）对于callback：
数据加进来以后会产生回调。即HandleCollatedSensorData

然后回到AddQueue函数的实现：

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

其实就是向queues_中加入新成员A*

小梳理一下 ----------------------------------------------------------------------------------------------------

sensor_collator_属于 trajectory_collator 类，它维护了一个容器trajectory_to_queue_，这个容器的每一个元素代表了一条轨迹对应的一个“多对象顺序队列”即ordered_multi_queue，简称“多列”。也就是说一个轨迹对应一个多列。

我用trajectory_id先索引到当前轨迹的多列，然后调用了多列的成员函数AddQueue。每一个传感器都会调用一次同样的多列，因为是同一条轨迹进行循环嘛

这个多列维护了一个容器queues_。queues_的成员是一个个的多列子，即多列维护的容器中的一个个儿子，他们是pair的形式：。QueueKey体现了轨迹id和传感器id。Queue体现了传感器数据队列和回调函数。

小梳理完毕 ----------------------------------------------------------------------------------------------------

传入的回调函数即为sensor_collator的HandleCollatedSensorData函数，其 实现在collated_trajectory_builder.cc文件中。

我们将HandleCollatedSensorData函数称为

“上层回调”

重点是HandleCollatedSensorData的实现中，其中以下语句：

  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());  
  // 重点看这个，支撑起回调函数HandleCollatedSensorData的主要作用：向位姿图中插入data

其实就是向位姿图中插入data。wrapped_trajectory_builder_是传入的CreateGlobalTrajectoryBuilder2D函数构造而来
具体见上述map_builder.cc的节选，即：

 CreateGlobalTrajectoryBuilder2D( 
                std::move(local_trajectory_builder), trajectory_id,		// GlobalTrajectoryBuilder类型，作为参数传入CollatedTrajectoryBuilder的构造
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)))

CreateGlobalTrajectoryBuilder2D函数的具体实现为：

global_trajectory_builder.cc

std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback) {
  return common::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback);
}

简单来说就是GlobalTrajectoryBuilder的一个构造函数。这里也有个回调函数：**local_slam_result_callback** 我们暂时称之为：

“下层回调”

往上回溯，这个下层回调就是曾经被bind的那个函数：OnLocalSlamResult

之所以叫下层回调，是因为上层回调HandleCollatedSensorData函数套了一个wrapped_trajectory_builder_，它是GlobalTrajectoryBuilder类型，这个类型绑定了上述的回调local_slam_result_callback。

先不讲下层回调，以后会涉及到，因为它涉及了数据进来以后的操作，所以在“建立壳”这一节讲并不合理。

这篇文章的最后会发现，上层回调和下层回调根本没有关系，也就是这样起名是不合理的。但是这是最后发现的，因此这里先做个提醒。

**** 至此，讲完了如何建立“壳”

也就是说，上述的一些东西还没涉及到data，也就是真正的传感器数据进来以后是如何操作的。之前只是建立了一些“壳”一样的东西等待传感器的进入，包括队列成员的设置呀，回调函数的设置呀，都是壳。
然后也只讲述可上层回调，没有进一步深入下层回调。

下面深入一些，进一步考虑传感器数据，即向壳中倒入传感器数据。

---

2.再讲“壳中物”

首先，回到Node::AddTrajectory函数，看看通过AddTrajectory建完壳后有什么操作：

Node.cc

```csharp
int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);  // 跟位姿估计有关
  AddSensorSamplers(trajectory_id, options);  // 传感器数据采集相关
  LaunchSubscribers(options, topics, trajectory_id);  // 重点关注这个
  is_active_trajectory_[trajectory_id] = true;
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

和输入的传感器数据有关联的是LaunchSubscribers

这个函数LaunchSubscribers启动了各类传感器消息订阅者，并开始订阅传感器消息

让我们看看它的具体实现节选：

    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, topic,
                                                &node_handle_, this),
         topic});
  }

topic即为传入的传感器话题。上述节选实现了imu传感器消息的订阅者

回调函数为：HandleImuMessage

void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  carto::common::MutexLocker lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

好！！！我们看到了，当真正的传感器数据进来以后，回调函数是如何处理的。
核心是这一句：sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);

sensor_bridge_ptr是由auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);得到的

我们回到map_builder_bridge.cc

map_builder_bridge.cc

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {  // 返回指定轨迹的sensor_bridge
  return sensor_bridges_.at(trajectory_id).get();
}

sensor_bridges_是个SensorBridge容器

std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

接下来看SensorBridge类型中的HandleImuMessage函数

sensor_bridge.h

void SensorBridge::HandleImuMessage(const std::string& sensor_id,  // 将imu信息转换为carto可处理的形式，并被trajectory_builder_加入
                                    const sensor_msgs::Imu::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}

终于

我们找到了最最最重要的，将cartographer_ros的传感器消息进行处理，并传入cartographer中的重要函数HandleImuMessage的实现

trajectory_builder_->AddSensorData是将数据传入cartographer的trajectory_builder_。因为trajectory_builder_类型是：

  ::cartographer::mapping::TrajectoryBuilderInterface* const
      trajectory_builder_;

在cartographer中，trajectory_builder的真正类型是collated_trajectory_builder，它继承了上述的TrajectoryBuilderInterface

所以让我们回到cartographer下的：

collated_trajectory_builder.h

找到了AddSensorData的实现：

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

调用了AddData：

collated_trajectory_builder.cc

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

sensor_collator_这个东西是不是很眼熟？

没错，他就是在我们第一讲 1.先讲“壳” 中的传感器收集者。最后会知道，它在cartographer中是所有的传感器数据的管理者。

其类型之前说过：

sensor_collator_的类型取决与mapbuilder的cc文件中，mapbuilder构造的时候使得sensor_collator为哪个类型——可能为sensor::TrajectoryCollator，也可能为sensor::Collator

还是假设他为：TrajectoryCollator类型。然后，看如何实现AddSensorData函数

trajectory_collator.cc

// 将传感器数据根据trajectory_id加入队列
void TrajectoryCollator::AddSensorData(const int trajectory_id,
                                       std::unique_ptr<Data> data) {
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  trajectory_to_queue_.at(trajectory_id)
      .Add(std::move(queue_key), std::move(data));
}

然后你会惊喜的发现！又出现了Queue这个令人兴奋的单词！trajectory_to_queue_这是个容器，之前说过：

这个容器的每一个元素代表了一条轨迹对应的一个“多对象顺序队列”，简称“多列”。也就是说一个轨迹对应一个多列。

Add函数就是多列的成员函数，让我们看看它：

ordered_multi_queue.cc

// 被trajectory_collator.cc调用，给队列中的队员以数据
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();
}

会继续惊喜的发现：it->second.queue.Push(std::move(data));这一句将数据，推入了，多列维护的queues_容器中的某一多列子的second元素即：Queue的queue中。

前面说过，queue维护了这一个传感器的数据队列：

common::BlockingQueue queue;

至此，壳中物可以被添加到壳子当中了。

可以看到，在一条轨迹中，同一个传感器一段时间序列的一系列数据是在同一个queue中维护的。

暂时总结：

我们知道sensor_collator_只有一个，但是它维护了一个容器：trajectory_to_queue_，总的来说，
这个容器包储存了所有的数据，即，不同轨迹的不同传感器的不同时间传入的数据

也就是说，在cartographer中，sensor_collator_是管理所有的传感器数据的最大BOSS。

---

3.再看回调

前面讲过一个上层回调，还有一个下层回调。我们将理论完善一些。

先回顾上层回调：* 上层回调的引出
再回顾下层回调：* 下层回调的引出

当把数据加入到queue中的时候，我们优先引发上层回调。即HandleCollatedSensorData。
然后这个函数的实现引发了global_trajectoryBuilder这个东西产生下层回调。我们具体分析一下。

从HandleCollatedSensorData的实现开始，其中以下语句：

  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());  
  // 重点看这个，支撑起回调函数HandleCollatedSensorData的主要作用：向位姿图中插入data

data为sensor::Data类型，看看其成员函数：

data.h

  virtual void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;

仅仅是个虚函数。需要被继承。

• LocalSlamResult2D (mapping/internal/2d/local_slam_result_2d.h)继承了LocalSlamResultData并实现了AddToTrajectoryBuilder.
• 在LocalSlamResultData的函数中有分别调用了TrajectoryBuilderInterface的成员函数和PoseGraph2D的成员函数。

所以你就暂时认为data就是local_slam_result_2d类型即可（因为还有3d的相应类型嘛，暂不考虑）

local_slam_result_2d.cc

void LocalSlamResult2D::AddToTrajectoryBuilder(
    TrajectoryBuilderInterface* const trajectory_builder) {
  trajectory_builder->AddLocalSlamResultData(
      common::make_unique<LocalSlamResult2D>(*this));
}

回想上面，我们传入的是wrapped_trajectory_builder_，之前说过它是一个GlobalTrajectoryBuilder2D类型。这个类型也是继承了TrajectoryBuilderInterface而来的。

所以我们得看GlobalTrajectoryBuilder2D的成员函数——AddLocalSlamResultData

global_trajectory_builder.cc

  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                                  local_slam_result_data) override {
    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
                                         "local_trajectory_builder_ present.";
    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
  }

我们会发现这个上层回调实际上是将data加入位姿图中。
回到mapping::LocalSlamResultData类型，查看其下的AddToPoseGraph函数。

local_slam_result_2d.cc

void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
                                       PoseGraph* pose_graph) const {
  DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph));
  CHECK_GE(local_slam_result_data_.submaps().size(), 1);
  CHECK(local_slam_result_data_.submaps(0).has_submap_2d());
  std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
  for (const auto& submap_proto : local_slam_result_data_.submaps()) {
    submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
  }
  static_cast<PoseGraph2D*>(pose_graph)
      ->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
                    mapping::FromProto(local_slam_result_data_.node_data())),
                trajectory_id, submaps);
}

最终下层回调追踪失败

也就是说，这个下层回调不是因为上层回调产生的，上层回调就是把数据加入位姿图中而已。并不引起下层回调。

也就是说，下层回调也就不能叫做下层回调。

我们现在将上层回调改名为回调A

我们现在将下层回调改名为回调B

回调A是数据加入了以后产生的，即自动将数据加入位姿图中。
回调B会在其他地方被用到，是用来在一个laser的前端匹配-估计位姿结束后决定是否将这个laser加入submap中进行的回调。但是要记住的是，它和GlobalTrajectoryBuilder这个东西是绑定的，回调B是由GlobalTrajectoryBuilder维护的。

二者没有联系。

---

4.总结

1.先讲壳这一节，我们给出了一个壳子。
2.再讲壳中物这一节，我们开启了订阅者们，订阅者们嗷嗷待哺等待数据到来，一旦数据到来则进入回调，会产生向壳中填入壳中物的效果。即将数据加入壳中，并产生回调A，即将刚进来的数据加入位姿图。