Extract assets writer class from static method (#827)

Extracted class Assets_Writer from RunAssetsWriterPipeline. 

The idea is to increase the re-usability and flexibility of the assets_writer: In next PR, the assets_writer will allow registering external points_processers to the points processing pipeline. This requires having a class instead of a static method to allow for different states.

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -54,17 +54,12 @@ namespace {
 constexpr char kTfStaticTopic[] = "/tf_static";
 namespace carto = ::cartographer;
 
-std::tuple<carto::mapping::proto::PoseGraph,
+carto::mapping::proto::PoseGraph LoadPoseGraph(
-           carto::mapping::proto::AllTrajectoryBuilderOptions>
+    const std::string& pose_graph_filename) {
-LoadPoseGraph(const std::string& pose_graph_filename) {
   carto::mapping::proto::PoseGraph pose_graph_proto;
-  carto::mapping::proto::AllTrajectoryBuilderOptions
-      all_trajectory_builder_options;
-
   carto::io::ProtoStreamReader reader(pose_graph_filename);
   CHECK(reader.ReadProto(&pose_graph_proto));
-  CHECK(reader.ReadProto(&all_trajectory_builder_options));
+  return pose_graph_proto;
-  return std::make_tuple(pose_graph_proto, all_trajectory_builder_options);
 }
 
 std::unique_ptr<carto::io::PointsProcessorPipelineBuilder>
@@ -90,7 +85,7 @@ CreatePipelineBuilder(
   return builder;
 }
 
-std::unique_ptr<carto::common::LuaParameterDictionary> CreateLuaDictionary(
+std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
     const std::string& configuration_directory,
     const std::string& configuration_basename) {
   auto file_resolver =
@@ -148,18 +143,50 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
   return points_batch;
 }
 
-void RunPipeline(
+}  // namespace
-    const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline,
+
-    const std::vector<std::string>& bag_filenames,
+AssetsWriter::AssetsWriter(const std::string& pose_graph_filename,
-    const carto::mapping::proto::PoseGraph& pose_graph_proto,
+                           const std::vector<std::string>& bag_filenames,
-    const std::string tracking_frame, const std::string& urdf_filename,
+                           const std::string& output_file_prefix)
-    const bool use_bag_transforms) {
+    : bag_filenames_(bag_filenames),
+      pose_graph_(LoadPoseGraph(pose_graph_filename)) {
+  CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size())
+      << "Pose graphs contains " << pose_graph_.trajectory_size()
+      << " trajectories while " << bag_filenames_.size()
+      << " bags were provided. This tool requires one bag for each "
+         "trajectory in the same order as the correponding trajectories in the "
+         "pose graph proto.";
+
+  // This vector must outlive the pipeline.
+  all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>(
+      pose_graph_.trajectory().begin(), pose_graph_.trajectory().end());
+
+  const std::string file_prefix = !output_file_prefix.empty()
+                                      ? output_file_prefix
+                                      : bag_filenames_.front() + "_";
+  point_pipeline_builder_ =
+      CreatePipelineBuilder(all_trajectories_, file_prefix);
+}
+
+void AssetsWriter::Run(const std::string& configuration_directory,
+                       const std::string& configuration_basename,
+                       const std::string& urdf_filename,
+                       const bool use_bag_transforms) {
+  const auto lua_parameter_dictionary =
+      LoadLuaDictionary(configuration_directory, configuration_basename);
+
+  std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
+      point_pipeline_builder_->CreatePipeline(
+          lua_parameter_dictionary->GetDictionary("pipeline").get());
+  const std::string tracking_frame =
+      lua_parameter_dictionary->GetString("tracking_frame");
+
   do {
-    for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size();
+    for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size();
          ++trajectory_id) {
       const carto::mapping::proto::Trajectory& trajectory_proto =
-          pose_graph_proto.trajectory(trajectory_id);
+          pose_graph_.trajectory(trajectory_id);
-      const std::string& bag_filename = bag_filenames[trajectory_id];
+      const std::string& bag_filename = bag_filenames_[trajectory_id];
       LOG(INFO) << "Processing " << bag_filename << "...";
       if (trajectory_proto.node_size() == 0) {
         continue;
@@ -236,47 +263,4 @@ void RunPipeline(
            carto::io::PointsProcessor::FlushResult::kRestartStream);
 }
 
-}  // namespace
-
-void RunAssetsWriterPipeline(const std::string& pose_graph_filename,
-                             const std::vector<std::string>& bag_filenames,
-                             const std::string& configuration_directory,
-                             const std::string& configuration_basename,
-                             const std::string& urdf_filename,
-                             const std::string& output_file_prefix,
-                             const bool use_bag_transforms) {
-  carto::mapping::proto::PoseGraph pose_graph_proto;
-  carto::mapping::proto::AllTrajectoryBuilderOptions
-      all_trajectory_builder_options;
-  std::tie(pose_graph_proto, all_trajectory_builder_options) =
-      LoadPoseGraph(pose_graph_filename);
-
-  CHECK_EQ(pose_graph_proto.trajectory_size(), bag_filenames.size())
-      << "Pose graphs contains " << pose_graph_proto.trajectory_size()
-      << " trajectories while " << bag_filenames.size()
-      << " bags were provided. This tool requires one bag for each "
-         "trajectory in the same order as the correponding trajectories in the "
-         "pose graph proto.";
-
-  // This vector must outlive the pipeline.
-  std::vector<carto::mapping::proto::Trajectory> all_trajectories(
-      pose_graph_proto.trajectory().begin(),
-      pose_graph_proto.trajectory().end());
-  const std::string& file_prefix = !output_file_prefix.empty()
-                                       ? output_file_prefix
-                                       : bag_filenames.front() + "_";
-  auto pipeline_builder = CreatePipelineBuilder(all_trajectories, file_prefix);
-
-  auto lua_parameter_dictionary =
-      CreateLuaDictionary(configuration_directory, configuration_basename);
-  std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
-      pipeline_builder->CreatePipeline(
-          lua_parameter_dictionary->GetDictionary("pipeline").get());
-  const std::string& tracking_frame =
-      lua_parameter_dictionary->GetString("tracking_frame");
-
-  RunPipeline(pipeline, bag_filenames, pose_graph_proto, tracking_frame,
-              urdf_filename, use_bag_transforms);
-}
-
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/assets_writer.h

@@ -17,20 +17,35 @@
 #include <string>
 #include <vector>
 
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/io/points_processor_pipeline_builder.h"
+#include "cartographer/mapping/proto/pose_graph.pb.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+
 #ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_
 #define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
 
 namespace cartographer_ros {
 
-// Configures a point processing pipeline and pushes the points from the bag
+class AssetsWriter {
-// through it.
+ public:
-void RunAssetsWriterPipeline(const std::string& pose_graph_filename,
+  AssetsWriter(const std::string& pose_graph_filename,
-                             const std::vector<std::string>& bag_filenames,
+               const std::vector<std::string>& bag_filenames,
-                             const std::string& configuration_directory,
+               const std::string& output_file_prefix);
-                             const std::string& configuration_basename,
+
-                             const std::string& urdf_filename,
+  // Configures a points processing pipeline and pushes the points from the
-                             const std::string& output_file_prefix,
+  // bag through the pipeline.
-                             bool use_bag_transforms);
+  void Run(const std::string& configuration_directory,
+           const std::string& configuration_basename,
+           const std::string& urdf_filename, bool use_bag_transforms);
+
+ private:
+  std::vector<std::string> bag_filenames_;
+  std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_;
+  ::cartographer::mapping::proto::PoseGraph pose_graph_;
+  std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder>
+      point_pipeline_builder_;
+};
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -54,9 +54,11 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_pose_graph_filename.empty())
       << "-pose_graph_filename is missing.";
 
-  ::cartographer_ros::RunAssetsWriterPipeline(
+  ::cartographer_ros::AssetsWriter asset_writer(
       FLAGS_pose_graph_filename,
       cartographer_ros::SplitString(FLAGS_bag_filenames, ','),
-      FLAGS_configuration_directory, FLAGS_configuration_basename,
+      FLAGS_output_file_prefix);
-      FLAGS_urdf_filename, FLAGS_output_file_prefix, FLAGS_use_bag_transforms);
+
+  asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename,
+                   FLAGS_urdf_filename, FLAGS_use_bag_transforms);
 }