Add GetAllSubmapPoses to gRPC interface. (#791)

cartographer_grpc/handlers/get_all_submap_poses.h

@@ -0,0 +1,54 @@
+/*
+ * 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_ALL_SUBMAP_POSES_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_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 GetAllSubmapPosesHandler
+    : public framework::RpcHandler<google::protobuf::Empty,
+                                   proto::GetAllSubmapPosesResponse> {
+ public:
+  void OnRequest(const google::protobuf::Empty& request) override {
+    auto submap_poses = GetContext<MapBuilderServer::MapBuilderContext>()
+                            ->map_builder()
+                            .pose_graph()
+                            ->GetAllSubmapPoses();
+    auto response =
+        cartographer::common::make_unique<proto::GetAllSubmapPosesResponse>();
+    for (const auto& submap_id_pose : submap_poses) {
+      auto* submap_pose = response->add_submap_poses();
+      submap_id_pose.id.ToProto(submap_pose->mutable_submap_id());
+      submap_pose->set_submap_version(submap_id_pose.data.version);
+      *submap_pose->mutable_global_pose() =
+          cartographer::transform::ToProto(submap_id_pose.data.pose);
+    }
+    Send(std::move(response));
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_ALL_SUBMAP_POSES_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_all_submap_poses.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"
@@ -124,6 +125,8 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler,
                                  proto::MapBuilderService>(
       "GetTrajectoryNodePoses");
+  server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler,
+                                 proto::MapBuilderService>("GetAllSubmapPoses");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -38,7 +38,23 @@ cartographer::mapping::MapById<
     cartographer::mapping::SubmapId,
     cartographer::mapping::PoseGraphInterface::SubmapPose>
 PoseGraphStub::GetAllSubmapPoses() {
-  LOG(FATAL) << "Not implemented";
+  grpc::ClientContext client_context;
+  google::protobuf::Empty request;
+  proto::GetAllSubmapPosesResponse response;
+  stub_->GetAllSubmapPoses(&client_context, request, &response);
+  cartographer::mapping::MapById<
+      cartographer::mapping::SubmapId,
+      cartographer::mapping::PoseGraphInterface::SubmapPose>
+      submap_poses;
+  for (const auto& submap_pose : response.submap_poses()) {
+    submap_poses.Insert(
+        cartographer::mapping::SubmapId{submap_pose.submap_id().trajectory_id(),
+                                        submap_pose.submap_id().submap_index()},
+        cartographer::mapping::PoseGraphInterface::SubmapPose{
+            submap_pose.submap_version(),
+            cartographer::transform::ToRigid3(submap_pose.global_pose())});
+  }
+  return submap_poses;
 }
 
 cartographer::transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(

cartographer_grpc/proto/map_builder_service.proto

@@ -93,6 +93,16 @@ message GetTrajectoryNodePosesResponse {
   repeated TrajectoryNodePose node_poses = 1;
 }
 
+message SubmapPose {
+  cartographer.mapping.proto.SubmapId submap_id = 1;
+  int32 submap_version = 2;
+  cartographer.transform.proto.Rigid3d global_pose = 3;
+}
+
+message GetAllSubmapPosesResponse {
+  repeated SubmapPose submap_poses = 1;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -128,4 +138,8 @@ service MapBuilderService {
   // Returns the current optimized trajectory poses.
   rpc GetTrajectoryNodePoses(google.protobuf.Empty)
       returns (GetTrajectoryNodePosesResponse);
+
+  // Returns the current optimized submap poses.
+  rpc GetAllSubmapPoses(google.protobuf.Empty)
+      returns (GetAllSubmapPosesResponse);
 }