cartographer生成ros格式地图

    较为简单的离线方法请参考：https://blog.csdn.net/xiekaikaibing/article/details/80320822（2018.05.15更新）

    cartographer建图时保存的地图格式默认为.pbstream。而机器人的navigation需要标准的ROS格式地图（.pgm+.yaml）。在此采用一种讨巧的方法，在保存.pbstream格式的地图后同时将其另存为标准的ROS格式地图。cartographer_ros中已有离线.pbstream格式地图转ros标准格式地图接口，据此写了个函数供建图时cartographer节点在线调用。函数主要参考了源程序中的pbstream_to_ros_map_main.cc。cartographer_ros-0.3.0版本为2018年1月左右最新版本。cartographer的作者们可能会在后续的版本中对地图转换的接口进行修改，为此需要做出相应修改。

     头文件pbstream_to_ros_map.h如下：  

#include <map>
#include <string>

#include "cartographer/io/proto_stream.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/submaps.h"
#include "cartographer_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "gflags/gflags.h"
#include "glog/logging.h"


namespace cartographer_ros{
  class Pbstream_to_ros_map{
	public:
	Pbstream_to_ros_map();
	~Pbstream_to_ros_map();
    Pbstream_to_ros_map(const Pbstream_to_ros_map&) = delete;
    Pbstream_to_ros_map& operator=(const Pbstream_to_ros_map&) = delete;
    void pgm_map_build(const std::string& pbstream_filename, const std::string& map_filestem,
         const double resolution);   
	};
}

     具体实现pbstream_to_ros_map.cc如下：

#include "cartographer_ros/pbstream_to_ros_map.h"

namespace cartographer_ros{
  Pbstream_to_ros_map::Pbstream_to_ros_map(){}
  Pbstream_to_ros_map::~Pbstream_to_ros_map(){}
   void Pbstream_to_ros_map::pgm_map_build(const std::string& pbstream_filename, const std::string& map_filestem,
         const double resolution){
  ::cartographer::io::ProtoStreamReader reader(pbstream_filename);

  ::cartographer::mapping::proto::PoseGraph pose_graph;
  CHECK(reader.ReadProto(&pose_graph));

  LOG(INFO) << "Loading submap slices from serialized data.";
  std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
      submap_slices;
  for (;;) {
    ::cartographer::mapping::proto::SerializedData proto;
    if (!reader.ReadProto(&proto)) {
      break;
    }
    if (proto.has_submap()) {
      const auto& submap = proto.submap();
      const ::cartographer::mapping::SubmapId id{
          submap.submap_id().trajectory_id(),
          submap.submap_id().submap_index()};
      const ::cartographer::transform::Rigid3d global_submap_pose =
          ::cartographer::transform::ToRigid3(
              pose_graph.trajectory(id.trajectory_id)
                  .submap(id.submap_index)
                  .pose());
      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
    }
  }
  CHECK(reader.eof());

  LOG(INFO) << "Generating combined map image from submap slices.";
  auto result =
      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);

  ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");

  ::cartographer::io::Image image(std::move(result.surface));
  WritePgm(image, resolution, &pgm_writer);

  const Eigen::Vector2d origin(
      -result.origin.x() * resolution,
      (result.origin.y() - image.height()) * resolution);

  ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
  WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
 }
}

    在生成.pbstream格式地图后，调用该函数即可得到ROS标准格式的地图。结束建图后，将会得到两种格式的地图。

    一个调用的例子如下：

Pbstream_to_ros_map pbstream_to_ros_map;
pbstream_to_ros_map.pgm_map_build(pbstream_filename_, pgm_filename_,0.05);

   假设：

   pbstream_filename_ =“/home/cabin/example.pbstream”

   pgm_filename_ = “/home/cabin/example”

   则函数的作用为将example.pbstream转换为分辨率为0.05m的ros标准格式地图后，以example命名后，保存于/home/cabin路径下，最终得到example.pgm，example.yaml。