From b77a1f217818db036c5dd4acdc346da2e3a632b5 Mon Sep 17 00:00:00 2001
From: gaschler <gaschlera@gmail.com>
Date: Tue, 5 Dec 2017 11:05:05 +0100
Subject: [PATCH] Remove implementation from TrajectoryBuilder. (#728)

To simplify the implementation of a gRPC-based trajectory builder,
this moves the existing implementation to CollatedTrajectoryBuilder
and organizes the interface by sensor type.
---
 .../mapping/collated_trajectory_builder.h     | 33 +++++++++++++++-
 cartographer/mapping/trajectory_builder.h     | 39 ++++++-------------
 2 files changed, 43 insertions(+), 29 deletions(-)

diff --git a/cartographer/mapping/collated_trajectory_builder.h b/cartographer/mapping/collated_trajectory_builder.h
index ed20905..7802a09 100644
--- a/cartographer/mapping/collated_trajectory_builder.h
+++ b/cartographer/mapping/collated_trajectory_builder.h
@@ -49,10 +49,39 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
   CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
       delete;
 
-  void AddSensorData(const std::string& sensor_id,
-                     std::unique_ptr<sensor::Data> data) override;
+  void AddRangefinderData(const std::string& sensor_id, common::Time time,
+                          const Eigen::Vector3f& origin,
+                          const sensor::TimedPointCloud& ranges) override {
+    AddSensorData(sensor_id,
+                  common::make_unique<sensor::DispatchableRangefinderData>(
+                      time, origin, ranges));
+  }
+
+  void AddImuData(const std::string& sensor_id, common::Time time,
+                  const Eigen::Vector3d& linear_acceleration,
+                  const Eigen::Vector3d& angular_velocity) override {
+    AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
+                                 time, linear_acceleration, angular_velocity}));
+  }
+
+  void AddOdometerData(const std::string& sensor_id, common::Time time,
+                       const transform::Rigid3d& odometer_pose) override {
+    AddSensorData(sensor_id, sensor::MakeDispatchable(
+                                 sensor::OdometryData{time, odometer_pose}));
+  }
+
+  void AddFixedFramePoseData(
+      const std::string& sensor_id, common::Time time,
+      const transform::Rigid3d& fixed_frame_pose) override {
+    AddSensorData(sensor_id,
+                  sensor::MakeDispatchable(
+                      sensor::FixedFramePoseData{time, fixed_frame_pose}));
+  }
 
  private:
+  void AddSensorData(const std::string& sensor_id,
+                     std::unique_ptr<sensor::Data> data);
+
   void HandleCollatedSensorData(const std::string& sensor_id,
                                 std::unique_ptr<sensor::Data> data);
 
diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h
index 02ca784..3c28d6e 100644
--- a/cartographer/mapping/trajectory_builder.h
+++ b/cartographer/mapping/trajectory_builder.h
@@ -46,36 +46,21 @@ class TrajectoryBuilder {
   TrajectoryBuilder(const TrajectoryBuilder&) = delete;
   TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
 
-  virtual void AddSensorData(const std::string& sensor_id,
-                             std::unique_ptr<sensor::Data> data) = 0;
+  virtual void AddRangefinderData(const std::string& sensor_id,
+                                  common::Time time,
+                                  const Eigen::Vector3f& origin,
+                                  const sensor::TimedPointCloud& ranges) = 0;
 
-  void AddRangefinderData(const std::string& sensor_id, common::Time time,
-                          const Eigen::Vector3f& origin,
-                          const sensor::TimedPointCloud& ranges) {
-    AddSensorData(sensor_id,
-                  common::make_unique<sensor::DispatchableRangefinderData>(
-                      time, origin, ranges));
-  }
+  virtual void AddImuData(const std::string& sensor_id, common::Time time,
+                          const Eigen::Vector3d& linear_acceleration,
+                          const Eigen::Vector3d& angular_velocity) = 0;
 
-  void AddImuData(const std::string& sensor_id, common::Time time,
-                  const Eigen::Vector3d& linear_acceleration,
-                  const Eigen::Vector3d& angular_velocity) {
-    AddSensorData(sensor_id, sensor::MakeDispatchable(sensor::ImuData{
-                                 time, linear_acceleration, angular_velocity}));
-  }
+  virtual void AddOdometerData(const std::string& sensor_id, common::Time time,
+                               const transform::Rigid3d& odometer_pose) = 0;
 
-  void AddOdometerData(const std::string& sensor_id, common::Time time,
-                       const transform::Rigid3d& odometer_pose) {
-    AddSensorData(sensor_id, sensor::MakeDispatchable(
-                                 sensor::OdometryData{time, odometer_pose}));
-  }
-
-  void AddFixedFramePoseData(const std::string& sensor_id, common::Time time,
-                             const transform::Rigid3d& fixed_frame_pose) {
-    AddSensorData(sensor_id,
-                  sensor::MakeDispatchable(
-                      sensor::FixedFramePoseData{time, fixed_frame_pose}));
-  }
+  virtual void AddFixedFramePoseData(
+      const std::string& sensor_id, common::Time time,
+      const transform::Rigid3d& fixed_frame_pose) = 0;
 };
 
 }  // namespace mapping