cartographer上实现自动保存建好的pgm地图

实现cartographer自动保存建好的地图 针对github tag 0.3.0版本

 

1.ros下建好的地图一般是使用命令$ rosrunmap_server map_saver -f map_name

一般使用gmapping hector建立的地图使用上述命令保存的地图是没问题的

 

2.但是在cartographer中使用上述命令也能保存地图但是 保存的地图有点问题,如下图

cartographer上实现自动保存建好的pgm地图_第1张图片

知乎上推荐   rosservice call /finish_trajectory "map" 默认会保存在.ros文件夹中 方法 会报错

改成 rosservice call /finish_trajectory 0 也会报错

 

3.于是要分析修改代码,在node_main.c文件添加2个自定义函

添加函数 1 FillSubmapSlice()

添加函数 2 save_to_tos_map(string map_pgm)

//map_pgm 要保存的地图路径跟名字,如"/home/aaa/map/2B_map"  地图存放的目录下

在run() 最后面对save_to_tos_map()调用即可自动保存建好的地图

整个node_main.c 文件 代码

/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"

#include 
#include 

#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 "glog/logging.h"



DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");
DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");
DEFINE_string(map_filename, "", "If non-empty, filename of a map to load.");
DEFINE_bool(
    start_trajectory_with_default_topics, true,
    "Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
    save_map_filename, "",
    "If non-empty, serialize state and write it to disk before shutting down.");

namespace cartographer_ros {
namespace {

// 注意一定要事先  建立home/tank/map/目录 不然会找不到目录
std::string map_path_name = "/home/tank/map/sick_map_data.pbstream";

void FillSubmapSlice(
    const ::cartographer::transform::Rigid3d& global_submap_pose,
    const ::cartographer::mapping::proto::Submap& proto,
    ::cartographer::io::SubmapSlice* const submap_slice) {
  ::cartographer::mapping::proto::SubmapQuery::Response response;
  ::cartographer::transform::Rigid3d local_pose;
  if (proto.has_submap_3d()) {
    ::cartographer::mapping_3d::Submap submap(proto.submap_3d());
    local_pose = submap.local_pose();
    submap.ToResponseProto(global_submap_pose, &response);
  } else {
    ::cartographer::mapping_2d::Submap submap(proto.submap_2d());
    local_pose = submap.local_pose();
    submap.ToResponseProto(global_submap_pose, &response);
  }
  submap_slice->pose = global_submap_pose;

  auto& texture_proto = response.textures(0);
  const SubmapTexture::Pixels pixels = UnpackTextureData(
      texture_proto.cells(), texture_proto.width(), texture_proto.height());
  submap_slice->width = texture_proto.width();
  submap_slice->height = texture_proto.height();
  submap_slice->resolution = texture_proto.resolution();
  submap_slice->slice_pose =
      ::cartographer::transform::ToRigid3(texture_proto.slice_pose());
  submap_slice->surface =
      DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(),
                  texture_proto.height(), &submap_slice->cairo_data);
}

void save_to_ros_map(const std::string& map_pgm)
{
   double resolution = 0.05;
  ::cartographer::io::ProtoStreamReader reader(map_path_name);

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

  LOG(INFO) << "Loading submap slices from serialized data. pbstream_filename"<
      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_pgm + ".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_pgm + ".yaml");
  WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
}



void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 1e6;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  Node node(node_options, &tf_buffer);
  if (!FLAGS_map_filename.empty()) {
  	LOG(ERROR)<<"-----------------LoadMap() FLAGS_map_filename:"<

4.终端启动 demo_sick_map.launch 文件 开始建图 建图图之后 终端crtl + C 结束建图    



   

  
  
  
  

  
   
  

  

   



cartographer上实现自动保存建好的pgm地图_第2张图片

下图   三面是透明玻璃 一面白墙 空旷地方建立的地图  

cartographer上实现自动保存建好的pgm地图_第3张图片


你可能感兴趣的:(SLAM,ROS机器人操作系统)