Struct SensorId (#839)

* WIP, started unordered_set<SensorId>

* struct SensorId. Works for cartographer without grpc.

* correct test

* SensorId in cartographer_grpc/

* clean up

* try to fix for trusty

* SensorId::operator==

* Ran clang-format.

cartographer/mapping/collated_trajectory_builder.cc

@@ -30,14 +30,18 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
 
 CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
     sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
-    const std::unordered_set<std::string>& expected_sensor_ids,
+    const std::set<SensorId>& expected_sensor_ids,
     std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
     : sensor_collator_(sensor_collator),
       trajectory_id_(trajectory_id),
       wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
       last_logging_time_(std::chrono::steady_clock::now()) {
+  std::unordered_set<std::string> expected_sensor_id_strings;
+  for (const auto& sensor_id : expected_sensor_ids) {
+    expected_sensor_id_strings.insert(sensor_id.id);
+  }
   sensor_collator_->AddTrajectory(
-      trajectory_id, expected_sensor_ids,
+      trajectory_id, expected_sensor_id_strings,
       [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
         HandleCollatedSensorData(sensor_id, std::move(data));
       });

cartographer/mapping/collated_trajectory_builder.h

@@ -20,8 +20,8 @@
 #include <chrono>
 #include <map>
 #include <memory>
+#include <set>
 #include <string>
-#include <unordered_set>
 
 #include "cartographer/common/port.h"
 #include "cartographer/common/rate_timer.h"
@@ -38,9 +38,11 @@ namespace mapping {
 // a mapping::TrajectoryBuilderInterface which is common for 2D and 3D.
 class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
  public:
+  using SensorId = TrajectoryBuilderInterface::SensorId;
+
   CollatedTrajectoryBuilder(
       sensor::CollatorInterface* sensor_collator, int trajectory_id,
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      const std::set<SensorId>& expected_sensor_ids,
       std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder);
   ~CollatedTrajectoryBuilder() override;
 

cartographer/mapping/map_builder.cc

@@ -77,7 +77,7 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
 MapBuilder::~MapBuilder() {}
 
 int MapBuilder::AddTrajectoryBuilder(
-    const std::unordered_set<std::string>& expected_sensor_ids,
+    const std::set<SensorId>& expected_sensor_ids,
     const proto::TrajectoryBuilderOptions& trajectory_options,
     LocalSlamResultCallback local_slam_result_callback) {
   const int trajectory_id = trajectory_builders_.size();

cartographer/mapping/map_builder.h

@@ -20,7 +20,7 @@
 #include "cartographer/mapping/map_builder_interface.h"
 
 #include <memory>
-#include <unordered_map>
+#include <set>
 
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/mapping/pose_graph_interface.h"
@@ -46,7 +46,7 @@ class MapBuilder : public MapBuilderInterface {
   MapBuilder& operator=(const MapBuilder&) = delete;
 
   int AddTrajectoryBuilder(
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      const std::set<SensorId>& expected_sensor_ids,
       const proto::TrajectoryBuilderOptions& trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) override;
 

cartographer/mapping/map_builder_interface.h

@@ -17,8 +17,8 @@
 #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_
 #define CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_
 
+#include <set>
 #include <string>
-#include <unordered_set>
 #include <vector>
 
 #include "Eigen/Geometry"
@@ -42,6 +42,8 @@ class MapBuilderInterface {
   using LocalSlamResultCallback =
       TrajectoryBuilderInterface::LocalSlamResultCallback;
 
+  using SensorId = TrajectoryBuilderInterface::SensorId;
+
   MapBuilderInterface() {}
   virtual ~MapBuilderInterface() {}
 
@@ -50,7 +52,7 @@ class MapBuilderInterface {
 
   // Creates a new trajectory builder and returns its index.
   virtual int AddTrajectoryBuilder(
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      const std::set<SensorId>& expected_sensor_ids,
       const proto::TrajectoryBuilderOptions& trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) = 0;
 

cartographer/mapping/map_builder_test.cc

@@ -25,12 +25,14 @@
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "gtest/gtest.h"
 
+using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+
 namespace cartographer {
 namespace mapping {
 namespace {
 
-constexpr char kRangeSensorId[] = "range";
-constexpr char kIMUSensorId[] = "imu";
+const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"};
+const SensorId kIMUSensorId{SensorId::SensorType::IMU, "imu"};
 constexpr double kDuration = 4.;         // Seconds.
 constexpr double kTimeStep = 0.1;        // Seconds.
 constexpr double kTravelDistance = 1.2;  // Meters.
@@ -96,9 +98,8 @@ class MapBuilderTest : public ::testing::Test {
 
 TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
   BuildMapBuilder();
-  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_builder_options_,
+      {kRangeSensorId}, trajectory_builder_options_,
       nullptr /* GetLocalSlamResultCallbackForSubscriptions */);
   EXPECT_EQ(1, map_builder_->num_trajectory_builders());
   EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr);
@@ -111,9 +112,8 @@ TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
 TEST_F(MapBuilderTest, TrajectoryAddFinish3D) {
   SetOptionsTo3D();
   BuildMapBuilder();
-  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_builder_options_,
+      {kRangeSensorId}, trajectory_builder_options_,
       nullptr /* GetLocalSlamResultCallbackForSubscriptions */);
   EXPECT_EQ(1, map_builder_->num_trajectory_builders());
   EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr);
@@ -125,16 +125,15 @@ TEST_F(MapBuilderTest, TrajectoryAddFinish3D) {
 
 TEST_F(MapBuilderTest, LocalSlam2D) {
   BuildMapBuilder();
-  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_builder_options_,
+      {kRangeSensorId}, trajectory_builder_options_,
       GetLocalSlamResultCallback());
   TrajectoryBuilderInterface* trajectory_builder =
       map_builder_->GetTrajectoryBuilder(trajectory_id);
   const auto measurements = test::GenerateFakeRangeMeasurements(
       kTravelDistance, kDuration, kTimeStep);
   for (const auto& measurement : measurements) {
-    trajectory_builder->AddSensorData(kRangeSensorId, measurement);
+    trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
   }
   map_builder_->FinishTrajectory(trajectory_id);
   map_builder_->pose_graph()->RunFinalOptimization();
@@ -149,19 +148,17 @@ TEST_F(MapBuilderTest, LocalSlam2D) {
 TEST_F(MapBuilderTest, LocalSlam3D) {
   SetOptionsTo3D();
   BuildMapBuilder();
-  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId,
-                                                               kIMUSensorId};
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_builder_options_,
+      {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_,
       GetLocalSlamResultCallback());
   TrajectoryBuilderInterface* trajectory_builder =
       map_builder_->GetTrajectoryBuilder(trajectory_id);
   const auto measurements = test::GenerateFakeRangeMeasurements(
       kTravelDistance, kDuration, kTimeStep);
   for (const auto& measurement : measurements) {
-    trajectory_builder->AddSensorData(kRangeSensorId, measurement);
+    trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
     trajectory_builder->AddSensorData(
-        kIMUSensorId,
+        kIMUSensorId.id,
         sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
                         Eigen::Vector3d::Zero()});
   }
@@ -178,16 +175,15 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
 TEST_F(MapBuilderTest, GlobalSlam2D) {
   SetOptionsEnableGlobalOptimization();
   BuildMapBuilder();
-  const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
   int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_builder_options_,
+      {kRangeSensorId}, trajectory_builder_options_,
       GetLocalSlamResultCallback());
   TrajectoryBuilderInterface* trajectory_builder =
       map_builder_->GetTrajectoryBuilder(trajectory_id);
   const auto measurements = test::GenerateFakeRangeMeasurements(
       kTravelDistance, kDuration, kTimeStep);
   for (const auto& measurement : measurements) {
-    trajectory_builder->AddSensorData(kRangeSensorId, measurement);
+    trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
   }
   map_builder_->FinishTrajectory(trajectory_id);
   map_builder_->pose_graph()->RunFinalOptimization();

cartographer/mapping/trajectory_builder_interface.h

@@ -62,6 +62,30 @@ class TrajectoryBuilderInterface {
                          sensor::RangeData /* in local frame */,
                          std::unique_ptr<const InsertionResult>)>;
 
+  struct SensorId {
+    enum class SensorType {
+      RANGE = 0,
+      IMU,
+      ODOMETRY,
+      FIXED_FRAME_POSE,
+      LANDMARK,
+      LOCAL_SLAM_RESULT
+    };
+
+    SensorType type;
+    std::string id;
+
+    bool operator==(const SensorId& other) const {
+      return std::forward_as_tuple(type, id) ==
+             std::forward_as_tuple(other.type, other.id);
+    }
+
+    bool operator<(const SensorId& other) const {
+      return std::forward_as_tuple(type, id) <
+             std::forward_as_tuple(other.type, other.id);
+    }
+  };
+
   TrajectoryBuilderInterface() {}
   virtual ~TrajectoryBuilderInterface() {}
 

cartographer_grpc/client_server_test.cc

@@ -30,13 +30,14 @@ using cartographer::mapping::MapBuilder;
 using cartographer::mapping::MapBuilderInterface;
 using cartographer::mapping::PoseGraphInterface;
 using cartographer::mapping::TrajectoryBuilderInterface;
+using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
 using testing::_;
 
 namespace cartographer_grpc {
 namespace {
 
-constexpr char kSensorId[] = "sensor";
-constexpr char kRangeSensorId[] = "range";
+const SensorId kSensorId{SensorId::SensorType::IMU, "sensor"};
+const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"};
 constexpr double kDuration = 4.;         // Seconds.
 constexpr double kTimeStep = 0.1;        // Seconds.
 constexpr double kTravelDistance = 1.2;  // Meters.
@@ -44,7 +45,7 @@ constexpr double kTravelDistance = 1.2;  // Meters.
 class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
  public:
   MOCK_METHOD3(AddTrajectoryBuilder,
-               int(const std::unordered_set<std::string>& expected_sensor_ids,
+               int(const std::set<SensorId>& expected_sensor_ids,
                    const cartographer::mapping::proto::TrajectoryBuilderOptions&
                        trajectory_options,
                    LocalSlamResultCallback local_slam_result_callback));
@@ -199,7 +200,7 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
   InitializeServerWithMockMapBuilder();
   server_->Start();
   InitializeStub();
-  std::unordered_set<std::string> expected_sensor_ids = {kSensorId};
+  std::set<SensorId> expected_sensor_ids = {kSensorId};
   EXPECT_CALL(
       *mock_map_builder_,
       AddTrajectoryBuilder(testing::ContainerEq(expected_sensor_ids), _, _))
@@ -227,7 +228,7 @@ TEST_F(ClientServerTest, AddSensorData) {
   cartographer::sensor::ImuData imu_data{
       cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8),
       Eigen::Vector3d::Zero()};
-  trajectory_stub->AddSensorData(kSensorId, imu_data);
+  trajectory_stub->AddSensorData(kSensorId.id, imu_data);
   stub_->FinishTrajectory(trajectory_id);
   server_->Shutdown();
 }
@@ -236,7 +237,7 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
   InitializeServerWithMockMapBuilder();
   server_->Start();
   InitializeStub();
-  std::unordered_set<std::string> expected_sensor_ids = {kSensorId};
+  std::set<SensorId> expected_sensor_ids = {kSensorId};
   EXPECT_CALL(
       *mock_map_builder_,
       AddTrajectoryBuilder(testing::ContainerEq(expected_sensor_ids), _, _))
@@ -253,10 +254,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
       Eigen::Vector3d::Zero()};
   EXPECT_CALL(
       *mock_trajectory_builder_,
-      AddSensorData(testing::StrEq(kSensorId),
+      AddSensorData(testing::StrEq(kSensorId.id),
                     testing::Matcher<const cartographer::sensor::ImuData&>(_)))
       .WillOnce(testing::Return());
-  trajectory_stub->AddSensorData(kSensorId, imu_data);
+  trajectory_stub->AddSensorData(kSensorId.id, imu_data);
   EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id));
   stub_->FinishTrajectory(trajectory_id);
   server_->Shutdown();
@@ -275,7 +276,7 @@ TEST_F(ClientServerTest, LocalSlam2D) {
       cartographer::mapping::test::GenerateFakeRangeMeasurements(
           kTravelDistance, kDuration, kTimeStep);
   for (const auto& measurement : measurements) {
-    trajectory_stub->AddSensorData(kRangeSensorId, measurement);
+    trajectory_stub->AddSensorData(kRangeSensorId.id, measurement);
   }
   WaitForLocalSlamResults(measurements.size());
   stub_->FinishTrajectory(trajectory_id);

cartographer_grpc/handlers/add_trajectory_handler.h

@@ -21,6 +21,7 @@
 #include "cartographer_grpc/framework/rpc_handler.h"
 #include "cartographer_grpc/map_builder_server.h"
 #include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "cartographer_grpc/sensor/serialization.h"
 
 namespace cartographer_grpc {
 namespace handlers {
@@ -33,9 +34,11 @@ class AddTrajectoryHandler
     auto local_slam_result_callback =
         GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
             ->GetLocalSlamResultCallbackForSubscriptions();
-    std::unordered_set<std::string> expected_sensor_ids(
-        request.expected_sensor_ids().begin(),
-        request.expected_sensor_ids().end());
+    std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+        expected_sensor_ids;
+    for (const auto& sensor_id : request.expected_sensor_ids()) {
+      expected_sensor_ids.insert(sensor::FromProto(sensor_id));
+    }
     const int trajectory_id =
         GetContext<MapBuilderServer::MapBuilderContext>()
             ->map_builder()

cartographer_grpc/local_trajectory_uploader.cc

@@ -18,6 +18,7 @@
 
 #include "cartographer/common/make_unique.h"
 #include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "cartographer_grpc/sensor/serialization.h"
 #include "glog/logging.h"
 
 namespace cartographer_grpc {
@@ -120,16 +121,15 @@ void LocalTrajectoryUploader::ProcessOdometryDataMessage(
 }
 
 void LocalTrajectoryUploader::AddTrajectory(
-    int local_trajectory_id,
-    const std::unordered_set<std::string> &expected_sensor_ids,
+    int local_trajectory_id, const std::set<SensorId> &expected_sensor_ids,
     const cartographer::mapping::proto::TrajectoryBuilderOptions
         &trajectory_options) {
   grpc::ClientContext client_context;
   proto::AddTrajectoryRequest request;
   proto::AddTrajectoryResponse result;
   *request.mutable_trajectory_builder_options() = trajectory_options;
-  for (const auto &sensor_id : expected_sensor_ids) {
-    *request.add_expected_sensor_ids() = sensor_id;
+  for (const SensorId &sensor_id : expected_sensor_ids) {
+    *request.add_expected_sensor_ids() = sensor::ToProto(sensor_id);
   }
   grpc::Status status =
       service_stub_->AddTrajectory(&client_context, request, &result);

cartographer_grpc/local_trajectory_uploader.h

@@ -19,12 +19,13 @@
 
 #include <map>
 #include <memory>
+#include <set>
 #include <string>
 #include <thread>
-#include <unordered_set>
 
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer_grpc/framework/client_writer.h"
 #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
 #include "grpc++/grpc++.h"
@@ -33,6 +34,8 @@ namespace cartographer_grpc {
 
 class LocalTrajectoryUploader {
  public:
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+
   LocalTrajectoryUploader(const std::string& uplink_server_address);
   ~LocalTrajectoryUploader();
 
@@ -44,8 +47,7 @@ class LocalTrajectoryUploader {
   void Shutdown();
 
   void AddTrajectory(
-      int local_trajectory_id,
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      int local_trajectory_id, const std::set<SensorId>& expected_sensor_ids,
       const cartographer::mapping::proto::TrajectoryBuilderOptions&
           trajectory_options);
   void FinishTrajectory(int local_trajectory_id);

cartographer_grpc/mapping/map_builder_stub.cc

@@ -17,6 +17,7 @@
 #include "cartographer_grpc/mapping/map_builder_stub.h"
 
 #include "cartographer_grpc/proto/map_builder_service.pb.h"
+#include "cartographer_grpc/sensor/serialization.h"
 #include "glog/logging.h"
 
 namespace cartographer_grpc {
@@ -29,7 +30,7 @@ MapBuilderStub::MapBuilderStub(const std::string& server_address)
       pose_graph_stub_(client_channel_, service_stub_.get()) {}
 
 int MapBuilderStub::AddTrajectoryBuilder(
-    const std::unordered_set<std::string>& expected_sensor_ids,
+    const std::set<SensorId>& expected_sensor_ids,
     const cartographer::mapping::proto::TrajectoryBuilderOptions&
         trajectory_options,
     LocalSlamResultCallback local_slam_result_callback) {
@@ -38,7 +39,7 @@ int MapBuilderStub::AddTrajectoryBuilder(
   proto::AddTrajectoryResponse result;
   *request.mutable_trajectory_builder_options() = trajectory_options;
   for (const auto& sensor_id : expected_sensor_ids) {
-    *request.add_expected_sensor_ids() = sensor_id;
+    *request.add_expected_sensor_ids() = sensor::ToProto(sensor_id);
   }
   grpc::Status status =
       service_stub_->AddTrajectory(&client_context, request, &result);

cartographer_grpc/mapping/map_builder_stub.h

@@ -36,7 +36,7 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
   MapBuilderStub& operator=(const MapBuilderStub&) = delete;
 
   int AddTrajectoryBuilder(
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      const std::set<SensorId>& expected_sensor_ids,
       const cartographer::mapping::proto::TrajectoryBuilderOptions&
           trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) override;

cartographer_grpc/proto/map_builder_service.proto

@@ -24,8 +24,22 @@ import "google/protobuf/empty.proto";
 
 package cartographer_grpc.proto;
 
+enum SensorType {
+  RANGE = 0;
+  IMU = 1;
+  ODOMETRY = 2;
+  FIXED_FRAME_POSE = 3;
+  LANDMARK = 4;
+  LOCAL_SLAM_RESULT = 5;
+}
+
+message SensorId {
+  string id = 1;
+  SensorType type = 2;
+}
+
 message AddTrajectoryRequest {
-  repeated string expected_sensor_ids = 1;
+  repeated SensorId expected_sensor_ids = 3;
   cartographer.mapping.proto.TrajectoryBuilderOptions
       trajectory_builder_options = 2;
 }

cartographer_grpc/sensor/serialization.cc

@@ -72,5 +72,69 @@ void CreateAddLandmarkDataRequest(
   *proto->mutable_landmark_data() = landmark_data;
 }
 
+proto::SensorId ToProto(
+    const cartographer::mapping::TrajectoryBuilderInterface::SensorId&
+        sensor_id) {
+  using SensorType =
+      cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType;
+  proto::SensorType type;
+  switch (sensor_id.type) {
+    case SensorType::RANGE:
+      type = proto::SensorType::RANGE;
+      break;
+    case SensorType::IMU:
+      type = proto::SensorType::IMU;
+      break;
+    case SensorType::ODOMETRY:
+      type = proto::SensorType::ODOMETRY;
+      break;
+    case SensorType::FIXED_FRAME_POSE:
+      type = proto::SensorType::FIXED_FRAME_POSE;
+      break;
+    case SensorType::LANDMARK:
+      type = proto::SensorType::LANDMARK;
+      break;
+    case SensorType::LOCAL_SLAM_RESULT:
+      type = proto::SensorType::LOCAL_SLAM_RESULT;
+      break;
+    default:
+      LOG(FATAL) << "unknown SensorType";
+  }
+  proto::SensorId proto;
+  proto.set_type(type);
+  proto.set_id(sensor_id.id);
+  return proto;
+}
+
+cartographer::mapping::TrajectoryBuilderInterface::SensorId FromProto(
+    const proto::SensorId& proto) {
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+  using SensorType = SensorId::SensorType;
+  SensorType type;
+  switch (proto.type()) {
+    case proto::SensorType::RANGE:
+      type = SensorType::RANGE;
+      break;
+    case proto::SensorType::IMU:
+      type = SensorType::IMU;
+      break;
+    case proto::SensorType::ODOMETRY:
+      type = SensorType::ODOMETRY;
+      break;
+    case proto::SensorType::FIXED_FRAME_POSE:
+      type = SensorType::FIXED_FRAME_POSE;
+      break;
+    case proto::SensorType::LANDMARK:
+      type = SensorType::LANDMARK;
+      break;
+    case proto::SensorType::LOCAL_SLAM_RESULT:
+      type = SensorType::LOCAL_SLAM_RESULT;
+      break;
+    default:
+      LOG(FATAL) << "unknown proto::SensorType";
+  }
+  return SensorId{type, proto.id()};
+}
+
 }  // namespace sensor
 }  // namespace cartographer_grpc

cartographer_grpc/sensor/serialization.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H
 #define CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H
 
+#include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/landmark_data.h"
@@ -53,6 +54,13 @@ void CreateAddLandmarkDataRequest(
     const cartographer::sensor::proto::LandmarkData& landmark_data,
     proto::AddLandmarkDataRequest* proto);
 
+proto::SensorId ToProto(
+    const cartographer::mapping::TrajectoryBuilderInterface::SensorId&
+        sensor_id);
+
+cartographer::mapping::TrajectoryBuilderInterface::SensorId FromProto(
+    const proto::SensorId& proto);
+
 }  // namespace sensor
 }  // namespace cartographer_grpc