Deduplicate loading options for offline node (#664)

This is preparation for #636. 

I noticed that there is duplicated code for loading options for the offline and GRPC offline node because they are needed while constructing the map builder for the non-GRPC offline node (and that step is the only difference between the offline node and the GRPC offline node).

I got around this by passing a map builder factory to `RunOfflineNode` instead, so we can deduplicate the code for loading options by doing it inside `RunOfflineNode`.

cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc

@@ -15,19 +15,10 @@
  */
 
 #include "cartographer_grpc/mapping/map_builder_stub.h"
-#include "cartographer_ros/node.h"
 #include "cartographer_ros/offline_node.h"
 #include "cartographer_ros/ros_log_sink.h"
-#include "cartographer_ros/split_string.h"
+#include "ros/ros.h"
 
-DEFINE_string(configuration_directory, "",
-              "First directory in which configuration files are searched, "
-              "second is always the Cartographer installation to allow "
-              "including files from there.");
-DEFINE_string(configuration_basename, "",
-              "Basename, i.e. not containing any directory prefix, of the "
-              "configuration file.");
-DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
 DEFINE_string(server_address, "localhost:50051",
               "gRPC server address to "
               "stream the sensor data to.");
@@ -36,33 +27,18 @@ int main(int argc, char** argv) {
   google::InitGoogleLogging(argv[0]);
   google::ParseCommandLineFlags(&argc, &argv, true);
 
-  CHECK(!FLAGS_configuration_directory.empty())
-      << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basename.empty())
-      << "-configuration_basename is missing.";
-  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
-
   ::ros::init(argc, argv, "cartographer_grpc_offline_node");
   ::ros::start();
 
   cartographer_ros::ScopedRosLogSink ros_log_sink;
 
-  cartographer_ros::NodeOptions node_options;
-  cartographer_ros::TrajectoryOptions trajectory_options;
-  std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
-      FLAGS_configuration_directory, FLAGS_configuration_basename);
+  const cartographer_ros::MapBuilderFactory map_builder_factory =
+      [](const ::cartographer::mapping::proto::MapBuilderOptions&) {
+        return ::cartographer::common::make_unique<
+            ::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address);
+      };
 
-  // Since we preload the transform buffer, we should never have to wait for a
-  // transform. When we finish processing the bag, we will simply drop any
-  // remaining sensor data that cannot be transformed due to missing transforms.
-  node_options.lookup_transform_timeout_sec = 0.;
-
-  auto map_builder = cartographer::common::make_unique<
-      ::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address);
-
-  cartographer_ros::RunOfflineNode(
-      std::move(map_builder), node_options, trajectory_options,
-      cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
+  cartographer_ros::RunOfflineNode(map_builder_factory);
 
   ::ros::shutdown();
 }

cartographer_ros/cartographer_ros/offline_node.cc

@@ -21,8 +21,10 @@
 #include <sys/resource.h>
 #include <time.h>
 #include <chrono>
+#include <queue>
 
 #include "cartographer_ros/node.h"
+#include "cartographer_ros/split_string.h"
 #include "cartographer_ros/urdf_reader.h"
 #include "ros/callback_queue.h"
 #include "rosbag/bag.h"
@@ -32,6 +34,14 @@
 #include "tf2_ros/static_transform_broadcaster.h"
 #include "urdf/model.h"
 
+DEFINE_string(configuration_directory, "",
+              "First directory in which configuration files are searched, "
+              "second is always the Cartographer installation to allow "
+              "including files from there.");
+DEFINE_string(configuration_basename, "",
+              "Basename, i.e. not containing any directory prefix, of the "
+              "configuration file.");
+DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
 DEFINE_string(
     urdf_filename, "",
     "URDF file that contains static links for your sensor configuration.");
@@ -51,11 +61,26 @@ constexpr char kTfTopic[] = "tf";
 constexpr double kClockPublishFrequencySec = 1. / 30.;
 constexpr int kSingleThreaded = 1;
 
