12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Dynamic dispatch of sensor::Data. (#475) 

This ensures at compile time that all types of data are handled,
and only keeps the data needed for each individual type.
master
Wolfgang Hess 2017-08-24 14:17:51 +02:00 committed by GitHub
parent 0671e8835e
commit a239b71a6e
7 changed files with 145 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -66,7 +66,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
.first; .first;
} }
it->second.Pulse(data->time); it->second.Pulse(data->GetTime());
if (std::chrono::steady_clock::now() - last_logging_time_ > if (std::chrono::steady_clock::now() - last_logging_time_ >
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
@ -76,29 +76,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
last_logging_time_ = std::chrono::steady_clock::now(); last_logging_time_ = std::chrono::steady_clock::now();
} }
switch (data->type) { data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
case sensor::Data::Type::kImu:
wrapped_trajectory_builder_->AddImuData(
sensor::ImuData{data->time, data->imu.linear_acceleration,
data->imu.angular_velocity});
return;
case sensor::Data::Type::kRangefinder:
wrapped_trajectory_builder_->AddRangefinderData(
data->time, data->rangefinder.origin, data->rangefinder.ranges);
return;
case sensor::Data::Type::kOdometer:
wrapped_trajectory_builder_->AddOdometerData(data->time,
data->odometer_pose);
return;
case sensor::Data::Type::kFixedFramePose:
wrapped_trajectory_builder_->AddFixedFramePoseData(
sensor::FixedFramePoseData{data->time, data->fixed_frame_pose.pose});
return;
}
LOG(FATAL);
} }
} // namespace mapping } // namespace mapping

20
cartographer/mapping/trajectory_builder.h
View File

@ -58,28 +58,30 @@ class TrajectoryBuilder {
const Eigen::Vector3f& origin, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) { const sensor::PointCloud& ranges) {
AddSensorData(sensor_id, AddSensorData(sensor_id,
common::make_unique<sensor::Data>( common::make_unique<sensor::DispatchableRangefinderData>(
time, sensor::Data::Rangefinder{origin, ranges})); time, origin, ranges));
} }
void AddImuData(const string& sensor_id, common::Time time, void AddImuData(const string& sensor_id, common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) { const Eigen::Vector3d& angular_velocity) {
AddSensorData(sensor_id, common::make_unique<sensor::Data>( AddSensorData(sensor_id, common::make_unique<sensor::DispatchableImuData>(
time, sensor::Data::Imu{linear_acceleration, sensor::ImuData{time, linear_acceleration,
angular_velocity})); angular_velocity}));
} }
void AddOdometerData(const string& sensor_id, common::Time time, void AddOdometerData(const string& sensor_id, common::Time time,
const transform::Rigid3d& odometer_pose) { const transform::Rigid3d& odometer_pose) {
AddSensorData(sensor_id, AddSensorData(sensor_id,
common::make_unique<sensor::Data>(time, odometer_pose)); common::make_unique<sensor::DispatchableOdometerData>(
time, odometer_pose));
} }
void AddFixedFramePose(const string& sensor_id, common::Time time, void AddFixedFramePoseData(const string& sensor_id, common::Time time,
const transform::Rigid3d& fixed_frame_pose) { const transform::Rigid3d& fixed_frame_pose) {
AddSensorData(sensor_id, AddSensorData(sensor_id,
common::make_unique<sensor::Data>(time, fixed_frame_pose)); common::make_unique<sensor::DispatchableFixedFramePoseData>(
time, fixed_frame_pose));
} }
}; };

78
cartographer/sensor/collator_test.cc
View File

