From 8165da873f2b9ace0978e1431b1722deb22ada87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Tue, 9 Jan 2018 14:25:28 +0100 Subject: [PATCH] Add GetConstraints() to gRPC service. (#798) --- cartographer/mapping/pose_graph.cc | 41 ++++++++------- cartographer/mapping/pose_graph.h | 1 + .../handlers/get_constraints_handler.h | 52 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 3 ++ cartographer_grpc/mapping/pose_graph_stub.cc | 7 ++- .../proto/map_builder_service.proto | 7 +++ 6 files changed, 91 insertions(+), 20 deletions(-) create mode 100644 cartographer_grpc/handlers/get_constraints_handler.h diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 272a4e5..d36c93a 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/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; diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 2821a2b..56f7535 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -134,6 +134,7 @@ std::vector FromProto( const ::google::protobuf::RepeatedPtrField< ::cartographer::mapping::proto::PoseGraph::Constraint>& constraint_protos); +proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint); } // namespace mapping } // namespace cartographer diff --git a/cartographer_grpc/handlers/get_constraints_handler.h b/cartographer_grpc/handlers/get_constraints_handler.h new file mode 100644 index 0000000..71781dd --- /dev/null +++ b/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 { + public: + void OnRequest(const google::protobuf::Empty& request) override { + auto constraints = GetContext() + ->map_builder() + .pose_graph() + ->constraints(); + auto response = + cartographer::common::make_unique(); + 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 diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 7f56c89..2f42214 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_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( "GetLocalToGlobalTransform"); + server_builder.RegisterHandler("GetConstraints"); grpc_server_ = server_builder.Build(); grpc_server_->SetExecutionContext( cartographer::common::make_unique(this)); diff --git a/cartographer_grpc/mapping/pose_graph_stub.cc b/cartographer_grpc/mapping/pose_graph_stub.cc index 6c9614c..406617d 100644 --- a/cartographer_grpc/mapping/pose_graph_stub.cc +++ b/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 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 diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index e13e387..ac062a5 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/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); }