Make sensor::Data dispatchable to TrajectoryBuilder. (#738)

This change allows to dispatch sensor::Data to implementations of mapping::TrajectoryBuilder, i.e. CollatedTrajectoryBuilder. We need this for cartographer_grpc as the incoming sensor data is inserted into a queue by the gRPC threads and dequeued by a SLAM threads that inserts them into a CollatedTrajectoryBuilder.

cartographer/mapping/collated_trajectory_builder.cc

@@ -72,7 +72,7 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
     last_logging_time_ = std::chrono::steady_clock::now();
   }
 
-  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
+  data->AddToGlobalTrajectoryBuilder(wrapped_trajectory_builder_.get());
 }
 
 }  // namespace mapping

cartographer/mapping/map_builder_interface.h

@@ -25,6 +25,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer/io/proto_stream.h"
+#include "cartographer/mapping/global_trajectory_builder_interface.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"

cartographer/mapping/trajectory_builder.h

@@ -28,7 +28,6 @@
 #include "cartographer/mapping/pose_estimate.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer/mapping/submaps.h"
-#include "cartographer/sensor/data.h"
 #include "cartographer/sensor/point_cloud.h"
 
 namespace cartographer {

cartographer/sensor/data.cc

@@ -0,0 +1,45 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/sensor/data.h"
+
+namespace cartographer {
+namespace sensor {
+
+template <>
+void Dispatchable<sensor::ImuData>::AddToTrajectoryBuilder(
+    mapping::TrajectoryBuilder* const trajectory_builder,
+    const std::string& sensor_id) {
+  trajectory_builder->AddImuData(
+      sensor_id, data_.time, data_.linear_acceleration, data_.angular_velocity);
+}
+
+template <>
+void Dispatchable<sensor::OdometryData>::AddToTrajectoryBuilder(
+    mapping::TrajectoryBuilder* const trajectory_builder,
+    const std::string& sensor_id) {
+  trajectory_builder->AddOdometerData(sensor_id, data_.time, data_.pose);
+}
+
+template <>
+void Dispatchable<sensor::FixedFramePoseData>::AddToTrajectoryBuilder(
+    mapping::TrajectoryBuilder* const trajectory_builder,
+    const std::string& sensor_id) {
+  trajectory_builder->AddFixedFramePoseData(sensor_id, data_.time, data_.pose);
+}
+
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/data.h

@@ -20,8 +20,10 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/global_trajectory_builder_interface.h"
+#include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/range_data.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -34,8 +36,11 @@ class Data {
   virtual ~Data() {}
 
   virtual common::Time GetTime() const = 0;
-  virtual void AddToTrajectoryBuilder(
+  virtual void AddToGlobalTrajectoryBuilder(
       mapping::GlobalTrajectoryBuilderInterface* trajectory_builder) = 0;
+  virtual void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilder* const trajectory_builder,
+      const std::string& sensor_id) = 0;
 };
 
 class DispatchableRangefinderData : public Data {
@@ -46,10 +51,16 @@ class DispatchableRangefinderData : public Data {
       : time_(time), origin_(origin), ranges_(ranges) {}
 
   common::Time GetTime() const override { return time_; }
-  void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
-                                  trajectory_builder) override {
+  void AddToGlobalTrajectoryBuilder(
+      mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
+      override {
     trajectory_builder->AddRangefinderData(time_, origin_, ranges_);
   }
+  void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilder* const trajectory_builder,
+      const std::string& sensor_id) override {
+    trajectory_builder->AddRangefinderData(sensor_id, time_, origin_, ranges_);
+  }
 
  private:
   const common::Time time_;
@@ -63,10 +74,14 @@ class Dispatchable : public Data {
   Dispatchable(const DataType& data) : data_(data) {}
 
   common::Time GetTime() const override { return data_.time; }
-  void AddToTrajectoryBuilder(mapping::GlobalTrajectoryBuilderInterface* const
-                                  trajectory_builder) override {
+  void AddToGlobalTrajectoryBuilder(
+      mapping::GlobalTrajectoryBuilderInterface* const trajectory_builder)
+      override {
     trajectory_builder->AddSensorData(data_);
   }
+  void AddToTrajectoryBuilder(
+      mapping::TrajectoryBuilder* const trajectory_builder,
+      const std::string& sensor_id) override;
 
  private:
   const DataType data_;