diff --git a/cartographer/sensor/dispatchable.h b/cartographer/sensor/dispatchable.h
index 876743c..5960512 100644
--- a/cartographer/sensor/dispatchable.h
+++ b/cartographer/sensor/dispatchable.h
@@ -34,6 +34,7 @@ class Dispatchable : public Data {
       mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
     trajectory_builder->AddSensorData(sensor_id_, data_);
   }
+  const DataType &data() const { return data_; }
 
  private:
   const DataType data_;
diff --git a/cartographer_grpc/framework/client.h b/cartographer_grpc/framework/client.h
index a485f98..3057073 100644
--- a/cartographer_grpc/framework/client.h
+++ b/cartographer_grpc/framework/client.h
@@ -17,6 +17,8 @@
 #ifndef CARTOGRAPHER_GRPC_FRAMEWORK_CLIENT_H
 #define CARTOGRAPHER_GRPC_FRAMEWORK_CLIENT_H
 
+#include "cartographer_grpc/framework/rpc_handler_interface.h"
+#include "cartographer_grpc/framework/type_traits.h"
 #include "grpc++/grpc++.h"
 #include "grpc++/impl/codegen/client_unary_call.h"
 #include "grpc++/impl/codegen/sync_stream.h"
diff --git a/cartographer_grpc/framework/server.h b/cartographer_grpc/framework/server.h
index 647349a..957dfe6 100644
--- a/cartographer_grpc/framework/server.h
+++ b/cartographer_grpc/framework/server.h
@@ -80,14 +80,13 @@ class Server {
                       typename RpcHandlerType::OutgoingType>::value,
               method_full_name});
     }
+    static std::tuple<std::string /* service_full_name */,
+                      std::string /* method_name */>
+    ParseMethodFullName(const std::string& method_full_name);
 
    private:
     using ServiceInfo = std::map<std::string, RpcHandlerInfo>;
 
