From d57c2441b8e913ae308ff882cc386664df63bb36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 8 Jan 2018 13:52:14 +0100 Subject: [PATCH] Implement gRPC submap query. (#794) --- cartographer/mapping/id.h | 6 +++ .../handlers/get_submap_handler.h | 50 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 4 ++ cartographer_grpc/mapping/map_builder_stub.cc | 11 +++- .../proto/map_builder_service.proto | 13 +++++ 5 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 cartographer_grpc/handlers/get_submap_handler.h diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index b18e3f6..e0c718a 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -30,6 +30,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" #include "glog/logging.h" namespace cartographer { @@ -75,6 +76,11 @@ struct SubmapId { return std::forward_as_tuple(trajectory_id, submap_index) < std::forward_as_tuple(other.trajectory_id, other.submap_index); } + + void ToProto(proto::SubmapId* proto) const { + proto->set_trajectory_id(trajectory_id); + proto->set_submap_index(submap_index); + } }; inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { diff --git a/cartographer_grpc/handlers/get_submap_handler.h b/cartographer_grpc/handlers/get_submap_handler.h new file mode 100644 index 0000000..f836b07 --- /dev/null +++ b/cartographer_grpc/handlers/get_submap_handler.h @@ -0,0 +1,50 @@ +/* + * 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_SUBMAP_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_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 GetSubmapHandler + : public framework::RpcHandler { + public: + void OnRequest(const proto::GetSubmapRequest &request) override { + auto response = + cartographer::common::make_unique(); + response->set_error_msg(GetContext() + ->map_builder() + .SubmapToProto( + cartographer::mapping::SubmapId{ + request.submap_id().trajectory_id(), + request.submap_id().submap_index()}, + response->mutable_submap_query_response())); + Send(std::move(response)); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_GET_SUBMAP_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index b766262..a999e8a 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/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_submap_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" @@ -116,6 +117,9 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler( "ReceiveLocalSlamResults"); + server_builder + .RegisterHandler( + "GetSubmap"); grpc_server_ = server_builder.Build(); grpc_server_->SetExecutionContext( cartographer::common::make_unique(this)); diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc index 92cb10a..140df68 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -76,8 +76,15 @@ void MapBuilderStub::FinishTrajectory(int trajectory_id) { std::string MapBuilderStub::SubmapToProto( const cartographer::mapping::SubmapId& submap_id, - cartographer::mapping::proto::SubmapQuery::Response* response) { - LOG(FATAL) << "Not implemented"; + cartographer::mapping::proto::SubmapQuery::Response* + submap_query_response) { + grpc::ClientContext client_context; + proto::GetSubmapRequest request; + submap_id.ToProto(request.mutable_submap_id()); + proto::GetSubmapResponse response; + CHECK(service_stub_->GetSubmap(&client_context, request, &response).ok()); + submap_query_response->CopyFrom(response.submap_query_response()); + return response.error_msg(); } void MapBuilderStub::SerializeState( diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index fca5e29..a029c98 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -15,6 +15,7 @@ syntax = "proto3"; import "cartographer/mapping/proto/pose_graph.proto"; +import "cartographer/mapping/proto/submap_visualization.proto"; import "cartographer/mapping/proto/trajectory_builder_options.proto"; import "cartographer/sensor/proto/sensor.proto"; import "cartographer/transform/proto/transform.proto"; @@ -73,6 +74,15 @@ message ReceiveLocalSlamResultsResponse { cartographer.mapping.proto.NodeId node_id = 5; } +message GetSubmapRequest { + cartographer.mapping.proto.SubmapId submap_id = 1; +} + +message GetSubmapResponse { + cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1; + string error_msg = 2; +} + service MapBuilderService { // Starts a new trajectory and returns its index. rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse); @@ -101,4 +111,7 @@ service MapBuilderService { // Marks a trajectory corresponding to 'trajectory_id' as finished, // i.e. no further sensor data is expected. rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty); + + // Retrieves a single submap. + rpc GetSubmap(GetSubmapRequest) returns (GetSubmapResponse); }