Add include_unfinished_submaps parameter to SerializeState() (#966)

- default to false in gRPC node (unsupported in `MapBuilderStub`)
- default to true in classic ROS nodes (as it was before)
- add as parameter to `write_state`

cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc

@@ -78,7 +78,8 @@ void Run() {
   node.RunFinalOptimization();
 
   if (!FLAGS_save_map_filename.empty()) {
-    node.SerializeState(FLAGS_save_map_filename);
+    node.SerializeState(FLAGS_save_map_filename,
+                        false /* include_unfinished_submaps */);
   }
 }
 

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -160,9 +160,10 @@ void MapBuilderBridge::RunFinalOptimization() {
   map_builder_->pose_graph()->RunFinalOptimization();
 }
 
-bool MapBuilderBridge::SerializeState(const std::string& filename) {
+bool MapBuilderBridge::SerializeState(const std::string& filename,
+                                      const bool include_unfinished_submaps) {
   cartographer::io::ProtoStreamWriter writer(filename);
-  map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
+  map_builder_->SerializeState(include_unfinished_submaps, &writer);
   return writer.Close();
 }
 

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -72,7 +72,8 @@ class MapBuilderBridge {
       const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
   void RunFinalOptimization();
-  bool SerializeState(const std::string& filename);
+  bool SerializeState(const std::string& filename,
+                      const bool include_unfinished_submaps);
 
   void HandleSubmapQuery(
       cartographer_ros_msgs::SubmapQuery::Request& request,

cartographer_ros/cartographer_ros/node.cc

@@ -639,7 +639,8 @@ bool Node::HandleWriteState(
     ::cartographer_ros_msgs::WriteState::Request& request,
     ::cartographer_ros_msgs::WriteState::Response& response) {
   carto::common::MutexLocker lock(&mutex_);
-  if (map_builder_bridge_.SerializeState(request.filename)) {
+  if (map_builder_bridge_.SerializeState(request.filename,
+                                         request.include_unfinished_submaps)) {
     response.status.code = cartographer_ros_msgs::StatusCode::OK;
     response.status.message = "State written to '" + request.filename + "'.";
   } else {
@@ -787,9 +788,11 @@ void Node::HandlePointCloud2Message(
       ->HandlePointCloud2Message(sensor_id, msg);
 }
 
-void Node::SerializeState(const std::string& filename) {
+void Node::SerializeState(const std::string& filename,
+                          const bool include_unfinished_submaps) {
   carto::common::MutexLocker lock(&mutex_);
-  CHECK(map_builder_bridge_.SerializeState(filename))
+  CHECK(
+      map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
       << "Could not write state.";
 }
 

cartographer_ros/cartographer_ros/node.h

@@ -112,7 +112,8 @@ class Node {
                                 const sensor_msgs::PointCloud2::ConstPtr& msg);
 
   // Serializes the complete Node state.
-  void SerializeState(const std::string& filename);
+  void SerializeState(const std::string& filename,
+                      const bool include_unfinished_submaps);
 
   // Loads a serialized SLAM state from a .pbstream file.
   void LoadState(const std::string& state_filename, bool load_frozen_state);

cartographer_ros/cartographer_ros/node_main.cc

@@ -74,7 +74,8 @@ void Run() {
   node.RunFinalOptimization();
 
   if (!FLAGS_save_state_filename.empty()) {
-    node.SerializeState(FLAGS_save_state_filename);
+    node.SerializeState(FLAGS_save_state_filename,
+                        true /* include_unfinished_submaps */);
   }
 }
 

cartographer_ros/cartographer_ros/offline_node.cc

@@ -372,7 +372,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     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);
+    node.SerializeState(state_output_filename,
+                        true /* include_unfinished_submaps */);
   }
   if (FLAGS_keep_running) {
     ::ros::waitForShutdown();

cartographer_ros_msgs/srv/WriteState.srv

@@ -13,5 +13,6 @@
 # limitations under the License.
 
 string filename
+bool include_unfinished_submaps
 ---
 cartographer_ros_msgs/StatusResponse status