其实对于cartographer建图来说，cartographer_ros只是为了用户方便调用接口，建图算法的核心在cartographer中，不过cargographer_ros对于用户使用来说，还是很便捷的。

先来看看cartographer建图的节点图。
在这里插入图片描述

除了bag数据包和机器人发布的坐标变换，真正有用的就两个节点，cartographer_node和cartographer_occupance_grid_node，前者为调用底层算法节点，后者为地图输出节点。下面分别做介绍。

一. 接口关系

为了便于理解，先来看看他们通信的桥梁接口。

cartographer_ros

map_builder_bridge.h中定义了：std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

map_bulider_bridege.h中还定义了：SensorBridge* sensor_bridge(int trajectory_id);，它在sensor_bridge.h中定义，而在它里面又定义了 ::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_;

cartographer

cartographer::mapping::MapBuilderInterface：它是一个抽象类，它的具体实现在map_builder.h中。

cartographer::mapping::TrajectoryBuilderInterface也是抽象类，它的实例化在mapping/internal/collated_trajector_builder.h。

看看TrajectoryBuilderInterface的实例化，在map_builder.h中,定义了

std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> trajectory_builders_;

然后又在map_builder.cc中实现了实例化：

//CollatedTrajectoryBuilder类型的对象才是所谓的轨迹跟踪器，轨迹跟踪器可能不止一条
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(//这个应该是用在后面的全局slam中
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

所以从上面可以看到，cartographer_ros的接口为map_builder_bridge.h，而cartographer的对接文件为map_builder_interface.h。最终数据都会在map_builder.h中

汇总它们的接口数据传输流程图：

二. cartographer_node

1. node_main.cc

该函数为cartographer_node的主函数，主要调用功能：

加载(LoadOptions)lua文件，并存入node_options和trajectory_options

监听tf：存入tf_buffer

Node构造函数(传入node_options，map_builder，tf_buffer)，订阅与发布

加载pbstream地图文件

开启默认轨迹(node.StartTrajectoryWithDefaultTopics)：传入trajectory_options

ros::spin()结束后：

完成轨迹：node.FinishAllTrajectories();

最终的地图优化：node.RunFinalOptimization();

地图系统状态： if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}

下面介绍构造函数和默认轨迹函数的流程。

2. node.cc

在node.h中定义了关键桥梁： map_builder_bridge_，也就是上面介绍的cartographer_ros的桥梁接口，数据会通过这个桥梁进行传输。

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

（1）构造函数(Node::Node)

在构造函数中，定义了一些发布器和服务器。注意在构造函数中没有进行订阅函数的定义。

发布器

变量	话题名称	说明
kSubmapListTopic	/submap_list	构建好的子图列表。
kTrajectoryNodeListTopic	/trajectory_node_list	跟踪轨迹路径点列表。
kLandmarkPosesListTopic	/landmark_poses_list	路标点位姿列表。
kConstraintListTopic	/constraint_list	优化约束。
kScanMatchedPointCloudTopic	/scan_matched_points2	匹配的2D点云数据。

这些发布器会通过定时器进行定时发布，分别定时发送上面五个发布器，定时器的时间在lua文件中定义。

wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this));
wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.pose_publish_period_sec), &Node::PublishTrajectoryStates, this));
wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(kConstraintPublishPeriodSec), &Node::PublishConstraintList, this));

服务器

总共有7个服务器，包括子图、开始轨迹、结束轨迹等的回调。需要注意的是建图过程中的地图显示是通过这个服务器回调实现的，之前我一直误以为通过话题订阅实现。

//地图显示服务器
service_servers_.push_back(node_handle_.advertiseService(kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
//结束建图
service_servers_.push_back(node_handle_.advertiseService(kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
//保存地图
service_servers_.push_back(node_handle_.advertiseService(kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(kReadMetricsServiceName, &Node::HandleReadMetrics, this));

（2）开启轨迹(Node::StartTrajectoryWithDefaultTopics)

除此之外，还可以调用服务"start_trajectory"来开启。两者的不同是，默认轨迹，使用系统默认的订阅主题，后者在调用服务的时候还需要提供轨迹配置和订阅主题。两者最终都是通过AddTrajectory来处理。这里以默认开启轨迹进行讲解。

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
    
    //传递前端参数
  absl::MutexLock lock(&mutex_);
  CHECK(ValidateTrajectoryOptions(options));
  AddTrajectory(options);
}

这里直接只有一个函数AddTrajectory(options)函数，因此，我们看看这个函数的实现。

int Node::AddTrajectory(const TrajectoryOptions& options) {
    
    
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);//处理一些重复的话题信息，比如scan_1,scan_2

       //接着通过接口map_builder_bridge_向Cartographer添加一条新的轨迹并获取轨迹的索引。然后以该索引为键值，
  //通过函数AddExtrapolator和AddSensorSamplers添加用于位姿插值(PoseExtrapolator)和传感器采样(TrajectorySensorSamplers)的对象。
  const int trajectory_id =//trajectory_id是0，第一条轨迹，而且是const类型，不会变
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);//通过cartographer实现Add
  AddExtrapolator(trajectory_id, options);//位姿
  AddSensorSamplers(trajectory_id, options);//传感器采样,会修改sensor_samplers_的值，然后影响订阅
  LaunchSubscribers(options, trajectory_id);//订阅传感器
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
       //最后记录当前正在更新的轨迹，以及订阅的主题，并返回新建的轨迹索引。 
  //在node对象中，通过成员函数LaunchSubscribers根据配置订阅需要的主题。
  for (const auto& sensor_id : expected_sensor_ids) {
    
    
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

（3）数据订阅(Node::LaunchSubscribers)

其中最重要的是订阅传感器数据函数LaunchSubscribers(options, trajectory_id);。我们来看看数据订阅的流程。
实际上在这个函数中，可以看到很多判断语句，因为需要判断lua中限定了订阅传感器的类型。在这里我们以雷达和imu数据进行举例。

Node::HandleLaserScanMessage

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;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

我们可以看到，订阅的雷达数据直接传入到map_builder_bridge_中去了，它属于和底层的通信接口。

这里我们还需要看到的是，它实际上调用的是sensor_bridge的接口。符合接口数据传输关系图。看看它的流程图：

传递的数据格式为：cartographer/sensor/point_cloud.h中的TimedPointCloud，如下图：

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

Node::HandleImuMessage

//下面是处理IMU的消息回调函数，除了要通过map_builder_bridge_传递给建图器之外
//还需要将IMU数据传递给位姿估计器。 
//这个位姿估计器的数据类型是定义在cartographer的PoseExtrapolator，
//同样是在函数AddTrajectory中通过调用AddExtrapolator完成初始化操作。
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;
  }
  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);//直接传到cartographer底层
}

在这里我们可以看到imu数据传给了两个模块：位姿态估计器和cartographer底层。传给底层的流程和雷达数据一样，通过map_builder_bridge_.sensor_bridge传输数据。传给位姿估计器的extrapolators_，实际上直接和cartographer对接，在node.h中定义如下：

std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;

三. cartographer_occupancy_grid_node

下面我们看看另外一个节点。

occupancy_grid_node_main.cc

我们可以看到这个函数中既有主函数，又有Node函数的一些定义。而主函数中直接转入到了Node的构造函数。构造函数只传递了分辨率和发布频率。

  ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);

