Introduce MapBuilderContextInterface (#869)

cartographer_grpc/framework/execution_context.h

@@ -49,7 +49,10 @@ class ExecutionContext {
     cartographer::common::MutexLocker locker_;
     ExecutionContext* execution_context_;
   };
+  ExecutionContext() = default;
   virtual ~ExecutionContext() = default;
+  ExecutionContext(const ExecutionContext&) = delete;
+  ExecutionContext& operator=(const ExecutionContext&) = delete;
   cartographer::common::Mutex* lock() { return &lock_; }
 
  private:

cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h

@@ -39,16 +39,16 @@ class AddFixedFramePoseDataHandler
     // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
     // thread-safe. Therefore it suffices to get an unsynchronized reference to
     // the 'MapBuilderContext'.
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
-        ->EnqueueSensorData(
+        request.sensor_metadata().trajectory_id(),
-            request.sensor_metadata().trajectory_id(),
+        cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
-            cartographer::sensor::FromProto(request.fixed_frame_pose_data()));
+            cartographer::sensor::FromProto(request.fixed_frame_pose_data())));
 
     // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
     // Therefore it suffices to get an unsynchronized reference to the
     // 'MapBuilderContext'.
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    if (GetUnsynchronizedContext<MapBuilderContext>()
             ->local_trajectory_uploader()) {
       auto data_request = cartographer::common::make_unique<
           proto::AddFixedFramePoseDataRequest>();
@@ -56,7 +56,7 @@ class AddFixedFramePoseDataHandler
           request.sensor_metadata().sensor_id(),
           request.sensor_metadata().trajectory_id(),
           request.fixed_frame_pose_data(), data_request.get());
-      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->EnqueueDataRequest(std::move(data_request));
     }

cartographer_grpc/handlers/add_imu_data_handler.h

@@ -38,23 +38,23 @@ class AddImuDataHandler
     // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
     // thread-safe. Therefore it suffices to get an unsynchronized reference to
     // the 'MapBuilderContext'.
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
-        ->EnqueueSensorData(
+        request.sensor_metadata().trajectory_id(),
-            request.sensor_metadata().trajectory_id(),
+        cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
-            cartographer::sensor::FromProto(request.imu_data()));
+            cartographer::sensor::FromProto(request.imu_data())));
 
     // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
     // Therefore it suffices to get an unsynchronized reference to the
     // 'MapBuilderContext'.
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    if (GetUnsynchronizedContext<MapBuilderContext>()
             ->local_trajectory_uploader()) {
       auto data_request =
           cartographer::common::make_unique<proto::AddImuDataRequest>();
       sensor::CreateAddImuDataRequest(request.sensor_metadata().sensor_id(),
                                       request.sensor_metadata().trajectory_id(),
                                       request.imu_data(), data_request.get());
-      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->EnqueueDataRequest(std::move(data_request));
     }

cartographer_grpc/handlers/add_landmark_data_handler.h

@@ -38,16 +38,16 @@ class AddLandmarkDataHandler
     // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
     // thread-safe. Therefore it suffices to get an unsynchronized reference to
     // the 'MapBuilderContext'.
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
-        ->EnqueueSensorData(
+        request.sensor_metadata().trajectory_id(),
-            request.sensor_metadata().trajectory_id(),
+        cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
-            cartographer::sensor::FromProto(request.landmark_data()));
+            cartographer::sensor::FromProto(request.landmark_data())));
 
     // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
     // Therefore it suffices to get an unsynchronized reference to the
     // 'MapBuilderContext'.
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    if (GetUnsynchronizedContext<MapBuilderContext>()
             ->local_trajectory_uploader()) {
       auto data_request =
           cartographer::common::make_unique<proto::AddLandmarkDataRequest>();
@@ -55,7 +55,7 @@ class AddLandmarkDataHandler
           request.sensor_metadata().sensor_id(),
           request.sensor_metadata().trajectory_id(), request.landmark_data(),
           data_request.get());
-      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->EnqueueDataRequest(std::move(data_request));
     }

cartographer_grpc/handlers/add_local_slam_result_data_handler.h

