Handle invalid requests. (#1222)

FIXES=#1221

cartographer/cloud/client/map_builder_stub.cc

@@ -84,7 +84,11 @@ int MapBuilderStub::AddTrajectoryForDeserialization(
 
 mapping::TrajectoryBuilderInterface* MapBuilderStub::GetTrajectoryBuilder(
     int trajectory_id) const {
-  return trajectory_builder_stubs_.at(trajectory_id).get();
+  auto it = trajectory_builder_stubs_.find(trajectory_id);
+  if (it == trajectory_builder_stubs_.end()) {
+    return nullptr;
+  }
+  return it->second.get();
 }
 
 void MapBuilderStub::FinishTrajectory(int trajectory_id) {

cartographer/cloud/internal/client_server_test.cc

@@ -106,6 +106,8 @@ class ClientServerTest : public ::testing::Test {
         mapping::testing::ResolveLuaParameters(kUploadingMapBuilderServerLua);
     uploading_map_builder_server_options_ = CreateMapBuilderServerOptions(
         uploading_map_builder_server_parameters.get());
+    EXPECT_NE(map_builder_server_options_.server_address(),
+              uploading_map_builder_server_options_.server_address());
 
     const std::string kTrajectoryBuilderLua = R"text(
       include "trajectory_builder.lua"
@@ -491,6 +493,56 @@ TEST_F(ClientServerTest, LoadState) {
 
 // TODO(gaschler): Test-cover LoadStateFromFile.
 
+TEST_F(ClientServerTest, LocalSlam2DHandlesInvalidRequests) {
+  InitializeRealServer();
+  server_->Start();
+  InitializeStub();
+  int trajectory_id =
+      stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_,
+                                  local_slam_result_callback_);
+  TrajectoryBuilderInterface* trajectory_stub =
+      stub_->GetTrajectoryBuilder(trajectory_id);
+  const auto measurements = mapping::testing::GenerateFakeRangeMeasurements(
+      kTravelDistance, kDuration, kTimeStep);
+  for (const auto& measurement : measurements) {
+    trajectory_stub->AddSensorData(kRangeSensorId.id, measurement);
+  }
+  WaitForLocalSlamResults(measurements.size());
+  stub_->pose_graph()->RunFinalOptimization();
+
+  const int kInvalidTrajectoryId = 7;
+  stub_->pose_graph()->DeleteTrajectory(kInvalidTrajectoryId);
+  EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(kInvalidTrajectoryId));
+  EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(kInvalidTrajectoryId));
+  EXPECT_EQ(nullptr, stub_->GetTrajectoryBuilder(kInvalidTrajectoryId));
+  stub_->FinishTrajectory(kInvalidTrajectoryId);
+  const mapping::SubmapId kInvalidSubmapId0{kInvalidTrajectoryId, 0},
+      kInvalidSubmapId1{trajectory_id, 424242};
+  mapping::proto::SubmapQuery::Response submap_query_response;
+  // Expect that it returns non-empty error string.
+  EXPECT_NE("",
+            stub_->SubmapToProto(kInvalidSubmapId0, &submap_query_response));
+  EXPECT_NE("",
+            stub_->SubmapToProto(kInvalidSubmapId1, &submap_query_response));
+
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::ACTIVE);
+  auto submap_poses = stub_->pose_graph()->GetAllSubmapPoses();
+  EXPECT_GT(submap_poses.size(), 0);
+  EXPECT_GT(stub_->pose_graph()->GetTrajectoryNodePoses().size(), 0);
+  stub_->FinishTrajectory(trajectory_id);
+  stub_->pose_graph()->DeleteTrajectory(trajectory_id);
+  stub_->pose_graph()->RunFinalOptimization();
+  mapping::SubmapId deleted_submap_id = submap_poses.begin()->id;
+  EXPECT_NE("",
+            stub_->SubmapToProto(deleted_submap_id, &submap_query_response));
+  EXPECT_EQ(stub_->pose_graph()->GetTrajectoryStates().at(trajectory_id),
+            PoseGraphInterface::TrajectoryState::DELETED);
+  // Make sure optimization runs with a deleted trajectory.
+  stub_->pose_graph()->RunFinalOptimization();
+  server_->Shutdown();
+}
+
 }  // namespace
 }  // namespace cloud
 }  // namespace cartographer

cartographer/mapping/id.h

@@ -289,7 +289,7 @@ class MapById {
   void Trim(const IdType& id) {
     auto& trajectory = trajectories_.at(id.trajectory_id);
     const auto it = trajectory.data_.find(GetIndex(id));
-    CHECK(it != trajectory.data_.end());
+    CHECK(it != trajectory.data_.end()) << id;
     if (std::next(it) == trajectory.data_.end()) {
       // We are removing the data with the highest index from this trajectory.
       // We assume that we will never append to it anymore. If we did, we would

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -467,7 +467,13 @@ void PoseGraph2D::WaitForAllComputations() {
 
 void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
-  data_.trajectories_state.at(trajectory_id).deletion_state =
+  auto it = data_.trajectories_state.find(trajectory_id);
+  if (it == data_.trajectories_state.end()) {
+    LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
+                 << trajectory_id;
+    return;
+  }
+  it->second.deletion_state =
       InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
     CHECK(data_.trajectories_state.at(trajectory_id).state !=

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -484,7 +484,13 @@ void PoseGraph3D::WaitForAllComputations() {
 
 void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
-  data_.trajectories_state.at(trajectory_id).deletion_state =
+  auto it = data_.trajectories_state.find(trajectory_id);
+  if (it == data_.trajectories_state.end()) {
+    LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
+                 << trajectory_id;
+    return;
+  }
+  it->second.deletion_state =
       InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
     CHECK(data_.trajectories_state.at(trajectory_id).state !=