From bd2fbbf1a13bec5d8a7cf6e93a272fd78128c77e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Mon, 8 Jan 2018 16:28:41 +0100
Subject: [PATCH] Implement GetLocalToGlobalTransform() in gRPC service (#797)

---
 .../get_local_to_global_transform_handler.h   | 45 +++++++++++++++++++
 cartographer_grpc/map_builder_server.cc       |  4 ++
 cartographer_grpc/mapping/pose_graph_stub.cc  |  8 +++-
 .../proto/map_builder_service.proto           | 12 +++++
 4 files changed, 68 insertions(+), 1 deletion(-)
 create mode 100644 cartographer_grpc/handlers/get_local_to_global_transform_handler.h

diff --git a/cartographer_grpc/handlers/get_local_to_global_transform_handler.h b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h
new file mode 100644
index 0000000..6c4c2fc
--- /dev/null
+++ b/cartographer_grpc/handlers/get_local_to_global_transform_handler.h
@@ -0,0 +1,45 @@
+/*
+ * 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_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class GetLocalToGlobalTransformHandler
+    : public framework::RpcHandler<proto::GetLocalToGlobalTransformRequest,
+                                   proto::GetLocalToGlobalTransformResponse> {
+ public:
+  void OnRequest(
+      const proto::GetLocalToGlobalTransformRequest& request) override {
+    auto response = cartographer::common::make_unique<
+        proto::GetLocalToGlobalTransformResponse>();
+    auto local_to_global =
+        GetContext<MapBuilderServer::MapBuilderContext>()
+            ->map_builder()
+            .pose_graph()
+            ->GetLocalToGlobalTransform(request.trajectory_id());
+    *response->mutable_local_to_global() =
+        cartographer::transform::ToProto(local_to_global);
+    Send(std::move(response));
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_LOCAL_TO_GLOBAL_TRANSFORM_HANDLER_H
diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc
index 170b085..7f56c89 100644
--- a/cartographer_grpc/map_builder_server.cc
+++ b/cartographer_grpc/map_builder_server.cc
@@ -23,6 +23,7 @@
 #include "cartographer_grpc/handlers/add_trajectory_handler.h"
 #include "cartographer_grpc/handlers/finish_trajectory_handler.h"
 #include "cartographer_grpc/handlers/get_all_submap_poses.h"
+#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/receive_local_slam_results_handler.h"
@@ -127,6 +128,9 @@ MapBuilderServer::MapBuilderServer(
       "GetTrajectoryNodePoses");
   server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler,
                                  proto::MapBuilderService>("GetAllSubmapPoses");
+  server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler,
+                                 proto::MapBuilderService>(
+      "GetLocalToGlobalTransform");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));
diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc
index af08230..6c9614c 100644
--- a/cartographer_grpc/mapping/pose_graph_stub.cc
+++ b/cartographer_grpc/mapping/pose_graph_stub.cc
@@ -59,7 +59,13 @@ PoseGraphStub::GetAllSubmapPoses() {
 
 cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
     int trajectory_id) {
-  LOG(FATAL) << "Not implemented";
+  grpc::ClientContext client_context;
+  proto::GetLocalToGlobalTransformRequest request;
+  request.set_trajectory_id(trajectory_id);
+  proto::GetLocalToGlobalTransformResponse response;
+  CHECK(stub_->GetLocalToGlobalTransform(&client_context, request, &response)
+            .ok());
+  return cartographer::transform::ToRigid3(response.local_to_global());
 }
 
 cartographer::mapping::MapById<cartographer::mapping::NodeId,
diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto
index 09ea6c6..e13e387 100644
--- a/cartographer_grpc/proto/map_builder_service.proto
+++ b/cartographer_grpc/proto/map_builder_service.proto
@@ -103,6 +103,14 @@ message GetAllSubmapPosesResponse {
   repeated SubmapPose submap_poses = 1;
 }
 
+message GetLocalToGlobalTransformRequest {
+  int32 trajectory_id = 1;
+}
+
+message GetLocalToGlobalTransformResponse {
+  cartographer.transform.proto.Rigid3d local_to_global = 1;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -142,4 +150,8 @@ service MapBuilderService {
   // Returns the current optimized submap poses.
   rpc GetAllSubmapPoses(google.protobuf.Empty)
       returns (GetAllSubmapPosesResponse);
+
+  // Returns the current local-to-global transform for the trajectory.
+  rpc GetLocalToGlobalTransform(GetLocalToGlobalTransformRequest)
+      returns (GetLocalToGlobalTransformResponse);
 }