From 683129243ee26b5c1b278430d39e462a95b32709 Mon Sep 17 00:00:00 2001
From: Kevin Daun <kdaun@google.com>
Date: Fri, 13 Jul 2018 00:52:49 +0200
Subject: [PATCH] Introduce value converter tables. (#937)

---
 .../cartographer_ros/pbstream_map_publisher_main.cc  | 12 ++++++++----
 .../cartographer_ros/pbstream_to_ros_map_main.cc     |  4 +++-
 .../ros_map_writing_points_processor.cc              |  4 ++--
 .../ros_map_writing_points_processor.h               |  2 ++
 4 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
index 640ce76..34b3f20 100644
--- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
+++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
@@ -29,6 +29,7 @@
 #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"
@@ -49,7 +50,8 @@ namespace cartographer_ros {
 namespace {
 
 std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
-    const std::string& pbstream_filename, const double resolution) {
+    const std::string& pbstream_filename, const double resolution,
+    ::cartographer::mapping::ValueConversionTables* conversion_lookup_tables) {
   ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
   ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
 
@@ -68,7 +70,8 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
                                                   .trajectory(id.trajectory_id)
                                                   .submap(id.submap_index)
                                                   .pose());
-      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
+      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
+                      conversion_lookup_tables);
     }
   }
   CHECK(reader.eof());
@@ -82,8 +85,9 @@ std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
 
 void Run(const std::string& pbstream_filename, const std::string& map_topic,
          const std::string& map_frame_id, const double resolution) {
-  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr =
-      LoadOccupancyGridMsg(pbstream_filename, resolution);
+  ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
+  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = LoadOccupancyGridMsg(
+      pbstream_filename, resolution, &conversion_lookup_tables);
 
   ::ros::NodeHandle node_handle("");
   ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(
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 b75fcc0..d3a29e4 100644
--- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
+++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
@@ -51,6 +51,7 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
   std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
       submap_slices;
   ::cartographer::mapping::proto::SerializedData proto;
+  ::cartographer::mapping::ValueConversionTables conversion_lookup_tables;
   while (deserializer.ReadNextSerializedData(&proto)) {
     if (proto.has_submap()) {
       const auto& submap = proto.submap();
@@ -62,7 +63,8 @@ void Run(const std::string& pbstream_filename, const std::string& map_filestem,
               pose_graph.trajectory(id.trajectory_id)
                   .submap(id.submap_index)
                   .pose());
-      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
+      FillSubmapSlice(global_submap_pose, submap, &submap_slices[id],
+                      &conversion_lookup_tables);
     }
   }
   CHECK(reader.eof());
diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
index c25cb3b..be16ca2 100644
--- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
+++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
@@ -34,8 +34,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
       next_(next),
       file_writer_factory_(file_writer_factory),
       range_data_inserter_(range_data_inserter_options),
-      probability_grid_(::cartographer::io::CreateProbabilityGrid(resolution)) {
-}
+      probability_grid_(::cartographer::io::CreateProbabilityGrid(
+          resolution, &conversion_tables_)) {}
 
 std::unique_ptr<RosMapWritingPointsProcessor>
 RosMapWritingPointsProcessor::FromDictionary(
diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
index 04dca75..71c165b 100644
--- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
+++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
@@ -23,6 +23,7 @@
 #include "cartographer/mapping/2d/probability_grid.h"
 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
 #include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h"
+#include "cartographer/mapping/value_conversion_tables.h"
 
 namespace cartographer_ros {
 
@@ -59,6 +60,7 @@ class RosMapWritingPointsProcessor
   ::cartographer::io::FileWriterFactory file_writer_factory_;
   ::cartographer::mapping::ProbabilityGridRangeDataInserter2D
       range_data_inserter_;
+  ::cartographer::mapping::ValueConversionTables conversion_tables_;
   ::cartographer::mapping::ProbabilityGrid probability_grid_;
 };