From 4c2104473c5f5950a5f06a06712a4b1acac15ca7 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Wed, 29 Aug 2018 10:55:55 +0200 Subject: [PATCH] Support gRPC requests for loading unfrozen states. (#1379) --- cartographer/cloud/client/map_builder_stub.cc | 7 +-- .../cloud/internal/client_server_test.cc | 49 ++++++++++++++++++- .../internal/handlers/load_state_handler.cc | 3 +- .../internal/handlers/load_state_handler.h | 1 + .../cloud/proto/map_builder_service.proto | 1 + 5 files changed, 55 insertions(+), 6 deletions(-) diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index 2c85e26..d68e02e 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/cartographer/cloud/client/map_builder_stub.cc @@ -150,9 +150,6 @@ void MapBuilderStub::SerializeState(bool include_unfinished_submaps, std::map MapBuilderStub::LoadState( io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) { - if (!load_frozen_state) { - LOG(FATAL) << "Not implemented"; - } async_grpc::Client client(client_channel_); { proto::LoadStateRequest request; @@ -165,6 +162,7 @@ std::map MapBuilderStub::LoadState( { proto::LoadStateRequest request; *request.mutable_serialization_header() = deserializer.header(); + request.set_load_frozen_state(load_frozen_state); CHECK(client.Write(request)); } // Request with a PoseGraph proto is sent second. @@ -172,6 +170,7 @@ std::map MapBuilderStub::LoadState( proto::LoadStateRequest request; *request.mutable_serialized_data()->mutable_pose_graph() = deserializer.pose_graph(); + request.set_load_frozen_state(load_frozen_state); CHECK(client.Write(request)); } // Request with an AllTrajectoryBuilderOptions should be third. @@ -180,12 +179,14 @@ std::map MapBuilderStub::LoadState( *request.mutable_serialized_data() ->mutable_all_trajectory_builder_options() = deserializer.all_trajectory_builder_options(); + request.set_load_frozen_state(load_frozen_state); CHECK(client.Write(request)); } // Multiple requests with SerializedData are sent after. proto::LoadStateRequest request; while ( deserializer.ReadNextSerializedData(request.mutable_serialized_data())) { + request.set_load_frozen_state(load_frozen_state); CHECK(client.Write(request)); } diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 148e276..33cbdd1 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -611,10 +611,8 @@ TEST_P(ClientServerTestByGridType, LoadStateAndDelete) { kAllTrajectoryBuilderOptionsProtoString, kSubmapProtoString, kNodeProtoString, - kTrajectoryDataProtoString, kImuDataProtoString, kOdometryDataProtoString, - kFixedFramePoseDataProtoString, kLandmarkDataProtoString, }); @@ -636,6 +634,53 @@ TEST_P(ClientServerTestByGridType, LoadStateAndDelete) { server_->Shutdown(); } +TEST_P(ClientServerTestByGridType, LoadUnfrozenStateAndDelete) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } + InitializeRealServer(); + server_->Start(); + InitializeStub(); + + // Load text proto into in_memory_reader. + auto reader = + ProtoReaderFromStrings(kSerializationHeaderProtoString, + { + kPoseGraphProtoString, + kAllTrajectoryBuilderOptionsProtoString, + kSubmapProtoString, + kNodeProtoString, + kImuDataProtoString, + kOdometryDataProtoString, + kLandmarkDataProtoString, + }); + + auto trajectory_remapping = + stub_->LoadState(reader.get(), false /* load_frozen_state */); + int expected_trajectory_id = 0; + EXPECT_EQ(trajectory_remapping.size(), 1); + EXPECT_EQ(trajectory_remapping.at(0), expected_trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(expected_trajectory_id)); + EXPECT_FALSE( + stub_->pose_graph()->IsTrajectoryFinished(expected_trajectory_id)); + EXPECT_EQ( + stub_->pose_graph()->GetTrajectoryStates().at(expected_trajectory_id), + PoseGraphInterface::TrajectoryState::ACTIVE); + stub_->FinishTrajectory(expected_trajectory_id); + EXPECT_EQ( + stub_->pose_graph()->GetTrajectoryStates().at(expected_trajectory_id), + PoseGraphInterface::TrajectoryState::FINISHED); + for (const auto& entry : trajectory_remapping) { + int trajectory_id = entry.second; + stub_->pose_graph()->DeleteTrajectory(trajectory_id); + stub_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id), + PoseGraphInterface::TrajectoryState::DELETED); + } + server_->Shutdown(); +} + // TODO(gaschler): Test-cover LoadStateFromFile. TEST_P(ClientServerTestByGridType, LocalSlam2DHandlesInvalidRequests) { diff --git a/cartographer/cloud/internal/handlers/load_state_handler.cc b/cartographer/cloud/internal/handlers/load_state_handler.cc index 562856b..652a9ad 100644 --- a/cartographer/cloud/internal/handlers/load_state_handler.cc +++ b/cartographer/cloud/internal/handlers/load_state_handler.cc @@ -40,12 +40,13 @@ void LoadStateHandler::OnRequest(const proto::LoadStateRequest& request) { default: LOG(FATAL) << "Unhandled proto::LoadStateRequest case."; } + load_frozen_state_ = request.load_frozen_state(); } void LoadStateHandler::OnReadsDone() { auto trajectory_remapping = GetContext()->map_builder().LoadState( - &reader_, true); + &reader_, load_frozen_state_); for (const auto& entry : trajectory_remapping) { GetContext()->RegisterClientIdForTrajectory( client_id_, entry.second); diff --git a/cartographer/cloud/internal/handlers/load_state_handler.h b/cartographer/cloud/internal/handlers/load_state_handler.h index f466ee1..325854c 100644 --- a/cartographer/cloud/internal/handlers/load_state_handler.h +++ b/cartographer/cloud/internal/handlers/load_state_handler.h @@ -38,6 +38,7 @@ class LoadStateHandler : public async_grpc::RpcHandler { private: io::InMemoryProtoStreamReader reader_; std::string client_id_; + bool load_frozen_state_; }; } // namespace handlers diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index 4d62cc8..282c9ee 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/cartographer/cloud/proto/map_builder_service.proto @@ -146,6 +146,7 @@ message LoadStateRequest { cartographer.mapping.proto.SerializationHeader serialization_header = 2; string client_id = 3; } + bool load_frozen_state = 4; } message TrajectoryRemapping {