diff --git a/cartographer_ros/cartographer_ros/bag_reader.cc b/cartographer_ros/cartographer_ros/bag_reader.cc index 6bc26af..9097c3a 100644 --- a/cartographer_ros/cartographer_ros/bag_reader.cc +++ b/cartographer_ros/cartographer_ros/bag_reader.cc @@ -52,9 +52,9 @@ std::unique_ptr ReadTransformsFromBag( } } } - LOG_EVERY_N(INFO, 100000) << "Processed " - << (msg.getTime() - begin_time).toSec() << " of " - << duration_in_seconds << " bag time seconds..."; + LOG_EVERY_N(INFO, 100000) + << "Processed " << (msg.getTime() - begin_time).toSec() << " of " + << duration_in_seconds << " bag time seconds..."; } bag.close(); diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 71a24c9..68b638e 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -39,7 +39,7 @@ namespace { constexpr int kInfiniteSubscriberQueueSize = 0; -void Run() { +NodeOptions LoadOptions() { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( std::vector{FLAGS_configuration_directory}); @@ -48,7 +48,11 @@ void Run() { cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); - const auto options = CreateNodeOptions(&lua_parameter_dictionary); + return CreateNodeOptions(&lua_parameter_dictionary); +} + +void Run() { + const auto options = LoadOptions(); constexpr double kTfBufferCacheTimeInSeconds = 1e6; tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)}; tf2_ros::TransformListener tf(tf_buffer); diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index b21aef7..349e73f 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -67,7 +67,7 @@ std::vector SplitString(const string& input, const char delimiter) { return tokens; } -void Run(std::vector bag_filenames) { +NodeOptions LoadOptions() { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( std::vector{FLAGS_configuration_directory}); @@ -76,6 +76,12 @@ void Run(std::vector bag_filenames) { cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); + return CreateNodeOptions(&lua_parameter_dictionary); +} + +void Run(const std::vector& bag_filenames) { + auto options = LoadOptions(); + auto tf_buffer = ::cartographer::common::make_unique(); if (!FLAGS_urdf_filename.empty()) { ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, tf_buffer.get()); @@ -87,7 +93,6 @@ void Run(std::vector bag_filenames) { } tf_buffer->setUsingDedicatedThread(true); - auto options = CreateNodeOptions(&lua_parameter_dictionary); // 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.