From 12318555a521c2e4a17ff81924aefb94cd5552b9 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Wed, 5 Jul 2017 16:47:50 +0200
Subject: [PATCH] Add support for loading a map. (#409)

For now we implement map loading via a commandline flag.

PAIR=SirVer
---
 cartographer_ros/cartographer_ros/map_builder_bridge.cc | 8 +++++++-
 cartographer_ros/cartographer_ros/map_builder_bridge.h  | 2 ++
 cartographer_ros/cartographer_ros/msg_conversion.cc     | 2 +-
 cartographer_ros/cartographer_ros/node.cc               | 5 +++++
 cartographer_ros/cartographer_ros/node.h                | 3 +++
 cartographer_ros/cartographer_ros/node_main.cc          | 4 ++++
 6 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index 6debdf5..6d5f730 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -33,6 +33,12 @@ MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
       map_builder_(node_options.map_builder_options),
       tf_buffer_(tf_buffer) {}
 
+void MapBuilderBridge::LoadMap(const std::string& map_filename) {
+  LOG(INFO) << "Loading map '" << map_filename << "'...";
+  cartographer::io::ProtoStreamReader stream(map_filename);
+  map_builder_.LoadMap(&stream);
+}
+
 int MapBuilderBridge::AddTrajectory(
     const std::unordered_set<string>& expected_sensor_ids,
     const TrajectoryOptions& trajectory_options) {
@@ -153,7 +159,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
 std::unique_ptr<nav_msgs::OccupancyGrid>
 MapBuilderBridge::BuildOccupancyGrid() {
   CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
-      << "Publishing OccupancyGrids for 3D data is not yet supported";
+      << "Publishing OccupancyGrids for 3D data is not yet supported.";
   const auto all_trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h
index 46fae8d..f99f890 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.h
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h
@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
 
 #include <memory>
+#include <string>
 #include <unordered_map>
 #include <unordered_set>
 
@@ -49,6 +50,7 @@ class MapBuilderBridge {
   MapBuilderBridge(const MapBuilderBridge&) = delete;
   MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
 
+  void LoadMap(const std::string& map_filename);
   int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
                     const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc
index 296adb9..d540d8b 100644
--- a/cartographer_ros/cartographer_ros/msg_conversion.cc
+++ b/cartographer_ros/cartographer_ros/msg_conversion.cc
@@ -43,8 +43,8 @@ namespace {
 // properly.
 constexpr float kPointCloudComponentFourMagic = 1.;
 
-using ::cartographer::transform::Rigid3d;
 using ::cartographer::sensor::PointCloudWithIntensities;
+using ::cartographer::transform::Rigid3d;
 
 sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
                                                    const string& frame_id,
diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index 29a8923..681a18d 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -494,4 +494,9 @@ void Node::FinishAllTrajectories() {
     }
   }
 }
+
+void Node::LoadMap(const std::string& map_filename) {
+  map_builder_bridge_.LoadMap(map_filename);
+}
+
 }  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h
index ce4ed55..31b6570 100644
--- a/cartographer_ros/cartographer_ros/node.h
+++ b/cartographer_ros/cartographer_ros/node.h
@@ -53,6 +53,9 @@ class Node {
   // Starts the first trajectory with the default topics.
   void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
 
+  // Loads a persisted state to use as a map.
+  void LoadMap(const std::string& map_filename);
+
   ::ros::NodeHandle* node_handle();
   MapBuilderBridge* map_builder_bridge();
 
diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc
index 5b8f6f0..5821cc1 100644
--- a/cartographer_ros/cartographer_ros/node_main.cc
+++ b/cartographer_ros/cartographer_ros/node_main.cc
@@ -34,6 +34,7 @@ DEFINE_string(configuration_directory, "",
 DEFINE_string(configuration_basename, "",
               "Basename, i.e. not containing any directory prefix, of the "
               "configuration file.");
+DEFINE_string(map_filename, "", "If non-empty, filename of a map to load.");
 
 namespace cartographer_ros {
 namespace {
@@ -60,6 +61,9 @@ void Run() {
   std::tie(node_options, trajectory_options) = LoadOptions();
 
   Node node(node_options, &tf_buffer);
+  if (!FLAGS_map_filename.empty()) {
+    node.LoadMap(FLAGS_map_filename);
+  }
   node.StartTrajectoryWithDefaultTopics(trajectory_options);
 
   ::ros::spin();