From dafb4149ed197d231cabd89cdf023e56ad207b70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Fri, 27 Jul 2018 17:20:42 +0200 Subject: [PATCH] Revert: Fix writing state of trajectories with unfinished submaps. #1286 (#1346) Reason: Break rviz visualization for submaps loaded from pbstreams. --- .../cloud/internal/client_server_test.cc | 20 ------------------- .../internal/mapping_state_serialization.cc | 3 +-- .../mapping/internal/2d/pose_graph_2d.cc | 15 ++++++-------- .../mapping/internal/3d/pose_graph_3d.cc | 15 ++++++-------- .../mapping/internal/testing/test_helpers.cc | 1 - cartographer/mapping/map_builder_test.cc | 2 +- 6 files changed, 14 insertions(+), 42 deletions(-) diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 05f54a3..3592b5a 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -452,26 +452,6 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) { } WaitForLocalSlamResults(measurements.size()); WaitForLocalSlamResultUploads(number_of_insertion_results_); - - std::queue> chunks; - io::ForwardingProtoStreamWriter writer( - [&chunks](const google::protobuf::Message* proto) -> bool { - if (!proto) { - return true; - } - std::unique_ptr p(proto->New()); - p->CopyFrom(*proto); - chunks.push(std::move(p)); - return true; - }); - stub_->SerializeState(&writer); - CHECK(writer.Close()); - - // Ensure it can be read. - io::InMemoryProtoStreamReader reader(std::move(chunks)); - io::ProtoStreamDeserializer deserializer(&reader); - EXPECT_EQ(deserializer.pose_graph().trajectory_size(), 1); - stub_for_uploading_server_->FinishTrajectory(trajectory_id); EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); EXPECT_NEAR(kTravelDistance, diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index e6dd101..88d24a7 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -85,8 +85,7 @@ void SerializeSubmaps( SerializedData proto; auto* const submap_proto = proto.mutable_submap(); *submap_proto = submap_id_data.data.submap->ToProto( - /*include_probability_grid_data=*/submap_id_data.data.submap - ->finished()); + /*include_probability_grid_data=*/true); submap_proto->mutable_submap_id()->set_trajectory_id( submap_id_data.id.trajectory_id); submap_proto->mutable_submap_id()->set_submap_index( diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 7b4d0ec..8f9cc9e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -636,15 +636,12 @@ void PoseGraph2D::AddSubmapFromProto( data_.global_submap_poses_2d.Insert( submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); } - const bool finished = submap.submap_2d().finished(); - AddWorkItem( - [this, submap_id, global_submap_pose_2d, finished]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); - data_.submap_data.at(submap_id).state = - finished ? SubmapState::kFinished : SubmapState::kActive; - optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); - return WorkItem::Result::kDoNotRunOptimization; - }); + AddWorkItem([this, submap_id, global_submap_pose_2d]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + data_.submap_data.at(submap_id).state = SubmapState::kFinished; + optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 9d43f96..1b77c65 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -648,15 +648,12 @@ void PoseGraph3D::AddSubmapFromProto( data_.global_submap_poses_3d.Insert( submap_id, optimization::SubmapSpec3D{global_submap_pose}); } - bool finished = submap.submap_3d().finished(); - AddWorkItem( - [this, submap_id, global_submap_pose, finished]() EXCLUDES(mutex_) { - common::MutexLocker locker(&mutex_); - data_.submap_data.at(submap_id).state = - finished ? SubmapState::kFinished : SubmapState::kActive; - optimization_problem_->InsertSubmap(submap_id, global_submap_pose); - return WorkItem::Result::kDoNotRunOptimization; - }); + AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + data_.submap_data.at(submap_id).state = SubmapState::kFinished; + optimization_problem_->InsertSubmap(submap_id, global_submap_pose); + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index d5413ab..18389be 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/cartographer/mapping/internal/testing/test_helpers.cc @@ -85,7 +85,6 @@ proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) { proto.mutable_submap_3d()->set_num_range_data(1); *proto.mutable_submap_3d()->mutable_local_pose() = transform::ToProto(transform::Rigid3d::Identity()); - proto.mutable_submap_3d()->set_finished(true); return proto; } diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index cf3dc10..6bf9190 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -417,7 +417,7 @@ TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) { ++num_cross_trajectory_constraints; } } - EXPECT_EQ(num_cross_trajectory_constraints, 3); + EXPECT_GT(num_cross_trajectory_constraints, 3); // TODO(gaschler): Subscribe global slam callback, verify that all nodes are // optimized. EXPECT_THAT(constraints, ::testing::Contains(::testing::Field(