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.

cartographer/mapping/collated_trajectory_builder.cc

@@ -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) {
+  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

cartographer/mapping/trajectory_builder.h

@@ -58,28 +58,30 @@ class TrajectoryBuilder {
                           const Eigen::Vector3f& origin,
                           const sensor::PointCloud& ranges) {
     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,
                   const Eigen::Vector3d& linear_acceleration,
                   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,
                        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) {
+                             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));
   }
 };
 

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -60,7 +60,7 @@ void GlobalTrajectoryBuilder::AddFixedFramePoseData(
     const sensor::FixedFramePoseData& fixed_frame_pose) {
   sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
 }
-  
+
 const mapping::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const {
   return local_trajectory_builder_.pose_estimate();
 }

cartographer/sensor/collator_test.cc

@@ -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{});
+  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.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],
+  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],
-                         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],
-                         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);
 }
 

cartographer/sensor/data.h

@@ -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
+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

cartographer/sensor/ordered_multi_queue.cc

@@ -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) {
+      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));
       }
     }
@@ -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 =
+        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

cartographer/sensor/ordered_multi_queue_test.cc

@@ -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>(
+    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_;
 };
 
@@ -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()));
   }
 }