二、数据分发

接收sensor数据 ——（node.cc中的sensor回调函数）

//  接受轮速计数据
void Node::HandleOdometryMessage(const int trajectory_id,
                                 const std::string& sensor_id,
                                 const nav_msgs::Odometry::ConstPtr& msg) {
  //  锁
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
    return;
  }
  //  获取该条轨迹的sensor_bridge
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  //  转换格式
  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
  if (odometry_data_ptr != nullptr) {
    //  Node位姿外推器加入数据,推算位姿
    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
  }
  //  传入sensor_bridge
  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}

//  接受
void Node::HandleNavSatFixMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::NavSatFix::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  //  采样器
  if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleNavSatFixMessage(sensor_id, msg);
}

void Node::HandleLandmarkMessage(
    const int trajectory_id, const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLandmarkMessage(sensor_id, msg);
}

//  接受ＩＭＵ数据
void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 　采样器
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  // 获取该条轨迹的sensor_bridge
  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) {
    //　Node位姿外推器加入数据，推算位姿
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  // 　传入sensor_bridge
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

// 
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);//锁,避免各个传感器之间数据修改
  //  传感器帧数采样
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  // 把ROS中的数据类型送到数据转换器sensor_bridge中转换成cartographer接受的雷达数据类型，
  // 使用HandleLaserScanMessage函数处理数据（sensor_bridge.cc）
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

void Node::HandleMultiEchoLaserScanMessage(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}

注：在Node.cc中，将数据通过trajecotry_id传进相应的SensorBridge中进行处理。
点击处理数据的函数HandleImuMessage、HandleLaserScanMessage、HandleMultiEchoLaserScanMessage等函数转到定义，发现这些函数都是SensorBridge类的函数，点击SensorBridge类的定义

SensorBridge类（sensor_bridge.h）

class SensorBridge {
 public:
  explicit SensorBridge(
      int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
      ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);

  SensorBridge(const SensorBridge&) = delete;
  SensorBridge& operator=(const SensorBridge&) = delete;

  //  转换数据类型
  std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
      const nav_msgs::Odometry::ConstPtr& msg);
  void HandleOdometryMessage(const std::string& sensor_id,
                             const nav_msgs::Odometry::ConstPtr& msg);
  void HandleNavSatFixMessage(const std::string& sensor_id,
                              const sensor_msgs::NavSatFix::ConstPtr& msg);
  void HandleLandmarkMessage(
      const std::string& sensor_id,
      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);

  //  转换数据类型
  std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
      const sensor_msgs::Imu::ConstPtr& msg);
  void HandleImuMessage(const std::string& sensor_id,
                        const sensor_msgs::Imu::ConstPtr& msg);
  void HandleLaserScanMessage(const std::string& sensor_id,
                              const sensor_msgs::LaserScan::ConstPtr& msg);
  void HandleMultiEchoLaserScanMessage(
      const std::string& sensor_id,
      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
  void HandlePointCloud2Message(const std::string& sensor_id,
                                const sensor_msgs::PointCloud2::ConstPtr& msg);

  const TfBridge& tf_bridge() const;

 private:
  void HandleLaserScan(
      const std::string& sensor_id, ::cartographer::common::Time start_time,
      const std::string& frame_id,
      const ::cartographer::sensor::PointCloudWithIntensities& points);
  void HandleRangefinder(const std::string& sensor_id,
                         ::cartographer::common::Time time,
                         const std::string& frame_id,
                         const ::cartographer::sensor::TimedPointCloud& ranges);

  const int num_subdivisions_per_laser_scan_;
  std::map<std::string, cartographer::common::Time>
      sensor_to_previous_subdivision_time_;
  const TfBridge tf_bridge_;
  ::cartographer::mapping::TrajectoryBuilderInterface* const
      trajectory_builder_;

  absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
};

重点关注：

处理数据 ——（Node.cc）

点击HandleLaserScan（）进入sensor_brideg.cc

HandleLaserScanMessage（）

void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  // 定义一个这个类型（带强度的点云）的变量point_cloud，在carto中,全局搜索，可以查看
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  // 把LaserScan类型转换为cartographer自定义的PointCloudWithIntensities类型
  // 点击ToPointCloudWithIntensities（）转到声明，进入msg_conversion.h
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  // 将LaserScan转换成点云后送到下面函数HandleLaserScan（）去处理
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

