Follow googlecartographer/cartographer#1353 (#959)

FIXES=#944

cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc

@@ -25,16 +25,10 @@
 #include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/3d/submap_3d.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/proto/trajectory_builder_options.pb.h"
-#include "cartographer/mapping/value_conversion_tables.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"
@@ -57,24 +51,9 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
   LOG(INFO) << "Loading submap slices from serialized data.";
   std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
       submap_slices;
-  ::cartographer::mapping::proto::SerializedData proto;
+  ::cartographer::mapping::ValueConversionTables conversion_tables;
-  ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
+  ::cartographer::io::DeserializeAndFillSubmapSlices(
-  while (deserializer.ReadNextSerializedData(&proto)) {
+      &deserializer, &submap_slices, &conversion_tables);
-    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(),
-          submap.submap_id().submap_index()};
-      const ::cartographer::transform::Rigid3d global_submap_pose =
-          ::cartographer::transform::ToRigid3(deserializer.pose_graph()
-                                                  .trajectory(id.trajectory_id)
-                                                  .submap(id.submap_index)
-                                                  .pose());
-      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
-                      &conversion_lookup_tables);
-    }
-  }
   CHECK(reader.eof());
 
   LOG(INFO) << "Generating combined map image from submap slices.";

cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc

@@ -23,12 +23,7 @@
 #include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/3d/submap_3d.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/proto/trajectory_builder_options.pb.h"
 #include "cartographer_ros/ros_map.h"
-#include "cartographer_ros/submap.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 
@@ -45,29 +40,12 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
   ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
   ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
 
-  const auto& pose_graph = deserializer.pose_graph();
-
   LOG(INFO) << "Loading submap slices from serialized data.";
   std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
       submap_slices;
-  ::cartographer::mapping::proto::SerializedData proto;
+  ::cartographer::mapping::ValueConversionTables conversion_tables;
-  ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
+  ::cartographer::io::DeserializeAndFillSubmapSlices(
-  while (deserializer.ReadNextSerializedData(&proto)) {
+      &deserializer, &submap_slices, &conversion_tables);
-    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(),
-          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],
-                      &conversion_lookup_tables);
-    }
-  }
   CHECK(reader.eof());
 
   LOG(INFO) << "Generating combined map image from submap slices.";

cartographer_ros/cartographer_ros/submap.cc

@@ -52,14 +52,4 @@ 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

cartographer_ros/cartographer_ros/submap.h

@@ -35,9 +35,6 @@ 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