@@ -37,16 +37,15 @@ class AddLocalSlamResultDataHandler
   }
   void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override {
     auto local_slam_result_data =
-        GetContext<MapBuilderServer::MapBuilderContext>()
+        GetContext<MapBuilderContext>()->ProcessLocalSlamResultData(
-            ->ProcessLocalSlamResultData(
+            request.sensor_metadata().sensor_id(),
-                request.sensor_metadata().sensor_id(),
+            cartographer::common::FromUniversal(
-                cartographer::common::FromUniversal(
+                request.local_slam_result_data().timestamp()),
-                    request.local_slam_result_data().timestamp()),
+            request.local_slam_result_data());
-                request.local_slam_result_data());
+    GetContext<MapBuilderContext>()->EnqueueLocalSlamResultData(
-    GetContext<MapBuilderServer::MapBuilderContext>()
+        request.sensor_metadata().trajectory_id(),
-        ->EnqueueLocalSlamResultData(request.sensor_metadata().trajectory_id(),
+        request.sensor_metadata().sensor_id(),
-                                     request.sensor_metadata().sensor_id(),
+        std::move(local_slam_result_data));
-                                     std::move(local_slam_result_data));
   }
 
   void OnReadsDone() override {

cartographer_grpc/handlers/add_odometry_data_handler.h

@@ -38,16 +38,16 @@ class AddOdometryDataHandler
     // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
     // thread-safe. Therefore it suffices to get an unsynchronized reference to
     // the 'MapBuilderContext'.
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
-        ->EnqueueSensorData(
+        request.sensor_metadata().trajectory_id(),
-            request.sensor_metadata().trajectory_id(),
+        cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
-            cartographer::sensor::FromProto(request.odometry_data()));
+            cartographer::sensor::FromProto(request.odometry_data())));
 
     // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
     // Therefore it suffices to get an unsynchronized reference to the
     // 'MapBuilderContext'.
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    if (GetUnsynchronizedContext<MapBuilderContext>()
             ->local_trajectory_uploader()) {
       auto data_request =
           cartographer::common::make_unique<proto::AddOdometryDataRequest>();
@@ -55,7 +55,7 @@ class AddOdometryDataHandler
           request.sensor_metadata().sensor_id(),
           request.sensor_metadata().trajectory_id(), request.odometry_data(),
           data_request.get());
-      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->EnqueueDataRequest(std::move(data_request));
     }

cartographer_grpc/handlers/add_rangefinder_data_handler.h

@@ -38,11 +38,11 @@ class AddRangefinderDataHandler
     // The 'BlockingQueue' returned by 'sensor_data_queue()' is already
     // thread-safe. Therefore it suffices to get an unsynchronized reference to
     // the 'MapBuilderContext'.
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+    GetUnsynchronizedContext<MapBuilderContext>()->EnqueueSensorData(
-        ->EnqueueSensorData(
+        request.sensor_metadata().trajectory_id(),
-            request.sensor_metadata().trajectory_id(),
+        cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
-            cartographer::sensor::FromProto(request.timed_point_cloud_data()));
+            cartographer::sensor::FromProto(request.timed_point_cloud_data())));
   }
 
   void OnReadsDone() override {

cartographer_grpc/handlers/add_trajectory_handler.h

@@ -35,7 +35,7 @@ class AddTrajectoryHandler
   }
   void OnRequest(const proto::AddTrajectoryRequest& request) override {
     auto local_slam_result_callback =
-        GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+        GetUnsynchronizedContext<MapBuilderContext>()
             ->GetLocalSlamResultCallbackForSubscriptions();
     std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
         expected_sensor_ids;
@@ -43,12 +43,10 @@ class AddTrajectoryHandler
       expected_sensor_ids.insert(sensor::FromProto(sensor_id));
     }
     const int trajectory_id =
