Support gRPC requests for loading unfrozen states. (#1379)

master
Michael Grupp 2018-08-29 10:55:55 +02:00 committed by Christoph Schütte
parent 81b75da9f4
commit 4c2104473c
5 changed files with 55 additions and 6 deletions

View File

@ -150,9 +150,6 @@ void MapBuilderStub::SerializeState(bool include_unfinished_submaps,
std::map<int, int> MapBuilderStub::LoadState(
io::ProtoStreamReaderInterface* reader, const bool load_frozen_state) {
if (!load_frozen_state) {
LOG(FATAL) << "Not implemented";
}
async_grpc::Client<handlers::LoadStateSignature> client(client_channel_);
{
proto::LoadStateRequest request;
@ -165,6 +162,7 @@ std::map<int, int> 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<int, int> 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<int, int> 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));
}

View File

@ -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) {

View File

@ -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<MapBuilderContextInterface>()->map_builder().LoadState(
&reader_, true);
&reader_, load_frozen_state_);
for (const auto& entry : trajectory_remapping) {
GetContext<MapBuilderContextInterface>()->RegisterClientIdForTrajectory(
client_id_, entry.second);

View File

@ -38,6 +38,7 @@ class LoadStateHandler : public async_grpc::RpcHandler<LoadStateSignature> {
private:
io::InMemoryProtoStreamReader reader_;
std::string client_id_;
bool load_frozen_state_;
};
} // namespace handlers

View File

@ -146,6 +146,7 @@ message LoadStateRequest {
cartographer.mapping.proto.SerializationHeader serialization_header = 2;
string client_id = 3;
}
bool load_frozen_state = 4;
}
message TrajectoryRemapping {