From 984553ee15d4d99f9ac25c8615d285dfbfd51a63 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 19 Jun 2018 18:01:39 +0200 Subject: [PATCH] LoadState(FromFile) endpoints (#1203) [RFC=0023](https://github.com/googlecartographer/rfcs/blob/master/text/0023-delete-load.md) --- cartographer/cloud/client/map_builder_stub.cc | 12 ++++-- .../cloud/internal/client_server_test.cc | 9 +++- .../handlers/load_state_from_file_handler.cc | 43 +++++++++++++++++++ .../handlers/load_state_from_file_handler.h | 42 ++++++++++++++++++ .../internal/handlers/load_state_handler.cc | 11 +++-- .../internal/handlers/load_state_handler.h | 3 +- .../cloud/internal/map_builder_server.cc | 2 + .../cloud/internal/mapping/serialization.cc | 40 +++++++++++++++++ .../cloud/internal/mapping/serialization.h | 32 ++++++++++++++ .../cloud/proto/map_builder_service.proto | 23 +++++++++- 10 files changed, 205 insertions(+), 12 deletions(-) create mode 100644 cartographer/cloud/internal/handlers/load_state_from_file_handler.cc create mode 100644 cartographer/cloud/internal/handlers/load_state_from_file_handler.h create mode 100644 cartographer/cloud/internal/mapping/serialization.cc create mode 100644 cartographer/cloud/internal/mapping/serialization.h diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index c326223..85562a6 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -21,8 +21,10 @@ #include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/get_submap_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" #include "cartographer/cloud/internal/handlers/load_state_handler.h" #include "cartographer/cloud/internal/handlers/write_state_handler.h" +#include "cartographer/cloud/internal/mapping/serialization.h" #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" #include "cartographer/io/proto_stream_deserializer.h" @@ -163,13 +165,17 @@ std::map MapBuilderStub::LoadState( CHECK(reader->eof()); CHECK(client.StreamWritesDone()); CHECK(client.StreamFinish().ok()); - // TODO(gaschler): Return trajectory remapping. - return {}; + return FromProto(client.response().trajectory_remapping()); } std::map MapBuilderStub::LoadStateFromFile( const std::string& filename) { - LOG(FATAL) << "not implemented"; + proto::LoadStateFromFileRequest request; + request.set_file_path(filename); + async_grpc::Client client( + client_channel_); + CHECK(client.Write(request)); + return FromProto(client.response().trajectory_remapping()); } int MapBuilderStub::num_trajectory_builders() const { diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index a39bed6..c4aaa97 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -474,8 +474,13 @@ TEST_F(ClientServerTest, LoadState) { kLandmarkDataProtoString, }); - stub_->LoadState(reader.get(), true); - EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFrozen(0)); + auto trajectory_remapping = stub_->LoadState(reader.get(), true); + int expected_trajectory_id = 0; + EXPECT_EQ(trajectory_remapping.size(), 1); + EXPECT_EQ(trajectory_remapping.at(0), expected_trajectory_id); + EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFrozen(expected_trajectory_id)); + EXPECT_FALSE( + stub_->pose_graph()->IsTrajectoryFinished(expected_trajectory_id)); server_->Shutdown(); } diff --git a/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc b/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc new file mode 100644 index 0000000..4319e7c --- /dev/null +++ b/cartographer/cloud/internal/handlers/load_state_from_file_handler.cc @@ -0,0 +1,43 @@ +/* + * 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. + */ + +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +void LoadStateFromFileHandler::OnRequest( + const proto::LoadStateFromFileRequest& request) { + // TODO(gaschler): This blocks a handler thread, consider working in + // background. + auto trajectory_remapping = + GetContext()->map_builder().LoadStateFromFile( + request.file_path()); + auto response = common::make_unique(); + *response->mutable_trajectory_remapping() = ToProto(trajectory_remapping); + Send(std::move(response)); +} + +} // namespace handlers +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/handlers/load_state_from_file_handler.h b/cartographer/cloud/internal/handlers/load_state_from_file_handler.h new file mode 100644 index 0000000..f9ec5d0 --- /dev/null +++ b/cartographer/cloud/internal/handlers/load_state_from_file_handler.h @@ -0,0 +1,42 @@ +/* + * 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_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H +#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H + +#include "async_grpc/rpc_handler.h" +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { +namespace handlers { + +DEFINE_HANDLER_SIGNATURE( + LoadStateFromFileSignature, proto::LoadStateFromFileRequest, + proto::LoadStateFromFileResponse, + "/cartographer.cloud.proto.MapBuilderService/LoadStateFromFile") + +class LoadStateFromFileHandler + : public async_grpc::RpcHandler { + public: + void OnRequest(const proto::LoadStateFromFileRequest& request) override; +}; + +} // namespace handlers +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_LOAD_STATE_FROM_FILE_HANDLER_H diff --git a/cartographer/cloud/internal/handlers/load_state_handler.cc b/cartographer/cloud/internal/handlers/load_state_handler.cc index 80caeac..4696b7d 100644 --- a/cartographer/cloud/internal/handlers/load_state_handler.cc +++ b/cartographer/cloud/internal/handlers/load_state_handler.cc @@ -18,9 +18,9 @@ #include "async_grpc/rpc_handler.h" #include "cartographer/cloud/internal/map_builder_context_interface.h" +#include "cartographer/cloud/internal/mapping/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" #include "cartographer/common/make_unique.h" -#include "google/protobuf/empty.pb.h" namespace cartographer { namespace cloud { @@ -40,9 +40,12 @@ void LoadStateHandler::OnRequest(const proto::LoadStateRequest& request) { } void LoadStateHandler::OnReadsDone() { - GetContext()->map_builder().LoadState(&reader_, - true); - Send(common::make_unique()); + auto trajectory_remapping = + GetContext()->map_builder().LoadState( + &reader_, true); + auto response = common::make_unique(); + *response->mutable_trajectory_remapping() = ToProto(trajectory_remapping); + Send(std::move(response)); } } // namespace handlers diff --git a/cartographer/cloud/internal/handlers/load_state_handler.h b/cartographer/cloud/internal/handlers/load_state_handler.h index 7f370c6..0d55aff 100644 --- a/cartographer/cloud/internal/handlers/load_state_handler.h +++ b/cartographer/cloud/internal/handlers/load_state_handler.h @@ -20,7 +20,6 @@ #include "async_grpc/rpc_handler.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" #include "cartographer/io/internal/in_memory_proto_stream.h" -#include "google/protobuf/empty.pb.h" namespace cartographer { namespace cloud { @@ -28,7 +27,7 @@ namespace handlers { DEFINE_HANDLER_SIGNATURE( LoadStateSignature, async_grpc::Stream, - google::protobuf::Empty, + proto::LoadStateResponse, "/cartographer.cloud.proto.MapBuilderService/LoadState") class LoadStateHandler : public async_grpc::RpcHandler { diff --git a/cartographer/cloud/internal/map_builder_server.cc b/cartographer/cloud/internal/map_builder_server.cc index 6752b4d..16be45e 100644 --- a/cartographer/cloud/internal/map_builder_server.cc +++ b/cartographer/cloud/internal/map_builder_server.cc @@ -33,6 +33,7 @@ #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h" #include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h" +#include "cartographer/cloud/internal/handlers/load_state_from_file_handler.h" #include "cartographer/cloud/internal/handlers/load_state_handler.h" #include "cartographer/cloud/internal/handlers/receive_global_slam_optimizations_handler.h" #include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" @@ -88,6 +89,7 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); + server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); server_builder.RegisterHandler(); diff --git a/cartographer/cloud/internal/mapping/serialization.cc b/cartographer/cloud/internal/mapping/serialization.cc new file mode 100644 index 0000000..d5a6dcf --- /dev/null +++ b/cartographer/cloud/internal/mapping/serialization.cc @@ -0,0 +1,40 @@ +/* + * 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. + */ + +#include "cartographer/cloud/internal/mapping/serialization.h" + +#include "cartographer/common/port.h" + +namespace cartographer { +namespace cloud { + +proto::TrajectoryRemapping ToProto( + const std::map& trajectory_remapping) { + proto::TrajectoryRemapping proto; + *proto.mutable_serialized_trajectories_to_trajectories() = + google::protobuf::Map(trajectory_remapping.begin(), + trajectory_remapping.end()); + return proto; +} + +std::map FromProto(const proto::TrajectoryRemapping& proto) { + return std::map( + proto.serialized_trajectories_to_trajectories().begin(), + proto.serialized_trajectories_to_trajectories().end()); +} + +} // namespace cloud +} // namespace cartographer diff --git a/cartographer/cloud/internal/mapping/serialization.h b/cartographer/cloud/internal/mapping/serialization.h new file mode 100644 index 0000000..4b21bf7 --- /dev/null +++ b/cartographer/cloud/internal/mapping/serialization.h @@ -0,0 +1,32 @@ +/* + * 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_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H +#define CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H + +#include "cartographer/cloud/proto/map_builder_service.pb.h" + +namespace cartographer { +namespace cloud { + +proto::TrajectoryRemapping ToProto( + const std::map& trajectory_remapping); +std::map FromProto(const proto::TrajectoryRemapping& proto); + +} // namespace cloud +} // namespace cartographer + +#endif // CARTOGRAPHER_CLOUD_INTERNAL_MAPPING_SERIALIZATION_H diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 0b5d4b5..465fa69 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -136,6 +136,23 @@ message LoadStateRequest { } } +message TrajectoryRemapping { + map + serialized_trajectories_to_trajectories = 1; +} + +message LoadStateResponse { + TrajectoryRemapping trajectory_remapping = 1; +} + +message LoadStateFromFileRequest { + string file_path = 1; +} + +message LoadStateFromFileResponse { + TrajectoryRemapping trajectory_remapping = 1; +} + message GetSubmapResponse { cartographer.mapping.proto.SubmapQuery.Response submap_query_response = 1; string error_msg = 2; @@ -290,7 +307,11 @@ service MapBuilderService { returns (google.protobuf.Empty); // Adds serialized SLAM state data in the order defined by ProtoStreamReader. - rpc LoadState(stream LoadStateRequest) returns (google.protobuf.Empty); + rpc LoadState(stream LoadStateRequest) returns (LoadStateResponse); + + // Loads a serialized SLAM state data from the host file system. + rpc LoadStateFromFile(LoadStateFromFileRequest) + returns (LoadStateFromFileResponse); // Receives serialized SLAM state data in the order defined by // ProtoStreamWriter.