-    std::tuple<std::string /* service_full_name */,
-               std::string /* method_name */>
-    ParseMethodFullName(const std::string& method_full_name);
-
     template <typename RpcHandlerType>
     void CheckHandlerCompatibility(const std::string& service_full_name,
                                    const std::string& method_name) {
diff --git a/cartographer_grpc/framework/testing/rpc_handler_test_server.h b/cartographer_grpc/framework/testing/rpc_handler_test_server.h
index 6a8a7c4..79e98f2 100644
--- a/cartographer_grpc/framework/testing/rpc_handler_test_server.h
+++ b/cartographer_grpc/framework/testing/rpc_handler_test_server.h
@@ -21,131 +21,91 @@
 #include <string>
 
 #include "cartographer/common/blocking_queue.h"
+#include "cartographer_grpc/framework/client.h"
+#include "cartographer_grpc/framework/rpc_handler_interface.h"
 #include "cartographer_grpc/framework/server.h"
 #include "cartographer_grpc/framework/testing/rpc_handler_wrapper.h"
+#include "grpc++/grpc++.h"
+#include "gtest/gtest.h"
 
 namespace cartographer_grpc {
 namespace framework {
 namespace testing {
 
 namespace {
-const std::string kMethodName = "method";
-const std::string kServiceName = "service";
-const std::string kFullyQualifiedMethodName =
-    "/" + kServiceName + "/" + kMethodName;
 const std::string kServerAddress = "localhost:50051";
 }  // namespace
 
-template <class RpcHandlerType>
+template <typename RpcHandlerType>
 class RpcHandlerTestServer : public Server {
  public:
-  RpcHandlerTestServer() : Server(Options{1, 1, kServerAddress}) {
-    // Register the handler under test.
-    this->AddService(kServiceName, {{kMethodName, GetRpcHandlerInfo()}});
-    // Starts the server and instantiates the handler under test.
+  RpcHandlerTestServer(std::unique_ptr<ExecutionContext> execution_context)
+      : Server(Options{1, 1, kServerAddress}),
+        channel_(grpc::CreateChannel(kServerAddress,
+                                     grpc::InsecureChannelCredentials())),
+        client_(channel_) {
+    std::string method_full_name_under_test =
+        RpcHandlerInterface::Instantiate<RpcHandlerType>()->method_name();
+    std::string service_full_name;
+    std::string method_name;
+    std::tie(service_full_name, method_name) =
+        Server::Builder::ParseMethodFullName(method_full_name_under_test);
+    this->AddService(
+        service_full_name,
+        {{method_name, GetRpcHandlerInfo(method_full_name_under_test)}});
+    this->SetExecutionContext(std::move(execution_context));
     this->Start();
-    // Depending on the RPC type might already trigger a NEW_CONNECTION
-    // event in the handler under test.
-    InstantiateReadersAndWriters();
   }
 
   ~RpcHandlerTestServer() { this->Shutdown(); };
 
+  void SendWrite(const typename RpcHandlerType::RequestType &message) {
+    EXPECT_TRUE(client_.Write(message));
+    WaitForHandlerCompletion(RpcHandlerWrapper<RpcHandlerType>::ON_REQUEST);
+  }
+
   // Parses a request message from the passed string and issues the
   // request against the handler, waits for the handler to complete
   // processing before returning.
   void SendWrite(const std::string &serialized_message) {
-    auto message = cartographer::common::make_unique<
-        typename RpcHandlerType::RequestType>();
-    message->ParseFromString(serialized_message);
-    switch (rpc_method_.method_type()) {
-      case ::grpc::internal::RpcMethod::CLIENT_STREAMING:
-        CHECK(client_writer_->Write(*message)) << "Write failed.";
-        break;
-      case ::grpc::internal::RpcMethod::BIDI_STREAMING:
-      case ::grpc::internal::RpcMethod::SERVER_STREAMING:
-      case ::grpc::internal::RpcMethod::NORMAL_RPC:
-        LOG(FATAL) << "Not implemented";
-        break;
-    }
-    WaitForHandlerCompletion(RpcHandlerWrapper::ON_REQUEST);
+    typename RpcHandlerType::RequestType message;
+    message.ParseFromString(serialized_message);
+    Write(message);
   }
 
   // Sends a WRITES_DONE event to the handler, waits for the handler
   // to finish processing the READS_DONE event before returning.
   void SendWritesDone() {
-    auto message = cartographer::common::make_unique<
-        typename RpcHandlerType::RequestType>();
-    switch (rpc_method_.method_type()) {
-      case ::grpc::internal::RpcMethod::CLIENT_STREAMING:
-        CHECK(client_writer_->WritesDone()) << "WritesDone failed.";
-        break;
-      case ::grpc::internal::RpcMethod::BIDI_STREAMING:
-      case ::grpc::internal::RpcMethod::SERVER_STREAMING:
-      case ::grpc::internal::RpcMethod::NORMAL_RPC:
-        LOG(FATAL) << "Not implemented";
-        break;
-    }
-    WaitForHandlerCompletion(RpcHandlerWrapper::ON_READS_DONE);
+    EXPECT_TRUE(client_.WritesDone());
+    WaitForHandlerCompletion(RpcHandlerWrapper<RpcHandlerType>::ON_READS_DONE);
   }
 
   // Sends a FINISH event to the handler under test, waits for the
   // handler to finish processing the event before returning.
   void SendFinish() {
-    switch (rpc_method_.method_type()) {
-      case ::grpc::internal::RpcMethod::CLIENT_STREAMING:
-        CHECK(client_writer_->Finish()) << "Finish failed.";
-        break;
-      case ::grpc::internal::RpcMethod::BIDI_STREAMING:
-      case ::grpc::internal::RpcMethod::SERVER_STREAMING:
-      case ::grpc::internal::RpcMethod::NORMAL_RPC:
-        LOG(FATAL) << "Not implemented";
-        break;
-    }
-    WaitForHandlerCompletion(RpcHandlerWrapper::ON_FINISH);
+    EXPECT_TRUE(client_.Finish().ok());
+    WaitForHandlerCompletion(RpcHandlerWrapper<RpcHandlerType>::ON_FINISH);
   }
 
  private:
   using ClientWriter = ::grpc::internal::ClientWriterFactory<
       typename RpcHandlerType::RequestType>;
 
-  void WaitForHandlerCompletion(RpcHandlerWrapper::RpcHandlerEvent event) {
+  void WaitForHandlerCompletion(
+      typename RpcHandlerWrapper<RpcHandlerType>::RpcHandlerEvent event) {
     CHECK_EQ(rpc_handler_event_queue_.Pop(), event);
   }
 
-  void InstantiateReadersAndWriters() {
-    switch (rpc_method_.method_type()) {
-      case ::grpc::internal::RpcMethod::CLIENT_STREAMING:
-        if (!response_) {
-          response_ = cartographer::common::make_unique<
-              typename RpcHandlerType::ResponseType>();
-        }
-        if (!client_writer_) {
-          client_writer_ = CreateClientWriter();
-        }
-        break;
-      case ::grpc::internal::RpcMethod::SERVER_STREAMING:
-      case ::grpc::internal::RpcMethod::NORMAL_RPC:
-      case ::grpc::internal::RpcMethod::BIDI_STREAMING:
-        LOG(FATAL) << "Not implemented";
-        break;
-    }
-  }
-
-  std::unique_ptr<ClientWriter> CreateClientWriter() {
-    return std::unique_ptr<ClientWriter>(ClientWriter::Create(
-        channel_.get(), rpc_method_, &context_, response_.get()));
-  }
-
-  RpcHandlerInfo GetRpcHandlerInfo() {
+  RpcHandlerInfo GetRpcHandlerInfo(const std::string &method_full_name) {
     ::grpc::internal::RpcMethod::RpcType rpc_type =
         RpcType<typename RpcHandlerType::IncomingType,
                 typename RpcHandlerType::OutgoingType>::value;
     auto event_callback =
-        [this](RpcHandlerWrapper<RpcHandlerType>::RpcHandlerEvent event) {
+        [this](
+            typename RpcHandlerWrapper<RpcHandlerType>::RpcHandlerEvent event) {
           rpc_handler_event_queue_.Push(event);
         };
-    auto handler_instantiator = [this](
+    auto handler_instantiator = [event_callback](
                                     Rpc *const rpc,
                                     ExecutionContext *const execution_context) {
       std::unique_ptr<RpcHandlerInterface> rpc_handler =
@@ -158,16 +118,13 @@ class RpcHandlerTestServer : public Server {
     return RpcHandlerInfo{
         RpcHandlerType::RequestType::default_instance().GetDescriptor(),
         RpcHandlerType::ResponseType::default_instance().GetDescriptor(),
-        handler_instantiator, rpc_type, kFullyQualifiedMethodName};
+        handler_instantiator, rpc_type, method_full_name};
   }
 
-  ::grpc::internal::RpcMethod rpc_method_;
-  ::grpc::ClientContext context_;
-  std::shared_ptr<::grpc::ChannelInterface> channel_;
-  std::unique_ptr<::grpc::ClientWriter<typename RpcHandlerType::RequestType>>
-      client_writer_;
-  std::unique_ptr<typename RpcHandlerType::ResponseType> response_;
-  cartographer::common::BlockingQueue<RpcHandlerWrapper::RpcHandlerEvent>
+  std::shared_ptr<::grpc::Channel> channel_;
+  cartographer_grpc::framework::Client<RpcHandlerType> client_;
+  cartographer::common::BlockingQueue<
+      typename RpcHandlerWrapper<RpcHandlerType>::RpcHandlerEvent>
       rpc_handler_event_queue_;
 };
 
diff --git a/cartographer_grpc/handlers/add_imu_data_handler.h b/cartographer_grpc/handlers/add_imu_data_handler.h
index ae40aad..45264ae 100644
--- a/cartographer_grpc/handlers/add_imu_data_handler.h
+++ b/cartographer_grpc/handlers/add_imu_data_handler.h
@@ -38,7 +38,7 @@ 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<MapBuilderContext>()->EnqueueSensorData(
+    GetUnsynchronizedContext<MapBuilderContextInterface>()->EnqueueSensorData(
         request.sensor_metadata().trajectory_id(),
         cartographer::sensor::MakeDispatchable(
             request.sensor_metadata().sensor_id(),
@@ -47,14 +47,14 @@ class AddImuDataHandler
     // The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
     // Therefore it suffices to get an unsynchronized reference to the
     // 'MapBuilderContext'.
-    if (GetUnsynchronizedContext<MapBuilderContext>()
+    if (GetUnsynchronizedContext<MapBuilderContextInterface>()
             ->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<MapBuilderContext>()
+      GetUnsynchronizedContext<MapBuilderContextInterface>()
           ->local_trajectory_uploader()
           ->EnqueueDataRequest(std::move(data_request));
     }
diff --git a/cartographer_grpc/handlers/add_imu_data_handler_test.cc b/cartographer_grpc/handlers/add_imu_data_handler_test.cc
new file mode 100644
index 0000000..8f13804
--- /dev/null
+++ b/cartographer_grpc/handlers/add_imu_data_handler_test.cc
@@ -0,0 +1,143 @@
+/*
+ * 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/handlers/add_imu_data_handler.h"
+#include "cartographer/common/make_unique.h"
+#include "cartographer_grpc/framework/testing/rpc_handler_test_server.h"
+#include "cartographer_grpc/testing/mock_local_trajectory_uploader.h"
+#include "cartographer_grpc/testing/mock_map_builder_context.h"
+#include "google/protobuf/text_format.h"
+#include "google/protobuf/util/message_differencer.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_grpc {
+namespace handlers {
+namespace {
+
+using ::testing::_;
+using ::testing::Eq;
+using ::testing::Pointee;
+using ::testing::Return;
+using ::testing::Test;
+using ::testing::Truly;
+
+const std::string kMessage = R"PROTO(
+  sensor_metadata {
+    trajectory_id: 1
+    sensor_id: "sensor_id"
+  }
+  imu_data {
+    timestamp: 2
+    linear_acceleration {
+      x: 3
+      y: 4
+      z: 5
+    }
+    angular_velocity {
+      x: 6
+      y: 7
+      z: 8
+    }
+  })PROTO";
+
+using DataPredicateType =
+    std::function<bool(const cartographer::sensor::Data &)>;
+using ProtoPredicateType =
+    std::function<bool(const google::protobuf::Message &)>;
+
+class AddImuDataHandlerTest : public Test {
+ public:
+  void SetUp() override {
+    test_server_ = cartographer::common::make_unique<
+        framework::testing::RpcHandlerTestServer<AddImuDataHandler>>(
+        cartographer::common::make_unique<testing::MockMapBuilderContext>());
+    mock_map_builder_context_ =
+        test_server_
+            ->GetUnsynchronizedContext<testing::MockMapBuilderContext>();
+    mock_local_trajectory_uploader_ = cartographer::common::make_unique<
+        testing::MockLocalTrajectoryUploader>();
+    EXPECT_TRUE(
+        google::protobuf::TextFormat::ParseFromString(kMessage, &request_));
+  }
+
+  void SetNoLocalTrajectoryUploader() {
+    EXPECT_CALL(*mock_map_builder_context_, local_trajectory_uploader())
+        .WillOnce(Return(nullptr));
+  }
+
+  void SetMockLocalTrajectoryUploader() {
+    EXPECT_CALL(*mock_map_builder_context_, local_trajectory_uploader())
+        .WillRepeatedly(Return(mock_local_trajectory_uploader_.get()));
+  }
+
+ protected:
+  std::unique_ptr<framework::testing::RpcHandlerTestServer<AddImuDataHandler>>
+      test_server_;
+  testing::MockMapBuilderContext *mock_map_builder_context_;
+  std::unique_ptr<testing::MockLocalTrajectoryUploader>
+      mock_local_trajectory_uploader_;
+  proto::AddImuDataRequest request_;
+};
+
+DataPredicateType BuildDataPredicateEquals(
+    const proto::AddImuDataRequest &proto) {
+  return [proto](const cartographer::sensor::Data &data) {
+    const auto *dispatchable =
+        dynamic_cast<const cartographer::sensor::Dispatchable<
+            cartographer::sensor::ImuData> *>(&data);
+    CHECK_NOTNULL(dispatchable);
+    return google::protobuf::util::MessageDifferencer::Equals(
+               cartographer::sensor::ToProto(dispatchable->data()),
+               proto.imu_data()) &&
+           dispatchable->GetSensorId() == proto.sensor_metadata().sensor_id();
+  };
+}
+
+ProtoPredicateType BuildProtoPredicateEquals(
+    const google::protobuf::Message *proto) {
+  return [proto](const google::protobuf::Message &message) {
+    return google::protobuf::util::MessageDifferencer::Equals(*proto, message);
+  };
+}
+
+TEST_F(AddImuDataHandlerTest, NoLocalSlamUploader) {
+  SetNoLocalTrajectoryUploader();
+  EXPECT_CALL(
+      *mock_map_builder_context_,
+      DoEnqueueSensorData(Eq(request_.sensor_metadata().trajectory_id()),
+                          Pointee(Truly(BuildDataPredicateEquals(request_)))));
+  test_server_->SendWrite(request_);
+  test_server_->SendWritesDone();
+  test_server_->SendFinish();
+}
+
+TEST_F(AddImuDataHandlerTest, WithMockLocalSlamUploader) {
+  SetMockLocalTrajectoryUploader();
+  EXPECT_CALL(
+      *mock_map_builder_context_,
+      DoEnqueueSensorData(Eq(request_.sensor_metadata().trajectory_id()),
+                          Pointee(Truly(BuildDataPredicateEquals(request_)))));
+  EXPECT_CALL(*mock_local_trajectory_uploader_,
+              DoEnqueueDataRequest(
+                  Pointee(Truly(BuildProtoPredicateEquals(&request_)))));
+  test_server_->SendWrite(request_);
+  test_server_->SendWritesDone();
+  test_server_->SendFinish();
+}
+
+}  // namespace
+}  // namespace handlers
+}  // namespace cartographer_grpc
diff --git a/cartographer_grpc/local_trajectory_uploader.h b/cartographer_grpc/local_trajectory_uploader.h
index 32f92ca..1eb3cd5 100644
--- a/cartographer_grpc/local_trajectory_uploader.h
+++ b/cartographer_grpc/local_trajectory_uploader.h
@@ -32,10 +32,27 @@
 
 namespace cartographer_grpc {
 
-class LocalTrajectoryUploader {
+class LocalTrajectoryUploaderInterface {
  public:
   using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
 
+  virtual ~LocalTrajectoryUploaderInterface() = default;
+
+  // Enqueue an Add*DataRequest message to be uploaded.
+  virtual void EnqueueDataRequest(
+      std::unique_ptr<google::protobuf::Message> data_request) = 0;
+  virtual void AddTrajectory(
+      int local_trajectory_id, const std::set<SensorId>& expected_sensor_ids,
+      const cartographer::mapping::proto::TrajectoryBuilderOptions&
+          trajectory_options) = 0;
+  virtual void FinishTrajectory(int local_trajectory_id) = 0;
+
+  virtual SensorId GetLocalSlamResultSensorId(
+      int local_trajectory_id) const = 0;
+};
+
+class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
+ public:
   LocalTrajectoryUploader(const std::string& uplink_server_address);
   ~LocalTrajectoryUploader();
 
@@ -49,14 +66,12 @@ class LocalTrajectoryUploader {
   void AddTrajectory(
       int local_trajectory_id, const std::set<SensorId>& expected_sensor_ids,
       const cartographer::mapping::proto::TrajectoryBuilderOptions&
-          trajectory_options);
-  void FinishTrajectory(int local_trajectory_id);
-
-  // Enqueue an Add*DataRequest message to be uploaded.
+          trajectory_options) override;
+  void FinishTrajectory(int local_trajectory_id) override;
   void EnqueueDataRequest(
-      std::unique_ptr<google::protobuf::Message> data_request);
+      std::unique_ptr<google::protobuf::Message> data_request) override;
 
-  SensorId GetLocalSlamResultSensorId(int local_trajectory_id) const {
+  SensorId GetLocalSlamResultSensorId(int local_trajectory_id) const override {
     return SensorId{SensorId::SensorType::LOCAL_SLAM_RESULT,
                     "local_slam_result_" + std::to_string(local_trajectory_id)};
   }
diff --git a/cartographer_grpc/map_builder_context.cc b/cartographer_grpc/map_builder_context.cc
index 8dec011..b176f53 100644
--- a/cartographer_grpc/map_builder_context.cc
+++ b/cartographer_grpc/map_builder_context.cc
@@ -172,7 +172,8 @@ MapBuilderContext::ProcessLocalSlamResultData(
   }
 }
 
-LocalTrajectoryUploader* MapBuilderContext::local_trajectory_uploader() {
+LocalTrajectoryUploaderInterface*
+MapBuilderContext::local_trajectory_uploader() {
   return map_builder_server_->local_trajectory_uploader_.get();
 }
 
@@ -192,4 +193,4 @@ void MapBuilderContext::EnqueueLocalSlamResultData(
           Data{trajectory_id, std::move(local_slam_result_data)}));
 }
 
-}  // namespace cartographer_grpc
\ No newline at end of file
+}  // namespace cartographer_grpc
diff --git a/cartographer_grpc/map_builder_context.h b/cartographer_grpc/map_builder_context.h
index d2b5d84..c27fa2b 100644
--- a/cartographer_grpc/map_builder_context.h
+++ b/cartographer_grpc/map_builder_context.h
@@ -41,7 +41,7 @@ class MapBuilderContext : public MapBuilderContextInterface {
   ProcessLocalSlamResultData(
       const std::string& sensor_id, cartographer::common::Time time,
       const cartographer::mapping::proto::LocalSlamResultData& proto) override;
-  LocalTrajectoryUploader* local_trajectory_uploader() override;
+  LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
   void EnqueueSensorData(
       int trajectory_id,
       std::unique_ptr<cartographer::sensor::Data> data) override;
@@ -64,4 +64,4 @@ class MapBuilderContext : public MapBuilderContextInterface {
 
 }  // namespace cartographer_grpc
 
-#endif  // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
\ No newline at end of file
+#endif  // CARTOGRAPHER_GRPC_MAP_BUILDER_CONTEXT_H
diff --git a/cartographer_grpc/map_builder_context_interface.h b/cartographer_grpc/map_builder_context_interface.h
index b4a3785..1506b50 100644
--- a/cartographer_grpc/map_builder_context_interface.h
+++ b/cartographer_grpc/map_builder_context_interface.h
@@ -74,7 +74,7 @@ class MapBuilderContextInterface : public framework::ExecutionContext {
   ProcessLocalSlamResultData(
       const std::string& sensor_id, cartographer::common::Time time,
       const cartographer::mapping::proto::LocalSlamResultData& proto) = 0;
-  virtual LocalTrajectoryUploader* local_trajectory_uploader() = 0;
+  virtual LocalTrajectoryUploaderInterface* local_trajectory_uploader() = 0;
   virtual void EnqueueSensorData(
       int trajectory_id, std::unique_ptr<cartographer::sensor::Data> data) = 0;
   virtual void EnqueueLocalSlamResultData(
diff --git a/cartographer_grpc/testing/mock_local_trajectory_uploader.h b/cartographer_grpc/testing/mock_local_trajectory_uploader.h
new file mode 100644
index 0000000..d55dc2d
--- /dev/null
+++ b/cartographer_grpc/testing/mock_local_trajectory_uploader.h
@@ -0,0 +1,46 @@
+/*
+ * 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_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H
+#define CARTOGRAPHER_GRPC_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H
+
+#include "cartographer_grpc/local_trajectory_uploader.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_grpc {
+namespace testing {
+
+class MockLocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
+ public:
+  MOCK_METHOD1(DoEnqueueDataRequest, void(google::protobuf::Message *));
+  void EnqueueDataRequest(
+      std::unique_ptr<google::protobuf::Message> data_request) override {
+    DoEnqueueDataRequest(data_request.get());
+  }
+  MOCK_METHOD3(
+      AddTrajectory,
+      void(int, const std::set<SensorId> &,
+           const cartographer::mapping::proto::TrajectoryBuilderOptions &));
+  MOCK_METHOD1(FinishTrajectory, void(int));
+  MOCK_CONST_METHOD1(GetLocalSlamResultSensorId, SensorId(int));
+};
+
+}  // namespace testing
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_TESTING_MOCK_LOCAL_TRAJECTORY_UPLOADER_H
diff --git a/cartographer_grpc/testing/mock_map_builder_context.h b/cartographer_grpc/testing/mock_map_builder_context.h
new file mode 100644
index 0000000..f160c1e
--- /dev/null
+++ b/cartographer_grpc/testing/mock_map_builder_context.h
@@ -0,0 +1,81 @@
+/*
+ * 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_TESTING_MOCK_MAP_BUILDER_CONTEXT_H
+#define CARTOGRAPHER_GRPC_TESTING_MOCK_MAP_BUILDER_CONTEXT_H
+
+#include "cartographer/mapping/local_slam_result_data.h"
+#include "cartographer_grpc/map_builder_context_interface.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer_grpc {
+namespace testing {
+
+class MockMapBuilderContext : public MapBuilderContextInterface {
+ public:
+  MOCK_METHOD0(map_builder, cartographer::mapping::MapBuilderInterface &());
+  MOCK_METHOD0(sensor_data_queue,
+               cartographer::common::BlockingQueue<
+                   std::unique_ptr<MapBuilderContextInterface::Data>> &());
+  MOCK_METHOD0(GetLocalSlamResultCallbackForSubscriptions,
+               cartographer::mapping::TrajectoryBuilderInterface::
+                   LocalSlamResultCallback());
+  MOCK_METHOD1(AddSensorDataToTrajectory,
+               void(const MapBuilderContextInterface::Data &));
+  MOCK_METHOD2(SubscribeLocalSlamResults,
+               MapBuilderContextInterface::SubscriptionId(
+                   int,
+                   MapBuilderContextInterface::LocalSlamSubscriptionCallback));
+  MOCK_METHOD1(UnsubscribeLocalSlamResults,
+               void(const MapBuilderContextInterface::SubscriptionId &));
+  MOCK_METHOD1(NotifyFinishTrajectory, void(int));
+  MOCK_METHOD3(DoProcessLocalSlamResultData,
+               cartographer::mapping::LocalSlamResultData *(
+                   const std::string &, cartographer::common::Time,
+                   const cartographer::mapping::proto::LocalSlamResultData &));
+  std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+  ProcessLocalSlamResultData(
+      const std::string &sensor_id, cartographer::common::Time time,
+      const cartographer::mapping::proto::LocalSlamResultData &proto) override {
+    return std::unique_ptr<cartographer::mapping::LocalSlamResultData>(
+        DoProcessLocalSlamResultData(sensor_id, time, proto));
+  }
+  MOCK_METHOD0(local_trajectory_uploader, LocalTrajectoryUploaderInterface *());
+
+  MOCK_METHOD2(DoEnqueueSensorData, void(int, cartographer::sensor::Data *));
+  void EnqueueSensorData(
+      int trajectory_id,
+      std::unique_ptr<cartographer::sensor::Data> data) override {
+    DoEnqueueSensorData(trajectory_id, data.get());
+  }
+  MOCK_METHOD3(DoEnqueueLocalSlamResultData,
+               void(int, const std::string &,
+                    cartographer::mapping::LocalSlamResultData *));
+  void EnqueueLocalSlamResultData(
+      int trajectory_id, const std::string &sensor_id,
+      std::unique_ptr<cartographer::mapping::LocalSlamResultData>
+          local_slam_result_data) {
+    DoEnqueueLocalSlamResultData(trajectory_id, sensor_id,
+                                 local_slam_result_data.get());
+  }
+};
+
+}  // namespace testing
+}  // namespace cartographer_grpc
+
+#endif  // CARTOGRAPHER_GRPC_TESTING_MOCK_MAP_BUILDER_CONTEXT_H