From 37ddf9e55063fcb29a1a069a11615b32468641a1 Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Thu, 25 Jan 2018 09:27:42 +0100
Subject: [PATCH]  Add gRPC version of LoadMap(). (#842)

Later they will be used in map_builder_stub and load_map_handler.
---
 cartographer_grpc/handlers/load_map_handler.h | 60 +++++++++++++++++++
 cartographer_grpc/map_builder_server.cc       |  4 ++
 cartographer_grpc/mapping/map_builder_stub.cc | 21 ++++++-
 .../proto/map_builder_service.proto           | 10 ++++
 4 files changed, 94 insertions(+), 1 deletion(-)
 create mode 100644 cartographer_grpc/handlers/load_map_handler.h

diff --git a/cartographer_grpc/handlers/load_map_handler.h b/cartographer_grpc/handlers/load_map_handler.h
new file mode 100644
index 0000000..36c6b47
--- /dev/null
+++ b/cartographer_grpc/handlers/load_map_handler.h
@@ -0,0 +1,60 @@
+/*
+ * 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_LOAD_MAP_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_LOAD_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_server.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class LoadMapHandler
+    : public framework::RpcHandler<framework::Stream<proto::LoadMapRequest>,
+                                   google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::LoadMapRequest& request) override {
+    switch (request.map_chunk_case()) {
+      case proto::LoadMapRequest::kPoseGraph:
+        reader_.AddProto(request.pose_graph());
+        break;
+      case proto::LoadMapRequest::kSerializedData:
+        reader_.AddProto(request.serialized_data());
+        break;
+      default:
+        LOG(FATAL) << "Unhandled proto::LoadMapRequest case.";
+    }
+  }
+
+  void OnReadsDone() override {
+    GetContext<MapBuilderServer::MapBuilderContext>()->map_builder().LoadMap(
+        &reader_);
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+
+ private:
+  cartographer::io::InMemoryProtoStreamReader reader_;
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H
diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc
index 9d4eccd..a1a74fd 100644
--- a/cartographer_grpc/map_builder_server.cc
+++ b/cartographer_grpc/map_builder_server.cc
@@ -29,6 +29,7 @@
 #include "cartographer_grpc/handlers/get_local_to_global_transform_handler.h"
 #include "cartographer_grpc/handlers/get_submap_handler.h"
 #include "cartographer_grpc/handlers/get_trajectory_node_poses_handler.h"
+#include "cartographer_grpc/handlers/load_map_handler.h"
 #include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "glog/logging.h"
@@ -249,6 +250,9 @@ MapBuilderServer::MapBuilderServer(
       "GetLocalToGlobalTransform");
   server_builder.RegisterHandler<handlers::GetConstraintsHandler,
                                  proto::MapBuilderService>("GetConstraints");
+  server_builder
+      .RegisterHandler<handlers::LoadMapHandler, proto::MapBuilderService>(
+          "LoadMap");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));
diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc
index 556607c..9280f0d 100644
--- a/cartographer_grpc/mapping/map_builder_stub.cc
+++ b/cartographer_grpc/mapping/map_builder_stub.cc
@@ -94,7 +94,26 @@ void MapBuilderStub::SerializeState(
 
 void MapBuilderStub::LoadMap(
     cartographer::io::ProtoStreamReaderInterface* reader) {
-  LOG(FATAL) << "Not implemented";
+  framework::ClientWriter<proto::LoadMapRequest> load_map_writer;
+  load_map_writer.client_writer = service_stub_->LoadMap(
+      &load_map_writer.client_context, &load_map_writer.response);
+  CHECK(load_map_writer.client_writer);
+
+  // Request with a PoseGraph proto is sent first.
+  {
+    proto::LoadMapRequest request;
+    CHECK(reader->ReadProto(request.mutable_pose_graph()));
+    CHECK(load_map_writer.client_writer->Write(request));
+  }
+
+  // Multiple requests with SerializedData are sent after.
+  proto::LoadMapRequest request;
+  while (reader->ReadProto(request.mutable_serialized_data())) {
+    CHECK(load_map_writer.client_writer->Write(request));
+  }
+
+  CHECK(reader->eof());
+  CHECK(load_map_writer.client_writer->WritesDone());
 }
 
 int MapBuilderStub::num_trajectory_builders() const {
diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto
index de107c9..fdf0a87 100644
--- a/cartographer_grpc/proto/map_builder_service.proto
+++ b/cartographer_grpc/proto/map_builder_service.proto
@@ -88,6 +88,13 @@ message GetSubmapRequest {
   cartographer.mapping.proto.SubmapId submap_id = 1;
 }
 
+message LoadMapRequest {
+  oneof map_chunk {
+    cartographer.mapping.proto.PoseGraph pose_graph = 1;
+    cartographer.mapping.proto.SerializedData serialized_data = 2;
+  }
+}
+
 message GetSubmapResponse {
   cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1;
   string error_msg = 2;
@@ -184,4 +191,7 @@ service MapBuilderService {
 
   // Returns the list of constraints in the current optimization problem.
   rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse);
+
+  // Adds map data in the order defined by ProtoStreamReader.
+  rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty);
 }