From 2eca0210036b7c8a466584dfb97559d3bccba45e Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Mon, 17 Oct 2016 10:18:17 +0200 Subject: [PATCH] Moves SensorCollator into sensor package. (#58) --- cartographer/mapping/CMakeLists.txt | 23 ----------- cartographer/sensor/CMakeLists.txt | 23 +++++++++++ .../sensor_collator.h => sensor/collator.h} | 32 ++++++++-------- .../collator_test.cc} | 38 +++++++++---------- 4 files changed, 58 insertions(+), 58 deletions(-) rename cartographer/{mapping/sensor_collator.h => sensor/collator.h} (82%) rename cartographer/{mapping/sensor_collator_test.cc => sensor/collator_test.cc} (68%) diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index 78a7688..1850be7 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/cartographer/mapping/CMakeLists.txt @@ -91,18 +91,6 @@ google_library(mapping_probability_values common_port ) -google_library(mapping_sensor_collator - USES_EIGEN - USES_GLOG - HDRS - sensor_collator.h - DEPENDS - common_make_unique - common_ordered_multi_queue - common_time - sensor_sensor_packet_period_histogram_builder -) - google_library(mapping_sparse_pose_graph USES_GLOG SRCS @@ -166,17 +154,6 @@ google_test(mapping_probability_values_test mapping_probability_values ) -google_test(mapping_sensor_collator_test - SRCS - sensor_collator_test.cc - DEPENDS - common_lua_parameter_dictionary_test_helpers - common_make_unique - common_time - mapping_sensor_collator - sensor_proto_sensor -) - google_test(mapping_sparse_pose_graph_test USES_GLOG SRCS diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index eb838d3..ecca7e6 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -14,6 +14,18 @@ add_subdirectory("proto") +google_library(sensor_collator + USES_EIGEN + USES_GLOG + HDRS + collator.h + DEPENDS + common_make_unique + common_ordered_multi_queue + common_time + sensor_sensor_packet_period_histogram_builder +) + google_library(sensor_compressed_point_cloud USES_EIGEN SRCS @@ -102,6 +114,17 @@ google_library(sensor_voxel_filter sensor_proto_adaptive_voxel_filter_options ) +google_test(sensor_collator_test + SRCS + collator_test.cc + DEPENDS + common_lua_parameter_dictionary_test_helpers + common_make_unique + common_time + sensor_collator + sensor_proto_sensor +) + google_test(sensor_compressed_point_cloud_test SRCS compressed_point_cloud_test.cc diff --git a/cartographer/mapping/sensor_collator.h b/cartographer/sensor/collator.h similarity index 82% rename from cartographer/mapping/sensor_collator.h rename to cartographer/sensor/collator.h index acd6fde..967ba84 100644 --- a/cartographer/mapping/sensor_collator.h +++ b/cartographer/sensor/collator.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_ -#define CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_ +#ifndef CARTOGRAPHER_SENSOR_COLLATOR_H_ +#define CARTOGRAPHER_SENSOR_COLLATOR_H_ #include #include @@ -32,32 +32,32 @@ #include "glog/logging.h" namespace cartographer { -namespace mapping { +namespace sensor { -struct SensorCollatorQueueKey { +struct CollatorQueueKey { int trajectory_id; string sensor_id; - bool operator<(const SensorCollatorQueueKey& other) const { + bool operator<(const CollatorQueueKey& other) const { return std::forward_as_tuple(trajectory_id, sensor_id) < std::forward_as_tuple(other.trajectory_id, other.sensor_id); } }; inline std::ostream& operator<<(std::ostream& out, - const SensorCollatorQueueKey& key) { + const CollatorQueueKey& key) { return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')'; } template -class SensorCollator { +class Collator { public: using Callback = std::function)>; - SensorCollator() {} + Collator() {} - SensorCollator(const SensorCollator&) = delete; - SensorCollator& operator=(const SensorCollator&) = delete; + Collator(const Collator&) = delete; + Collator& operator=(const Collator&) = delete; // Adds a trajectory to produce sorted sensor output for. Calls 'callback' // for each collated sensor data. @@ -65,7 +65,7 @@ class SensorCollator { const std::unordered_set& expected_sensor_ids, const Callback callback) { for (const auto& sensor_id : expected_sensor_ids) { - const auto queue_key = SensorCollatorQueueKey{trajectory_id, sensor_id}; + const auto queue_key = CollatorQueueKey{trajectory_id, sensor_id}; queue_.AddQueue(queue_key, [callback](std::unique_ptr value) { callback(value->timestamp, std::move(value->sensor_data)); }); @@ -89,7 +89,7 @@ class SensorCollator { sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp, sensor_id); queue_.Add( - SensorCollatorQueueKey{trajectory_id, sensor_id}, timestamp, + CollatorQueueKey{trajectory_id, sensor_id}, timestamp, common::make_unique(Value{timestamp, std::move(sensor_data)})); } @@ -117,15 +117,15 @@ class SensorCollator { }; // Queue keys are a pair of trajectory ID and sensor identifier. - common::OrderedMultiQueue queue_; + common::OrderedMultiQueue queue_; // Map of trajectory ID to all associated QueueKeys. - std::unordered_map> queue_keys_; + std::unordered_map> queue_keys_; sensor::SensorPacketPeriodHistogramBuilder sensor_packet_period_histogram_builder_; }; -} // namespace mapping +} // namespace sensor } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_ +#endif // CARTOGRAPHER_SENSOR_COLLATOR_H_ diff --git a/cartographer/mapping/sensor_collator_test.cc b/cartographer/sensor/collator_test.cc similarity index 68% rename from cartographer/mapping/sensor_collator_test.cc rename to cartographer/sensor/collator_test.cc index 99c2451..3a44c8b 100644 --- a/cartographer/mapping/sensor_collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/sensor_collator.h" +#include "cartographer/sensor/collator.h" #include @@ -25,14 +25,14 @@ #include "gtest/gtest.h" namespace cartographer { -namespace mapping { +namespace sensor { namespace { struct TestData { string frame_id; }; -TEST(SensorCollator, Ordering) { +TEST(Collator, Ordering) { TestData first{"horizontal_laser"}; TestData second{"vertical_laser"}; TestData third{"imu"}; @@ -43,25 +43,25 @@ TEST(SensorCollator, Ordering) { const std::unordered_set frame_ids = { "horizontal_laser", "vertical_laser", "imu", "something"}; std::vector> received; - SensorCollator sensor_collator; - sensor_collator.AddTrajectory( + Collator collator; + collator.AddTrajectory( 0, frame_ids, [&received](const int64 timestamp, std::unique_ptr packet) { received.push_back(std::make_pair(timestamp, *packet)); }); - sensor_collator.AddSensorData(0, 100, first.frame_id, - common::make_unique(first)); - sensor_collator.AddSensorData(0, 600, sixth.frame_id, - common::make_unique(sixth)); - sensor_collator.AddSensorData(0, 400, fourth.frame_id, - common::make_unique(fourth)); - sensor_collator.AddSensorData(0, 200, second.frame_id, - common::make_unique(second)); - sensor_collator.AddSensorData(0, 500, fifth.frame_id, - common::make_unique(fifth)); - sensor_collator.AddSensorData(0, 300, third.frame_id, - common::make_unique(third)); + 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)); EXPECT_EQ(3, received.size()); EXPECT_EQ(100, received[0].first); @@ -71,7 +71,7 @@ TEST(SensorCollator, Ordering) { EXPECT_EQ(300, received[2].first); EXPECT_EQ("imu", received[2].second.frame_id); - sensor_collator.Flush(); + collator.Flush(); ASSERT_EQ(6, received.size()); EXPECT_EQ("horizontal_laser", received[3].second.frame_id); @@ -82,5 +82,5 @@ TEST(SensorCollator, Ordering) { } } // namespace -} // namespace mapping +} // namespace sensor } // namespace cartographer