diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc
index 25ddc50..1166838 100644
--- a/cartographer_grpc/client_server_test.cc
+++ b/cartographer_grpc/client_server_test.cc
@@ -19,11 +19,41 @@
 #include "cartographer_grpc/map_builder_server_options.h"
 #include "cartographer_grpc/mapping/map_builder_stub.h"
 #include "glog/logging.h"
+#include "gmock/gmock.h"
 #include "gtest/gtest.h"
 
+using cartographer::mapping::MapBuilder;
+using cartographer::mapping::MapBuilderInterface;
+using cartographer::mapping::PoseGraphInterface;
+using cartographer::mapping::TrajectoryBuilderInterface;
+using testing::_;
+
 namespace cartographer_grpc {
 namespace {
 
+constexpr char kSensorId[] = "sensor";
+
+class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
+ public:
+  MOCK_METHOD3(AddTrajectoryBuilder,
+               int(const std::unordered_set<std::string>& expected_sensor_ids,
+                   const cartographer::mapping::proto::TrajectoryBuilderOptions&
+                       trajectory_options,
+                   LocalSlamResultCallback local_slam_result_callback));
+  MOCK_METHOD0(AddTrajectoryForDeserialization, int());
+  MOCK_CONST_METHOD1(GetTrajectoryBuilder,
+                     TrajectoryBuilderInterface*(int trajectory_id));
+  MOCK_METHOD1(FinishTrajectory, void(int trajectory_id));
+  MOCK_METHOD2(
+      SubmapToProto,
+      std::string(const cartographer::mapping::SubmapId&,
+                  cartographer::mapping::proto::SubmapQuery::Response*));
+  MOCK_METHOD1(SerializeState, void(cartographer::io::ProtoStreamWriter*));
+  MOCK_METHOD1(LoadMap, void(cartographer::io::ProtoStreamReader*));
+  MOCK_CONST_METHOD0(num_trajectory_builders, int());
+  MOCK_METHOD0(pose_graph, PoseGraphInterface*());
+};
+
 class ClientServerTest : public ::testing::Test {
  protected:
   void SetUp() override {
@@ -42,24 +72,83 @@ class ClientServerTest : public ::testing::Test {
       return MAP_BUILDER_SERVER)text";
     auto map_builder_server_parameters =
         cartographer::mapping::test::ResolveLuaParameters(kMapBuilderServerLua);
-    auto map_builder_server_options =
+    map_builder_server_options_ =
         CreateMapBuilderServerOptions(map_builder_server_parameters.get());
-    auto map_builder =
-        cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
-            map_builder_server_options.map_builder_options());
+    const std::string kTrajectoryBuilderLua = R"text(
+      include "trajectory_builder.lua"
+      return TRAJECTORY_BUILDER)text";
+    auto trajectory_builder_parameters =
+        cartographer::mapping::test::ResolveLuaParameters(
+            kTrajectoryBuilderLua);
+    trajectory_builder_options_ =
+        cartographer::mapping::CreateTrajectoryBuilderOptions(
+            trajectory_builder_parameters.get());
+  }
+
+  void InitializeRealServer() {
+    auto map_builder = cartographer::common::make_unique<MapBuilder>(
+        map_builder_server_options_.map_builder_options());
     server_ = cartographer::common::make_unique<MapBuilderServer>(
-        map_builder_server_options, std::move(map_builder));
+        map_builder_server_options_, std::move(map_builder));
     EXPECT_TRUE(server_ != nullptr);
   }
 
+  void InitializeServerWithMockMapBuilder() {
+    auto mock_map_builder = cartographer::common::make_unique<MockMapBuilder>();
+    mock_map_builder_ = mock_map_builder.get();
+    server_ = cartographer::common::make_unique<MapBuilderServer>(
+        map_builder_server_options_, std::move(mock_map_builder));
+    EXPECT_TRUE(server_ != nullptr);
+  }
+
+  void InitializeStub() {
+    stub_ = cartographer::common::make_unique<mapping::MapBuilderStub>(
+        map_builder_server_options_.server_address());
+    EXPECT_TRUE(stub_ != nullptr);
+  }
+
+  proto::MapBuilderServerOptions map_builder_server_options_;
+  MockMapBuilder* mock_map_builder_;
+  cartographer::mapping::proto::TrajectoryBuilderOptions
+      trajectory_builder_options_;
   std::unique_ptr<MapBuilderServer> server_;
   std::unique_ptr<mapping::MapBuilderStub> stub_;
 };
 
 TEST_F(ClientServerTest, StartAndStopServer) {
+  InitializeRealServer();
   server_->Start();
   server_->Shutdown();
 }
 
