diff --git a/cartographer_grpc/handlers/write_map_handler.h b/cartographer_grpc/handlers/write_map_handler.h new file mode 100644 index 0000000..882eb4a --- /dev/null +++ b/cartographer_grpc/handlers/write_map_handler.h @@ -0,0 +1,67 @@ +/* + * 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_WRITE_MAP_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_WRITE_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_context_interface.h" +#include "cartographer_grpc/map_builder_server.h" +#include "cartographer_grpc/proto/map_builder_service.pb.h" + +namespace cartographer_grpc { +namespace handlers { + +class WriteMapHandler + : public framework::RpcHandler> { + public: + std::string method_name() const override { + return "/cartographer_grpc.proto.MapBuilderService/WriteMap"; + } + void OnRequest(const google::protobuf::Empty& request) override { + auto writer = GetWriter(); + cartographer::io::ForwardingProtoStreamWriter proto_stream_writer( + [writer](const google::protobuf::Message* proto) { + if (!proto) { + writer.WritesDone(); + return true; + } + + auto response = + cartographer::common::make_unique(); + if (proto->GetTypeName() == "cartographer.mapping.proto.PoseGraph") { + response->mutable_pose_graph()->CopyFrom(*proto); + } else if (proto->GetTypeName() == + "cartographer.mapping.proto.SerializedData") { + response->mutable_serialized_data()->CopyFrom(*proto); + } else { + LOG(FATAL) << "Unsupported message type: " << proto->GetTypeName(); + } + writer.Write(std::move(response)); + return true; + }); + GetContext()->map_builder().SerializeState( + &proto_stream_writer); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_WRITE_MAP_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index f01ebee..9f53ad5 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -32,6 +32,7 @@ #include "cartographer_grpc/handlers/load_map_handler.h" #include "cartographer_grpc/handlers/receive_local_slam_results_handler.h" #include "cartographer_grpc/handlers/run_final_optimization_handler.h" +#include "cartographer_grpc/handlers/write_map_handler.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "cartographer_grpc/sensor/serialization.h" #include "glog/logging.h" @@ -75,6 +76,7 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); 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 6747564..b498cc8 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/cartographer_grpc/mapping/map_builder_stub.cc @@ -90,7 +90,14 @@ std::string MapBuilderStub::SubmapToProto( void MapBuilderStub::SerializeState( cartographer::io::ProtoStreamWriterInterface* writer) { - LOG(FATAL) << "Not implemented"; + grpc::ClientContext client_context; + google::protobuf::Empty request; + auto reader = service_stub_->WriteMap(&client_context, request); + proto::WriteMapResponse response; + while (reader->Read(&response)) { + writer->WriteProto(response); + } + CHECK(writer->Close()); } void MapBuilderStub::LoadMap( diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index b3021e0..184e476 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -151,6 +151,13 @@ message AddLocalSlamResultDataRequest { cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2; } +message WriteMapResponse { + oneof map_chunk { + cartographer.mapping.proto.PoseGraph pose_graph = 1; + cartographer.mapping.proto.SerializedData serialized_data = 2; + } +} + service MapBuilderService { // Starts a new trajectory and returns its index. rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse); @@ -212,4 +219,7 @@ service MapBuilderService { // Adds map data in the order defined by ProtoStreamReader. rpc LoadMap(stream LoadMapRequest) returns (google.protobuf.Empty); + + // Receives map data in the order defined by ProtoStreamWriter. + rpc WriteMap(google.protobuf.Empty) returns (stream WriteMapResponse); }