-        GetContext<MapBuilderServer::MapBuilderContext>()
+        GetContext<MapBuilderContext>()->map_builder().AddTrajectoryBuilder(
-            ->map_builder()
+            expected_sensor_ids, request.trajectory_builder_options(),
-            .AddTrajectoryBuilder(expected_sensor_ids,
+            local_slam_result_callback);
-                                  request.trajectory_builder_options(),
+    if (GetUnsynchronizedContext<MapBuilderContext>()
-                                  local_slam_result_callback);
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
             ->local_trajectory_uploader()) {
       auto trajectory_builder_options = request.trajectory_builder_options();
 
@@ -58,7 +56,7 @@ class AddTrajectoryHandler
       trajectory_builder_options.clear_trajectory_builder_2d_options();
       trajectory_builder_options.clear_trajectory_builder_3d_options();
 
-      GetContext<MapBuilderServer::MapBuilderContext>()
+      GetContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->AddTrajectory(trajectory_id, expected_sensor_ids,
                           trajectory_builder_options);

cartographer_grpc/handlers/finish_trajectory_handler.h

@@ -34,14 +34,13 @@ class FinishTrajectoryHandler
     return "/cartographer_grpc.proto.MapBuilderService/FinishTrajectory";
   }
   void OnRequest(const proto::FinishTrajectoryRequest& request) override {
-    GetContext<MapBuilderServer::MapBuilderContext>()
+    GetContext<MapBuilderContext>()->map_builder().FinishTrajectory(
-        ->map_builder()
+        request.trajectory_id());
-        .FinishTrajectory(request.trajectory_id());
+    GetUnsynchronizedContext<MapBuilderContext>()->NotifyFinishTrajectory(
-    GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+        request.trajectory_id());
-        ->NotifyFinishTrajectory(request.trajectory_id());
+    if (GetUnsynchronizedContext<MapBuilderContext>()
-    if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
             ->local_trajectory_uploader()) {
-      GetContext<MapBuilderServer::MapBuilderContext>()
+      GetContext<MapBuilderContext>()
           ->local_trajectory_uploader()
           ->FinishTrajectory(request.trajectory_id());
     }

cartographer_grpc/handlers/get_all_submap_poses.h

@@ -34,7 +34,7 @@ class GetAllSubmapPosesHandler
     return "/cartographer_grpc.proto.MapBuilderService/GetAllSubmapPoses";
   }
   void OnRequest(const google::protobuf::Empty& request) override {
-    auto submap_poses = GetContext<MapBuilderServer::MapBuilderContext>()
+    auto submap_poses = GetContext<MapBuilderContext>()
                             ->map_builder()
                             .pose_graph()
                             ->GetAllSubmapPoses();

cartographer_grpc/handlers/get_constraints_handler.h

@@ -35,7 +35,7 @@ class GetConstraintsHandler
     return "/cartographer_grpc.proto.MapBuilderService/GetConstraints";
   }
   void OnRequest(const google::protobuf::Empty& request) override {
-    auto constraints = GetContext<MapBuilderServer::MapBuilderContext>()
+    auto constraints = GetContext<MapBuilderContext>()
                            ->map_builder()
                            .pose_graph()
                            ->constraints();

cartographer_grpc/handlers/get_local_to_global_transform_handler.h

@@ -33,7 +33,7 @@ class GetLocalToGlobalTransformHandler
     auto response = cartographer::common::make_unique<
         proto::GetLocalToGlobalTransformResponse>();
     auto local_to_global =
-        GetContext<MapBuilderServer::MapBuilderContext>()
+        GetContext<MapBuilderContext>()
             ->map_builder()
             .pose_graph()
             ->GetLocalToGlobalTransform(request.trajectory_id());

cartographer_grpc/handlers/get_submap_handler.h

@@ -36,13 +36,11 @@ class GetSubmapHandler
   void OnRequest(const proto::GetSubmapRequest &request) override {
     auto response =
         cartographer::common::make_unique<proto::GetSubmapResponse>();
-    response->set_error_msg(GetContext<MapBuilderServer::MapBuilderContext>()
+    response->set_error_msg(
-                                ->map_builder()
+        GetContext<MapBuilderContext>()->map_builder().SubmapToProto(
-                                .SubmapToProto(
+            cartographer::mapping::SubmapId{request.submap_id().trajectory_id(),
-                                    cartographer::mapping::SubmapId{
+                                            request.submap_id().submap_index()},
-                                        request.submap_id().trajectory_id(),
+            response->mutable_submap_query_response()));
-                                        request.submap_id().submap_index()},
-                                    response->mutable_submap_query_response()));
     Send(std::move(response));
   }
 };

