发布地图（map话题）

一、顶层逻辑

1. [cartographer_node节点]Node::Node构造函数
   1、创建“submap_query”Server。
   2、创建间隔submap_publish_period_sec秒(默认0.3秒)的定时器，处理例程Node::PublishSubmapList，用于定时发布“submap_list”话题。
2. [cartographer_occupancy_grid节点]Node::Node构造函数
   1、创建“submap_query”Client。
   2、订阅“submap_list”话题。
   3、创建间隔publish_period_sec秒(不是lua配置，默认1秒)的定时器，处理例程Node::DrawAndPublish，用于定时发布“map”话题。
3. [cartographer_occupancy_grid节点]收到“submap_list”话题，回调Node::HandleSubmapList()，负责根据子图列表生成submap_slices_，处理后，submap_slices_已将概率图转成cario图面，但没有将子图拼成大地图。处理过程中，“submap_list”给出了子图索引、绝对位姿、概率图版本，但没有该子图概率图。一旦没有得到过该子图的概率图或概率图版本发生变化，会以子图索引为参数调用“submap_query”服务获得概率图。
4. [cartographer_occupancy_grid节点]cpu调度到Node::DrawAndPublish定时器
   1、把submap_slices_中子图拼成大地图，结果存放在painted_slices。
   2、把painted_slices转成msgs::OccupancyGrid，基于它发布个“map”。

void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
  absl::MutexLock locker(&mutex_);
  // last_frame_id_: "map"
  if (submap_slices_.empty() || last_frame_id_.empty()) {
    return;
  }
  // submap_slices_存储着最新子图列表，submap_slices_[n].surface存储着cairo_surface_t类型的图面。
  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
  // painted_slices存储拼出的大地图。painted_slices.surface是cairo_surface_t类型的图面，
  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
      painted_slices, resolution_, last_frame_id_, last_timestamp_);
  occupancy_grid_publisher_.publish(*msg_ptr);
}

二、拼接地图：PaintSubmapSlices

PaintSubmapSlices执行着DrawAndPublish的第一个任务：把submap_slices_中子图拼成大地图。

<cartographer>/cartographer/io/submap_painter.cc
------
PaintSubmapSlicesResult PaintSubmapSlices(
    const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
    const double resolution) {
  Eigen::AlignedBox2f bounding_box;
  {
    auto surface = MakeUniqueCairoSurfacePtr(
        cairo_image_surface_create(kCairoFormat, 1, 1));
    auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
    const auto update_bounding_box = [&bounding_box, &cr](double x, double y) {
      cairo_user_to_device(cr.get(), &x, &y);
      bounding_box.extend(Eigen::Vector2f(x, y));
    };

    // 第一个CairoPaintSubmapSlices，算出地图尺寸、map坐标系原点。
    CairoPaintSubmapSlices(
        1. / resolution, submaps, cr.get(),
        [&update_bounding_box](const SubmapSlice& submap_slice) {
          update_bounding_box(0, 0);
          update_bounding_box(submap_slice.width, 0);
          update_bounding_box(0, submap_slice.height);
          update_bounding_box(submap_slice.width, submap_slice.height);
        });
  }

  const int kPaddingPixel = 5;  
  // 最终地图的宽度、高度各多了2*kPaddingPixel个栅格。
  const Eigen::Array2i size(
      std::ceil(bounding_box.sizes().x()) + 2 * kPaddingPixel,
      std::ceil(bounding_box.sizes().y()) + 2 * kPaddingPixel);
  const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel,
                              -bounding_box.min().y() + kPaddingPixel);

  // size指示地图尺寸，有了它便可创建用于存储地图的cairo_surface_t图面了
  auto surface = MakeUniqueCairoSurfacePtr(
      cairo_image_surface_create(kCairoFormat, size.x(), size.y()));
  {
    auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
    cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.);
    cairo_paint(cr.get());
    cairo_translate(cr.get(), origin.x(), origin.y());
    // 第二个CairoPaintSubmapSlices，将各子图画到地图
    CairoPaintSubmapSlices(1. / resolution, submaps, cr.get(),
                           [&cr](const SubmapSlice& submap_slice) {
                             cairo_set_source_surface(
                                 cr.get(), submap_slice.surface.get(), 0., 0.);
                             cairo_paint(cr.get());
                           });
    cairo_surface_flush(surface.get());
  }
  return PaintSubmapSlicesResult(std::move(surface), origin);
}

PaintSubmapSlices依次执行两次CairoPaintSubmapSlices，第一次的目的算出地图尺寸、map坐标系原点；第二次目的是把子图画到地图。

void CairoPaintSubmapSlices(
    const double scale,
    const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
    cairo_t* cr, std::function<void(const SubmapSlice&)> draw_callback) {
  cairo_scale(cr, scale, scale);

  for (auto& pair : submaps) {
    const auto& submap_slice = pair.second;
    if (submap_slice.surface == nullptr) {
      return;
    }
    // submap_slice.pose。来自“submap_list”消息。值就是Global SLAM优化出的该子图的全局位姿。
    // submap_slice.slice_pose。来自“submap_query”服务。表示概率图的laser原点。2D时，旋转部分一定是0。
    const Eigen::Matrix4d homo =
        ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix();

    cairo_save(cr);
    cairo_matrix_t matrix;
    cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1),
                      homo(0, 3), -homo(1, 3));
    cairo_transform(cr, &matrix);

    const double submap_resolution = submap_slice.resolution;
    cairo_scale(cr, submap_resolution, submap_resolution);

    // Invokes caller's callback to utilize slice data in global cooridnate
    // frame. e.g. finds bounding box, paints slices.
    draw_callback(submap_slice);
    cairo_restore(cr);
  }
}

CairoPaintSubmapSlices会枚举submaps中各子图，对每张子图，先是准备好环境，然后调用真实执行操作的draw_callback。准备环境时，一个重要操作是要准备好cairo变换关系，即变量matrix表示的二维变换矩阵。matrix来自两个位姿：submap_slice.pose、submap_slice.slice_pose。

• submap_slice.pose。来自“submap_list”消息。值就是Global SLAM优化出的该子图的全局位姿。即使是2D时，旋转部分一般不是0。
• submap_slice.slice_pose。来自“submap_query”服务。表示概率图的laser原点。2D时，旋转部分一定是0。参考“概率栅格图(ProbabilityGrid)”中“DrawToSubmapTexture”。

homo是这两个位姿的“和”，至于接下homo生成matrix，个人能力不足，还没理解。但如果不考虑submap_slice.pose旋转部分，即当旋转是0时，变换可拆分为两个步骤：先顺时针旋转90度，再平移。这里给出一个update_bounding_box时示例，子图尺寸是(203 x 97)，要update_bounding_box的四个点依次是A(0, 0)、B(203, 0)、C(0, 97)、D(207, 97)。

关于PaintSubmapSlices的未解决疑问。

• 在表示概率栅格图坐标系时，我把y表示成由上到下增，和直角坐标系相反，这是对的吗？（这么做一个原因，计算origin时取的是“-bounding_box.min().y()”，认为min左上角，前面有个负号）。
• map坐标系原点origin直接等于(-bounding_box.min().x() + kPaddingPixel, -bounding_box.min().y() + kPaddingPixel)，这个bounding_box不应该是包含所有子图的矩形吗？它的左上角怎么成了了origin？
• ProbabilityGrid由点云生成概率时，x、y是互换的，是在哪换回来？
• matrix矩阵中，偏移中的y加了负号，为什么？