Add GetConstraints() to gRPC service. (#798)

cartographer/mapping/pose_graph.cc

@@ -96,6 +96,24 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
   return options;
 }
 
+proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) {
+  proto::PoseGraph::Constraint constraint_proto;
+  *constraint_proto.mutable_relative_pose() =
+      transform::ToProto(constraint.pose.zbar_ij);
+  constraint_proto.set_translation_weight(constraint.pose.translation_weight);
+  constraint_proto.set_rotation_weight(constraint.pose.rotation_weight);
+  constraint_proto.mutable_submap_id()->set_trajectory_id(
+      constraint.submap_id.trajectory_id);
+  constraint_proto.mutable_submap_id()->set_submap_index(
+      constraint.submap_id.submap_index);
+  constraint_proto.mutable_node_id()->set_trajectory_id(
+      constraint.node_id.trajectory_id);
+  constraint_proto.mutable_node_id()->set_node_index(
+      constraint.node_id.node_index);
+  constraint_proto.set_tag(mapping::ToProto(constraint.tag));
+  return constraint_proto;
+}
+
 proto::PoseGraph PoseGraph::ToProto() {
   proto::PoseGraph proto;
 
@@ -130,25 +148,10 @@ proto::PoseGraph PoseGraph::ToProto() {
         transform::ToProto(submap_id_data.data.pose);
   }
 
-  for (const auto& constraint : constraints()) {
-    auto* const constraint_proto = proto.add_constraint();
-    *constraint_proto->mutable_relative_pose() =
-        transform::ToProto(constraint.pose.zbar_ij);
-    constraint_proto->set_translation_weight(
-        constraint.pose.translation_weight);
-    constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
-
-    constraint_proto->mutable_submap_id()->set_trajectory_id(
-        constraint.submap_id.trajectory_id);
-    constraint_proto->mutable_submap_id()->set_submap_index(
-        constraint.submap_id.submap_index);
-
-    constraint_proto->mutable_node_id()->set_trajectory_id(
-        constraint.node_id.trajectory_id);
-    constraint_proto->mutable_node_id()->set_node_index(
-        constraint.node_id.node_index);
-
-    constraint_proto->set_tag(mapping::ToProto(constraint.tag));
+  auto constraints_copy = constraints();
+  proto.mutable_constraint()->Reserve(constraints_copy.size());
+  for (const auto& constraint : constraints_copy) {
+    *proto.add_constraint() = cartographer::mapping::ToProto(constraint);
   }
 
   return proto;

cartographer/mapping/pose_graph.h

@@ -134,6 +134,7 @@ std::vector<PoseGraph::Constraint> FromProto(
     const ::google::protobuf::RepeatedPtrField<
         ::cartographer::mapping::proto::PoseGraph::Constraint>&
         constraint_protos);
+proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint);
 
 }  // namespace mapping
 }  // namespace cartographer

cartographer_grpc/handlers/get_constraints_handler.h

@@ -0,0 +1,52 @@
+/*
+ * 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_CONSTRAINTS_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_GET_CONSTRAINTS_HANDLER_H
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/pose_graph.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 GetConstraintsHandler
+    : public framework::RpcHandler<google::protobuf::Empty,
+                                   proto::GetConstraintsResponse> {
+ public:
+  void OnRequest(const google::protobuf::Empty& request) override {
+    auto constraints = GetContext<MapBuilderServer::MapBuilderContext>()
+                           ->map_builder()
+                           .pose_graph()
+                           ->constraints();
+    auto response =
+        cartographer::common::make_unique<proto::GetConstraintsResponse>();
+    response->mutable_constraints()->Reserve(constraints.size());
+    for (const auto& constraint : constraints) {
+      *response->add_constraints() = cartographer::mapping::ToProto(constraint);
+    }
+    Send(std::move(response));
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_GET_CONSTRAINTS_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_all_submap_poses.h"
+#include "cartographer_grpc/handlers/get_constraints_handler.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"
@@ -131,6 +132,8 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler,
                                  proto::MapBuilderService>(
       "GetLocalToGlobalTransform");
+  server_builder.RegisterHandler<handlers::GetConstraintsHandler,
+                                 proto::MapBuilderService>("GetConstraints");
   grpc_server_ = server_builder.Build();
   grpc_server_->SetExecutionContext(
       cartographer::common::make_unique<MapBuilderContext>(this));

cartographer_grpc/mapping/pose_graph_stub.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer_grpc/mapping/pose_graph_stub.h"
 
+#include "cartographer/mapping/pose_graph.h"
 #include "glog/logging.h"
 
 namespace cartographer_grpc {
@@ -101,7 +102,11 @@ bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
 
 std::vector<cartographer::mapping::PoseGraphInterface::Constraint>
 PoseGraphStub::constraints() {
-  LOG(FATAL) << "Not implemented";
+  grpc::ClientContext client_context;
+  google::protobuf::Empty request;
+  proto::GetConstraintsResponse response;
+  stub_->GetConstraints(&client_context, request, &response);
+  return cartographer::mapping::FromProto(response.constraints());
 }
 
 }  // namespace mapping

cartographer_grpc/proto/map_builder_service.proto

@@ -111,6 +111,10 @@ message GetLocalToGlobalTransformResponse {
   cartographer.transform.proto.Rigid3d local_to_global = 1;
 }
 
+message GetConstraintsResponse {
+  repeated cartographer.mapping.proto.PoseGraph.Constraint constraints = 1;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -154,4 +158,7 @@ service MapBuilderService {
   // Returns the current local-to-global transform for the trajectory.
   rpc GetLocalToGlobalTransform(GetLocalToGlobalTransformRequest)
       returns (GetLocalToGlobalTransformResponse);
+
+  // Returns the list of constraints in the current optimization problem.
+  rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse);
 }