From 22ea782fad57caed372fd80bc56b4d658bf85977 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 2 Jul 2018 13:05:28 +0200 Subject: [PATCH] Handle invalid requests. (#1222) FIXES=#1221 --- cartographer/cloud/client/map_builder_stub.cc | 6 ++- .../cloud/internal/client_server_test.cc | 52 +++++++++++++++++++ cartographer/mapping/id.h | 2 +- .../mapping/internal/2d/pose_graph_2d.cc | 8 ++- .../mapping/internal/3d/pose_graph_3d.cc | 8 ++- 5 files changed, 72 insertions(+), 4 deletions(-) diff --git a/cartographer/cloud/client/map_builder_stub.cc b/cartographer/cloud/client/map_builder_stub.cc index 85562a6..8ac4d8e 100644 --- a/cartographer/cloud/client/map_builder_stub.cc +++ b/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) { diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 697398b..1b43183 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/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 diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 49662c2..504e3d0 100644 --- a/cartographer/mapping/id.h +++ b/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 diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 505805d..84faa89 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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 != diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 8d15378..4fb1649 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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 !=