Add pbstream_map_publisher_main.cc (#711)

Implements [RFC 06](https://github.com/googlecartographer/rfcs/blob/master/text/0006-serve-ros-map-from-pbstream.md)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -91,6 +91,17 @@ install(TARGETS cartographer_pbstream_to_ros_map
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+google_binary(cartographer_pbstream_map_publisher
+  SRCS
+    pbstream_map_publisher_main.cc
+)
+
+install(TARGETS cartographer_pbstream_map_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 # TODO(cschuet): Add support for shared library case.
 if (${BUILD_GRPC})
   google_binary(cartographer_grpc_node

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -21,6 +21,7 @@
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/io/submap_painter.h"
 #include "cartographer/transform/proto/transform.pb.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer_ros/time_conversion.h"
@@ -30,6 +31,7 @@
 #include "geometry_msgs/TransformStamped.h"
 #include "geometry_msgs/Vector3.h"
 #include "glog/logging.h"
+#include "nav_msgs/OccupancyGrid.h"
 #include "ros/ros.h"
 #include "ros/serialization.h"
 #include "sensor_msgs/Imu.h"
@@ -284,4 +286,53 @@ cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(
   return cartographer::transform::Rigid3d(rotation * -translation, rotation);
 }
 
+std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
+    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
+    const double resolution, const std::string& frame_id,
+    const ros::Time& time) {
+  auto occupancy_grid =
+      ::cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
+
+  const int width = cairo_image_surface_get_width(painted_slices.surface.get());
+  const int height =
+      cairo_image_surface_get_height(painted_slices.surface.get());
+  const ros::Time now = ros::Time::now();
+
+  occupancy_grid->header.stamp = time;
+  occupancy_grid->header.frame_id = frame_id;
+  occupancy_grid->info.map_load_time = time;
+  occupancy_grid->info.resolution = resolution;
+  occupancy_grid->info.width = width;
+  occupancy_grid->info.height = height;
+  occupancy_grid->info.origin.position.x =
+      -painted_slices.origin.x() * resolution;
+  occupancy_grid->info.origin.position.y =
+      (-height + painted_slices.origin.y()) * resolution;
+  occupancy_grid->info.origin.position.z = 0.;
+  occupancy_grid->info.origin.orientation.w = 1.;
+  occupancy_grid->info.origin.orientation.x = 0.;
+  occupancy_grid->info.origin.orientation.y = 0.;
+  occupancy_grid->info.origin.orientation.z = 0.;
+
+  const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
+      cairo_image_surface_get_data(painted_slices.surface.get()));
+  occupancy_grid->data.reserve(width * height);
+  for (int y = height - 1; y >= 0; --y) {
+    for (int x = 0; x < width; ++x) {
+      const uint32_t packed = pixel_data[y * width + x];
+      const unsigned char color = packed >> 16;
+      const unsigned char observed = packed >> 8;
+      const int value =
+          observed == 0
+              ? -1
+              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
+      CHECK_LE(-1, value);
+      CHECK_GE(100, value);
+      occupancy_grid->data.push_back(value);
+    }
+  }
+
+  return occupancy_grid;
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -19,11 +19,13 @@
 
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/io/submap_painter.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "geometry_msgs/Pose.h"
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
+#include "nav_msgs/OccupancyGrid.h"
 #include "pcl/point_cloud.h"
 #include "pcl/point_types.h"
 #include "pcl_conversions/pcl_conversions.h"
@@ -79,6 +81,13 @@ Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
 cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude,
                                                               double longitude);
 
+// Points to an occupancy grid message at a specific resolution from painted
+// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...).
+std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
+    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
+    const double resolution, const std::string& frame_id,
+    const ros::Time& time);
+
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_MSG_CONVERSION_H_

cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc

@@ -0,0 +1,115 @@
+/*
+ * Copyright 2018 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.
+ */
+
+// Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps.
+
+#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/msg_conversion.h"
+#include "cartographer_ros/node_constants.h"
+#include "cartographer_ros/ros_log_sink.h"
+#include "cartographer_ros/ros_map.h"
+#include "cartographer_ros/submap.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "ros/ros.h"
+
+DEFINE_string(pbstream_filename, "",
+              "Filename of a pbstream to draw a map from.");
+DEFINE_string(map_topic, "map", "Name of the published map topic.");
+DEFINE_string(map_frame_id, "map", "Frame ID of the published map.");
+DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
+
+namespace cartographer_ros {
+namespace {
+
+void Run(const std::string& pbstream_filename, const std::string& map_topic,
+         const std::string& map_frame_id, 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());
+
+  ::ros::NodeHandle node_handle("");
+  ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(
+      map_topic, kLatestOnlyPublisherQueueSize, true /*latched */);
+
+  LOG(INFO) << "Generating combined map image from submap slices.";
+  const auto painted_slices =
+      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
+  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
+      painted_slices, resolution, FLAGS_map_frame_id, ros::Time::now());
+
+  LOG(INFO) << "Publishing occupancy grid topic " << map_topic
+            << " (frame_id: " << map_frame_id
+            << ", resolution:" << std::to_string(resolution) << ").";
+  pub.publish(*msg_ptr);
+  ::ros::spin();
+  ::ros::shutdown();
+}
+
+}  // namespace
+}  // namespace cartographer_ros
+
+int main(int argc, char** argv) {
+  FLAGS_alsologtostderr = true;
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
+
+  ::ros::init(argc, argv, "cartographer_pbstream_map_publisher");
+  ::ros::start();
+
+  cartographer_ros::ScopedRosLogSink ros_log_sink;
+
+  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic,
+                          FLAGS_map_frame_id, FLAGS_resolution);
+}