+TEST_F(ClientServerTest, AddTrajectoryBuilder) {
+  InitializeRealServer();
+  server_->Start();
+  InitializeStub();
+  int trajectory_id = stub_->AddTrajectoryBuilder(
+      {kSensorId}, trajectory_builder_options_, nullptr);
+  stub_->FinishTrajectory(trajectory_id);
+  server_->Shutdown();
+}
+
+TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
+  InitializeServerWithMockMapBuilder();
+  server_->Start();
+  InitializeStub();
+  std::unordered_set<std::string> expected_sensor_ids = {kSensorId};
+  EXPECT_CALL(
+      *mock_map_builder_,
+      AddTrajectoryBuilder(testing::ContainerEq(expected_sensor_ids), _, _))
+      .WillOnce(testing::Return(3));
+  EXPECT_CALL(*mock_map_builder_, GetTrajectoryBuilder(_))
+      .WillRepeatedly(testing::Return(nullptr));
+  int trajectory_id = stub_->AddTrajectoryBuilder(
+      expected_sensor_ids, trajectory_builder_options_, nullptr);
+  EXPECT_EQ(trajectory_id, 3);
+  EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id));
+  stub_->FinishTrajectory(trajectory_id);
+  server_->Shutdown();
+}
+
 }  // namespace
 }  // namespace cartographer_grpc
diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc
index 8c5db4d..b1f9423 100644
--- a/cartographer_grpc/map_builder_server.cc
+++ b/cartographer_grpc/map_builder_server.cc
@@ -37,7 +37,7 @@ MapBuilderServer::MapBuilderContext::MapBuilderContext(
     MapBuilderServer* map_builder_server)
     : map_builder_server_(map_builder_server) {}
 
-cartographer::mapping::MapBuilder&
+cartographer::mapping::MapBuilderInterface&
 MapBuilderServer::MapBuilderContext::map_builder() {
   return *map_builder_server_->map_builder_;
 }
@@ -72,7 +72,7 @@ void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
 
 MapBuilderServer::MapBuilderServer(
     const proto::MapBuilderServerOptions& map_builder_server_options,
-    std::unique_ptr<cartographer::mapping::MapBuilder> map_builder)
+    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
     : map_builder_(std::move(map_builder)) {
   framework::Server::Builder server_builder;
   server_builder.SetServerAddress(map_builder_server_options.server_address());
diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h
index 2b11de9..91cd0c2 100644
--- a/cartographer_grpc/map_builder_server.h
+++ b/cartographer_grpc/map_builder_server.h
@@ -48,7 +48,7 @@ class MapBuilderServer {
   class MapBuilderContext : public framework::ExecutionContext {
    public:
     MapBuilderContext(MapBuilderServer* map_builder_server);
-    cartographer::mapping::MapBuilder& map_builder();
+    cartographer::mapping::MapBuilderInterface& map_builder();
     cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
     sensor_data_queue();
     cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
@@ -75,7 +75,7 @@ class MapBuilderServer {
 
   MapBuilderServer(
       const proto::MapBuilderServerOptions& map_builder_server_options,
-      std::unique_ptr<cartographer::mapping::MapBuilder> map_builder);
+      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder);
 
   // Starts the gRPC server and the SLAM thread.
   void Start();
@@ -106,7 +106,7 @@ class MapBuilderServer {
   bool shutting_down_ = false;
   std::unique_ptr<std::thread> slam_thread_;
   std::unique_ptr<framework::Server> grpc_server_;
-  std::unique_ptr<cartographer::mapping::MapBuilder> map_builder_;
+  std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
   cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>
       sensor_data_queue_;
   cartographer::common::Mutex local_slam_subscriptions_lock_;