cartographer_ros: pbstream_to_ros_map

Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include <map>
00018 #include <string>
00019 
00020 #include "cartographer/io/proto_stream.h"
00021 #include "cartographer/io/proto_stream_deserializer.h"
00022 #include "cartographer/io/submap_painter.h"
00023 #include "cartographer/mapping/2d/probability_grid.h"
00024 #include "cartographer/mapping/2d/submap_2d.h"
00025 #include "cartographer/mapping/3d/submap_3d.h"
00026 #include "cartographer_ros/ros_map.h"
00027 #include "gflags/gflags.h"
00028 #include "glog/logging.h"
00029 
00030 DEFINE_string(pbstream_filename, "",
00031               "Filename of a pbstream to draw a map from.");
00032 DEFINE_string(map_filestem, "map", "Stem of the output files.");
00033 DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
00034 
00035 namespace cartographer_ros {
00036 namespace {
00037 
00038 void Run(const std::string& pbstream_filename, const std::string& map_filestem,
00039          const double resolution) {
00040   ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
00041   ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
00042 
00043   LOG(INFO) << "Loading submap slices from serialized data.";
00044   std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
00045       submap_slices;
00046   ::cartographer::mapping::ValueConversionTables conversion_tables;
00047   ::cartographer::io::DeserializeAndFillSubmapSlices(
00048       &deserializer, &submap_slices, &conversion_tables);
00049   CHECK(reader.eof());
00050 
00051   LOG(INFO) << "Generating combined map image from submap slices.";
00052   auto result =
00053       ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
00054 
00055   ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
00056 
00057   ::cartographer::io::Image image(std::move(result.surface));
00058   WritePgm(image, resolution, &pgm_writer);
00059 
00060   const Eigen::Vector2d origin(
00061       -result.origin.x() * resolution,
00062       (result.origin.y() - image.height()) * resolution);
00063 
00064   ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
00065   WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
00066 }
00067 
00068 }  // namespace
00069 }  // namespace cartographer_ros
00070 
00071 int main(int argc, char** argv) {
00072   FLAGS_alsologtostderr = true;
00073   google::InitGoogleLogging(argv[0]);
00074   google::ParseCommandLineFlags(&argc, &argv, true);
00075 
00076   CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
00077   CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing.";
00078 
00079   ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem,
00080                           FLAGS_resolution);
00081 }