-void RunOfflineNode(
-    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-    const cartographer_ros::NodeOptions& node_options,
-    const cartographer_ros::TrajectoryOptions& trajectory_options,
-    const std::vector<std::string>& bag_filenames) {
+void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
+  CHECK(!FLAGS_configuration_directory.empty())
+      << "-configuration_directory is missing.";
+  CHECK(!FLAGS_configuration_basename.empty())
+      << "-configuration_basename is missing.";
+  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
+  const auto bag_filenames =
+      cartographer_ros::SplitString(FLAGS_bag_filenames, ',');
+  cartographer_ros::NodeOptions node_options;
+  cartographer_ros::TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
+      FLAGS_configuration_directory, FLAGS_configuration_basename);
+
+  // Since we preload the transform buffer, we should never have to wait for a
+  // transform. When we finish processing the bag, we will simply drop any
+  // remaining sensor data that cannot be transformed due to missing transforms.
+  node_options.lookup_transform_timeout_sec = 0.;
+
+  auto map_builder = map_builder_factory(node_options.map_builder_options);
+
   const std::chrono::time_point<std::chrono::steady_clock> start_time =
       std::chrono::steady_clock::now();
 

cartographer_ros/cartographer_ros/offline_node.h

@@ -17,6 +17,8 @@
 #ifndef CARTOGRAPHER_ROS_OFFLINE_NODE_H_
 #define CARTOGRAPHER_ROS_OFFLINE_NODE_H_
 
+#include <functional>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -25,11 +27,11 @@
 
 namespace cartographer_ros {
 
-void RunOfflineNode(
-    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-    const cartographer_ros::NodeOptions& node_options,
-    const cartographer_ros::TrajectoryOptions& trajectory_options,
-    const std::vector<std::string>& bag_filenames);
+using MapBuilderFactory =
+    std::function<std::unique_ptr<::cartographer::mapping::MapBuilderInterface>(
+        const ::cartographer::mapping::proto::MapBuilderOptions&)>;
+
+void RunOfflineNode(const MapBuilderFactory& map_builder_factory);
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -15,52 +15,27 @@
  */
 
 #include "cartographer/mapping/map_builder.h"
-#include "cartographer_ros/node.h"
 #include "cartographer_ros/offline_node.h"
 #include "cartographer_ros/ros_log_sink.h"
-#include "cartographer_ros/split_string.h"
-
-DEFINE_string(configuration_directory, "",
-              "First directory in which configuration files are searched, "
-              "second is always the Cartographer installation to allow "
-              "including files from there.");
-DEFINE_string(configuration_basename, "",
-              "Basename, i.e. not containing any directory prefix, of the "
-              "configuration file.");
-DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
+#include "ros/ros.h"
 
 int main(int argc, char** argv) {
   google::InitGoogleLogging(argv[0]);
   google::ParseCommandLineFlags(&argc, &argv, true);
 
-  CHECK(!FLAGS_configuration_directory.empty())
-      << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basename.empty())
-      << "-configuration_basename is missing.";
-  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
-
   ::ros::init(argc, argv, "cartographer_offline_node");
   ::ros::start();
 
   cartographer_ros::ScopedRosLogSink ros_log_sink;
 
-  cartographer_ros::NodeOptions node_options;
-  cartographer_ros::TrajectoryOptions trajectory_options;
-  std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
-      FLAGS_configuration_directory, FLAGS_configuration_basename);
+  const cartographer_ros::MapBuilderFactory map_builder_factory =
+      [](const ::cartographer::mapping::proto::MapBuilderOptions&
+             map_builder_options) {
+        return ::cartographer::common::make_unique<
+            ::cartographer::mapping::MapBuilder>(map_builder_options);
+      };
 
-  // Since we preload the transform buffer, we should never have to wait for a
-  // transform. When we finish processing the bag, we will simply drop any
-  // remaining sensor data that cannot be transformed due to missing transforms.
-  node_options.lookup_transform_timeout_sec = 0.;
-
-  auto map_builder =
-      cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
-          node_options.map_builder_options);
-
-  cartographer_ros::RunOfflineNode(
-      std::move(map_builder), node_options, trajectory_options,
-      cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
+  cartographer_ros::RunOfflineNode(map_builder_factory);
 
   ::ros::shutdown();
 }