Unfrozen trajectories (#710)

Unfrozen trajectories

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -105,10 +105,18 @@ MapBuilderBridge::MapBuilderBridge(
       map_builder_(std::move(map_builder)),
       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);
+void MapBuilderBridge::LoadState(const std::string& state_filename,
+                                 bool load_frozen_state) {
+  // Check if suffix of the state file is ".pbstream".
+  const std::string suffix = ".pbstream";
+  CHECK_EQ(state_filename.substr(
+               std::max<int>(state_filename.size() - suffix.size(), 0)),
+           suffix)
+      << "The file containing the state to be loaded must be a "
+         ".pbstream file.";
+  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
+  cartographer::io::ProtoStreamReader stream(state_filename);
+  map_builder_->LoadState(&stream, load_frozen_state);
 }
 
 int MapBuilderBridge::AddTrajectory(

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -63,7 +63,7 @@ class MapBuilderBridge {
   MapBuilderBridge(const MapBuilderBridge&) = delete;
   MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
 
-  void LoadMap(const std::string& map_filename);
+  void LoadState(const std::string& state_filename, bool load_frozen_state);
   int AddTrajectory(
       const std::set<
           ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&

cartographer_ros/cartographer_ros/node.cc

@@ -657,9 +657,10 @@ void Node::SerializeState(const std::string& filename) {
       << "Could not write state.";
 }
 
-void Node::LoadMap(const std::string& map_filename) {
+void Node::LoadState(const std::string& state_filename,
+                     const bool load_frozen_state) {
   carto::common::MutexLocker lock(&mutex_);
-  map_builder_bridge_.LoadMap(map_filename);
+  map_builder_bridge_.LoadState(state_filename, load_frozen_state);
 }
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node.h

@@ -108,8 +108,8 @@ class Node {
   // Serializes the complete Node state.
   void SerializeState(const std::string& filename);
 
-  // Loads a persisted state to use as a map.
-  void LoadMap(const std::string& map_filename);
+  // Loads a serialized SLAM state from a .pbstream file.
+  void LoadState(const std::string& state_filename, bool load_frozen_state);
 
   ::ros::NodeHandle* node_handle();
 

cartographer_ros/cartographer_ros/node_main.cc

@@ -28,12 +28,16 @@ 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.");
+DEFINE_string(load_state_filename, "",
+              "If non-empty, filename of a .pbstream file to load, containing "
+              "a saved SLAM state.");
+DEFINE_bool(load_frozen_state, true,
+            "Load the saved state as frozen (non-optimized) trajectories.");
 DEFINE_bool(
     start_trajectory_with_default_topics, true,
     "Enable to immediately start the first trajectory with default topics.");
 DEFINE_string(
-    save_map_filename, "",
+    save_state_filename, "",
     "If non-empty, serialize state and write it to disk before shutting down.");
 
 namespace cartographer_ros {
@@ -52,8 +56,8 @@ void Run() {
       cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
           node_options.map_builder_options);
   Node node(node_options, std::move(map_builder), &tf_buffer);
-  if (!FLAGS_map_filename.empty()) {
-    node.LoadMap(FLAGS_map_filename);
+  if (!FLAGS_load_state_filename.empty()) {
+    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
   }
 
   if (FLAGS_start_trajectory_with_default_topics) {
@@ -65,8 +69,8 @@ void Run() {
   node.FinishAllTrajectories();
   node.RunFinalOptimization();
 
-  if (!FLAGS_save_map_filename.empty()) {
-    node.SerializeState(FLAGS_save_map_filename);
+  if (!FLAGS_save_state_filename.empty()) {
+    node.SerializeState(FLAGS_save_state_filename);
   }
 }
 

cartographer_ros/cartographer_ros/offline_node.cc

@@ -52,8 +52,11 @@ DEFINE_string(urdf_filenames, "",
               "static links for the sensor configuration(s).");
 DEFINE_bool(use_bag_transforms, true,
             "Whether to read, use and republish transforms from bags.");
-DEFINE_string(pbstream_filename, "",
-              "If non-empty, filename of a pbstream to load.");
+DEFINE_string(load_state_filename, "",
+              "If non-empty, filename of a .pbstream file to load, containing "
+              "a saved SLAM state.");
+DEFINE_bool(load_frozen_state, true,
+            "Load the saved state as frozen (non-optimized) trajectories.");
 DEFINE_bool(keep_running, false,
             "Keep running the offline node after all messages from the bag "
             "have been processed.");
@@ -75,7 +78,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
       << "-configuration_directory is missing.";
   CHECK(!FLAGS_configuration_basenames.empty())
       << "-configuration_basenames is missing.";
-  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
+  CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty()))
+      << "-bag_filenames and -load_state_filename cannot both be unspecified.";
   const auto bag_filenames =
       cartographer_ros::SplitString(FLAGS_bag_filenames, ',');
   cartographer_ros::NodeOptions node_options;
@@ -95,7 +99,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     }
     bag_trajectory_options.push_back(current_trajectory_options);
   }
-  CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
+  if (bag_filenames.size() > 0) {
+    CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
+  }
 
   // Since we preload the transform buffer, we should never have to wait for a
   // transform. When we finish processing the bag, we will simply drop any
@@ -122,10 +128,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
   tf_buffer.setUsingDedicatedThread(true);
 
   Node node(node_options, std::move(map_builder), &tf_buffer);
-  if (!FLAGS_pbstream_filename.empty()) {
-    // TODO(jihoonl): LoadMap should be replaced by some better deserialization
-    // of full SLAM state as non-frozen trajectories once possible
-    node.LoadMap(FLAGS_pbstream_filename);
+  if (!FLAGS_load_state_filename.empty()) {
+    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
   }
 
   ::ros::Publisher tf_publisher =
@@ -308,10 +312,12 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
   LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
 #endif
 
-  if (::ros::ok()) {
-    const std::string output_filename = bag_filenames.front() + ".pbstream";
-    LOG(INFO) << "Writing state to '" << output_filename << "'...";
-    node.SerializeState(output_filename);
+  if (::ros::ok() && bag_filenames.size() > 0) {
+    const std::string output_filename = bag_filenames.front();
+    const std::string suffix = ".pbstream";
+    const std::string state_output_filename = output_filename + suffix;
+    LOG(INFO) << "Writing state to '" << state_output_filename << "'...";
+    node.SerializeState(state_output_filename);
   }
   if (FLAGS_keep_running) {
     ::ros::waitForShutdown();

cartographer_ros/launch/demo_backpack_2d_localization.launch

@@ -27,7 +27,7 @@
       type="cartographer_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
           -configuration_basename backpack_2d_localization.lua
-          -map_filename $(arg map_filename)"
+          -load_state_filename $(arg load_state_filename)"
       output="screen">
     <remap from="echoes" to="horizontal_laser_2d" />
   </node>

docs/source/demos.rst

@@ -30,7 +30,7 @@ Pure localization
     roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
     # Run pure localization:
     roslaunch cartographer_ros demo_backpack_2d_localization.launch \
-       map_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
+       load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
        bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag
 
     # Pure localization demo in 3D: We use 2 different 3D bags from the Deutsche
@@ -42,7 +42,7 @@ Pure localization
     roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag
     # Run pure localization:
     roslaunch cartographer_ros demo_backpack_3d_localization.launch \
-       map_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \
+       load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \
        bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag
 
 Revo LDS

docs/source/ros_api.rst

@@ -38,10 +38,14 @@ TODO(hrapp): Should these not be removed? It seems duplicated efforts documentin
   Basename (i.e. not containing any directory prefix) of the configuration file
   (e.g. backpack_3d.lua).
 
-\-\-map_filename
-  A Cartographer state file that will be loaded from disk. This allows to
-  add new trajectories SLAMing from an earlier state, but the loaded state is
-  frozen.
+\-\-load_state_filename
+  A Cartographer .pbstream state file that will be loaded from disk. This allows
+  to add new trajectories SLAMing from an earlier state.
+
+\-\-load_frozen_state
+  This boolean parameter controls if the saved state, specified using the option
+  \-\-load_state_filename, is going to be loaded as a set of frozen (not
+  optimized) trajectories.
 
 Subscribed Topics
 -----------------