Add LocalSlamResultData retrieval and submap management (#810)

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)

cartographer/mapping/local_slam_result_data.cc

@@ -24,9 +24,14 @@ LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id,
                                          common::Time time)
     : Data(sensor_id), time_(time) {}
 
-LocalSlamResult2D::LocalSlamResult2D(const std::string &sensor_id,
+LocalSlamResult2D::LocalSlamResult2D(
-                                     common::Time time)
+    const std::string &sensor_id, common::Time time,
-    : LocalSlamResultData(sensor_id, time) {}
+    std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
+    const std::vector<std::shared_ptr<const mapping_2d::Submap>>
+        &insertion_submaps)
+    : LocalSlamResultData(sensor_id, time),
+      node_data_(node_data),
+      insertion_submaps_(insertion_submaps) {}
 
 void LocalSlamResult2D::AddToTrajectoryBuilder(
     mapping::TrajectoryBuilderInterface *const trajectory_builder) {
@@ -39,12 +44,17 @@ void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
   DCHECK(dynamic_cast<mapping_2d::PoseGraph *>(pose_graph));
   mapping_2d::PoseGraph *pose_graph_2d =
       static_cast<mapping_2d::PoseGraph *>(pose_graph);
-  pose_graph_2d->AddNode(constant_data, trajectory_id, insertion_submaps);
+  pose_graph_2d->AddNode(node_data_, trajectory_id, insertion_submaps_);
 }
 
-LocalSlamResult3D::LocalSlamResult3D(const std::string &sensor_id,
+LocalSlamResult3D::LocalSlamResult3D(
-                                     common::Time time)
+    const std::string &sensor_id, common::Time time,
-    : LocalSlamResultData(sensor_id, time) {}
+    std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
+    const std::vector<std::shared_ptr<const mapping_3d::Submap>>
+        &insertion_submaps)
+    : LocalSlamResultData(sensor_id, time),
+      node_data_(node_data),
+      insertion_submaps_(insertion_submaps) {}
 
 void LocalSlamResult3D::AddToTrajectoryBuilder(
     mapping::TrajectoryBuilderInterface *const trajectory_builder) {
@@ -57,7 +67,7 @@ void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
   DCHECK(dynamic_cast<mapping_3d::PoseGraph *>(pose_graph));
   mapping_3d::PoseGraph *pose_graph_3d =
       static_cast<mapping_3d::PoseGraph *>(pose_graph);
-  pose_graph_3d->AddNode(constant_data, trajectory_id, insertion_submaps);
+  pose_graph_3d->AddNode(node_data_, trajectory_id, insertion_submaps_);
 }
 
 }  // namespace mapping

cartographer/mapping/local_slam_result_data.h

