Cartographer_learn_2

bool Node::HandleStartTrajectory(
    ::cartographer_ros_msgs::StartTrajectory::Request& request,
    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
    ......//前面是一系列的检查，检查即设置，比如检查有没有初始位姿，如果由便设置，检查是否存在生成轨迹的必要配置信息，没有就报错等
    else {
    response.status.message = "Success.";
    response.trajectory_id = AddTrajectory(trajectory_options);   //真正生成轨迹
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
  }
  return true;
 }

##生成轨迹的函数
int Node::AddTrajectory(const TrajectoryOptions& options) {

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);
  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);
  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

我们看到其实轨迹的生成也是调用了map_builder_bridge_来具体生成的，下一节我们再来具体的看看Node::AddTrajectory这个函数，很重要。
同样的这里直接给出结论Node::HandleFinishTrajectory这个回调函数是调用了Node::FinishTrajectoryUnderLock，再由FinishTrajectoryUnderLock调用map_builder_bridge_去具体实现功能。
所以我们得到了结论，再提供ros服务这方面，Node这个类只是一个封装，其具体实现是依靠它的成员变量*MapBuilderBridge map_builder_bridge_***具体实现。

#函数Node::AddTrajectory
这个函数虽然调用了map_builder_bridge_去具体的生成一条轨迹，但是我们看到生成一条轨迹后它还做了大量的工作
（1）生成位姿估计器（还没学完，但是我认为（瞎猜）是利用imu和里程计给出位姿的初值，便于在前端做迭代求解），这个位姿估计器生成后存储在map<int, ::cartographer::mapping::PoseExtrapolator> Node::extrapolators_中，其中索引是轨迹的id
（2）生成传感器采样器，这里也有目前想不明白的点，是要丢弃掉一些传感器数据吗？生成后存储在unordered_map<int, TrajectorySensorSamplers> Node::sensor_samplers_中，索引是轨迹的id
（3）重点又来了（怎么全是重点，吐槽），订阅一些话题。这里说一下目前我对轨迹的理解，还不全面，至少做一次slam过程就要生成一条轨迹，那么一条轨迹要怎么接受传感器数据呢？就在这，通过订阅一些话题去接收传感器数据。又上代码了

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  // multi_echo_laser_scans的订阅与注册回调函数
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  // point_clouds 的订阅与注册回调函数
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, kImuTopic,
                                                &node_handle_, this),
         kImuTopic});
  }
  // odometry 的订阅与注册回调函数
  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, kOdometryTopic,
                                                  &node_handle_, this),
         kOdometryTopic});
  }
  // gps 的订阅与注册回调函数
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             &node_handle_, this),
         kNavSatFixTopic});
  }
  // landmarks 的订阅与注册回调函数
  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, 
             trajectory_id, 
             kLandmarkTopic,
             &node_handle_, 
             this),
         kLandmarkTopic});
  }
}

我们看到订阅了laser_scan，multi_echo_laser_scans，点云，imu（根据需要），里程计（根据需要），gps（根据需要）和landmark（根据需要）的数据。同时我们也看到了一系列的数据处理的函数，就是这些订阅者的回调函数。如果你有兴趣，可以打开这些回调函数看看，大多是先通过前面的传感器采样器判断要不要接收本次数据，要的话又又又调用*map_builder_bridge_*去具体处理，当然有点小小不同，这次调用的map_builder_bridge_中的一个sensor_bridge去具体处理，相信后面我们还会具体讨论它。

#Node发布的话题
这里就简单的提一下，可以在Cartographer_learn_1这篇文章中看到，这些话题的发布都是通过与定时器绑定去发布的，所以我们可以在Node构造函数的step4中看到它们的回调函数，打开这些回调函数具体看会发现这些话题发布的数据来源依然是map_builder_bridge_。看到这，有一种感觉，就是这个Node好像就是个封装，其具体的实现大多都在map_builder_bridge_中。