From fddb1e32a0a295268e89ceb7ab6dd9124051859d Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Tue, 18 Oct 2016 16:07:18 +0200 Subject: [PATCH] Moves time into sensor::Data. (#76) --- cartographer/sensor/collator.h | 18 +++--- cartographer/sensor/collator_test.cc | 58 +++++++++---------- cartographer/sensor/data.h | 21 +++++-- cartographer/sensor/ordered_multi_queue.h | 48 ++++++--------- .../sensor/ordered_multi_queue_test.cc | 37 ++++++------ 5 files changed, 84 insertions(+), 98 deletions(-) diff --git a/cartographer/sensor/collator.h b/cartographer/sensor/collator.h index 364ce6a..8774235 100644 --- a/cartographer/sensor/collator.h +++ b/cartographer/sensor/collator.h @@ -37,7 +37,7 @@ namespace sensor { class Collator { public: - using Callback = std::function)>; + using Callback = std::function)>; Collator() {} @@ -51,9 +51,8 @@ class Collator { const Callback callback) { for (const auto& sensor_id : expected_sensor_ids) { const auto queue_key = QueueKey{trajectory_id, sensor_id}; - queue_.AddQueue(queue_key, [callback](const common::Time time, - std::unique_ptr data) { - callback(common::ToUniversal(time), std::move(data)); + queue_.AddQueue(queue_key, [callback](std::unique_ptr data) { + callback(std::move(data)); }); queue_keys_[trajectory_id].push_back(queue_key); } @@ -69,12 +68,11 @@ class Collator { // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid // sensor data. Sensor packets with matching 'sensor_id' must be added in time // order. - void AddSensorData(const int trajectory_id, const int64 timestamp, - const string& sensor_id, std::unique_ptr data) { - sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp, - sensor_id); - queue_.Add(QueueKey{trajectory_id, sensor_id}, - common::FromUniversal(timestamp), std::move(data)); + void AddSensorData(const int trajectory_id, const string& sensor_id, + std::unique_ptr data) { + sensor_packet_period_histogram_builder_.Add( + trajectory_id, common::ToUniversal(data->time), sensor_id); + queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data)); } // Dispatches all queued sensor packets. May only be called once. diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index 4d020a1..ba59ec9 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -29,52 +29,46 @@ namespace sensor { namespace { TEST(Collator, Ordering) { - Data first("horizontal_laser", sensor::LaserFan{}); - Data second("vertical_laser", sensor::LaserFan{}); - Data third("imu", Data::Imu{}); - Data fourth("horizontal_laser", sensor::LaserFan{}); - Data fifth("vertical_laser", sensor::LaserFan{}); - Data sixth("odometry", Data::Odometry{}); + Data first(common::FromUniversal(100), "horizontal_laser", sensor::LaserFan{}); + Data second(common::FromUniversal(200),"vertical_laser", sensor::LaserFan{}); + Data third(common::FromUniversal(300),"imu", Data::Imu{}); + Data fourth(common::FromUniversal(400),"horizontal_laser", sensor::LaserFan{}); + Data fifth(common::FromUniversal(500),"vertical_laser", sensor::LaserFan{}); + Data sixth(common::FromUniversal(600),"odometry", Data::Odometry{}); const std::unordered_set frame_ids = { "horizontal_laser", "vertical_laser", "imu", "odometry"}; - std::vector> received; + std::vector received; Collator collator; collator.AddTrajectory( 0, frame_ids, - [&received](const int64 timestamp, std::unique_ptr packet) { - received.push_back(std::make_pair(timestamp, *packet)); + [&received](std::unique_ptr data) { + received.push_back(*data); }); - collator.AddSensorData(0, 100, first.frame_id, - common::make_unique(first)); - collator.AddSensorData(0, 600, sixth.frame_id, - common::make_unique(sixth)); - collator.AddSensorData(0, 400, fourth.frame_id, - common::make_unique(fourth)); - collator.AddSensorData(0, 200, second.frame_id, - common::make_unique(second)); - collator.AddSensorData(0, 500, fifth.frame_id, - common::make_unique(fifth)); - collator.AddSensorData(0, 300, third.frame_id, - common::make_unique(third)); + collator.AddSensorData(0, first.frame_id, common::make_unique(first)); + collator.AddSensorData(0, sixth.frame_id, common::make_unique(sixth)); + collator.AddSensorData(0, fourth.frame_id, common::make_unique(fourth)); + collator.AddSensorData(0, second.frame_id, common::make_unique(second)); + collator.AddSensorData(0, fifth.frame_id, common::make_unique(fifth)); + collator.AddSensorData(0, third.frame_id, common::make_unique(third)); EXPECT_EQ(3, received.size()); - EXPECT_EQ(100, received[0].first); - EXPECT_EQ("horizontal_laser", received[0].second.frame_id); - EXPECT_EQ(200, received[1].first); - EXPECT_EQ("vertical_laser", received[1].second.frame_id); - EXPECT_EQ(300, received[2].first); - EXPECT_EQ("imu", received[2].second.frame_id); + EXPECT_EQ(100, common::ToUniversal(received[0].time)); + EXPECT_EQ("horizontal_laser", received[0].frame_id); + EXPECT_EQ(200, common::ToUniversal(received[1].time)); + EXPECT_EQ("vertical_laser", received[1].frame_id); + EXPECT_EQ(300, common::ToUniversal(received[2].time)); + EXPECT_EQ("imu", received[2].frame_id); collator.Flush(); ASSERT_EQ(6, received.size()); - EXPECT_EQ("horizontal_laser", received[3].second.frame_id); - EXPECT_EQ(500, received[4].first); - EXPECT_EQ("vertical_laser", received[4].second.frame_id); - EXPECT_EQ(600, received[5].first); - EXPECT_EQ("odometry", received[5].second.frame_id); + EXPECT_EQ("horizontal_laser", received[3].frame_id); + EXPECT_EQ(500, common::ToUniversal(received[4].time)); + EXPECT_EQ("vertical_laser", received[4].frame_id); + EXPECT_EQ(600, common::ToUniversal(received[5].time)); + EXPECT_EQ("odometry", received[5].frame_id); } } // namespace diff --git a/cartographer/sensor/data.h b/cartographer/sensor/data.h index 1b693fb..0c8987f 100644 --- a/cartographer/sensor/data.h +++ b/cartographer/sensor/data.h @@ -19,6 +19,7 @@ #include +#include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/sensor/laser.h" #include "cartographer/transform/rigid_transform.h" @@ -42,17 +43,25 @@ struct Data { Eigen::Vector3d angular_velocity; }; - Data(const string& frame_id, const Imu& imu) - : type(Type::kImu), frame_id(frame_id), imu(imu) {} + Data(const common::Time time, const string& frame_id, const Imu& imu) + : type(Type::kImu), time(time), frame_id(frame_id), imu(imu) {} - Data(const string& frame_id, + Data(const common::Time time, const string& frame_id, const ::cartographer::sensor::LaserFan& laser_fan) - : type(Type::kLaserFan), frame_id(frame_id), laser_fan(laser_fan) {} + : type(Type::kLaserFan), + time(time), + frame_id(frame_id), + laser_fan(laser_fan) {} - Data(const string& frame_id, const Odometry& odometry) - : type(Type::kOdometry), frame_id(frame_id), odometry(odometry) {} + Data(const common::Time time, const string& frame_id, + const Odometry& odometry) + : type(Type::kOdometry), + time(time), + frame_id(frame_id), + odometry(odometry) {} Type type; + common::Time time; string frame_id; Imu imu; sensor::LaserFan laser_fan; diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h index 284e6f0..6691933 100644 --- a/cartographer/sensor/ordered_multi_queue.h +++ b/cartographer/sensor/ordered_multi_queue.h @@ -51,9 +51,9 @@ inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) { // sorted order. This class is thread-compatible. class OrderedMultiQueue { public: - using Callback = std::function)>; + using Callback = std::function)>; - // Will wait to see at least one value for each 'expected_queue_keys' before + // Will wait to see at least one value for each unfinished queue before // dispatching the next smallest value across all queues. OrderedMultiQueue() {} ~OrderedMultiQueue() {} @@ -74,17 +74,14 @@ class OrderedMultiQueue { return queues_.count(queue_key) != 0; } - void Add(const QueueKey& queue_key, const common::Time& sort_key, - std::unique_ptr value) { + void Add(const QueueKey& queue_key, std::unique_ptr data) { auto* queue = FindOrNull(queue_key); if (queue == nullptr) { - // TODO(damonkohler): This will not work for every value of "queue_key". - LOG_EVERY_N(WARNING, 1000) << "Ignored value for queue: '" << queue_key + LOG_EVERY_N(WARNING, 1000) << "Ignored data for queue: '" << queue_key << "'"; return; } - queue->queue.Push( - common::make_unique(sort_key, std::move(value))); + queue->queue.Push(std::move(data)); Dispatch(); } @@ -108,15 +105,8 @@ class OrderedMultiQueue { } private: - struct KeyValuePair { - KeyValuePair(const common::Time& sort_key, std::unique_ptr value) - : sort_key(sort_key), value(std::move(value)) {} - common::Time sort_key; - std::unique_ptr value; - }; - struct Queue { - common::BlockingQueue> queue; + common::BlockingQueue> queue; Callback callback; bool finished = false; }; @@ -140,11 +130,11 @@ class OrderedMultiQueue { void Dispatch() { while (true) { Queue* next_queue = nullptr; - const KeyValuePair* next_key_value_pair = nullptr; + const Data* next_data = nullptr; for (auto it = queues_.begin(); it != queues_.end();) { auto& queue = it->second.queue; - const auto* key_value_pair = queue.template Peek(); - if (key_value_pair == nullptr) { + const auto* data = queue.Peek(); + if (data == nullptr) { if (it->second.finished) { queues_.erase(it++); continue; @@ -152,24 +142,22 @@ class OrderedMultiQueue { CannotMakeProgress(); return; } - if (next_key_value_pair == nullptr || - std::forward_as_tuple(key_value_pair->sort_key, it->first) < - std::forward_as_tuple(next_key_value_pair->sort_key, - it->first)) { - next_key_value_pair = key_value_pair; + if (next_data == nullptr || + std::forward_as_tuple(data->time, it->first) < + std::forward_as_tuple(next_data->time, it->first)) { + next_data = data; next_queue = &it->second; } - CHECK_LE(last_dispatched_key_, next_key_value_pair->sort_key) - << "Non-sorted values added to queue: '" << it->first << "'"; + CHECK_LE(last_dispatched_key_, next_data->time) + << "Non-sorted data added to queue: '" << it->first << "'"; ++it; } - if (next_key_value_pair == nullptr) { + if (next_data == nullptr) { CHECK(queues_.empty()); return; } - last_dispatched_key_ = next_key_value_pair->sort_key; - next_queue->callback(last_dispatched_key_, - std::move(next_queue->queue.Pop()->value)); + last_dispatched_key_ = next_data->time; + next_queue->callback(std::move(next_queue->queue.Pop())); } } diff --git a/cartographer/sensor/ordered_multi_queue_test.cc b/cartographer/sensor/ordered_multi_queue_test.cc index 26f9064..d58132a 100644 --- a/cartographer/sensor/ordered_multi_queue_test.cc +++ b/cartographer/sensor/ordered_multi_queue_test.cc @@ -33,12 +33,9 @@ class OrderedMultiQueueTest : public ::testing::Test { void SetUp() { for (const auto& queue_key : {kFirst, kSecond, kThird}) { - queue_.AddQueue(queue_key, [this](const common::Time time, - std::unique_ptr data) { - EXPECT_EQ(common::ToUniversal(time), data->imu.linear_acceleration.x()); + queue_.AddQueue(queue_key, [this](std::unique_ptr data) { if (!values_.empty()) { - EXPECT_GT(data->imu.linear_acceleration.x(), - values_.back().imu.linear_acceleration.x()); + EXPECT_GT(data->time, values_.back().time); } values_.push_back(*data); }); @@ -47,8 +44,8 @@ class OrderedMultiQueueTest : public ::testing::Test { std::unique_ptr MakeImu(const int ordinal) { return common::make_unique( - "unused_frame_id", - Data::Imu{ordinal * Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero()}); + common::FromUniversal(ordinal), "unused_frame_id", + Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); } std::vector values_; @@ -56,30 +53,30 @@ class OrderedMultiQueueTest : public ::testing::Test { }; TEST_F(OrderedMultiQueueTest, Ordering) { - queue_.Add(kFirst, common::FromUniversal(4), MakeImu(4)); - queue_.Add(kFirst, common::FromUniversal(5), MakeImu(5)); - queue_.Add(kFirst, common::FromUniversal(6), MakeImu(6)); + queue_.Add(kFirst, MakeImu(4)); + queue_.Add(kFirst, MakeImu(5)); + queue_.Add(kFirst, MakeImu(6)); EXPECT_TRUE(values_.empty()); - queue_.Add(kSecond, common::FromUniversal(1), MakeImu(1)); + queue_.Add(kSecond, MakeImu(1)); EXPECT_TRUE(values_.empty()); - queue_.Add(kThird, common::FromUniversal(2), MakeImu(2)); + queue_.Add(kThird, MakeImu(2)); EXPECT_EQ(values_.size(), 1); - queue_.Add(kSecond, common::FromUniversal(3), MakeImu(3)); + queue_.Add(kSecond, MakeImu(3)); EXPECT_EQ(values_.size(), 2); - queue_.Add(kSecond, common::FromUniversal(7), MakeImu(7)); - queue_.Add(kThird, common::FromUniversal(8), MakeImu(8)); + queue_.Add(kSecond, MakeImu(7)); + queue_.Add(kThird, MakeImu(8)); queue_.Flush(); EXPECT_EQ(8, values_.size()); for (size_t i = 0; i < values_.size(); ++i) { - EXPECT_EQ(i + 1, values_[i].imu.linear_acceleration.x()); + EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); } } TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { - queue_.Add(kFirst, common::FromUniversal(1), MakeImu(1)); - queue_.Add(kFirst, common::FromUniversal(2), MakeImu(2)); - queue_.Add(kFirst, common::FromUniversal(3), MakeImu(3)); + queue_.Add(kFirst, MakeImu(1)); + queue_.Add(kFirst, MakeImu(2)); + queue_.Add(kFirst, MakeImu(3)); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kFirst); EXPECT_TRUE(values_.empty()); @@ -89,7 +86,7 @@ TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { EXPECT_EQ(3, values_.size()); for (size_t i = 0; i < values_.size(); ++i) { - EXPECT_EQ(i + 1, values_[i].imu.linear_acceleration.x()); + EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); } }