Add GetTrajectoryNodePoses() to gRPC service (#796)

cartographer/mapping/id.h

@@ -53,6 +53,11 @@ struct NodeId {
     return std::forward_as_tuple(trajectory_id, node_index) <
            std::forward_as_tuple(other.trajectory_id, other.node_index);
   }
+
+  void ToProto(proto::NodeId* proto) const {
+    proto->set_trajectory_id(trajectory_id);
+    proto->set_node_index(node_index);
+  }
 };
 
 inline std::ostream& operator<<(std::ostream& os, const NodeId& v) {

cartographer_grpc/handlers/get_trajectory_node_poses_handler.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_TRAJECTORY_NODE_POSES_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_TRAJECTORY_NODE_POSES_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 GetTrajectoryNodePosesHandler
+    : public framework::RpcHandler<google::protobuf::Empty,
+                                   proto::GetTrajectoryNodePosesResponse> {
+ public:
+  void OnRequest(const google::protobuf::Empty& request) override {
+    auto node_poses = GetContext<MapBuilderServer::MapBuilderContext>()
+                          ->map_builder()
+                          .pose_graph()
+                          ->GetTrajectoryNodePoses();
+    auto response = cartographer::common::make_unique<
+        proto::GetTrajectoryNodePosesResponse>();
+    for (const auto& node_id_pose : node_poses) {
+      auto* node_pose = response->add_node_poses();
+      node_id_pose.id.ToProto(node_pose->mutable_node_id());
+      *node_pose->mutable_global_pose() =
+          cartographer::transform::ToProto(node_id_pose.data.global_pose);
+      node_pose->set_has_constant_data(node_id_pose.data.has_constant_data);
+    }
+    Send(std::move(response));
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_TRAJECTORY_NODE_POSES_HANDLER_H

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_submap_handler.h"
+#include "cartographer_grpc/handlers/get_trajectory_node_poses_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"
@@ -120,6 +121,9 @@ MapBuilderServer::MapBuilderServer(
   server_builder
       .RegisterHandler<handlers::GetSubmapHandler, proto::MapBuilderService>(
           "GetSubmap");
+  server_builder.RegisterHandler<handlers::GetTrajectoryNodePosesHandler,
+                                 proto::MapBuilderService>(
+      "GetTrajectoryNodePoses");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -55,7 +55,22 @@ PoseGraphStub::GetTrajectoryNodes() {
 cartographer::mapping::MapById<cartographer::mapping::NodeId,
                                cartographer::mapping::TrajectoryNodePose>
 PoseGraphStub::GetTrajectoryNodePoses() {
-  LOG(FATAL) << "Not implemented";
+  grpc::ClientContext client_context;
+  google::protobuf::Empty request;
+  proto::GetTrajectoryNodePosesResponse response;
+  stub_->GetTrajectoryNodePoses(&client_context, request, &response);
+  cartographer::mapping::MapById<cartographer::mapping::NodeId,
+                                 cartographer::mapping::TrajectoryNodePose>
+      node_poses;
+  for (const auto& node_pose : response.node_poses()) {
+    node_poses.Insert(
+        cartographer::mapping::NodeId{node_pose.node_id().trajectory_id(),
+                                      node_pose.node_id().node_index()},
+        cartographer::mapping::TrajectoryNodePose{
+            node_pose.has_constant_data(),
+            cartographer::transform::ToRigid3(node_pose.global_pose())});
+  }
+  return node_poses;
 }
 
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {

cartographer_grpc/proto/map_builder_service.proto

@@ -83,6 +83,16 @@ message GetSubmapResponse {
   string error_msg = 2;
 }
 
+message TrajectoryNodePose {
+  cartographer.mapping.proto.NodeId node_id = 1;
+  cartographer.transform.proto.Rigid3d global_pose = 2;
+  bool has_constant_data = 3;
+}
+
+message GetTrajectoryNodePosesResponse {
+  repeated TrajectoryNodePose node_poses = 1;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -114,4 +124,8 @@ service MapBuilderService {
 
   // Retrieves a single submap.
   rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse);
+
+  // Returns the current optimized trajectory poses.
+  rpc GetTrajectoryNodePoses(google.protobuf.Empty)
+      returns (GetTrajectoryNodePosesResponse);
 }