Implement gRPC submap query. (#794)

cartographer/mapping/id.h

@@ -30,6 +30,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -75,6 +76,11 @@ struct SubmapId {
     return std::forward_as_tuple(trajectory_id, submap_index) <
            std::forward_as_tuple(other.trajectory_id, other.submap_index);
   }
+
+  void ToProto(proto::SubmapId* proto) const {
+    proto->set_trajectory_id(trajectory_id);
+    proto->set_submap_index(submap_index);
+  }
 };
 
 inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {

cartographer_grpc/handlers/get_submap_handler.h

@@ -0,0 +1,50 @@
+/*
+ * 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_GET_SUBMAP_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_HANDLER_H
+
+#include "cartographer/common/make_unique.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 GetSubmapHandler
+    : public framework::RpcHandler<proto::GetSubmapRequest,
+                                   proto::GetSubmapResponse> {
+ public:
+  void OnRequest(const proto::GetSubmapRequest &request) override {
+    auto response =
+        cartographer::common::make_unique<proto::GetSubmapResponse>();
+    response->set_error_msg(GetContext<MapBuilderServer::MapBuilderContext>()
+                                ->map_builder()
+                                .SubmapToProto(
+                                    cartographer::mapping::SubmapId{
+                                        request.submap_id().trajectory_id(),
+                                        request.submap_id().submap_index()},
+                                    response->mutable_submap_query_response()));
+    Send(std::move(response));
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -22,6 +22,7 @@
 #include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
 #include "cartographer_grpc/handlers/add_trajectory_handler.h"
 #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
+#include "cartographer_grpc/handlers/get_submap_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"
@@ -116,6 +117,9 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler,
                                  proto::MapBuilderService>(
       "ReceiveLocalSlamResults");
+  server_builder
+      .RegisterHandler<handlers::GetSubmapHandler, proto::MapBuilderService>(
+          "GetSubmap");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/mapping/map_builder_stub.cc

@@ -76,8 +76,15 @@ void MapBuilderStub::FinishTrajectory(int trajectory_id) {
 
 std::string MapBuilderStub::SubmapToProto(
     const cartographer::mapping::SubmapId& submap_id,
-    cartographer::mapping::proto::SubmapQuery::Response* response) {
+    cartographer::mapping::proto::SubmapQuery::Response*
-  LOG(FATAL) << "Not implemented";
+        submap_query_response) {
+  grpc::ClientContext client_context;
+  proto::GetSubmapRequest request;
+  submap_id.ToProto(request.mutable_submap_id());
+  proto::GetSubmapResponse response;
+  CHECK(service_stub_->GetSubmap(&client_context, request, &response).ok());
+  submap_query_response->CopyFrom(response.submap_query_response());
+  return response.error_msg();
 }
 
 void MapBuilderStub::SerializeState(

cartographer_grpc/proto/map_builder_service.proto

@@ -15,6 +15,7 @@
 syntax = "proto3";
 
 import "cartographer/mapping/proto/pose_graph.proto";
+import "cartographer/mapping/proto/submap_visualization.proto";
 import "cartographer/mapping/proto/trajectory_builder_options.proto";
 import "cartographer/sensor/proto/sensor.proto";
 import "cartographer/transform/proto/transform.proto";
@@ -73,6 +74,15 @@ message ReceiveLocalSlamResultsResponse {
   cartographer.mapping.proto.NodeId node_id = 5;
 }
 
+message GetSubmapRequest {
+  cartographer.mapping.proto.SubmapId submap_id = 1;
+}
+
+message GetSubmapResponse {
+  cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1;
+  string error_msg = 2;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -101,4 +111,7 @@ service MapBuilderService {
   // Marks a trajectory corresponding to 'trajectory_id' as finished,
   // i.e. no further sensor data is expected.
   rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);
+
+  // Retrieves a single submap.
+  rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse);
 }