void SensorBridge::HandleMultiEchoLaserScanMessage(
    const std::string& sensor_id,
    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

点击ToPointCloudWithIntensities（）转到声明，进入msg_conversion.h

//  点击转到定义，msg_conversion.cc
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);

点击转到定义，进入 msg_conversion.cc

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
  return LaserScanToPointCloudWithIntensities(msg);
}

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
  return LaserScanToPointCloudWithIntensities(msg);
}

点击LaserScanToPointCloudWithIntensities（）转到其定义（msg_conversion.cc），正式处理，将laserscan转成pointcloudwithIntensities

LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
  CHECK_GE(msg.range_min, 0.f);//检查，最小距离大于0
  CHECK_GE(msg.range_max, msg.range_min);//最大距离大于最小距离
  if (msg.angle_increment > 0.f) {
    CHECK_GT(msg.angle_max, msg.angle_min);//angle_increment，点云之间的角度增量；angle_max是最后一个点的角度，angle_min是第一个点的角度
  } else {
    CHECK_GT(msg.angle_min, msg.angle_max);
  }
  PointCloudWithIntensities point_cloud;//定义变量point_cloud，最后作为返回值返回
  float angle = msg.angle_min;//第一个点
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
    const auto& echoes = msg.ranges[i];//每个测距的echoes激光回波
    if (HasEcho(echoes)) {
      const float first_echo = GetFirstEcho(echoes);//一般取激光最强返回值，也就是第一次回波，返回类型为float
      //接下来将距离值转换成坐标值，使用轴角方式转换
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());//定义一个轴角：以z轴为轴，以角度angle为角度
        //定义一个点point，将其算出并push_back到point_cloud下的points中，作为返回值返回
        const cartographer::sensor::TimedRangefinderPoint point{
            rotation * (first_echo * Eigen::Vector3f::UnitX()),
            i * msg.time_increment};// Eigen::Vector3f::UnitX()表示X轴上的单位向量*第一次回波距离值
            //i是一帧激光的总点数，而time_increment是点与点之间的时间增量，i * msg.time_increment，就是一帧激光的时间间隔duration
        point_cloud.points.push_back(point);
        //将每个点的强度push_back到point_cloud中
        if (msg.intensities.size() > 0) {
          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
          const auto& echo_intensities = msg.intensities[i];
          CHECK(HasEcho(echo_intensities));
          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
        } else {
          point_cloud.intensities.push_back(0.f);
        }
      }
    }
    //转到下一个点point
    angle += msg.angle_increment;
  }
  //timestamp是定义的carto中的时间戳，但刚开始拿到的是ROS中的时间戳，所以需要做个计算换成carto中的时间戳
  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);//ROS中laser-scan的的时间戳是第一个点的时间戳
  //cartographer中一帧激光的时间戳是最后一个点的时间戳，故需转换
  //激光雷达每一帧数据都有时间戳，激光雷达需与主机或者其他传感器（GPS）时间同步，
  if (!point_cloud.points.empty()) {
    const double duration = point_cloud.points.back().time;//一帧激光的最后一个点的时间
    //此时，carto中的时间戳timestamp是ROS中的第一个点+duration，故，变成了一帧激光的最后一个点
    timestamp += cartographer::common::FromSeconds(duration);//一帧数据的每一个点都要加上这一帧数据的duration
    for (auto& point : point_cloud.points) {
      //相对时间戳
      point.time -= duration;//cartographer中时间戳是最后第一个点，所以每个点减去总时长duration，最后一个点前的点都是负值
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

至此，返回了带时间戳的点云，回到sensor_brage.cc的HandleLaserScan（）函数中，接着将LaserScan转换成点云后送到下面函数HandleLaserScan（）去处理，处理点云数据，在sensor_bridge.cc里

void SensorBridge::HandleLaserScan(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,
    const carto::sensor::PointCloudWithIntensities& points) {
  if (points.points.empty()) {
    return;
  }
  
  // 检查：点云中的最后一个点的时间小于等于0
  CHECK_LE(points.points.back().time, 0.f);
  // TODO(gaschler): Use per-point time instead of subdivisions.
  // 把一帧点云分成好几份，方便畸变处理
  // 以backpack2D为例，找到其配置文件backpack_2D.lua查看num_subdivisions_per_laser_scan是10
  // 就是把一帧数据分成了10份
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
    // 初始索引 = 一帧激光所有点 / 一小份激光的点 × i = 一份的点云数量 × i = 每一份的第一个点
    const size_t start_index =
        points.points.size() * i / num_subdivisions_per_laser_scan_;
    // 结束索引 = 一帧激光所有点 / 一小份激光点 × （i+1） = 一份的点云数量 × （i+1） = 每一份的最后一个点
    const size_t end_index =
        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
    // 得到一小块点云的数据subdivision
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    // 判断一帧激光数据是否完结，跳出循环
    if (start_index == end_index) {
      continue;
    }
    // 得到一小块数据的最后一个点的时间，相对时间戳（相对一最后一个点的时间，是负值）。  
    const double time_to_subdivision_end = subdivision.back().time;
    // `subdivision_time` is the end of the measurement so sensor::Collator will
    // send all other sensor data first.
    // time 指的是整帧激光的绝对时间戳；time_to_subdivision_end是一小段激光的相对时间戳
    // subdivision_time是一小段激光的最后一个点的绝对时间戳 = time + 负值
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end);
    // 查看 sensor_to_previous_subdivision_time_的定义（sensor_bridge.h），是一个map类型（带to的要么是方向，要么是map类型）
    // 表示上一次记录的子块的时间戳
    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
    // 这种情况出现在一台机器同时有两台激光雷达，且激光雷达的topic名称一样，很多原因导致（份数、雷达扫的点数）数据交替
    // 判断条件，it：上次记录，找到了但不等于上一次的时间end && it（上一次）的绝对时间 >= 当前子块的绝对时间
    // 此时说明当前帧的当前子块的点云被另一帧的某个子块（上一个subdivision）全部包含，古可将当前子块舍弃
    if (it != sensor_to_previous_subdivision_time_.end() &&
        it->second >= subdivision_time) {
      // 打印出log报waring：忽视当前sensorId的当前帧的subdivision小块，因为另一帧上一subdivision小块的绝对时间大于这一帧此小块的绝对时间
      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                   << sensor_id << " because previous subdivision time " 
                   << it->second << " is not before current subdivision time "
                   << subdivision_time;
      continue;
    }
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    for (auto& point : subdivision) {
      // time_to_subdivision_end 是这一小块最后时间与这一帧最后时间的相对时间，
      // point.time表示当前点相对于这一帧数据的相对时间
      // 如 ： time_to_subdivision_end=-7 point.time=-8 则计算后point.time变成了相对于子块最后时间的相对时间（-8 -（-7） = -1）
      point.time -= time_to_subdivision_end;
    }
    // 检查每个小分块的最后时间小于等于0
    CHECK_EQ(subdivision.back().time, 0.f);
    // 送去此HandleRangefinder函数处理（sensor_bridge.cc）
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  }
}

