Implement WriteMapHandler. (#851)

cartographer_grpc/handlers/write_map_handler.h

@@ -0,0 +1,67 @@
+/*
+ * 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_GRPC_HANDLERS_WRITE_MAP_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/io/in_memory_proto_stream.h"
+#include "cartographer_grpc/framework/rpc_handler.h"
+#include "cartographer_grpc/map_builder_context_interface.h"
+#include "cartographer_grpc/map_builder_server.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class WriteMapHandler
+    : public framework::RpcHandler<google::protobuf::Empty,
+                                   framework::Stream<proto::WriteMapResponse>> {
+ public:
+  std::string method_name() const override {
+    return "/cartographer_grpc.proto.MapBuilderService/WriteMap";
+  }
+  void OnRequest(const google::protobuf::Empty& request) override {
+    auto writer = GetWriter();
+    cartographer::io::ForwardingProtoStreamWriter proto_stream_writer(
+        [writer](const google::protobuf::Message* proto) {
+          if (!proto) {
+            writer.WritesDone();
+            return true;
+          }
+
+          auto response =
+              cartographer::common::make_unique<proto::WriteMapResponse>();
+          if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") {
+            response->mutable_pose_graph()->CopyFrom(*proto);
+          } else if (proto->GetTypeName() ==
+                     "cartographer.mapping.proto.SerializedData") {
+            response->mutable_serialized_data()->CopyFrom(*proto);
+          } else {
+            LOG(FATAL) << "Unsupported message type: " << proto->GetTypeName();
+          }
+          writer.Write(std::move(response));
+          return true;
+        });
+    GetContext<MapBuilderContextInterface>()->map_builder().SerializeState(
+        &proto_stream_writer);
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -32,6 +32,7 @@
 #include "cartographer_grpc/handlers/load_map_handler.h"
 #include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
 #include "cartographer_grpc/handlers/run_final_optimization_handler.h"
+#include "cartographer_grpc/handlers/write_map_handler.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "cartographer_grpc/sensor/serialization.h"
 #include "glog/logging.h"
@@ -75,6 +76,7 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
   server_builder.RegisterHandler<handlers::LoadMapHandler>();
   server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
+  server_builder.RegisterHandler<handlers::WriteMapHandler>();
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/mapping/map_builder_stub.cc

@@ -90,7 +90,14 @@ std::string MapBuilderStub::SubmapToProto(
 
 void MapBuilderStub::SerializeState(
     cartographer::io::ProtoStreamWriterInterface* writer) {
-  LOG(FATAL) << "Not implemented";
+  grpc::ClientContext client_context;
+  google::protobuf::Empty request;
+  auto reader = service_stub_->WriteMap(&client_context, request);
+  proto::WriteMapResponse response;
+  while (reader->Read(&response)) {
+    writer->WriteProto(response);
+  }
+  CHECK(writer->Close());
 }
 
 void MapBuilderStub::LoadMap(

cartographer_grpc/proto/map_builder_service.proto

@@ -151,6 +151,13 @@ message AddLocalSlamResultDataRequest {
   cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
 }
 
+message WriteMapResponse {
+  oneof map_chunk {
+    cartographer.mapping.proto.PoseGraph pose_graph = 1;
+    cartographer.mapping.proto.SerializedData serialized_data = 2;
+  }
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -212,4 +219,7 @@ service MapBuilderService {
 
   // Adds map data in the order defined by ProtoStreamReader.
   rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty);
+
+  // Receives map data in the order defined by ProtoStreamWriter.
+  rpc WriteMap(google.protobuf.Empty) returns (stream WriteMapResponse);
 }