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
GitHub
parent
0671e8835e
commit
a239b71a6e
|
|
@ -66,7 +66,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
|
|||
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
|
||||
.first;
|
||||
}
|
||||
it->second.Pulse(data->time);
|
||||
it->second.Pulse(data->GetTime());
|
||||
|
||||
if (std::chrono::steady_clock::now() - last_logging_time_ >
|
||||
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
|
||||
|
|
@ -76,29 +76,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
|
|||
last_logging_time_ = std::chrono::steady_clock::now();
|
||||
}
|
||||
|
||||
switch (data->type) {
|
||||
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);
|
||||
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
|
|
|
|||
|
|
@ -58,28 +58,30 @@ class TrajectoryBuilder {
|
|||
const Eigen::Vector3f& origin,
|
||||
const sensor::PointCloud& ranges) {
|
||||
AddSensorData(sensor_id,
|
||||
common::make_unique<sensor::Data>(
|
||||
time, sensor::Data::Rangefinder{origin, ranges}));
|
||||
common::make_unique<sensor::DispatchableRangefinderData>(
|
||||
time, origin, ranges));
|
||||
}
|
||||
|
||||
void AddImuData(const string& sensor_id, common::Time time,
|
||||
const Eigen::Vector3d& linear_acceleration,
|
||||
const Eigen::Vector3d& angular_velocity) {
|
||||
AddSensorData(sensor_id, common::make_unique<sensor::Data>(
|
||||
time, sensor::Data::Imu{linear_acceleration,
|
||||
AddSensorData(sensor_id, common::make_unique<sensor::DispatchableImuData>(
|
||||
sensor::ImuData{time, linear_acceleration,
|
||||
angular_velocity}));
|
||||
}
|
||||
|
||||
void AddOdometerData(const string& sensor_id, common::Time time,
|
||||
const transform::Rigid3d& odometer_pose) {
|
||||
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) {
|
||||
AddSensorData(sensor_id,
|
||||
common::make_unique<sensor::Data>(time, fixed_frame_pose));
|
||||
common::make_unique<sensor::DispatchableFixedFramePoseData>(
|
||||
time, fixed_frame_pose));
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -32,62 +32,76 @@ namespace {
|
|||
TEST(Collator, Ordering) {
|
||||
const std::array<string, 4> kSensorId = {
|
||||
{"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}};
|
||||
Data zero(common::FromUniversal(0), Data::Rangefinder{});
|
||||
Data first(common::FromUniversal(100), Data::Rangefinder{});
|
||||
Data second(common::FromUniversal(200), Data::Rangefinder{});
|
||||
Data third(common::FromUniversal(300), Data::Imu{});
|
||||
Data fourth(common::FromUniversal(400), Data::Rangefinder{});
|
||||
Data fifth(common::FromUniversal(500), Data::Rangefinder{});
|
||||
Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity());
|
||||
DispatchableRangefinderData zero(common::FromUniversal(0),
|
||||
Eigen::Vector3f::Zero(), {});
|
||||
DispatchableRangefinderData first(common::FromUniversal(100),
|
||||
Eigen::Vector3f::Zero(), {});
|
||||
DispatchableRangefinderData second(common::FromUniversal(200),
|
||||
Eigen::Vector3f::Zero(), {});
|
||||
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.AddTrajectory(
|
||||
0, std::unordered_set<string>(kSensorId.begin(), kSensorId.end()),
|
||||
[&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;
|
||||
|
||||
// Establish a common start time.
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<Data>(zero));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<Data>(zero));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[2],
|
||||
common::make_unique<Data>(zero));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[3],
|
||||
common::make_unique<Data>(zero));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<DispatchableRangefinderData>(zero));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<DispatchableRangefinderData>(zero));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[2],
|
||||
common::make_unique<DispatchableRangefinderData>(zero));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[3],
|
||||
common::make_unique<DispatchableRangefinderData>(zero));
|
||||
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<Data>(first));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<DispatchableRangefinderData>(first));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[3],
|
||||
common::make_unique<Data>(sixth));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<Data>(fourth));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<Data>(second));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<Data>(fifth));
|
||||
common::make_unique<DispatchableOdometerData>(sixth));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[0],
|
||||
common::make_unique<DispatchableRangefinderData>(fourth));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<DispatchableRangefinderData>(second));
|
||||
collator.AddSensorData(
|
||||
kTrajectoryId, kSensorId[1],
|
||||
common::make_unique<DispatchableRangefinderData>(fifth));
|
||||
collator.AddSensorData(kTrajectoryId, kSensorId[2],
|
||||
common::make_unique<Data>(third));
|
||||
common::make_unique<DispatchableImuData>(third));
|
||||
|
||||
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(200, common::ToUniversal(received[5].second.time));
|
||||
EXPECT_EQ(200, common::ToUniversal(received[5].second));
|
||||
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);
|
||||
|
||||
collator.Flush();
|
||||
|
||||
ASSERT_EQ(10, received.size());
|
||||
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(600, common::ToUniversal(received[9].second.time));
|
||||
EXPECT_EQ(600, common::ToUniversal(received[9].second));
|
||||
EXPECT_EQ(kSensorId[3], received[9].first);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@
|
|||
#define CARTOGRAPHER_MAPPING_DATA_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/range_data.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
|
|
@ -25,46 +28,79 @@
|
|||
namespace cartographer {
|
||||
namespace sensor {
|
||||
|
||||
// This type is a logical union, i.e. only one type of sensor data is actually
|
||||
// filled in. It is only used for time ordering sensor data before passing it
|
||||
// on.
|
||||
struct Data {
|
||||
enum class Type { kImu, kRangefinder, kOdometer, kFixedFramePose };
|
||||
class Data {
|
||||
public:
|
||||
virtual ~Data() {}
|
||||
|
||||
struct Imu {
|
||||
Eigen::Vector3d linear_acceleration;
|
||||
Eigen::Vector3d angular_velocity;
|
||||
};
|
||||
virtual common::Time GetTime() const = 0;
|
||||
virtual void AddToTrajectoryBuilder(
|
||||
mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
|
||||
};
|
||||
|
||||
struct Rangefinder {
|
||||
Eigen::Vector3f origin;
|
||||
PointCloud ranges;
|
||||
};
|
||||
class DispatchableImuData : public Data {
|
||||
public:
|
||||
DispatchableImuData(const ImuData& imu_data) : imu_data_(imu_data) {}
|
||||
|
||||
struct FixedFramePose {
|
||||
transform::Rigid3d pose;
|
||||
};
|
||||
common::Time GetTime() const override { return imu_data_.time; }
|
||||
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
|
||||
trajectory_builder) override {
|
||||
trajectory_builder->AddImuData(imu_data_);
|
||||
}
|
||||
|
||||
Data(const common::Time time, const Imu& imu)
|
||||
: type(Type::kImu), time(time), imu(imu) {}
|
||||
private:
|
||||
const ImuData imu_data_;
|
||||
};
|
||||
|
||||
Data(const common::Time time, const Rangefinder& rangefinder)
|
||||
: type(Type::kRangefinder), time(time), rangefinder(rangefinder) {}
|
||||
class DispatchableRangefinderData : public Data {
|
||||
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)
|
||||
: type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {}
|
||||
common::Time GetTime() const override { return time_; }
|
||||
void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
|
||||
trajectory_builder) override {
|
||||
trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
|
||||
}
|
||||
|
||||
Data(const common::Time time, const FixedFramePose& fixed_frame_pose)
|
||||
: type(Type::kFixedFramePose),
|
||||
time(time),
|
||||
fixed_frame_pose(fixed_frame_pose) {}
|
||||
private:
|
||||
const common::Time time_;
|
||||
const Eigen::Vector3f origin_;
|
||||
const PointCloud ranges_;
|
||||
};
|
||||
|
||||
Type type;
|
||||
common::Time time;
|
||||
Imu imu;
|
||||
Rangefinder rangefinder;
|
||||
transform::Rigid3d odometer_pose;
|
||||
FixedFramePose fixed_frame_pose;
|
||||
class DispatchableOdometerData : public Data {
|
||||
public:
|
||||
DispatchableOdometerData(const common::Time time,
|
||||
const transform::Rigid3d& odometer_pose)
|
||||
: time_(time), odometer_pose_(odometer_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
|
||||
|
|
|
|||
|
|
@ -104,12 +104,12 @@ void OrderedMultiQueue::Dispatch() {
|
|||
CannotMakeProgress(it->first);
|
||||
return;
|
||||
}
|
||||
if (next_data == nullptr || data->time < next_data->time) {
|
||||
if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
|
||||
next_data = data;
|
||||
next_queue = &it->second;
|
||||
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 << "'";
|
||||
++it;
|
||||
}
|
||||
|
|
@ -123,9 +123,9 @@ void OrderedMultiQueue::Dispatch() {
|
|||
const common::Time common_start_time =
|
||||
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.
|
||||
last_dispatched_time_ = next_data->time;
|
||||
last_dispatched_time_ = next_data->GetTime();
|
||||
next_queue->callback(next_queue->queue.Pop());
|
||||
} else if (next_queue->queue.Size() < 2) {
|
||||
if (!next_queue->finished) {
|
||||
|
|
@ -133,15 +133,15 @@ void OrderedMultiQueue::Dispatch() {
|
|||
CannotMakeProgress(next_queue_key);
|
||||
return;
|
||||
}
|
||||
last_dispatched_time_ = next_data->time;
|
||||
last_dispatched_time_ = next_data->GetTime();
|
||||
next_queue->callback(next_queue->queue.Pop());
|
||||
} else {
|
||||
// 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
|
||||
// first packet to dispatch from this queue.
|
||||
std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
|
||||
if (next_queue->queue.Peek<Data>()->time > common_start_time) {
|
||||
last_dispatched_time_ = next_data->time;
|
||||
if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
|
||||
last_dispatched_time_ = next_data->GetTime();
|
||||
next_queue->callback(std::move(next_data_owner));
|
||||
}
|
||||
}
|
||||
|
|
@ -165,8 +165,8 @@ common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) {
|
|||
if (emplace_result.second) {
|
||||
for (auto& entry : queues_) {
|
||||
if (entry.first.trajectory_id == trajectory_id) {
|
||||
common_start_time =
|
||||
std::max(common_start_time, entry.second.queue.Peek<Data>()->time);
|
||||
common_start_time = std::max(
|
||||
common_start_time, entry.second.queue.Peek<Data>()->GetTime());
|
||||
}
|
||||
}
|
||||
LOG(INFO) << "All sensor data for trajectory " << trajectory_id
|
||||
|
|
|
|||
|
|
@ -36,20 +36,20 @@ class OrderedMultiQueueTest : public ::testing::Test {
|
|||
for (const auto& queue_key : {kFirst, kSecond, kThird}) {
|
||||
queue_.AddQueue(queue_key, [this](std::unique_ptr<Data> data) {
|
||||
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) {
|
||||
return common::make_unique<Data>(
|
||||
common::FromUniversal(ordinal),
|
||||
Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()});
|
||||
return common::make_unique<DispatchableImuData>(
|
||||
sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(),
|
||||
Eigen::Vector3d::Zero()});
|
||||
}
|
||||
|
||||
std::vector<Data> values_;
|
||||
std::vector<std::unique_ptr<Data>> values_;
|
||||
OrderedMultiQueue queue_;
|
||||
};
|
||||
|
||||
|
|
@ -73,7 +73,7 @@ TEST_F(OrderedMultiQueueTest, Ordering) {
|
|||
|
||||
EXPECT_EQ(11, values_.size());
|
||||
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());
|
||||
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()));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue