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.
master
gaschler 2018-01-26 15:07:49 +01:00 committed by Alexander Belyaev
parent 1d2613c8e2
commit dab69e0ca0
16 changed files with 170 additions and 49 deletions

View File

@ -30,14 +30,18 @@ constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
sensor::CollatorInterface* const sensor_collator, const int trajectory_id, 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) std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator), : sensor_collator_(sensor_collator),
trajectory_id_(trajectory_id), trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) { 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( 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) { [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data)); HandleCollatedSensorData(sensor_id, std::move(data));
}); });

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -62,6 +62,30 @@ class TrajectoryBuilderInterface {
sensor::RangeData /* in local frame */, sensor::RangeData /* in local frame */,
std::unique_ptr<const InsertionResult>)>; 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() {} TrajectoryBuilderInterface() {}
virtual ~TrajectoryBuilderInterface() {} virtual ~TrajectoryBuilderInterface() {}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -24,8 +24,22 @@ import "google/protobuf/empty.proto";
package cartographer_grpc.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 { message AddTrajectoryRequest {
repeated string expected_sensor_ids = 1; repeated SensorId expected_sensor_ids = 3;
cartographer.mapping.proto.TrajectoryBuilderOptions cartographer.mapping.proto.TrajectoryBuilderOptions
trajectory_builder_options = 2; trajectory_builder_options = 2;
} }

View File

@ -72,5 +72,69 @@ void CreateAddLandmarkDataRequest(
*proto->mutable_landmark_data() = landmark_data; *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 sensor
} // namespace cartographer_grpc } // namespace cartographer_grpc

View File

@ -17,6 +17,7 @@
#ifndef CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H #ifndef CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H
#define 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/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
@ -53,6 +54,13 @@ void CreateAddLandmarkDataRequest(
const cartographer::sensor::proto::LandmarkData& landmark_data, const cartographer::sensor::proto::LandmarkData& landmark_data,
proto::AddLandmarkDataRequest* proto); 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 sensor
} // namespace cartographer_grpc } // namespace cartographer_grpc