From 42440452b6fdd94ed08d46b9b4a3738888f8cf07 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Wed, 30 Nov 2016 16:48:03 +0100 Subject: [PATCH] Adds multi-trajectory support to offline mapping. (#204) --- .../cartographer_ros/map_builder_bridge.cc | 2 +- .../cartographer_ros/offline_node_main.cc | 116 ++++++++++-------- 2 files changed, 67 insertions(+), 51 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 18e7c2f..2218d85 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -59,7 +59,7 @@ void MapBuilderBridge::WriteAssets(const string& stem) { if (trajectory_nodes.empty()) { LOG(WARNING) << "No data was collected and no assets will be written."; } else { - LOG(INFO) << "Writing assets to '" << stem << "'..."; + LOG(INFO) << "Writing assets with stem '" << stem << "'..."; cartographer_ros::WriteAssets(trajectory_nodes, options_, stem); } } diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 97b0aa3..330ce14 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -14,6 +14,7 @@ * limitations under the License. */ +#include #include #include @@ -37,8 +38,7 @@ DEFINE_string(configuration_directory, "", DEFINE_string(configuration_basename, "", "Basename, i.e. not containing any directory prefix, of the " "configuration file."); -// TODO(damonkohler): Support multi-trajectory. -DEFINE_string(bag_filename, "", "Bag to process."); +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."); @@ -49,8 +49,17 @@ namespace { constexpr int kLatestOnlyPublisherQueueSize = 1; constexpr char kClockTopic[] = "clock"; -void Run() { - // TODO(damonkohler): Pull out this common code across binaries. +std::vector SplitString(const string& input, const char delimiter) { + std::stringstream stream(input); + string token; + std::vector tokens; + while (std::getline(stream, token, delimiter)) { + tokens.push_back(token); + } + return tokens; +} + +void Run(std::vector bag_filenames) { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( std::vector{FLAGS_configuration_directory}); @@ -106,61 +115,67 @@ void Run() { node.node_handle()->resolveName(kOdometryTopic, false /* remap */)); } - // TODO(damonkohler): Support multi-trajectory. - const int trajectory_id = node.map_builder_bridge()->AddTrajectory( - expected_sensor_ids, options.tracking_frame); - ::ros::Publisher clock_publisher = node.node_handle()->advertise( kClockTopic, kLatestOnlyPublisherQueueSize); - rosbag::Bag bag; - bag.open(FLAGS_bag_filename, rosbag::bagmode::Read); - for (const rosbag::MessageInstance& msg : rosbag::View(bag)) { + for (const string& bag_filename : bag_filenames) { if (!::ros::ok()) { - break; + return; } - const string& topic = node.node_handle()->resolveName(msg.getTopic()); - if (expected_sensor_ids.count( - node.node_handle()->resolveName(msg.getTopic())) == 0) { - continue; - } - if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleLaserScanMessage(topic, - msg.instantiate()); - } else if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage( - topic, msg.instantiate()); - } else if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandlePointCloud2Message( - topic, msg.instantiate()); - } else if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleImuMessage(topic, msg.instantiate()); - } else if (msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleOdometryMessage(topic, msg.instantiate()); + const int trajectory_id = node.map_builder_bridge()->AddTrajectory( + expected_sensor_ids, options.tracking_frame); + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + + for (const rosbag::MessageInstance& msg : rosbag::View(bag)) { + if (!::ros::ok()) { + return; + } + + const string topic = node.node_handle()->resolveName(msg.getTopic()); + if (expected_sensor_ids.count(topic) == 0) { + continue; + } + if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleLaserScanMessage(topic, + msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage( + topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandlePointCloud2Message( + topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleImuMessage(topic, msg.instantiate()); + } else if (msg.isType()) { + node.map_builder_bridge() + ->sensor_bridge(trajectory_id) + ->HandleOdometryMessage(topic, + msg.instantiate()); + } + + rosgraph_msgs::Clock clock; + clock.clock = msg.getTime(); + clock_publisher.publish(clock); + + ::ros::spinOnce(); } - rosgraph_msgs::Clock clock; - clock.clock = msg.getTime(); - clock_publisher.publish(clock); - - ::ros::spinOnce(); + bag.close(); + node.map_builder_bridge()->FinishTrajectory(trajectory_id); } - bag.close(); - node.map_builder_bridge()->FinishTrajectory(trajectory_id); - node.map_builder_bridge()->WriteAssets(FLAGS_bag_filename); + node.map_builder_bridge()->WriteAssets(bag_filenames.front()); } } // namespace @@ -174,14 +189,15 @@ int main(int argc, char** argv) { << "-configuration_directory is missing."; CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing."; - CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing."; CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing."; ::ros::init(argc, argv, "cartographer_offline_node"); ::ros::start(); cartographer_ros::ScopedRosLogSink ros_log_sink; - cartographer_ros::Run(); + cartographer_ros::Run( + cartographer_ros::SplitString(FLAGS_bag_filenames, ',')); ::ros::shutdown(); }