// 将数据从激光雷达坐标系转换到tracking坐标系，并将转换后的数据传给SensorBridge的trajectory_builder_，
// 该trajectory_builder_是MapBuiler中的CollectorTrajectoryBuilder
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  if (!ranges.empty()) {
    // 检查每个小分块的最后的相对时间戳是不是小于等于0
    CHECK_LE(ranges.back().time, 0.f);
  }
  // sensor_to_tracking表示当前传感器要转换到tracking下，tracking指的是SLAM实际跟踪的坐标系，比如base_link 
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  if (sensor_to_tracking != nullptr) {
    // sensor_to_tracking不空，说明，有传感器数据，则需要给轨迹构建器添加sensor数据
    // sensor_bridge.cc中的trajectory_builder_来自哪里？
    // trajectory_builder_在carto中，去到，给trajectory_builder_添加数据
    // 在carto中搜索collated_Trajectory_builder.h
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())});
  }
}

查看trajectory_builder_定义

  // TrajectoryBuilderInterface是一个虚基类（抽象类），TrajectoryBuilder继承与此虚基类
  // 而trajectory_builder_是此类的实例化对象，可以调用此类的成员函数和变量
  ::cartographer::mapping::TrajectoryBuilderInterface* const
      trajectory_builder_;

trajectory_builder_类型为TrajectoryBuilderInterface，是一个虚基类（抽象类），TrajectoryBuilder继承与此虚基类，故在carto中搜索collated_Trajectory_builder.h，查看其是否有成员函数AddSensorData（）