@ -32,62 +32,76 @@ namespace {
TEST(Collator, Ordering) { TEST(Collator, Ordering) {
const std::array<string, 4> kSensorId = { const std::array<string, 4> kSensorId = {
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
Data zero(common::FromUniversal(0), Data::Rangefinder{}); DispatchableRangefinderData zero(common::FromUniversal(0),
Data first(common::FromUniversal(100), Data::Rangefinder{}); Eigen::Vector3f::Zero(), {});
Data second(common::FromUniversal(200), Data::Rangefinder{}); DispatchableRangefinderData first(common::FromUniversal(100),
Data third(common::FromUniversal(300), Data::Imu{}); Eigen::Vector3f::Zero(), {});
Data fourth(common::FromUniversal(400), Data::Rangefinder{}); DispatchableRangefinderData second(common::FromUniversal(200),
Data fifth(common::FromUniversal(500), Data::Rangefinder{}); Eigen::Vector3f::Zero(), {});
Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity()); DispatchableImuData third(ImuData{common::FromUniversal(300)});
DispatchableRangefinderData fourth(common::FromUniversal(400),
Eigen::Vector3f::Zero(), {});
DispatchableRangefinderData fifth(common::FromUniversal(500),
Eigen::Vector3f::Zero(), {});
DispatchableOdometerData sixth(common::FromUniversal(600),
transform::Rigid3d::Identity());
std::vector<std::pair<string, Data>> received; std::vector<std::pair<string, common::Time>> received;
Collator collator; Collator collator;
collator.AddTrajectory( collator.AddTrajectory(
0, std::unordered_set<string>(kSensorId.begin(), kSensorId.end()), 0, std::unordered_set<string>(kSensorId.begin(), kSensorId.end()),
[&received](const string& sensor_id, std::unique_ptr<Data> data) { [&received](const string& sensor_id, std::unique_ptr<Data> data) {
received.push_back(std::make_pair(sensor_id, *data)); received.push_back(std::make_pair(sensor_id, data->GetTime()));
}); });
constexpr int kTrajectoryId = 0; constexpr int kTrajectoryId = 0;
// Establish a common start time. // Establish a common start time.
collator.AddSensorData(kTrajectoryId, kSensorId[0], collator.AddSensorData(
common::make_unique<Data>(zero)); kTrajectoryId, kSensorId[0],
collator.AddSensorData(kTrajectoryId, kSensorId[1], common::make_unique<DispatchableRangefinderData>(zero));
common::make_unique<Data>(zero)); collator.AddSensorData(
collator.AddSensorData(kTrajectoryId, kSensorId[2], kTrajectoryId, kSensorId[1],
common::make_unique<Data>(zero)); common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(kTrajectoryId, kSensorId[3], collator.AddSensorData(
common::make_unique<Data>(zero)); kTrajectoryId, kSensorId[2],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(
kTrajectoryId, kSensorId[3],
common::make_unique<DispatchableRangefinderData>(zero));
collator.AddSensorData(kTrajectoryId, kSensorId[0], collator.AddSensorData(
common::make_unique<Data>(first)); kTrajectoryId, kSensorId[0],
common::make_unique<DispatchableRangefinderData>(first));
collator.AddSensorData(kTrajectoryId, kSensorId[3], collator.AddSensorData(kTrajectoryId, kSensorId[3],
common::make_unique<Data>(sixth)); common::make_unique<DispatchableOdometerData>(sixth));
collator.AddSensorData(kTrajectoryId, kSensorId[0], collator.AddSensorData(
common::make_unique<Data>(fourth)); kTrajectoryId, kSensorId[0],
collator.AddSensorData(kTrajectoryId, kSensorId[1], common::make_unique<DispatchableRangefinderData>(fourth));
common::make_unique<Data>(second)); collator.AddSensorData(
collator.AddSensorData(kTrajectoryId, kSensorId[1], kTrajectoryId, kSensorId[1],
common::make_unique<Data>(fifth)); common::make_unique<DispatchableRangefinderData>(second));
collator.AddSensorData(
kTrajectoryId, kSensorId[1],
common::make_unique<DispatchableRangefinderData>(fifth));
collator.AddSensorData(kTrajectoryId, kSensorId[2], collator.AddSensorData(kTrajectoryId, kSensorId[2],
common::make_unique<Data>(third)); common::make_unique<DispatchableImuData>(third));
ASSERT_EQ(7, received.size()); ASSERT_EQ(7, received.size());
EXPECT_EQ(100, common::ToUniversal(received[4].second.time)); EXPECT_EQ(100, common::ToUniversal(received[4].second));
EXPECT_EQ(kSensorId[0], received[4].first); EXPECT_EQ(kSensorId[0], received[4].first);
EXPECT_EQ(200, common::ToUniversal(received[5].second.time)); EXPECT_EQ(200, common::ToUniversal(received[5].second));
EXPECT_EQ(kSensorId[1], received[5].first); EXPECT_EQ(kSensorId[1], received[5].first);
EXPECT_EQ(300, common::ToUniversal(received[6].second.time)); EXPECT_EQ(300, common::ToUniversal(received[6].second));
EXPECT_EQ(kSensorId[2], received[6].first); EXPECT_EQ(kSensorId[2], received[6].first);
collator.Flush(); collator.Flush();
ASSERT_EQ(10, received.size()); ASSERT_EQ(10, received.size());
EXPECT_EQ(kSensorId[0], received[7].first); EXPECT_EQ(kSensorId[0], received[7].first);
EXPECT_EQ(500, common::ToUniversal(received[8].second.time)); EXPECT_EQ(500, common::ToUniversal(received[8].second));
EXPECT_EQ(kSensorId[1], received[8].first); EXPECT_EQ(kSensorId[1], received[8].first);
EXPECT_EQ(600, common::ToUniversal(received[9].second.time)); EXPECT_EQ(600, common::ToUniversal(received[9].second));
EXPECT_EQ(kSensorId[3], received[9].first); EXPECT_EQ(kSensorId[3], received[9].first);
} }

100
cartographer/sensor/data.h
View File

@ -18,6 +18,9 @@
#define CARTOGRAPHER_MAPPING_DATA_H_ #define CARTOGRAPHER_MAPPING_DATA_H_
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -25,46 +28,79 @@
namespace cartographer { namespace cartographer {
namespace sensor { namespace sensor {
// This type is a logical union, i.e. only one type of sensor data is actually class Data {
// filled in. It is only used for time ordering sensor data before passing it public:
// on. virtual ~Data() {}
struct Data {
enum class Type { kImu, kRangefinder, kOdometer, kFixedFramePose };
struct Imu { virtual common::Time GetTime() const = 0;
Eigen::Vector3d linear_acceleration; virtual void AddToTrajectoryBuilder(
Eigen::Vector3d angular_velocity; mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
}; };
struct Rangefinder { class DispatchableImuData : public Data {
Eigen::Vector3f origin; public:
PointCloud ranges; DispatchableImuData(const ImuData& imu_data) : imu_data_(imu_data) {}
};
struct FixedFramePose { common::Time GetTime() const override { return imu_data_.time; }
transform::Rigid3d pose; void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
}; trajectory_builder) override {
trajectory_builder->AddImuData(imu_data_);
}
Data(const common::Time time, const Imu& imu) private:
: type(Type::kImu), time(time), imu(imu) {} const ImuData imu_data_;
};
Data(const common::Time time, const Rangefinder& rangefinder) class DispatchableRangefinderData : public Data {
: type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} public:
DispatchableRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const PointCloud& ranges)
: time_(time), origin_(origin), ranges_(ranges) {}
Data(const common::Time time, const transform::Rigid3d& odometer_pose) common::Time GetTime() const override { return time_; }
: type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
}
Data(const common::Time time, const FixedFramePose& fixed_frame_pose) private:
: type(Type::kFixedFramePose), const common::Time time_;
time(time), const Eigen::Vector3f origin_;
fixed_frame_pose(fixed_frame_pose) {} const PointCloud ranges_;
};
Type type; class DispatchableOdometerData : public Data {
common::Time time; public:
Imu imu; DispatchableOdometerData(const common::Time time,
Rangefinder rangefinder; const transform::Rigid3d& odometer_pose)
transform::Rigid3d odometer_pose; : time_(time), odometer_pose_(odometer_pose) {}
FixedFramePose fixed_frame_pose;
common::Time GetTime() const override { return time_; }
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddOdometerData(time_, odometer_pose_);
}
private:
const common::Time time_;
const transform::Rigid3d odometer_pose_;
};
class DispatchableFixedFramePoseData : public Data {
public:
DispatchableFixedFramePoseData(const common::Time time,
const transform::Rigid3d& fixed_frame_pose)
: fixed_frame_pose_data_{time, fixed_frame_pose} {}
common::Time GetTime() const override { return fixed_frame_pose_data_.time; }
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
trajectory_builder) override {
trajectory_builder->AddFixedFramePoseData(fixed_frame_pose_data_);
}
private:
const FixedFramePoseData fixed_frame_pose_data_;
}; };
} // namespace sensor } // namespace sensor

18
cartographer/sensor/ordered_multi_queue.cc
View File

@ -104,12 +104,12 @@ void OrderedMultiQueue::Dispatch() {
CannotMakeProgress(it->first); CannotMakeProgress(it->first);
return; return;
} }
if (next_data == nullptr || data->time < next_data->time) { if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
next_data = data; next_data = data;
next_queue = &it->second; next_queue = &it->second;
next_queue_key = it->first; next_queue_key = it->first;
} }
CHECK_LE(last_dispatched_time_, next_data->time) CHECK_LE(last_dispatched_time_, next_data->GetTime())
<< "Non-sorted data added to queue: '" << it->first << "'"; << "Non-sorted data added to queue: '" << it->first << "'";
++it; ++it;
} }
@ -123,9 +123,9 @@ void OrderedMultiQueue::Dispatch() {
const common::Time common_start_time = const common::Time common_start_time =
GetCommonStartTime(next_queue_key.trajectory_id); GetCommonStartTime(next_queue_key.trajectory_id);
if (next_data->time >= common_start_time) { if (next_data->GetTime() >= common_start_time) {
// Happy case, we are beyond the 'common_start_time' already. // Happy case, we are beyond the 'common_start_time' already.
last_dispatched_time_ = next_data->time; last_dispatched_time_ = next_data->GetTime();
next_queue->callback(next_queue->queue.Pop()); next_queue->callback(next_queue->queue.Pop());
} else if (next_queue->queue.Size() < 2) { } else if (next_queue->queue.Size() < 2) {
if (!next_queue->finished) { if (!next_queue->finished) {
@ -133,15 +133,15 @@ void OrderedMultiQueue::Dispatch() {
CannotMakeProgress(next_queue_key); CannotMakeProgress(next_queue_key);
return; return;
} }
last_dispatched_time_ = next_data->time; last_dispatched_time_ = next_data->GetTime();
next_queue->callback(next_queue->queue.Pop()); next_queue->callback(next_queue->queue.Pop());
} else { } else {
// We take a peek at the time after next data. If it also is not beyond // We take a peek at the time after next data. If it also is not beyond
// 'common_start_time' we drop 'next_data', otherwise we just found the // 'common_start_time' we drop 'next_data', otherwise we just found the
// first packet to dispatch from this queue. // first packet to dispatch from this queue.
std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop(); std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
if (next_queue->queue.Peek<Data>()->time > common_start_time) { if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
last_dispatched_time_ = next_data->time; last_dispatched_time_ = next_data->GetTime();
next_queue->callback(std::move(next_data_owner)); next_queue->callback(std::move(next_data_owner));
} }
} }
@ -165,8 +165,8 @@ common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) {
if (emplace_result.second) { if (emplace_result.second) {
for (auto& entry : queues_) { for (auto& entry : queues_) {
if (entry.first.trajectory_id == trajectory_id) { if (entry.first.trajectory_id == trajectory_id) {
common_start_time = common_start_time = std::max(
std::max(common_start_time, entry.second.queue.Peek<Data>()->time); common_start_time, entry.second.queue.Peek<Data>()->GetTime());
} }
} }
LOG(INFO) << "All sensor data for trajectory " << trajectory_id LOG(INFO) << "All sensor data for trajectory " << trajectory_id

16
cartographer/sensor/ordered_multi_queue_test.cc
View File

@ -36,20 +36,20 @@ class OrderedMultiQueueTest : public ::testing::Test {
for (const auto& queue_key : {kFirst, kSecond, kThird}) { for (const auto& queue_key : {kFirst, kSecond, kThird}) {
queue_.AddQueue(queue_key, [this](std::unique_ptr<Data> data) { queue_.AddQueue(queue_key, [this](std::unique_ptr<Data> data) {
if (!values_.empty()) { if (!values_.empty()) {
EXPECT_GE(data->time, values_.back().time); EXPECT_GE(data->GetTime(), values_.back()->GetTime());
} }
values_.push_back(*data); values_.push_back(std::move(data));
}); });
} }
} }
std::unique_ptr<Data> MakeImu(const int ordinal) { std::unique_ptr<Data> MakeImu(const int ordinal) {
return common::make_unique<Data>( return common::make_unique<DispatchableImuData>(
common::FromUniversal(ordinal), sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); Eigen::Vector3d::Zero()});
} }
std::vector<Data> values_; std::vector<std::unique_ptr<Data>> values_;
OrderedMultiQueue queue_; OrderedMultiQueue queue_;
}; };
@ -73,7 +73,7 @@ TEST_F(OrderedMultiQueueTest, Ordering) {
EXPECT_EQ(11, values_.size()); EXPECT_EQ(11, values_.size());
for (size_t i = 0; i < values_.size() - 1; ++i) { for (size_t i = 0; i < values_.size() - 1; ++i) {
EXPECT_LE(values_[i].time, values_[i + 1].time); EXPECT_LE(values_[i]->GetTime(), values_[i + 1]->GetTime());
} }
} }
@ -90,7 +90,7 @@ TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) {
EXPECT_EQ(3, values_.size()); EXPECT_EQ(3, values_.size());
for (size_t i = 0; i < values_.size(); ++i) { for (size_t i = 0; i < values_.size(); ++i) {
EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); EXPECT_EQ(i + 1, common::ToUniversal(values_[i]->GetTime()));
} }
} }