@@ -39,7 +39,11 @@ class LocalSlamResultData : public cartographer::sensor::Data {
 
 class LocalSlamResult2D : public LocalSlamResultData {
  public:
-  LocalSlamResult2D(const std::string &sensor_id, common::Time time);
+  LocalSlamResult2D(
+      const std::string &sensor_id, common::Time time,
+      std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
+      const std::vector<std::shared_ptr<const mapping_2d::Submap>>
+          &insertion_submaps);
 
   void AddToTrajectoryBuilder(
       mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
@@ -47,13 +51,17 @@ class LocalSlamResult2D : public LocalSlamResultData {
                       mapping::PoseGraph *pose_graph) const override;
 
  private:
-  std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
+  std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;
-  std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps;
+  std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps_;
 };
 
 class LocalSlamResult3D : public LocalSlamResultData {
  public:
-  LocalSlamResult3D(const std::string &sensor_id, common::Time time);
+  LocalSlamResult3D(
+      const std::string &sensor_id, common::Time time,
+      std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
+      const std::vector<std::shared_ptr<const mapping_3d::Submap>>
+          &insertion_submaps);
 
   void AddToTrajectoryBuilder(
       mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
@@ -61,8 +69,8 @@ class LocalSlamResult3D : public LocalSlamResultData {
                       mapping::PoseGraph *pose_graph) const override;
 
  private:
-  std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
+  std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;
-  std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps;
+  std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps_;
 };
 
 }  // namespace mapping

cartographer/mapping/proto/serialization.proto

@@ -54,3 +54,9 @@ message SerializedData {
   OdometryData odometry_data = 4;
   FixedFramePoseData fixed_frame_pose_data = 5;
 }
+
+message LocalSlamResultData {
+  int64 timestamp = 1;
+  TrajectoryNodeData node_data = 2;
+  repeated Submap submaps = 3;
+}

cartographer/mapping/submaps.h

@@ -64,6 +64,7 @@ class Submap {
   virtual ~Submap() {}
 
   virtual void ToProto(proto::Submap* proto) const = 0;
+  virtual void UpdateFromProto(const proto::Submap& proto) = 0;
 
   // Pose of this submap in the local map frame.
   transform::Rigid3d local_pose() const { return local_pose_; }
@@ -76,7 +77,6 @@ class Submap {
       const transform::Rigid3d& global_submap_pose,
       proto::SubmapQuery::Response* response) const = 0;
 
- protected:
   void SetNumRangeData(const int num_range_data) {
     num_range_data_ = num_range_data;
   }

cartographer/mapping_2d/submaps.cc

@@ -82,6 +82,16 @@ void Submap::ToProto(mapping::proto::Submap* const proto) const {
   *submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
 }
 
+void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
+  CHECK(proto.has_submap_2d());
+  const auto& submap_2d = proto.submap_2d();
+  SetNumRangeData(submap_2d.num_range_data());
+  finished_ = submap_2d.finished();
+  if (submap_2d.has_probability_grid()) {
+    probability_grid_ = ProbabilityGrid(submap_2d.probability_grid());
+  }
+}
+
 void Submap::ToResponseProto(
     const transform::Rigid3d&,
     mapping::proto::SubmapQuery::Response* const response) const {

cartographer/mapping_2d/submaps.h

@@ -48,6 +48,7 @@ class Submap : public mapping::Submap {
   explicit Submap(const mapping::proto::Submap2D& proto);
 
   void ToProto(mapping::proto::Submap* proto) const override;
+  void UpdateFromProto(const mapping::proto::Submap& proto) override;
 
   const ProbabilityGrid& probability_grid() const { return probability_grid_; }
   bool finished() const { return finished_; }

cartographer/mapping_3d/submaps.cc

@@ -226,6 +226,27 @@ void Submap::ToProto(mapping::proto::Submap* const proto) const {
       low_resolution_hybrid_grid().ToProto();
 }
 
+void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
+  CHECK(proto.has_submap_3d());
+  const auto& submap_3d = proto.submap_3d();
+  SetNumRangeData(submap_3d.num_range_data());
+  finished_ = submap_3d.finished();
+  if (submap_3d.has_high_resolution_hybrid_grid()) {
+    high_resolution_hybrid_grid_ =
+        submap_3d.has_high_resolution_hybrid_grid()
+            ? common::make_unique<HybridGrid>(
+                  submap_3d.high_resolution_hybrid_grid())
+            : nullptr;
+  }
+  if (submap_3d.has_low_resolution_hybrid_grid()) {
+    low_resolution_hybrid_grid_ =
+        submap_3d.has_low_resolution_hybrid_grid()
+            ? common::make_unique<HybridGrid>(
+                  submap_3d.low_resolution_hybrid_grid())
+            : nullptr;
+  }
+}
+
 void Submap::ToResponseProto(
     const transform::Rigid3d& global_submap_pose,
     mapping::proto::SubmapQuery::Response* const response) const {

cartographer/mapping_3d/submaps.h

@@ -48,6 +48,7 @@ class Submap : public mapping::Submap {
   explicit Submap(const mapping::proto::Submap3D& proto);
 
   void ToProto(mapping::proto::Submap* proto) const override;
+  void UpdateFromProto(const mapping::proto::Submap& proto) override;
 
   const HybridGrid& high_resolution_hybrid_grid() const {
     return *high_resolution_hybrid_grid_;

cartographer_grpc/handlers/add_local_slam_result_data_handler.h

@@ -0,0 +1,57 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
+#define CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/mapping/trajectory_node.h"
+#include "cartographer_grpc/framework/rpc_handler.h"
+#include "cartographer_grpc/map_builder_server.h"
+#include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "google/protobuf/empty.pb.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+
+class AddLocalSlamResultDataHandler
+    : public framework::RpcHandler<
+          framework::Stream<proto::AddLocalSlamResultDataRequest>,
+          google::protobuf::Empty> {
+ public:
+  void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override {
+    auto local_slam_result_data =
+        GetContext<MapBuilderServer::MapBuilderContext>()
+            ->ProcessLocalSlamResultData(
+                request.sensor_metadata().sensor_id(),
+                cartographer::common::FromUniversal(
+                    request.local_slam_result_data().timestamp()),
+                request.local_slam_result_data());
+    GetContext<MapBuilderServer::MapBuilderContext>()
+        ->EnqueueLocalSlamResultData(request.sensor_metadata().trajectory_id(),
+                                     request.sensor_metadata().sensor_id(),
+                                     std::move(local_slam_result_data));
+  }
+
+  void OnReadsDone() override {
+    Send(cartographer::common::make_unique<google::protobuf::Empty>());
+  }
+};
+
+}  // namespace handlers
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H

cartographer_grpc/map_builder_server.cc

@@ -18,6 +18,7 @@
 
 #include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
 #include "cartographer_grpc/handlers/add_imu_data_handler.h"
+#include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
 #include "cartographer_grpc/handlers/add_odometry_data_handler.h"
 #include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
 #include "cartographer_grpc/handlers/add_trajectory_handler.h"
@@ -48,10 +49,9 @@ MapBuilderServer::MapBuilderContext::map_builder() {
   return *map_builder_server_->map_builder_;
 }
 
-cartographer::common::BlockingQueue<
+cartographer::common::BlockingQueue<std::unique_ptr<MapBuilderServer::Data>>&
-    std::unique_ptr<MapBuilderServer::SensorData>>&
 MapBuilderServer::MapBuilderContext::sensor_data_queue() {
-  return map_builder_server_->sensor_data_queue_;
+  return map_builder_server_->incoming_data_queue_;
 }
 
 cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
@@ -72,8 +72,8 @@ MapBuilderServer::MapBuilderContext::
 }
 
 void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
-    const SensorData& sensor_data) {
+    const Data& sensor_data) {
-  sensor_data.sensor_data->AddToTrajectoryBuilder(
+  sensor_data.data->AddToTrajectoryBuilder(
       map_builder_server_->map_builder_->GetTrajectoryBuilder(
           sensor_data.trajectory_id));
 }
@@ -95,6 +95,108 @@ void MapBuilderServer::MapBuilderContext::NotifyFinishTrajectory(
   map_builder_server_->NotifyFinishTrajectory(trajectory_id);
 }
 
+std::shared_ptr<cartographer::mapping_2d::Submap>
+MapBuilderServer::MapBuilderContext::UpdateSubmap2D(
+    const cartographer::mapping::proto::Submap& proto) {
+  CHECK(proto.has_submap_2d());
+  cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
+                                            proto.submap_id().submap_index()};
+  std::shared_ptr<cartographer::mapping_2d::Submap> submap_2d_ptr;
+  auto submap_it = unfinished_submaps_.find(submap_id);
+  if (submap_it == unfinished_submaps_.end()) {
+    // Seeing a submap for the first time it should never be finished.
+    CHECK(!proto.submap_2d().finished());
+    submap_2d_ptr =
+        std::make_shared<cartographer::mapping_2d::Submap>(proto.submap_2d());
+    unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
+  } else {
+    submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping_2d::Submap>(
+        submap_it->data);
+    CHECK(submap_2d_ptr);
+    submap_2d_ptr->UpdateFromProto(proto);
+
+    // If the submap was just finished by the recent update, remove it from the
+    // list of unfinished submaps.
+    if (submap_2d_ptr->finished()) {
+      unfinished_submaps_.Trim(submap_id);
+    } else {
+      // If the submap is unfinished set the 'num_range_data' to 0 since we
+      // haven't changed the HybridGrid.
+      submap_2d_ptr->SetNumRangeData(0);
+    }
+  }
+  return submap_2d_ptr;
+}
+
+std::shared_ptr<cartographer::mapping_3d::Submap>
+MapBuilderServer::MapBuilderContext::UpdateSubmap3D(
+    const cartographer::mapping::proto::Submap& proto) {
+  CHECK(proto.has_submap_3d());
+  cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
+                                            proto.submap_id().submap_index()};
+  std::shared_ptr<cartographer::mapping_3d::Submap> submap_3d_ptr;
+  auto submap_it = unfinished_submaps_.find(submap_id);
+  if (submap_it == unfinished_submaps_.end()) {
+    // Seeing a submap for the first time it should never be finished.
+    CHECK(!proto.submap_3d().finished());
+    submap_3d_ptr =
+        std::make_shared<cartographer::mapping_3d::Submap>(proto.submap_3d());
+    unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
+    submap_it = unfinished_submaps_.find(submap_id);
+  } else {
+    submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping_3d::Submap>(
+        submap_it->data);
+    CHECK(submap_3d_ptr);
+
+    // Update submap with information in incoming request.
+    submap_3d_ptr->UpdateFromProto(proto);
+
+    // If the submap was just finished by the recent update, remove it from the
+    // list of unfinished submaps.
+    if (submap_3d_ptr->finished()) {
+      unfinished_submaps_.Trim(submap_id);
+    } else {
+      // If the submap is unfinished set the 'num_range_data' to 0 since we
+      // haven't changed the HybridGrid.
+      submap_3d_ptr->SetNumRangeData(0);
+    }
+  }
+  return submap_3d_ptr;
+}
+
+std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+MapBuilderServer::MapBuilderContext::ProcessLocalSlamResultData(
+    const std::string& sensor_id, cartographer::common::Time time,
+    const cartographer::mapping::proto::LocalSlamResultData& proto) {
+  CHECK_GE(proto.submaps().size(), 0);
+  CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
+  if (proto.submaps(0).has_submap_2d()) {
+    std::vector<std::shared_ptr<const cartographer::mapping_2d::Submap>>
+        submaps;
+    for (const auto& submap_proto : proto.submaps()) {
+      submaps.push_back(UpdateSubmap2D(submap_proto));
+    }
+    return cartographer::common::make_unique<
+        cartographer::mapping::LocalSlamResult2D>(
+        sensor_id, time,
+        std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
+            cartographer::mapping::FromProto(proto.node_data())),
+        submaps);
+  } else {
+    std::vector<std::shared_ptr<const cartographer::mapping_3d::Submap>>
+        submaps;
+    for (const auto& submap_proto : proto.submaps()) {
+      submaps.push_back(UpdateSubmap3D(submap_proto));
+    }
+    return cartographer::common::make_unique<
+        cartographer::mapping::LocalSlamResult3D>(
+        sensor_id, time,
+        std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
+            cartographer::mapping::FromProto(proto.node_data())),
+        std::move(submaps));
+  }
+}
+
 MapBuilderServer::MapBuilderServer(
     const proto::MapBuilderServerOptions& map_builder_server_options,
     std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
@@ -118,6 +220,9 @@ MapBuilderServer::MapBuilderServer(
   server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
                                  proto::MapBuilderService>(
       "AddFixedFramePoseData");
+  server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler,
+                                 proto::MapBuilderService>(
+      "AddLocalSlamResultData");
   server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
                                  proto::MapBuilderService>("FinishTrajectory");
   server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler,
@@ -165,8 +270,8 @@ void MapBuilderServer::Shutdown() {
 void MapBuilderServer::ProcessSensorDataQueue() {
   LOG(INFO) << "Starting SLAM thread.";
   while (!shutting_down_) {
-    std::unique_ptr<SensorData> sensor_data =
+    std::unique_ptr<Data> sensor_data =
-        sensor_data_queue_.PopWithTimeout(kPopTimeout);
+        incoming_data_queue_.PopWithTimeout(kPopTimeout);
     if (sensor_data) {
       grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
           *sensor_data);
@@ -232,7 +337,7 @@ void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) {
 }
 
 void MapBuilderServer::WaitUntilIdle() {
-  sensor_data_queue_.WaitUntilEmpty();
+  incoming_data_queue_.WaitUntilEmpty();
   map_builder_->pose_graph()->RunFinalOptimization();
 }
 

cartographer_grpc/map_builder_server.h

@@ -19,6 +19,7 @@
 
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/common/time.h"
+#include "cartographer/mapping/local_slam_result_data.h"
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/sensor/dispatchable.h"
@@ -42,9 +43,9 @@ class MapBuilderServer {
   // Calling with 'nullptr' signals subscribers that the subscription has ended.
   using LocalSlamSubscriptionCallback =
       std::function<void(std::unique_ptr<LocalSlamResult>)>;
-  struct SensorData {
+  struct Data {
     int trajectory_id;
-    std::unique_ptr<cartographer::sensor::Data> sensor_data;
+    std::unique_ptr<cartographer::sensor::Data> data;
   };
   struct SubscriptionId {
     const int trajectory_id;
@@ -55,28 +56,51 @@ class MapBuilderServer {
    public:
     MapBuilderContext(MapBuilderServer* map_builder_server);
     cartographer::mapping::MapBuilderInterface& map_builder();
-    cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
+    cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
     sensor_data_queue();
     cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
     GetLocalSlamResultCallbackForSubscriptions();
-    void AddSensorDataToTrajectory(const SensorData& sensor_data);
+    void AddSensorDataToTrajectory(const Data& sensor_data);
     SubscriptionId SubscribeLocalSlamResults(
         int trajectory_id, LocalSlamSubscriptionCallback callback);
     void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
     void NotifyFinishTrajectory(int trajectory_id);
+    std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+    ProcessLocalSlamResultData(
+        const std::string& sensor_id, cartographer::common::Time time,
+        const cartographer::mapping::proto::LocalSlamResultData& proto);
 
-    template <typename SensorDataType>
+    template <typename DataType>
     void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
-                           const SensorDataType& sensor_data) {
+                           const DataType& data) {
-      map_builder_server_->sensor_data_queue_.Push(
+      map_builder_server_->incoming_data_queue_.Push(
-          cartographer::common::make_unique<MapBuilderServer::SensorData>(
+          cartographer::common::make_unique<MapBuilderServer::Data>(
-              MapBuilderServer::SensorData{
+              MapBuilderServer::Data{
-                  trajectory_id, cartographer::sensor::MakeDispatchable(
+                  trajectory_id,
-                                     sensor_id, sensor_data)}));
+                  cartographer::sensor::MakeDispatchable(sensor_id, data)}));
+    }
+
+    void EnqueueLocalSlamResultData(
+        int trajectory_id, const std::string& sensor_id,
+        std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+            local_slam_result_data) {
+      map_builder_server_->incoming_data_queue_.Push(
+          cartographer::common::make_unique<MapBuilderServer::Data>(
+              MapBuilderServer::Data{trajectory_id,
+                                     std::move(local_slam_result_data)}));
     }
 
    private:
+    std::shared_ptr<cartographer::mapping_2d::Submap> UpdateSubmap2D(
+        const cartographer::mapping::proto::Submap& proto);
+    std::shared_ptr<cartographer::mapping_3d::Submap> UpdateSubmap3D(
+        const cartographer::mapping::proto::Submap& proto);
+
     MapBuilderServer* map_builder_server_;
+    cartographer::mapping::MapById<
+        cartographer::mapping::SubmapId,
+        std::shared_ptr<cartographer::mapping::Submap>>
+        unfinished_submaps_;
   };
   friend MapBuilderContext;
 
@@ -120,8 +144,8 @@ class MapBuilderServer {
   std::unique_ptr<std::thread> slam_thread_;
   std::unique_ptr<framework::Server> grpc_server_;
   std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
-  cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>
+  cartographer::common::BlockingQueue<std::unique_ptr<Data>>
-      sensor_data_queue_;
+      incoming_data_queue_;
   cartographer::common::Mutex local_slam_subscriptions_lock_;
   int current_subscription_index_ = 0;
   std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>

cartographer_grpc/proto/map_builder_service.proto

@@ -15,6 +15,7 @@
 syntax = "proto3";
 
 import "cartographer/mapping/proto/pose_graph.proto";
+import "cartographer/mapping/proto/serialization.proto";
 import "cartographer/mapping/proto/submap_visualization.proto";
 import "cartographer/mapping/proto/trajectory_builder_options.proto";
 import "cartographer/sensor/proto/sensor.proto";
@@ -119,6 +120,11 @@ message GetConstraintsResponse {
   repeated cartographer.mapping.proto.PoseGraph.Constraint constraints = 1;
 }
 
+message AddLocalSlamResultDataRequest {
+  SensorMetadata sensor_metadata = 1;
+  cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
+}
+
 service MapBuilderService {
   // Starts a new trajectory and returns its index.
   rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@@ -139,6 +145,10 @@ service MapBuilderService {
   rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
       returns (google.protobuf.Empty);
 
+  // Adds a local SLAM result.
+  rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
+      returns (google.protobuf.Empty);
+
   // Requests the server to send a stream of local SLAM results for the given
   // 'trajectory_id'.
   rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest)