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

@@ -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;

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);

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,

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

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();
 

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();