From dab69e0ca034df59773004649354150568e57e2d Mon Sep 17 00:00:00 2001 From: gaschler Date: Fri, 26 Jan 2018 15:07:49 +0100 Subject: [PATCH] Struct SensorId (#839) * WIP, started unordered_set * 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. --- .../mapping/collated_trajectory_builder.cc | 8 ++- .../mapping/collated_trajectory_builder.h | 6 +- cartographer/mapping/map_builder.cc | 2 +- cartographer/mapping/map_builder.h | 4 +- cartographer/mapping/map_builder_interface.h | 6 +- cartographer/mapping/map_builder_test.cc | 30 ++++----- .../mapping/trajectory_builder_interface.h | 24 +++++++ cartographer_grpc/client_server_test.cc | 19 +++--- .../handlers/add_trajectory_handler.h | 9 ++- .../local_trajectory_uploader.cc | 8 +-- cartographer_grpc/local_trajectory_uploader.h | 8 ++- cartographer_grpc/mapping/map_builder_stub.cc | 5 +- cartographer_grpc/mapping/map_builder_stub.h | 2 +- .../proto/map_builder_service.proto | 16 ++++- cartographer_grpc/sensor/serialization.cc | 64 +++++++++++++++++++ cartographer_grpc/sensor/serialization.h | 8 +++ 16 files changed, 170 insertions(+), 49 deletions(-) diff --git a/cartographer/mapping/collated_trajectory_builder.cc b/cartographer/mapping/collated_trajectory_builder.cc index b50b3d4..4245988 100644 --- a/cartographer/mapping/collated_trajectory_builder.cc +++ b/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& expected_sensor_ids, + const std::set& expected_sensor_ids, std::unique_ptr 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 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 data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h index de6f221..6d4ebf1 100644 --- a/cartographer/mapping/collated_trajectory_builder.h +++ b/cartographer/mapping/collated_trajectory_builder.h @@ -20,8 +20,8 @@ #include #include #include +#include #include -#include #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& expected_sensor_ids, + const std::set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder); ~CollatedTrajectoryBuilder() override; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index a6ee1d1..69e3d84 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -77,7 +77,7 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) MapBuilder::~MapBuilder() {} int MapBuilder::AddTrajectoryBuilder( - const std::unordered_set& expected_sensor_ids, + const std::set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) { const int trajectory_id = trajectory_builders_.size(); diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index de17686..87c3b54 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -20,7 +20,7 @@ #include "cartographer/mapping/map_builder_interface.h" #include -#include +#include #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& expected_sensor_ids, + const std::set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index b4b0ba6..ec19cd1 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/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 #include -#include #include #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& expected_sensor_ids, + const std::set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) = 0; diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index f11dd50..dfb16c0 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/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 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 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 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 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 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(); diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index 4c386e9..cc220bd 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -62,6 +62,30 @@ class TrajectoryBuilderInterface { sensor::RangeData /* in local frame */, std::unique_ptr)>; + 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() {} diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index 8ddb6d5..3ca6dc9 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/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& expected_sensor_ids, + int(const std::set& 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 expected_sensor_ids = {kSensorId}; + std::set 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 expected_sensor_ids = {kSensorId}; + std::set 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(_))) .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); diff --git a/cartographer_grpc/handlers/add_trajectory_handler.h b/cartographer_grpc/handlers/add_trajectory_handler.h index 17598a5..e217947 100644 --- a/cartographer_grpc/handlers/add_trajectory_handler.h +++ b/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() ->GetLocalSlamResultCallbackForSubscriptions(); - std::unordered_set expected_sensor_ids( - request.expected_sensor_ids().begin(), - request.expected_sensor_ids().end()); + std::set + 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() ->map_builder() diff --git a/cartographer_grpc/local_trajectory_uploader.cc b/cartographer_grpc/local_trajectory_uploader.cc index 66ba0d0..64585ba 100644 --- a/cartographer_grpc/local_trajectory_uploader.cc +++ b/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 &expected_sensor_ids, + int local_trajectory_id, const std::set &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); diff --git a/cartographer_grpc/local_trajectory_uploader.h b/cartographer_grpc/local_trajectory_uploader.h index 0b7e8bc..5788f6b 100644 --- a/cartographer_grpc/local_trajectory_uploader.h +++ b/cartographer_grpc/local_trajectory_uploader.h @@ -19,12 +19,13 @@ #include #include +#include #include #include -#include #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& expected_sensor_ids, + int local_trajectory_id, const std::set& expected_sensor_ids, const cartographer::mapping::proto::TrajectoryBuilderOptions& trajectory_options); void FinishTrajectory(int local_trajectory_id); diff --git a/cartographer_grpc/mapping/map_builder_stub.cc b/cartographer_grpc/mapping/map_builder_stub.cc index 61efb16..6747564 100644 --- a/cartographer_grpc/mapping/map_builder_stub.cc +++ b/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& expected_sensor_ids, + const std::set& 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); diff --git a/cartographer_grpc/mapping/map_builder_stub.h b/cartographer_grpc/mapping/map_builder_stub.h index e375a2b..7bb98d8 100644 --- a/cartographer_grpc/mapping/map_builder_stub.h +++ b/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& expected_sensor_ids, + const std::set& expected_sensor_ids, const cartographer::mapping::proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) override; diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index ac3d09d..b3021e0 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/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; } diff --git a/cartographer_grpc/sensor/serialization.cc b/cartographer_grpc/sensor/serialization.cc index 8ed1039..67ffe2d 100644 --- a/cartographer_grpc/sensor/serialization.cc +++ b/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 diff --git a/cartographer_grpc/sensor/serialization.h b/cartographer_grpc/sensor/serialization.h index 529b7ad..8559ac4 100644 --- a/cartographer_grpc/sensor/serialization.h +++ b/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