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

cartographer-project · Aug 29, 2018 · 4c21044 · 4c21044
1 parent 81b75da
commit 4c21044
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 6 deletions.
diff --git 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<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,13 +162,15 @@ 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.
   {
     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));
   }
 

diff --git 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
@@ -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);

diff --git 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<LoadStateSignature> {
  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
@@ -146,6 +146,7 @@ message LoadStateRequest {
     cartographer.mapping.proto.SerializationHeader serialization_header = 2;
     string client_id = 3;
   }
+  bool load_frozen_state = 4;
 }
 
 message TrajectoryRemapping {