Update unfinished submap list on SLAM thread (#1061)

Fixes #1052

cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.cc

@@ -31,14 +31,9 @@ namespace handlers {
 
 void AddLocalSlamResultDataHandler::OnRequest(
     const proto::AddLocalSlamResultDataRequest& request) {
-  auto local_slam_result_data =
-      GetContext<MapBuilderContextInterface>()->ProcessLocalSlamResultData(
-          request.sensor_metadata().sensor_id(),
-          common::FromUniversal(request.local_slam_result_data().timestamp()),
-          request.local_slam_result_data());
   GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
       request.sensor_metadata().trajectory_id(),
-      request.sensor_metadata().sensor_id(), std::move(local_slam_result_data));
+      request.sensor_metadata().sensor_id(), request.local_slam_result_data());
 }
 
 void AddLocalSlamResultDataHandler::OnReadsDone() {

cartographer/cloud/internal/map_builder_context_impl.cc

@@ -24,39 +24,23 @@ namespace cartographer {
 namespace cloud {
 
 template <>
-std::unique_ptr<mapping::LocalSlamResultData>
+void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
-MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
+    int trajectory_id, const std::string& sensor_id,
-    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& local_slam_result_data) {
-    const mapping::proto::LocalSlamResultData& proto) {
+  map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
-  CHECK_GE(proto.submaps().size(), 1);
+      Data{trajectory_id,
-  CHECK(proto.submaps(0).has_submap_2d());
+           common::make_unique<mapping::LocalSlamResult2D>(
-  std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
+               sensor_id, local_slam_result_data, &submap_controller_)}));
-  for (const auto& submap_proto : proto.submaps()) {
-    submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
-  }
-  return common::make_unique<mapping::LocalSlamResult2D>(
-      sensor_id, time,
-      std::make_shared<const mapping::TrajectoryNode::Data>(
-          mapping::FromProto(proto.node_data())),
-      submaps);
 }
 
 template <>
-std::unique_ptr<mapping::LocalSlamResultData>
+void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
-MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
+    int trajectory_id, const std::string& sensor_id,
-    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& local_slam_result_data) {
-    const mapping::proto::LocalSlamResultData& proto) {
+  map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
-  CHECK_GE(proto.submaps().size(), 1);
+      Data{trajectory_id,
-  CHECK(proto.submaps(0).has_submap_3d());
+           common::make_unique<mapping::LocalSlamResult3D>(
-  std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
+               sensor_id, local_slam_result_data, &submap_controller_)}));
-  for (const auto& submap_proto : proto.submaps()) {
-    submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
-  }
-  return common::make_unique<mapping::LocalSlamResult3D>(
-      sensor_id, time,
-      std::make_shared<const mapping::TrajectoryNode::Data>(
-          mapping::FromProto(proto.node_data())),
-      submaps);
 }
 
 }  // namespace cloud

cartographer/cloud/internal/map_builder_context_impl.h

@@ -92,24 +92,14 @@ void MapBuilderContext<SubmapType>::EnqueueSensorData(
       common::make_unique<Data>(Data{trajectory_id, std::move(data)}));
 }
 
-template <class SubmapType>
+template <>
-void MapBuilderContext<SubmapType>::EnqueueLocalSlamResultData(
+void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
     int trajectory_id, const std::string& sensor_id,
-    std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) {
+    const mapping::proto::LocalSlamResultData& local_slam_result_data);
-  map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
-      Data{trajectory_id, std::move(local_slam_result_data)}));
-}
-
 template <>
-std::unique_ptr<mapping::LocalSlamResultData>
+void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
-MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
+    int trajectory_id, const std::string& sensor_id,
-    const std::string& sensor_id, common::Time time,
+    const mapping::proto::LocalSlamResultData& local_slam_result_data);
-    const mapping::proto::LocalSlamResultData& proto);
-template <>
-std::unique_ptr<mapping::LocalSlamResultData>
-MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
-    const std::string& sensor_id, common::Time time,
-    const mapping::proto::LocalSlamResultData& proto);
 
 }  // namespace cloud
 }  // namespace cartographer

cartographer/cloud/internal/map_builder_context_interface.h

@@ -21,6 +21,7 @@
 #include "cartographer/cloud/internal/local_trajectory_uploader.h"
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/sensor/data.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -68,16 +69,12 @@ class MapBuilderContextInterface : public async_grpc::ExecutionContext {
   virtual void UnsubscribeLocalSlamResults(
       const SubscriptionId& subscription_id) = 0;
   virtual void NotifyFinishTrajectory(int trajectory_id) = 0;
-  virtual std::unique_ptr<mapping::LocalSlamResultData>
-  ProcessLocalSlamResultData(
-      const std::string& sensor_id, common::Time time,
-      const mapping::proto::LocalSlamResultData& proto) = 0;
   virtual LocalTrajectoryUploaderInterface* local_trajectory_uploader() = 0;
   virtual void EnqueueSensorData(int trajectory_id,
                                  std::unique_ptr<sensor::Data> data) = 0;
   virtual void EnqueueLocalSlamResultData(
       int trajectory_id, const std::string& sensor_id,
-      std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
+      const mapping::proto::LocalSlamResultData& local_slam_result_data) = 0;
 };
 
 }  // namespace cloud

