Revert: Fix writing state of trajectories with unfinished submaps. #1286

(#1346) Reason: Break rviz visualization for submaps loaded from pbstreams.
cartographer-project · Jul 27, 2018 · dafb414 · dafb414
1 parent 3af5086
commit dafb414
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 42 deletions.
diff --git 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<std::unique_ptr<google::protobuf::Message>> chunks;
-  io::ForwardingProtoStreamWriter writer(
-      [&chunks](const google::protobuf::Message* proto) -> bool {
-        if (!proto) {
-          return true;
-        }
-        std::unique_ptr<google::protobuf::Message> 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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(