From 4b39ee68c7a4d518bf8d01a509331e2bc1f514a0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Thu, 19 Jul 2018 19:51:47 +0200
Subject: [PATCH] Fix pbstream exporting binaries (#945)

googlecartographer/cartographer#1286 modified Submap::ToProto such that grids for unfinished submaps are no longer serialized. This commit fixes the breakage this introduced in the pbstream exporting binaries.
---
 .../cartographer_ros/pbstream_map_publisher_main.cc    |  3 ++-
 .../cartographer_ros/pbstream_to_ros_map_main.cc       |  3 ++-
 cartographer_ros/cartographer_ros/submap.cc            | 10 ++++++++++
 cartographer_ros/cartographer_ros/submap.h             |  3 +++
 4 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
index 3da3ca7..796d24c 100644
--- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
+++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
@@ -60,7 +60,8 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
   ::cartographer::mapping::proto::SerializedData proto;
   ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
   while (deserializer.ReadNextSerializedData(&proto)) {
-    if (proto.has_submap()) {
+    if (proto.has_submap() &&
+        (Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
       const auto& submap = proto.submap();
       const ::cartographer::mapping::SubmapId id{
           submap.submap_id().trajectory_id(),
diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
index d3a29e4..fecdbfb 100644
--- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
+++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
@@ -53,7 +53,8 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
   ::cartographer::mapping::proto::SerializedData proto;
   ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
   while (deserializer.ReadNextSerializedData(&proto)) {
-    if (proto.has_submap()) {
+    if (proto.has_submap() &&
+        (Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
       const auto& submap = proto.submap();
       const ::cartographer::mapping::SubmapId id{
           submap.submap_id().trajectory_id(),
diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc
index 4a679f1..1c8ef74 100644
--- a/cartographer_ros/cartographer_ros/submap.cc
+++ b/cartographer_ros/cartographer_ros/submap.cc
@@ -53,4 +53,14 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
   return response;
 }
 
+bool Has2DGrid(const ::cartographer::mapping::proto::Submap& submap) {
+  return submap.has_submap_2d() && submap.submap_2d().has_grid();
+}
+
+bool Has3DGrids(const ::cartographer::mapping::proto::Submap& submap) {
+  return submap.has_submap_3d() &&
+         submap.submap_3d().has_low_resolution_hybrid_grid() &&
+         submap.submap_3d().has_high_resolution_hybrid_grid();
+}
+
 }  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h
index 0df2215..38d19f0 100644
--- a/cartographer_ros/cartographer_ros/submap.h
+++ b/cartographer_ros/cartographer_ros/submap.h
@@ -35,6 +35,9 @@ std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client);
 
+bool Has2DGrid(const ::cartographer::mapping::proto::Submap& submap);
+bool Has3DGrids(const ::cartographer::mapping::proto::Submap& submap);
+
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H