cartographer/cloud/internal/map_builder_server.h

@@ -21,13 +21,13 @@
 #include "async_grpc/server.h"
 #include "cartographer/cloud/internal/local_trajectory_uploader.h"
 #include "cartographer/cloud/internal/map_builder_context_interface.h"
-#include "cartographer/cloud/internal/submap_controller.h"
 #include "cartographer/cloud/map_builder_server_interface.h"
 #include "cartographer/cloud/proto/map_builder_server_options.pb.h"
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer/mapping/internal/submap_controller.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
@@ -53,21 +53,17 @@ class MapBuilderContext : public MapBuilderContextInterface {
   void UnsubscribeLocalSlamResults(
       const SubscriptionId& subscription_id) override;
   void NotifyFinishTrajectory(int trajectory_id) override;
-  std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
-      const std::string& sensor_id, common::Time time,
-      const mapping::proto::LocalSlamResultData& proto) override;
   LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
-
   void EnqueueSensorData(int trajectory_id,
                          std::unique_ptr<sensor::Data> data) override;
   void EnqueueLocalSlamResultData(int trajectory_id,
                                   const std::string& sensor_id,
-                                  std::unique_ptr<mapping::LocalSlamResultData>
+                                  const mapping::proto::LocalSlamResultData&
                                       local_slam_result_data) override;
 
  private:
   MapBuilderServer* map_builder_server_;
-  SubmapController<SubmapType> submap_controller_;
+  mapping::SubmapController<SubmapType> submap_controller_;
 };
 
 class MapBuilderServer : public MapBuilderServerInterface {

cartographer/cloud/internal/testing/mock_map_builder_context.h

@@ -45,16 +45,6 @@ class MockMapBuilderContext : public MapBuilderContextInterface {
   MOCK_METHOD1(UnsubscribeLocalSlamResults,
                void(const MapBuilderContextInterface::SubscriptionId &));
   MOCK_METHOD1(NotifyFinishTrajectory, void(int));
-  MOCK_METHOD3(DoProcessLocalSlamResultData,
-               mapping::LocalSlamResultData *(
-                   const std::string &, common::Time,
-                   const mapping::proto::LocalSlamResultData &));
-  std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
-      const std::string &sensor_id, common::Time time,
-      const mapping::proto::LocalSlamResultData &proto) override {
-    return std::unique_ptr<mapping::LocalSlamResultData>(
-        DoProcessLocalSlamResultData(sensor_id, time, proto));
-  }
   MOCK_METHOD0(local_trajectory_uploader, LocalTrajectoryUploaderInterface *());
 
   MOCK_METHOD2(DoEnqueueSensorData, void(int, sensor::Data *));
@@ -62,15 +52,9 @@ class MockMapBuilderContext : public MapBuilderContextInterface {
                          std::unique_ptr<sensor::Data> data) override {
     DoEnqueueSensorData(trajectory_id, data.get());
   }
-  MOCK_METHOD3(DoEnqueueLocalSlamResultData,
+  MOCK_METHOD3(EnqueueLocalSlamResultData,
-               void(int, const std::string &, mapping::LocalSlamResultData *));
+               void(int, const std::string &,
-  void EnqueueLocalSlamResultData(int trajectory_id,
+                    const mapping::proto::LocalSlamResultData &));
-                                  const std::string &sensor_id,
-                                  std::unique_ptr<mapping::LocalSlamResultData>
-                                      local_slam_result_data) override {
-    DoEnqueueLocalSlamResultData(trajectory_id, sensor_id,
-                                 local_slam_result_data.get());
-  }
 };
 
 }  // namespace testing

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

@@ -29,8 +29,16 @@ void LocalSlamResult2D::AddToTrajectoryBuilder(
 void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
                                        PoseGraph* pose_graph) const {
   DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph));
+  CHECK_GE(local_slam_result_data_.submaps().size(), 1);
+  CHECK(local_slam_result_data_.submaps(0).has_submap_2d());
+  std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
+  for (const auto& submap_proto : local_slam_result_data_.submaps()) {
+    submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
+  }
   static_cast<PoseGraph2D*>(pose_graph)
-      ->AddNode(node_data_, trajectory_id, insertion_submaps_);
+      ->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
+                    mapping::FromProto(local_slam_result_data_.node_data())),
+                trajectory_id, submaps);
 }
 
 }  // namespace mapping

cartographer/mapping/internal/2d/local_slam_result_2d.h

@@ -17,7 +17,7 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_
 