cartographer_grpc/handlers/get_trajectory_node_poses_handler.h

@@ -34,7 +34,7 @@ class GetTrajectoryNodePosesHandler
     return "/cartographer_grpc.proto.MapBuilderService/GetTrajectoryNodePoses";
   }
   void OnRequest(const google::protobuf::Empty& request) override {
-    auto node_poses = GetContext<MapBuilderServer::MapBuilderContext>()
+    auto node_poses = GetContext<MapBuilderContext>()
                           ->map_builder()
                           .pose_graph()
                           ->GetTrajectoryNodePoses();

cartographer_grpc/handlers/load_map_handler.h

@@ -48,8 +48,7 @@ class LoadMapHandler
   }
 
   void OnReadsDone() override {
-    GetContext<MapBuilderServer::MapBuilderContext>()->map_builder().LoadMap(
+    GetContext<MapBuilderContext>()->map_builder().LoadMap(&reader_);
-        &reader_);
     Send(cartographer::common::make_unique<google::protobuf::Empty>());
   }
 

cartographer_grpc/handlers/receive_local_slam_results_handler.h

@@ -36,12 +36,13 @@ class ReceiveLocalSlamResultsHandler
   void OnRequest(
       const proto::ReceiveLocalSlamResultsRequest& request) override {
     auto writer = GetWriter();
-    MapBuilderServer::SubscriptionId subscription_id =
+    MapBuilderContextInterface::SubscriptionId subscription_id =
-        GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+        GetUnsynchronizedContext<MapBuilderContext>()
             ->SubscribeLocalSlamResults(
                 request.trajectory_id(),
-                [writer](std::unique_ptr<MapBuilderServer::LocalSlamResult>
+                [writer](
-                             local_slam_result) {
+                    std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
+                        local_slam_result) {
                   if (local_slam_result) {
                     writer.Write(
                         GenerateResponse(std::move(local_slam_result)));
@@ -52,14 +53,13 @@ class ReceiveLocalSlamResultsHandler
                   }
                 });
 
-    subscription_id_ =
+    subscription_id_ = cartographer::common::make_unique<
-        cartographer::common::make_unique<MapBuilderServer::SubscriptionId>(
+        MapBuilderContextInterface::SubscriptionId>(subscription_id);
-            subscription_id);
   }
 
   static std::unique_ptr<proto::ReceiveLocalSlamResultsResponse>
-  GenerateResponse(
+  GenerateResponse(std::unique_ptr<MapBuilderContextInterface::LocalSlamResult>
-      std::unique_ptr<MapBuilderServer::LocalSlamResult> local_slam_result) {
+                       local_slam_result) {
     auto response = cartographer::common::make_unique<
         proto::ReceiveLocalSlamResultsResponse>();
     response->set_trajectory_id(local_slam_result->trajectory_id);
@@ -80,13 +80,13 @@ class ReceiveLocalSlamResultsHandler
 
   void OnFinish() override {
     if (subscription_id_) {
-      GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContext>()
           ->UnsubscribeLocalSlamResults(*subscription_id_);
     }
   }
 
  private:
-  std::unique_ptr<MapBuilderServer::SubscriptionId> subscription_id_;
+  std::unique_ptr<MapBuilderContextInterface::SubscriptionId> subscription_id_;
 };
 
 }  // namespace handlers

cartographer_grpc/handlers/run_final_optimization_handler.h

@@ -35,7 +35,7 @@ class RunFinalOptimizationHandler
     return "/cartographer_grpc.proto.MapBuilderService/RunFinalOptimization";
   }
   void OnRequest(const google::protobuf::Empty& request) override {
-    GetContext<MapBuilderServer::MapBuilderContext>()
+    GetContext<MapBuilderContext>()
         ->map_builder()
         .pose_graph()
         ->RunFinalOptimization();

cartographer_grpc/map_builder_context.cc

@@ -0,0 +1,195 @@
+/*
+ * 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.
+ */
+
+#include "cartographer_grpc/map_builder_context.h"
+#include "cartographer_grpc/map_builder_server.h"
+
+namespace cartographer_grpc {
+
+MapBuilderContext::MapBuilderContext(MapBuilderServer* map_builder_server)
+    : map_builder_server_(map_builder_server) {}
+
+cartographer::mapping::MapBuilderInterface& MapBuilderContext::map_builder() {
+  return *map_builder_server_->map_builder_;
+}
+
+cartographer::common::BlockingQueue<
+    std::unique_ptr<MapBuilderContextInterface::Data>>&
+MapBuilderContext::sensor_data_queue() {
+  return map_builder_server_->incoming_data_queue_;
+}
+
+cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
+MapBuilderContext::GetLocalSlamResultCallbackForSubscriptions() {
+  MapBuilderServer* map_builder_server = map_builder_server_;
+  return [map_builder_server](
+             int trajectory_id, cartographer::common::Time time,
+             cartographer::transform::Rigid3d local_pose,
+             cartographer::sensor::RangeData range_data,
+             std::unique_ptr<const cartographer::mapping::
+                                 TrajectoryBuilderInterface::InsertionResult>
+                 insertion_result) {
+    map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
+                                          std::move(range_data),
+                                          std::move(insertion_result));
+  };
+}
+
+void MapBuilderContext::AddSensorDataToTrajectory(const Data& sensor_data) {
+  sensor_data.data->AddToTrajectoryBuilder(
+      map_builder_server_->map_builder_->GetTrajectoryBuilder(
+          sensor_data.trajectory_id));
+}
+
+MapBuilderContextInterface::SubscriptionId
+MapBuilderContext::SubscribeLocalSlamResults(
+    int trajectory_id, LocalSlamSubscriptionCallback callback) {
+  return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
+                                                        callback);
+}
+
+void MapBuilderContext::UnsubscribeLocalSlamResults(
+    const SubscriptionId& subscription_id) {
+  map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
+}
+
+void MapBuilderContext::NotifyFinishTrajectory(int trajectory_id) {
+  map_builder_server_->NotifyFinishTrajectory(trajectory_id);
+}
+
+std::shared_ptr<cartographer::mapping_2d::Submap>
+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>
+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>
+MapBuilderContext::ProcessLocalSlamResultData(
+    const std::string& sensor_id, cartographer::common::Time time,
+    const cartographer::mapping::proto::LocalSlamResultData& proto) {
+  CHECK_GE(proto.submaps().size(), 1);
+  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));
+  }
+}
+
+LocalTrajectoryUploader* MapBuilderContext::local_trajectory_uploader() {
+  return map_builder_server_->local_trajectory_uploader_.get();
+}
+
+void MapBuilderContext::EnqueueSensorData(
+    int trajectory_id, std::unique_ptr<cartographer::sensor::Data> data) {
+  map_builder_server_->incoming_data_queue_.Push(
+      cartographer::common::make_unique<Data>(
+          Data{trajectory_id, std::move(data)}));
+}
+
+void MapBuilderContext::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<Data>(
+          Data{trajectory_id, std::move(local_slam_result_data)}));
+}
+
+}  // namespace cartographer_grpc