在构造函数中，定义了一个发布器、一个订阅器和一个服务器。

订阅器类型：submap_list
发布器类型： nav_msgs::OccupancyGrid类型。

Node::Node(const double resolution, const double publish_period_sec)
    : resolution_(resolution),
      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
          kSubmapQueryServiceName)),//服务器
      submap_list_subscriber_(node_handle_.subscribe(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize,
          boost::function<void(
              const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
    
    
                HandleSubmapList(msg);
              }))),//订阅器
      occupancy_grid_publisher_(
          node_handle_.advertise<::nav_msgs::OccupancyGrid>(
              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
              true /* latched */)),//发布器
      occupancy_grid_publisher_timer_(//定时器
          node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
                                       &Node::DrawAndPublish, this)) {
    
    }

然后我们直接看订阅函数的回调函数。在这个回调函数中，我们直接对msg进行处理，然后传递给submap_slices_，它是需要发布的数据。(submap_slices_经过加锁)

  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);

void Node::HandleSubmapList(
    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
    
    
  absl::MutexLock locker(&mutex_);

  // We do not do any work if nobody listens.
  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
    
    
    return;
  }

  // Keep track of submap IDs that don't appear in the message anymore.
  std::set<SubmapId> submap_ids_to_delete;
  for (const auto& pair : submap_slices_) {
    
    
    submap_ids_to_delete.insert(pair.first);
  }

  for (const auto& submap_msg : msg->submap) {
    
    
    const SubmapId id{
    
    submap_msg.trajectory_id, submap_msg.submap_index};
    submap_ids_to_delete.erase(id);
    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
    
    
      continue;
    }
    SubmapSlice& submap_slice = submap_slices_[id];
    submap_slice.pose = ToRigid3d(submap_msg.pose);
    submap_slice.metadata_version = submap_msg.submap_version;
    if (submap_slice.surface != nullptr &&
        submap_slice.version == submap_msg.submap_version) {
    
    
      continue;
    }

    auto fetched_textures =
        ::cartographer_ros::FetchSubmapTextures(id, &client_);
    if (fetched_textures == nullptr) {
    
    
      continue;
    }
    CHECK(!fetched_textures->textures.empty());
    submap_slice.version = fetched_textures->version;

    // We use the first texture only. By convention this is the highest
    // resolution texture and that is the one we want to use to construct the
    // map for ROS.
    const auto fetched_texture = fetched_textures->textures.begin();
    submap_slice.width = fetched_texture->width;
    submap_slice.height = fetched_texture->height;
    submap_slice.slice_pose = fetched_texture->slice_pose;
    submap_slice.resolution = fetched_texture->resolution;
    submap_slice.cairo_data.clear();
    submap_slice.surface = ::cartographer::io::DrawTexture(
        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
        fetched_texture->width, fetched_texture->height,
        &submap_slice.cairo_data);
  }

  // Delete all submaps that didn't appear in the message.
  for (const auto& id : submap_ids_to_delete) {
    
    
    submap_slices_.erase(id);
  }

  last_timestamp_ = msg->header.stamp;
  last_frame_id_ = msg->header.frame_id;
}

四. 其他说明

4.1 保存地图脚本

#!/bin/bash
rosservice call /finish_trajectory 0
rosservice call /write_state "filename: '$(pwd)/map.pbstream'"

killall rosmaster

4.2 结束建图

rosservice call /finish_trajectory 0

4.3 保存地图

rosservice call /write_state "filename: '$(pwd)/map.pbstream'"

cartorapher_ros：理解cartographer_ros之源码解读