数据传入carto ——（trajectory_builder_->AddSensorData）

在cartographer_ros中接受的sensor数据经过一定处理，传入到cartographer中使用（collated_trajectory_builder）

collated_trajectory_builder.h

// 由sensor_bridge的HandleRangefinder函数调用，将sensor_bridge中处理好的激光数据传给CollatedTrajectoryBuilder
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    // MakeDispatchable是什么？使分发
    // 数据经过makedispatchable之后可以给到轨迹构建器，AddData数据同步器，查看定义
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

  // 由sensor_bridge的HandleImuMessage函数调用，将sensor_bridge中处理好的IMU数据传给CollatedTrajectoryBuilder
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
  }

  // 由sensor_bridge的HandleOdometryMessage函数调用，将sensor_bridge中处理好的轮速计数据传给CollatedTrajectoryBuilder
  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
  }

查看MakeDispatchable（），进去dispatchable.h

数据统一组织 —— MakeDispatchable（）

template <typename DataType>
// Dispatchable类
class Dispatchable : public Data {
 public:
  Dispatchable(const std::string &sensor_id, const DataType &data)
      : Data(sensor_id), data_(data) {}

  common::Time GetTime() const override { return data_.time; }
  // 重要函数：sensor数据经过数据同步之后，添加到地图构建器
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }
  const DataType &data() const { return data_; }

 private:
  const DataType data_;//数据本身data_
};

template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
  //以data为实参，构建了一个类Dispatchable
  return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}

查看collated_trajectory_builder.h数据同一调用接口AddData（），将数据放入MapBuilder的数据分发器中

void 
  // 传入的数据是makedispatched的数据
CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  // sensor数据等通过该函数统一将数据送入MapBuilder数据分发器sensor_collator_中
  // sensor_collator_的类型为sensor::collator（继承与虚基类CollatorInterface），进入collator.h中查看
  // sensor_collator_数据收集器，添加完数据进行同步，同步完了进行分发dispatch，分发给谁？
  // 一种传感器数据都有一个队列，每个队列都有callback，开启一条轨迹，sensor_collator_会管制一条轨迹
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

collator.h查看sensor_collator_的类型

class Collator : public CollatorInterface {
 public:
  Collator() {}

  Collator(const Collator&) = delete;
  Collator& operator=(const Collator&) = delete;

  // CollatedTrajectoryBuilder的HandleCollatedSensorData作为回调函数callback传进来
  void AddTrajectory(
      int trajectory_id,
      const absl::flat_hash_set<std::string>& expected_sensor_ids,
      const Callback& callback) override;

  void FinishTrajectory(int trajectory_id) override;

  // 数据传入接口
  void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;

  void Flush() override;

  absl::optional<int> GetBlockingTrajectoryId() const override;

 private:
  // Queue keys are a pair of trajectory ID and sensor identifier.
  // 队列里存放着所有的数据，包括轨迹的轮速计数据和激光数据，并且根据QueueKey分发到不同的轨迹中，并执行相应的回调函数
  OrderedMultiQueue queue_;//有序的添加队列

  // Map of trajectory ID to all associated QueueKeys.
  // 队列queue_keys_里管理着<trajectory_id, <trajectory_id, sensor_id>>
  absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;
};

查看AddSensorData（）函数，进入collated.cc

void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data) {
  // 构造QueueKey
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  // 将sensor::Dispatchable及其对应的QueueKey放入queue_(OrderedMultiQueue::Add)里
  // 点击queue_查看其定义
  queue_.Add(std::move(queue_key), std::move(data));
}

点击queue_查看其定义，进入collated.h，是collator类的私有成员变量

 private:
  // Queue keys are a pair of trajectory ID and sensor identifier.
  // 队列里存放着所有的数据，包括轨迹的轮速计数据和激光数据，并且根据QueueKey分发到不同的轨迹中，并执行相应的回调函数
  OrderedMultiQueue queue_;//有序的添加队列

queue_是OrderedMultiQueue类型的对象，查看OrderedMultiQueue类型，进入ordered_multi_queue.h

class OrderedMultiQueue {
 public:
  using Callback = std::function<void(std::unique_ptr<Data>)>;

  OrderedMultiQueue();
  OrderedMultiQueue(OrderedMultiQueue&& queue) = default;

