From 2538ac3e45ccaf553e956bbb7e745c26008460bf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Mon, 29 Jan 2018 15:43:25 +0100
Subject: [PATCH] Offline node: better support for sequential bags. (#694)

Allow same topics to be used in different bags (a previously supported use case).
Remove unused variable `current_bag_sensor_topics`.
Touch up flag descriptions.

Fixes #693.

pair=@gaschler
---
 .../cartographer_ros/offline_node.cc          | 31 ++++++++++---------
 1 file changed, 16 insertions(+), 15 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc
index 31439bb..4d21e5d 100644
--- a/cartographer_ros/cartographer_ros/offline_node.cc
+++ b/cartographer_ros/cartographer_ros/offline_node.cc
@@ -40,12 +40,13 @@ DEFINE_string(
     configuration_basenames, "",
     "Comma-separated list of basenames, i.e. not containing any "
     "directory prefix, of the configuration files for each trajectory. "
-    "The first configuration file will be used for node options."
+    "The first configuration file will be used for node options. "
     "If less configuration files are specified than trajectories, the "
     "first file will be used the remaining trajectories.");
 DEFINE_string(
     bag_filenames, "",
-    "Comma-separated list of bags to process. One bag per trajectory.");
+    "Comma-separated list of bags to process. One bag per trajectory. "
+    "Any combination of simultaneous and sequential bags is supported.");
 DEFINE_string(urdf_filenames, "",
               "Comma-separated list of one or more URDF files that contain "
               "static links for the sensor configuration(s).");
@@ -153,7 +154,7 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
 
   auto bag_expected_sensor_ids =
       node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
-  std::map<std::string,
+  std::map<std::pair<int /* bag_index */, std::string>,
            cartographer::mapping::TrajectoryBuilderInterface::SensorId>
       bag_topic_to_sensor_id;
   PlayableBagMultiplexer playable_bag_multiplexer;
@@ -163,20 +164,19 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     if (!::ros::ok()) {
       return;
     }
-    std::unordered_set<std::string> current_bag_sensor_topics;
     for (const auto& expected_sensor_id :
          bag_expected_sensor_ids.at(current_bag_index)) {
-      std::string resolved_topic =
-          node.node_handle()->resolveName(expected_sensor_id.id);
-      if (bag_topic_to_sensor_id.count(resolved_topic) != 0) {
-        LOG(ERROR) << "Sensor " << expected_sensor_id.id
-                   << " resolves to topic " << resolved_topic
-                   << " which is already used by "
+      const auto bag_resolved_topic = std::make_pair(
+          static_cast<int>(current_bag_index),
+          node.node_handle()->resolveName(expected_sensor_id.id));
+      if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
+        LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
+                   << current_bag_index << " resolves to topic "
+                   << bag_resolved_topic.second << " which is already used by "
                    << " sensor "
-                   << bag_topic_to_sensor_id.at(resolved_topic).id;
+                   << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
       }
-      bag_topic_to_sensor_id[resolved_topic] = expected_sensor_id;
-      current_bag_sensor_topics.insert(resolved_topic);
+      bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
     }
 
     playable_bag_multiplexer.AddPlayableBag(PlayableBag(
@@ -243,8 +243,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     if (!::ros::ok()) {
       return;
     }
-    const std::string bag_topic =
-        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
+    const auto bag_topic = std::make_pair(
+        bag_index,
+        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
     auto it = bag_topic_to_sensor_id.find(bag_topic);
     if (it != bag_topic_to_sensor_id.end()) {
       const std::string& sensor_id = it->second.id;