diff --git a/cartographer_grpc/handlers/load_map_handler.h b/cartographer_grpc/handlers/load_map_handler.h new file mode 100644 index 0000000..36c6b47 --- /dev/null +++ b/cartographer_grpc/handlers/load_map_handler.h @@ -0,0 +1,60 @@ +/* + * 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_LOAD_MAP_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H + +#include "cartographer/common/make_unique.h" +#include "cartographer/io/in_memory_proto_stream.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 LoadMapHandler + : public framework::RpcHandler, + google::protobuf::Empty> { + public: + void OnRequest(const proto::LoadMapRequest& request) override { + switch (request.map_chunk_case()) { + case proto::LoadMapRequest::kPoseGraph: + reader_.AddProto(request.pose_graph()); + break; + case proto::LoadMapRequest::kSerializedData: + reader_.AddProto(request.serialized_data()); + break; + default: + LOG(FATAL) << "Unhandled proto::LoadMapRequest case."; + } + } + + void OnReadsDone() override { + GetContext()->map_builder().LoadMap( + &reader_); + Send(cartographer::common::make_unique()); + } + + private: + cartographer::io::InMemoryProtoStreamReader reader_; +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_LOAD_MAP_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index 9d4eccd..a1a74fd 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -29,6 +29,7 @@ #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" +#include "cartographer_grpc/handlers/load_map_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" @@ -249,6 +250,9 @@ MapBuilderServer::MapBuilderServer( "GetLocalToGlobalTransform"); server_builder.RegisterHandler("GetConstraints"); + server_builder + .RegisterHandler( + "LoadMap"); 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 556607c..9280f0d 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -94,7 +94,26 @@ void MapBuilderStub::SerializeState( void MapBuilderStub::LoadMap( cartographer::io::ProtoStreamReaderInterface* reader) { - LOG(FATAL) << "Not implemented"; + framework::ClientWriter load_map_writer; + load_map_writer.client_writer = service_stub_->LoadMap( + &load_map_writer.client_context, &load_map_writer.response); + CHECK(load_map_writer.client_writer); + + // Request with a PoseGraph proto is sent first. + { + proto::LoadMapRequest request; + CHECK(reader->ReadProto(request.mutable_pose_graph())); + CHECK(load_map_writer.client_writer->Write(request)); + } + + // Multiple requests with SerializedData are sent after. + proto::LoadMapRequest request; + while (reader->ReadProto(request.mutable_serialized_data())) { + CHECK(load_map_writer.client_writer->Write(request)); + } + + CHECK(reader->eof()); + CHECK(load_map_writer.client_writer->WritesDone()); } int MapBuilderStub::num_trajectory_builders() const { diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index de107c9..fdf0a87 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -88,6 +88,13 @@ message GetSubmapRequest { cartographer.mapping.proto.SubmapId submap_id = 1; } +message LoadMapRequest { + oneof map_chunk { + cartographer.mapping.proto.PoseGraph pose_graph = 1; + cartographer.mapping.proto.SerializedData serialized_data = 2; + } +} + message GetSubmapResponse { cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1; string error_msg = 2; @@ -184,4 +191,7 @@ service MapBuilderService { // Returns the list of constraints in the current optimization problem. rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse); + + // Adds map data in the order defined by ProtoStreamReader. + rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty); }