  ~OrderedMultiQueue();

  // Adds a new queue with key 'queue_key' which must not already exist.
  // 'callback' will be called whenever data from this queue can be dispatched.
  void AddQueue(const QueueKey& queue_key, Callback callback);

  // Marks a queue as finished, i.e. no further data can be added. The queue
  // will be removed once the last piece of data from it has been dispatched.
  void MarkQueueAsFinished(const QueueKey& queue_key);

  // Adds 'data' to a queue with the given 'queue_key'. Data must be added
  // sorted per queue.
  void Add(const QueueKey& queue_key, std::unique_ptr<Data> data);

  // Dispatches all remaining values in sorted order and removes the underlying
  // queues.
  void Flush();

  // Must only be called if at least one unfinished queue exists. Returns the
  // key of a queue that needs more data before the OrderedMultiQueue can
  // dispatch data.
  QueueKey GetBlocker() const;

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

加入数据 —— OrderedMultiQueue::Add（）

在collator.cc中查看有此成员函数Add（）

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

将数据的回调函数及queue_key加入队列：

// 将轨迹中QueueKey和对应的回调函数，放到queues_里
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

分发数据 —— OrderedMultiQueue::Dispatch()

查看dispatch（）函数，进入ordered_multi_queue.cc

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) {
        // 删除finished队列，不再给finished的trajectory分发数据
        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;
    }
    // 队列为空的情况
    if (next_data == nullptr) {
      CHECK(queues_.empty());
      return;
    }

    // 分发最老的数据next_data
    // 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);

    // 如果待分发数据时间戳大于等于起始时间，则正常分发
    if (next_data->GetTime() >= common_start_time) {
      // Happy case, we are beyond the 'common_start_time' already.
      last_dispatched_time_ = next_data->GetTime();
      // 执行queue的callbac函数，该callback即是在OrderedMultiQueue::AddQueue函数中加入的
      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());
    } 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));
      }
    }
  }
}

至此，从node的sensor数据经过处理，按照时间分发到相应的GlobalTrajectoryBuilder，并通过GlobalTrajectoryBuilder的AddSensorData（）函数，将数据传到前端和后端

sensor数据传入前后端

GlobalTrajectoryBuilder.cc中的AddSensorData，将相关sensor数据传入前后端

  // 激光数据接口
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    CHECK(local_trajectory_builder_)
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
    // 将激光数据送入LocalTrajectoryBuilder进行CSM匹配，并生成submap
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);
    if (matching_result == nullptr) {
      // The range data has not been fully accumulated yet.
      return;
    }
    kLocalSlamMatchingResults->Increment();
    std::unique_ptr<InsertionResult> insertion_result;
    if (matching_result->insertion_result != nullptr) {
      kLocalSlamInsertionResults->Increment();
      // 将前端计算的Node传入后端PoseGraph中计算回环和参与优化
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);
      CHECK_EQ(node_id.trajectory_id, trajectory_id_);
      insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
          node_id, matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});
    }
    if (local_slam_result_callback_) {
      local_slam_result_callback_(
          trajectory_id_, matching_result->time, matching_result->local_pose,
          std::move(matching_result->range_data_in_local),
          std::move(insertion_result));
    }
  }

  // IMU数据接口
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    if (local_trajectory_builder_) {
      // 前端接收IMU数据，放在位姿外推器中，校正运动畸变和提供CSM初值
      local_trajectory_builder_->AddImuData(imu_data);
    }
    // 后端接收IMU数据，在3DSLAM里，会构建IMU残差，参与优化，而在2D里，不会参与优化
    pose_graph_->AddImuData(trajectory_id_, imu_data);
  }

  // 轮速里程计数据接口
  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
    if (local_trajectory_builder_) {
      // 前端接收轮速计数据，放在位姿外推器中，校正运动畸变和提供CSM初值
      local_trajectory_builder_->AddOdometryData(odometry_data);
    }
    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
    // odometry could be marginalized between nodes in the pose graph.
    // Related issue: cartographer-project/cartographer/#1768
    if (pose_graph_odometry_motion_filter_.has_value() &&
        pose_graph_odometry_motion_filter_.value().IsSimilar(
            odometry_data.time, odometry_data.pose)) {
      return;
    }
    // 后端接收轮速计数据，用于构建轮速计边参与优化
    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
  }