cartographer_grpc/map_builder_context.h

@@ -0,0 +1,67 @@
+/*
+ * 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_MAP_BUILDER_CONTEXT_H
+#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
+
+#include "cartographer/mapping_2d/submaps.h"
+#include "cartographer/mapping_3d/submaps.h"
+#include "cartographer_grpc/map_builder_context_interface.h"
+
+namespace cartographer_grpc {
+
+class MapBuilderContext : public MapBuilderContextInterface {
+ public:
+  MapBuilderContext(MapBuilderServer* map_builder_server);
+  cartographer::mapping::MapBuilderInterface& map_builder() override;
+  cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
+  sensor_data_queue() override;
+  cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
+  GetLocalSlamResultCallbackForSubscriptions() override;
+  void AddSensorDataToTrajectory(const Data& sensor_data) override;
+  SubscriptionId SubscribeLocalSlamResults(
+      int trajectory_id, LocalSlamSubscriptionCallback callback) override;
+  void UnsubscribeLocalSlamResults(
+      const SubscriptionId& subscription_id) override;
+  void NotifyFinishTrajectory(int trajectory_id) override;
+  std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+  ProcessLocalSlamResultData(
+      const std::string& sensor_id, cartographer::common::Time time,
+      const cartographer::mapping::proto::LocalSlamResultData& proto) override;
+  LocalTrajectoryUploader* local_trajectory_uploader() override;
+  void EnqueueSensorData(
+      int trajectory_id,
+      std::unique_ptr<cartographer::sensor::Data> data) override;
+  void EnqueueLocalSlamResultData(
+      int trajectory_id, const std::string& sensor_id,
+      std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+          local_slam_result_data) override;
+
+ 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_;
+};
+
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H

cartographer_grpc/map_builder_context_interface.h

@@ -0,0 +1,88 @@
+/*
+ * 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_MAP_BUILDER_CONTEXT_INTERFACE_H
+#define CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_INTERFACE_H
+
+#include "cartographer/common/blocking_queue.h"
+#include "cartographer/mapping/map_builder_interface.h"
+#include "cartographer/sensor/data.h"
+#include "cartographer/sensor/range_data.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer_grpc/framework/execution_context.h"
+#include "cartographer_grpc/local_trajectory_uploader.h"
+
+namespace cartographer_grpc {
+
+class MapBuilderServer;
+class MapBuilderContextInterface : public framework::ExecutionContext {
+ public:
+  struct LocalSlamResult {
+    int trajectory_id;
+    cartographer::common::Time time;
+    cartographer::transform::Rigid3d local_pose;
+    std::shared_ptr<const cartographer::sensor::RangeData> range_data;
+    std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
+                        InsertionResult>
+        insertion_result;
+  };
+  // Calling with 'nullptr' signals subscribers that the subscription has ended.
+  using LocalSlamSubscriptionCallback =
+      std::function<void(std::unique_ptr<LocalSlamResult>)>;
+  struct Data {
+    int trajectory_id;
+    std::unique_ptr<cartographer::sensor::Data> data;
+  };
+  struct SubscriptionId {
+    const int trajectory_id;
+    const int subscription_index;
+  };
+
+  MapBuilderContextInterface() = default;
+  ~MapBuilderContextInterface() = default;
+
+  MapBuilderContextInterface(const MapBuilderContextInterface&) = delete;
+  MapBuilderContextInterface& operator=(const MapBuilderContextInterface&) =
+      delete;
+
+  virtual cartographer::mapping::MapBuilderInterface& map_builder() = 0;
+  virtual cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
+  sensor_data_queue() = 0;
+  virtual cartographer::mapping::TrajectoryBuilderInterface::
+      LocalSlamResultCallback
+      GetLocalSlamResultCallbackForSubscriptions() = 0;
+  virtual void AddSensorDataToTrajectory(const Data& sensor_data) = 0;
+  virtual SubscriptionId SubscribeLocalSlamResults(
+      int trajectory_id, LocalSlamSubscriptionCallback callback) = 0;
+  virtual void UnsubscribeLocalSlamResults(
+      const SubscriptionId& subscription_id) = 0;
+  virtual void NotifyFinishTrajectory(int trajectory_id) = 0;
+  virtual std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+  ProcessLocalSlamResultData(
+      const std::string& sensor_id, cartographer::common::Time time,
+      const cartographer::mapping::proto::LocalSlamResultData& proto) = 0;
+  virtual LocalTrajectoryUploader* local_trajectory_uploader() = 0;
+  virtual void EnqueueSensorData(
+      int trajectory_id, std::unique_ptr<cartographer::sensor::Data> data) = 0;
+  virtual void EnqueueLocalSlamResultData(
+      int trajectory_id, const std::string& sensor_id,
+      std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+          local_slam_result_data) = 0;
+};
+
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_INTERFACE_H

cartographer_grpc/map_builder_server.cc

@@ -44,163 +44,6 @@ const cartographer::common::Duration kPopTimeout =
 
 }  // namespace
 
-MapBuilderServer::MapBuilderContext::MapBuilderContext(
-    MapBuilderServer* map_builder_server)
-    : map_builder_server_(map_builder_server) {}
-
-cartographer::mapping::MapBuilderInterface&
-MapBuilderServer::MapBuilderContext::map_builder() {
-  return *map_builder_server_->map_builder_;
-}
-
-cartographer::common::BlockingQueue<std::unique_ptr<MapBuilderServer::Data>>&
-MapBuilderServer::MapBuilderContext::sensor_data_queue() {
-  return map_builder_server_->incoming_data_queue_;
-}
-
-cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
-MapBuilderServer::MapBuilderContext::
-    GetLocalSlamResultCallbackForSubscriptions() {
-  MapBuilderServer* map_builder_server = map_builder_server_;
-  return [map_builder_server](
-             int trajectory_id, cartographer::common::Time time,
-             cartographer::transform::Rigid3d local_pose,
-             cartographer::sensor::RangeData range_data,
-             std::unique_ptr<const cartographer::mapping::
-                                 TrajectoryBuilderInterface::InsertionResult>
-                 insertion_result) {
-    map_builder_server->OnLocalSlamResult(trajectory_id, time, local_pose,
-                                          std::move(range_data),
-                                          std::move(insertion_result));
-  };
-}
-
-void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
-    const Data& sensor_data) {
-  sensor_data.data->AddToTrajectoryBuilder(
-      map_builder_server_->map_builder_->GetTrajectoryBuilder(
-          sensor_data.trajectory_id));
-}
-
-MapBuilderServer::SubscriptionId
-MapBuilderServer::MapBuilderContext::SubscribeLocalSlamResults(
-    int trajectory_id, LocalSlamSubscriptionCallback callback) {
-  return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
-                                                        callback);
-}
-
-void MapBuilderServer::MapBuilderContext::UnsubscribeLocalSlamResults(
-    const SubscriptionId& subscription_id) {
-  map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
-}
-
-void MapBuilderServer::MapBuilderContext::NotifyFinishTrajectory(
-    int trajectory_id) {
-  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(), 1);
-  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)
@@ -267,7 +110,7 @@ void MapBuilderServer::Shutdown() {
 void MapBuilderServer::ProcessSensorDataQueue() {
   LOG(INFO) << "Starting SLAM thread.";
   while (!shutting_down_) {
-    std::unique_ptr<Data> sensor_data =
+    std::unique_ptr<MapBuilderContextInterface::Data> sensor_data =
         incoming_data_queue_.PopWithTimeout(kPopTimeout);
     if (sensor_data) {
       grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
@@ -324,23 +167,29 @@ void MapBuilderServer::OnLocalSlamResult(
                   const cartographer::mapping::TrajectoryBuilderInterface::
                       InsertionResult>(*insertion_result)
             : nullptr;
-    LocalSlamSubscriptionCallback callback = entry.second;
+    MapBuilderContextInterface::LocalSlamSubscriptionCallback callback =
-    callback(cartographer::common::make_unique<LocalSlamResult>(
+        entry.second;
-        LocalSlamResult{trajectory_id, time, local_pose, shared_range_data,
+    callback(cartographer::common::make_unique<
-                        std::move(copy_of_insertion_result)}));
+             MapBuilderContextInterface::LocalSlamResult>(
+        MapBuilderContextInterface::LocalSlamResult{
+            trajectory_id, time, local_pose, shared_range_data,
+            std::move(copy_of_insertion_result)}));
   }
 }
 
-MapBuilderServer::SubscriptionId MapBuilderServer::SubscribeLocalSlamResults(
+MapBuilderContextInterface::SubscriptionId
-    int trajectory_id, LocalSlamSubscriptionCallback callback) {
+MapBuilderServer::SubscribeLocalSlamResults(
+    int trajectory_id,
+    MapBuilderContextInterface::LocalSlamSubscriptionCallback callback) {
   cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
   local_slam_subscriptions_[trajectory_id].emplace(current_subscription_index_,
                                                    callback);
-  return SubscriptionId{trajectory_id, current_subscription_index_++};
+  return MapBuilderContextInterface::SubscriptionId{
+      trajectory_id, current_subscription_index_++};
 }
 
 void MapBuilderServer::UnsubscribeLocalSlamResults(
-    const SubscriptionId& subscription_id) {
+    const MapBuilderContextInterface::SubscriptionId& subscription_id) {
   cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
   CHECK_EQ(local_slam_subscriptions_[subscription_id.trajectory_id].erase(
                subscription_id.subscription_index),
@@ -350,7 +199,8 @@ void MapBuilderServer::UnsubscribeLocalSlamResults(
 void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) {
   cartographer::common::MutexLocker locker(&local_slam_subscriptions_lock_);
   for (auto& entry : local_slam_subscriptions_[trajectory_id]) {
-    LocalSlamSubscriptionCallback callback = entry.second;
+    MapBuilderContextInterface::LocalSlamSubscriptionCallback callback =
+        entry.second;
     // 'nullptr' signals subscribers that the trajectory finished.
     callback(nullptr);
   }

cartographer_grpc/map_builder_server.h

@@ -26,86 +26,13 @@
 #include "cartographer_grpc/framework/execution_context.h"
 #include "cartographer_grpc/framework/server.h"
 #include "cartographer_grpc/local_trajectory_uploader.h"
+#include "cartographer_grpc/map_builder_context.h"
 #include "cartographer_grpc/proto/map_builder_server_options.pb.h"
 
 namespace cartographer_grpc {
 
 class MapBuilderServer {
  public:
-  struct LocalSlamResult {
-    int trajectory_id;
-    cartographer::common::Time time;
-    cartographer::transform::Rigid3d local_pose;
-    std::shared_ptr<const cartographer::sensor::RangeData> range_data;
-    std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
-                        InsertionResult>
-        insertion_result;
-  };
-  // Calling with 'nullptr' signals subscribers that the subscription has ended.
-  using LocalSlamSubscriptionCallback =
-      std::function<void(std::unique_ptr<LocalSlamResult>)>;
-  struct Data {
-    int trajectory_id;
-    std::unique_ptr<cartographer::sensor::Data> data;
-  };
-  struct SubscriptionId {
-    const int trajectory_id;
-    const int subscription_index;
-  };
-
-  class MapBuilderContext : public framework::ExecutionContext {
-   public:
-    MapBuilderContext(MapBuilderServer* map_builder_server);
-    cartographer::mapping::MapBuilderInterface& map_builder();
-    cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
-    sensor_data_queue();
-    cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
-    GetLocalSlamResultCallbackForSubscriptions();
-    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);
-    LocalTrajectoryUploader* local_trajectory_uploader() {
-      return map_builder_server_->local_trajectory_uploader_.get();
-    }
-
-    template <typename DataType>
-    void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
-                           const DataType& data) {
-      map_builder_server_->incoming_data_queue_.Push(
-          cartographer::common::make_unique<MapBuilderServer::Data>(
-              MapBuilderServer::Data{
-                  trajectory_id,
-                  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;
 
   MapBuilderServer(
@@ -129,7 +56,8 @@ class MapBuilderServer {
 
  private:
   using LocalSlamResultHandlerSubscriptions =
-      std::map<int /* subscription_index */, LocalSlamSubscriptionCallback>;
+      std::map<int /* subscription_index */,
+               MapBuilderContextInterface::LocalSlamSubscriptionCallback>;
 
   void ProcessSensorDataQueue();
   void StartSlamThread();
@@ -140,16 +68,19 @@ class MapBuilderServer {
       std::unique_ptr<const cartographer::mapping::TrajectoryBuilderInterface::
                           InsertionResult>
           insertion_result);
-  SubscriptionId SubscribeLocalSlamResults(
+  MapBuilderContextInterface::SubscriptionId SubscribeLocalSlamResults(
-      int trajectory_id, LocalSlamSubscriptionCallback callback);
+      int trajectory_id,
-  void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
+      MapBuilderContextInterface::LocalSlamSubscriptionCallback callback);
+  void UnsubscribeLocalSlamResults(
+      const MapBuilderContextInterface::SubscriptionId& subscription_id);
   void NotifyFinishTrajectory(int trajectory_id);
 
   bool shutting_down_ = false;
   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<Data>>
+  cartographer::common::BlockingQueue<
+      std::unique_ptr<MapBuilderContextInterface::Data>>
       incoming_data_queue_;
   cartographer::common::Mutex local_slam_subscriptions_lock_;
   int current_subscription_index_ = 0;