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。

评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值