cartographer_ros: pbstream_map_publisher

Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 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 // Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps.
00018 
00019 #include <map>
00020 #include <string>
00021 
00022 #include "cartographer/io/proto_stream.h"
00023 #include "cartographer/io/proto_stream_deserializer.h"
00024 #include "cartographer/io/submap_painter.h"
00025 #include "cartographer/mapping/2d/probability_grid.h"
00026 #include "cartographer/mapping/2d/submap_2d.h"
00027 #include "cartographer/mapping/3d/submap_3d.h"
00028 #include "cartographer_ros/msg_conversion.h"
00029 #include "cartographer_ros/node_constants.h"
00030 #include "cartographer_ros/ros_log_sink.h"
00031 #include "cartographer_ros/ros_map.h"
00032 #include "gflags/gflags.h"
00033 #include "glog/logging.h"
00034 #include "nav_msgs/OccupancyGrid.h"
00035 #include "ros/ros.h"
00036 
00037 DEFINE_string(pbstream_filename, "",
00038               "Filename of a pbstream to draw a map from.");
00039 DEFINE_string(map_topic, "map", "Name of the published map topic.");
00040 DEFINE_string(map_frame_id, "map", "Frame ID of the published map.");
00041 DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
00042 
00043 namespace cartographer_ros {
00044 namespace {
00045 
00046 std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
00047     const std::string& pbstream_filename, const double resolution) {
00048   ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
00049   ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
00050 
00051   LOG(INFO) << "Loading submap slices from serialized data.";
00052   std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
00053       submap_slices;
00054   ::cartographer::mapping::ValueConversionTables conversion_tables;
00055   ::cartographer::io::DeserializeAndFillSubmapSlices(
00056       &deserializer, &submap_slices, &conversion_tables);
00057   CHECK(reader.eof());
00058 
00059   LOG(INFO) << "Generating combined map image from submap slices.";
00060   const auto painted_slices =
00061       ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
00062   return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id,
00063                                 ros::Time::now());
00064 }
00065 
00066 void Run(const std::string& pbstream_filename, const std::string& map_topic,
00067          const std::string& map_frame_id, const double resolution) {
00068   std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr =
00069       LoadOccupancyGridMsg(pbstream_filename, resolution);
00070 
00071   ::ros::NodeHandle node_handle("");
00072   ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(
00073       map_topic, kLatestOnlyPublisherQueueSize, true /*latched */);
00074 
00075   LOG(INFO) << "Publishing occupancy grid topic " << map_topic
00076             << " (frame_id: " << map_frame_id
00077             << ", resolution:" << std::to_string(resolution) << ").";
00078   pub.publish(*msg_ptr);
00079   ::ros::spin();
00080   ::ros::shutdown();
00081 }
00082 
00083 }  // namespace
00084 }  // namespace cartographer_ros
00085 
00086 int main(int argc, char** argv) {
00087   google::InitGoogleLogging(argv[0]);
00088   google::ParseCommandLineFlags(&argc, &argv, true);
00089 
00090   CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
00091 
00092   ::ros::init(argc, argv, "cartographer_pbstream_map_publisher");
00093   ::ros::start();
00094 
00095   cartographer_ros::ScopedRosLogSink ros_log_sink;
00096 
00097   ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic,
00098                           FLAGS_map_frame_id, FLAGS_resolution);
00099 }