-#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/internal/submap_controller.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 
@@ -27,20 +27,23 @@ namespace mapping {
 class LocalSlamResult2D : public LocalSlamResultData {
  public:
   LocalSlamResult2D(
-      const std::string& sensor_id, common::Time time,
+      const std::string& sensor_id,
-      std::shared_ptr<const TrajectoryNode::Data> node_data,
+      const mapping::proto::LocalSlamResultData local_slam_result_data,
-      const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
+      SubmapController<mapping::Submap2D>* submap_controller)
-      : LocalSlamResultData(sensor_id, time),
+      : LocalSlamResultData(sensor_id, common::FromUniversal(
-        node_data_(node_data),
+                                           local_slam_result_data.timestamp())),
-        insertion_submaps_(insertion_submaps) {}
+        sensor_id_(sensor_id),
+        local_slam_result_data_(local_slam_result_data),
+        submap_controller_(submap_controller) {}
 
   void AddToTrajectoryBuilder(
       TrajectoryBuilderInterface* const trajectory_builder) override;
   void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
 
  private:
-  std::shared_ptr<const TrajectoryNode::Data> node_data_;
+  const std::string sensor_id_;
-  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps_;
+  const mapping::proto::LocalSlamResultData local_slam_result_data_;
+  SubmapController<mapping::Submap2D>* submap_controller_;
 };
 
 }  // namespace mapping

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

@@ -29,8 +29,16 @@ void LocalSlamResult3D::AddToTrajectoryBuilder(
 void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
                                        PoseGraph* pose_graph) const {
   DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph));
+  CHECK_GE(local_slam_result_data_.submaps().size(), 1);
+  CHECK(local_slam_result_data_.submaps(0).has_submap_3d());
+  std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
+  for (const auto& submap_proto : local_slam_result_data_.submaps()) {
+    submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
+  }
   static_cast<PoseGraph3D*>(pose_graph)
-      ->AddNode(node_data_, trajectory_id, insertion_submaps_);
+      ->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
+                    mapping::FromProto(local_slam_result_data_.node_data())),
+                trajectory_id, submaps);
 }
 
 }  // namespace mapping

cartographer/mapping/internal/3d/local_slam_result_3d.h

@@ -17,7 +17,7 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_
 
-#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer/mapping/internal/submap_controller.h"
 #include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 
@@ -27,20 +27,23 @@ namespace mapping {
 class LocalSlamResult3D : public LocalSlamResultData {
  public:
   LocalSlamResult3D(
-      const std::string& sensor_id, common::Time time,
+      const std::string& sensor_id,
-      std::shared_ptr<const TrajectoryNode::Data> node_data,
+      const mapping::proto::LocalSlamResultData local_slam_result_data,
-      const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
+      SubmapController<mapping::Submap3D>* submap_controller)
-      : LocalSlamResultData(sensor_id, time),
+      : LocalSlamResultData(sensor_id, common::FromUniversal(
-        node_data_(node_data),
+                                           local_slam_result_data.timestamp())),
-        insertion_submaps_(insertion_submaps) {}
+        sensor_id_(sensor_id),
+        local_slam_result_data_(local_slam_result_data),
+        submap_controller_(submap_controller) {}
 
   void AddToTrajectoryBuilder(
       TrajectoryBuilderInterface* const trajectory_builder) override;
   void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
 
  private:
-  std::shared_ptr<const TrajectoryNode::Data> node_data_;
+  const std::string sensor_id_;
-  std::vector<std::shared_ptr<const Submap3D>> insertion_submaps_;
+  const mapping::proto::LocalSlamResultData local_slam_result_data_;
+  SubmapController<mapping::Submap3D>* submap_controller_;
 };
 
 }  // namespace mapping

cartographer/mapping/internal/submap_controller.cc

@@ -14,10 +14,10 @@
  * limitations under the License.
  */
 
-#include "cartographer/cloud/internal/submap_controller.h"
+#include "cartographer/mapping/internal/submap_controller.h"
 
 namespace cartographer {
-namespace cloud {
+namespace mapping {
 
 template <>
 std::shared_ptr<mapping::Submap2D>
@@ -33,5 +33,5 @@ SubmapController<mapping::Submap3D>::CreateSubmap(
   return std::make_shared<mapping::Submap3D>(proto.submap_3d());
 }
 
-}  // namespace cloud
+}  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/submap_controller.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
-#define CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
+#define CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
 
 #include "cartographer/mapping/2d/submap_2d.h"
 #include "cartographer/mapping/3d/submap_3d.h"
@@ -23,7 +23,7 @@
 #include "cartographer/mapping/proto/serialization.pb.h"
 
 namespace cartographer {
-namespace cloud {
+namespace mapping {
 
 template <class SubmapType>
 class SubmapController {
@@ -71,7 +71,7 @@ std::shared_ptr<mapping::Submap3D>
 SubmapController<mapping::Submap3D>::CreateSubmap(
     const mapping::proto::Submap& proto);
 
-}  // namespace cloud
+}  // namespace mapping
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H