Implement MapBuilderService.WriteStateToFile() (#1424)

cartographer/cloud/client/map_builder_stub.cc

@@ -24,6 +24,7 @@
 #include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h"
 #include "cartographer/cloud/internal/handlers/load_state_handler.h"
 #include "cartographer/cloud/internal/handlers/write_state_handler.h"
+#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h"
 #include "cartographer/cloud/internal/mapping/serialization.h"
 #include "cartographer/cloud/internal/sensor/serialization.h"
 #include "cartographer/cloud/proto/map_builder_service.pb.h"
@@ -148,6 +149,22 @@ void MapBuilderStub::SerializeState(bool include_unfinished_submaps,
   }
 }
 
+void MapBuilderStub::SerializeStateToFile(bool include_unfinished_submaps,
+                                          const std::string& filename) {
+  if (include_unfinished_submaps) {
+    LOG(WARNING) << "Serializing unfinished submaps is currently unsupported. "
+                    "Proceeding to write the state without them.";
+  }
+  proto::WriteStateToFileRequest request;
+  request.set_filename(filename);
+  ::grpc::Status status;
+  async_grpc::Client<handlers::WriteStateToFileSignature> client(
+      client_channel_);
+  CHECK(client.Write(request, &status))
+      << "code: " << status.error_code()
+      << " reason: " << status.error_message();
+}
+
 std::map<int, int> MapBuilderStub::LoadState(
     io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) {
   async_grpc::Client<handlers::LoadStateSignature> client(client_channel_);

cartographer/cloud/client/map_builder_stub.h

@@ -50,6 +50,8 @@ class MapBuilderStub : public mapping::MapBuilderInterface {
       mapping::proto::SubmapQuery::Response* response) override;
   void SerializeState(bool include_unfinished_submaps,
                       io::ProtoStreamWriterInterface* writer) override;
+  void SerializeStateToFile(bool include_unfinished_submaps,
+                            const std::string& filename) override;
   std::map<int, int> LoadState(io::ProtoStreamReaderInterface* reader,
                                bool load_frozen_state) override;
   std::map<int, int> LoadStateFromFile(const std::string& filename,

cartographer/cloud/internal/handlers/write_state_to_file_handler.cc

@@ -0,0 +1,41 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h"
+
+#include "async_grpc/rpc_handler.h"
+#include "cartographer/cloud/internal/map_builder_context_interface.h"
+#include "cartographer/cloud/internal/map_builder_server.h"
+#include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "cartographer/io/proto_stream.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+
+void WriteStateToFileHandler::OnRequest(
+    const proto::WriteStateToFileRequest& request) {
+  if (request.filename().empty()) {
+    Finish(::grpc::Status(::grpc::INVALID_ARGUMENT, "Filename empty."));
+    return;
+  }
+  GetContext<MapBuilderContextInterface>()->map_builder().SerializeStateToFile(
+      /*include_unfinished_submaps=*/false, request.filename());
+}
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer

cartographer/cloud/internal/handlers/write_state_to_file_handler.h

@@ -0,0 +1,43 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H
+#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H
+
+#include "async_grpc/rpc_handler.h"
+#include "cartographer/cloud/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer {
+namespace cloud {
+namespace handlers {
+
+DEFINE_HANDLER_SIGNATURE(
+    WriteStateToFileSignature, proto::WriteStateToFileRequest,
+    google::protobuf::Empty,
+    "/cartographer.cloud.proto.MapBuilderService/WriteStateToFile")
+
+class WriteStateToFileHandler
+    : public async_grpc::RpcHandler<WriteStateToFileSignature> {
+ public:
+  void OnRequest(const proto::WriteStateToFileRequest& request) override;
+};
+
+}  // namespace handlers
+}  // namespace cloud
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_WRITE_STATE_TO_FILE_HANDLER_H

cartographer/cloud/internal/map_builder_server.cc

@@ -41,6 +41,7 @@
 #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h"
 #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h"
 #include "cartographer/cloud/internal/handlers/write_state_handler.h"
+#include "cartographer/cloud/internal/handlers/write_state_to_file_handler.h"
 #include "cartographer/cloud/internal/sensor/serialization.h"
 #include "glog/logging.h"
 
@@ -95,6 +96,7 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::LoadStateFromFileHandler>();
   server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
   server_builder.RegisterHandler<handlers::WriteStateHandler>();
+  server_builder.RegisterHandler<handlers::WriteStateToFileHandler>();
   server_builder.RegisterHandler<handlers::SetLandmarkPoseHandler>();
   grpc_server_ = server_builder.Build();
   if (map_builder_server_options.map_builder_options()

cartographer/cloud/proto/map_builder_service.proto

@@ -228,6 +228,10 @@ message WriteStateResponse {
   }
 }
 
+message WriteStateToFileRequest {
+  string filename = 1;
+}
+
 message IsTrajectoryFinishedRequest {
   int32 trajectory_id = 1;
 }
@@ -339,4 +343,7 @@ service MapBuilderService {
   // Receives serialized SLAM state data in the order defined by
   // ProtoStreamWriter.
   rpc WriteState(google.protobuf.Empty) returns (stream WriteStateResponse);
+
+  // Writes the serialized SLAM state to the host file system.
+  rpc WriteStateToFile(WriteStateToFileRequest) returns (google.protobuf.Empty);
 }

cartographer/mapping/internal/testing/mock_map_builder.h

@@ -48,6 +48,7 @@ class MockMapBuilder : public mapping::MapBuilderInterface {
                std::string(const mapping::SubmapId &,
                            mapping::proto::SubmapQuery::Response *));
   MOCK_METHOD2(SerializeState, void(bool, io::ProtoStreamWriterInterface *));
+  MOCK_METHOD2(SerializeStateToFile, void(bool, const std::string &));
   MOCK_METHOD2(LoadState,
                std::map<int, int>(io::ProtoStreamReaderInterface *, bool));
   MOCK_METHOD2(LoadStateFromFile,

cartographer/mapping/map_builder.cc

@@ -212,6 +212,14 @@ void MapBuilder::SerializeState(bool include_unfinished_submaps,
                     include_unfinished_submaps);
 }
 
+void MapBuilder::SerializeStateToFile(bool include_unfinished_submaps,
+                                      const std::string& filename) {
+  io::ProtoStreamWriter writer(filename);
+  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer,
+                    include_unfinished_submaps);
+  writer.Close();
+}
+
 std::map<int, int> MapBuilder::LoadState(
     io::ProtoStreamReaderInterface* const reader, bool load_frozen_state) {
   io::ProtoStreamDeserializer deserializer(reader);

cartographer/mapping/map_builder.h

@@ -59,6 +59,9 @@ class MapBuilder : public MapBuilderInterface {
   void SerializeState(bool include_unfinished_submaps,
                       io::ProtoStreamWriterInterface *writer) override;
 
+  void SerializeStateToFile(bool include_unfinished_submaps,
+                            const std::string &filename) override;
+
   std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
                                bool load_frozen_state) override;
 

cartographer/mapping/map_builder_interface.h

@@ -84,6 +84,13 @@ class MapBuilderInterface {
   virtual void SerializeState(bool include_unfinished_submaps,
                               io::ProtoStreamWriterInterface* writer) = 0;
 
+  // Serializes the current state to a proto stream file on the host system. If
+  // 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
+  // submaps that have not yet received all rangefinder data insertions, will
+  // be included in the serialized state.
+  virtual void SerializeStateToFile(bool include_unfinished_submaps,
+                                    const std::string& filename) = 0;
+
   // Loads the SLAM state from a proto stream. Returns the remapping of new
   // trajectory_ids.
   virtual std::map<int /* trajectory id in proto */, int /* trajectory id */>