From 1dd49247a49a049c3249515b0bb32dcca6c79179 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= <schwoere@users.noreply.github.com>
Date: Thu, 19 Apr 2018 18:42:42 +0200
Subject: [PATCH] Registration of external points processors in AssetsWriter
 (#830)

Added RegisterPointsProcessor method to AssetsWriter class. This allows to register new points processors to the pipeline builder.

As the new points processors may write files to the disk, the CreateFileWriterFactory method is exposed.
---
 .../cartographer_ros/assets_writer.cc         | 21 +++++++++++++++----
 .../cartographer_ros/assets_writer.h          | 11 ++++++++++
 2 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc
index 66d5d6f..7367ef3 100644
--- a/cartographer_ros/cartographer_ros/assets_writer.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer.cc
@@ -66,10 +66,8 @@ std::unique_ptr<carto::io::PointsProcessorPipelineBuilder>
 CreatePipelineBuilder(
     const std::vector<carto::mapping::proto::Trajectory>& trajectories,
     const std::string file_prefix) {
-  const auto file_writer_factory = [file_prefix](const std::string& filename) {
-    return carto::common::make_unique<carto::io::StreamFileWriter>(file_prefix +
-                                                                   filename);
-  };
+  const auto file_writer_factory =
+      AssetsWriter::CreateFileWriterFactory(file_prefix);
   auto builder =
       carto::common::make_unique<carto::io::PointsProcessorPipelineBuilder>();
   carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory,
@@ -168,6 +166,12 @@ AssetsWriter::AssetsWriter(const std::string& pose_graph_filename,
       CreatePipelineBuilder(all_trajectories_, file_prefix);
 }
 
+void AssetsWriter::RegisterPointsProcessor(
+    const std::string& name,
+    cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction factory) {
+  point_pipeline_builder_->Register(name, factory);
+}
+
 void AssetsWriter::Run(const std::string& configuration_directory,
                        const std::string& configuration_basename,
                        const std::string& urdf_filename,
@@ -263,4 +267,13 @@ void AssetsWriter::Run(const std::string& configuration_directory,
            carto::io::PointsProcessor::FlushResult::kRestartStream);
 }
 
+::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory(
+    const std::string& file_path) {
+  const auto file_writer_factory = [file_path](const std::string& filename) {
+    return carto::common::make_unique<carto::io::StreamFileWriter>(file_path +
+                                                                   filename);
+  };
+  return file_writer_factory;
+}
+
 }  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h
index 13ef7cb..3c3bf80 100644
--- a/cartographer_ros/cartographer_ros/assets_writer.h
+++ b/cartographer_ros/cartographer_ros/assets_writer.h
@@ -33,12 +33,23 @@ class AssetsWriter {
                const std::vector<std::string>& bag_filenames,
                const std::string& output_file_prefix);
 
+  // Registers a new PointsProcessor type uniquly identified by 'name' which
+  // will be created using 'factory'.
+  void RegisterPointsProcessor(
+      const std::string& name,
+      cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction
+          factory);
+
   // Configures a points processing pipeline and pushes the points from the
   // bag through the pipeline.
   void Run(const std::string& configuration_directory,
            const std::string& configuration_basename,
            const std::string& urdf_filename, bool use_bag_transforms);
 
+  // Creates a FileWriterFactory which creates a FileWriter for storing assets.
+  static ::cartographer::io::FileWriterFactory CreateFileWriterFactory(
+      const std::string& file_path);
+
  private:
   std::vector<std::string> bag_filenames_;
   std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_;