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`.
master
Juraj Oršulić 2018-01-19 10:20:47 +01:00 committed by Wally B. Feed
parent 1eda46a941
commit 7cc4fec316
4 changed files with 52 additions and 74 deletions

View File

@ -15,19 +15,10 @@
*/ */
#include "cartographer_grpc/mapping/map_builder_stub.h" #include "cartographer_grpc/mapping/map_builder_stub.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/offline_node.h" #include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.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", DEFINE_string(server_address, "localhost:50051",
"gRPC server address to " "gRPC server address to "
"stream the sensor data to."); "stream the sensor data to.");
@ -36,33 +27,18 @@ int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]); google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true); 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::init(argc, argv, "cartographer_grpc_offline_node");
::ros::start(); ::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::NodeOptions node_options; const cartographer_ros::MapBuilderFactory map_builder_factory =
cartographer_ros::TrajectoryOptions trajectory_options; [](const ::cartographer::mapping::proto::MapBuilderOptions&) {
std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions( return ::cartographer::common::make_unique<
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 = cartographer::common::make_unique<
::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address); ::cartographer_grpc::mapping::MapBuilderStub>(FLAGS_server_address);
};
cartographer_ros::RunOfflineNode( cartographer_ros::RunOfflineNode(map_builder_factory);
std::move(map_builder), node_options, trajectory_options,
cartographer_ros::SplitString(FLAGS_bag_filenames, ','));
::ros::shutdown(); ::ros::shutdown();
} }

View File

@ -21,8 +21,10 @@
#include <sys/resource.h> #include <sys/resource.h>
#include <time.h> #include <time.h>
#include <chrono> #include <chrono>
#include <queue>
#include "cartographer_ros/node.h" #include "cartographer_ros/node.h"
#include "cartographer_ros/split_string.h"
#include "cartographer_ros/urdf_reader.h" #include "cartographer_ros/urdf_reader.h"
#include "ros/callback_queue.h" #include "ros/callback_queue.h"
#include "rosbag/bag.h" #include "rosbag/bag.h"
@ -32,6 +34,14 @@
#include "tf2_ros/static_transform_broadcaster.h" #include "tf2_ros/static_transform_broadcaster.h"
#include "urdf/model.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( DEFINE_string(
urdf_filename, "", urdf_filename, "",
"URDF file that contains static links for your sensor configuration."); "URDF file that contains static links for your sensor configuration.");
@ -51,11 +61,26 @@ constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.; constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1; constexpr int kSingleThreaded = 1;
void RunOfflineNode( void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, CHECK(!FLAGS_configuration_directory.empty())
const cartographer_ros::NodeOptions& node_options, << "-configuration_directory is missing.";
const cartographer_ros::TrajectoryOptions& trajectory_options, CHECK(!FLAGS_configuration_basename.empty())
const std::vector<std::string>& bag_filenames) { << "-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 = const std::chrono::time_point<std::chrono::steady_clock> start_time =
std::chrono::steady_clock::now(); std::chrono::steady_clock::now();

View File

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

View File

@ -15,52 +15,27 @@
*/ */
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/offline_node.h" #include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.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.");
int main(int argc, char** argv) { int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]); google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true); 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::init(argc, argv, "cartographer_offline_node");
::ros::start(); ::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::NodeOptions node_options; const cartographer_ros::MapBuilderFactory map_builder_factory =
cartographer_ros::TrajectoryOptions trajectory_options; [](const ::cartographer::mapping::proto::MapBuilderOptions&
std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions( map_builder_options) {
FLAGS_configuration_directory, FLAGS_configuration_basename); 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 cartographer_ros::RunOfflineNode(map_builder_factory);
// 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, ','));
::ros::shutdown(); ::ros::shutdown();
} }