Simultaneous offline multi trajectories (#636)

RFC=[0009](https://github.com/googlecartographer/rfcs/pull/4)

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -257,8 +257,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
   visualization_msgs::Marker residual_inter_same_trajectory_marker =
       constraint_intra_marker;
   residual_inter_same_trajectory_marker.id = marker_id++;
-  residual_inter_same_trajectory_marker.ns =
+  residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory";
-      "Inter residuals, same trajectory";
   residual_inter_same_trajectory_marker.pose.position.z = 0.1;
 
   visualization_msgs::Marker constraint_inter_diff_trajectory_marker =
@@ -309,7 +308,7 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
         // Bright orange
         color_constraint.a = 1.0;
         color_constraint.r = 1.0;
-        color_constraint.g = 165./255.;
+        color_constraint.g = 165. / 255.;
       }
       // Bright cyan
       color_residual.a = 1.0;

cartographer_ros/cartographer_ros/node.cc

@@ -122,7 +122,7 @@ Node::Node(
       &Node::PublishConstraintList, this));
 }
 
-Node::~Node() {}
+Node::~Node() { FinishAllTrajectories(); }
 
 ::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
 

cartographer_ros/cartographer_ros/offline_node.cc

@@ -21,17 +21,14 @@
 #include <sys/resource.h>
 #include <time.h>
 #include <chrono>
-#include <queue>
 
 #include "cartographer_ros/node.h"
+#include "cartographer_ros/playable_bag.h"
 #include "cartographer_ros/split_string.h"
 #include "cartographer_ros/urdf_reader.h"
 #include "gflags/gflags.h"
 #include "ros/callback_queue.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
 #include "rosgraph_msgs/Clock.h"
-#include "tf2_msgs/TFMessage.h"
 #include "tf2_ros/static_transform_broadcaster.h"
 #include "urdf/model.h"
 
@@ -39,20 +36,35 @@ 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, "",
+    configuration_basenames, "",
-    "URDF file that contains static links for your sensor configuration.");
+    "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."
+    "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.");
+DEFINE_string(urdf_filenames, "",
+              "Comma-separated list of one or more URDF files that contain "
+              "static links for the sensor configuration(s).");
 DEFINE_bool(use_bag_transforms, true,
-            "Whether to read, use and republish the transforms from the bag.");
+            "Whether to read, use and republish transforms from bags.");
 DEFINE_string(pbstream_filename, "",
               "If non-empty, filename of a pbstream to load.");
 DEFINE_bool(keep_running, false,
             "Keep running the offline node after all messages from the bag "
             "have been processed.");
+DEFINE_string(sensor_topics, "",
+              "Optional comma-separated list of colon-separated lists of "
+              "sensor topics for each trajectory. A single trajectory's topics "
+              "are delimited with colons, while different trajectories are "
+              "delimited with commas. If a blank list is given for a "
+              "trajectory, the default topics are used. If a single "
+              "colon-separated list is given, it will be used for all "
+              "trajectories. If omitted, default topics will be used "
+              "for all trajectories.");
 
 namespace cartographer_ros {
 
@@ -61,19 +73,37 @@ constexpr char kTfStaticTopic[] = "/tf_static";
 constexpr char kTfTopic[] = "tf";
 constexpr double kClockPublishFrequencySec = 1. / 30.;
 constexpr int kSingleThreaded = 1;
+// We publish tf messages one second earlier than other messages. Under
+// the assumption of higher frequency tf this should ensure that tf can
+// always interpolate.
+const ::ros::Duration kDelay = ::ros::Duration(1.0);
 
 void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
   CHECK(!FLAGS_configuration_directory.empty())
       << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basename.empty())
+  CHECK(!FLAGS_configuration_basenames.empty())
-      << "-configuration_basename is missing.";
+      << "-configuration_basenames 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;
+  const auto configuration_basenames =
-  std::tie(node_options, trajectory_options) = cartographer_ros::LoadOptions(
+      cartographer_ros::SplitString(FLAGS_configuration_basenames, ',');
-      FLAGS_configuration_directory, FLAGS_configuration_basename);
+  std::vector<TrajectoryOptions> bag_trajectory_options(1);
+  std::tie(node_options, bag_trajectory_options.at(0)) =
+      LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));
+
+  for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
+    TrajectoryOptions current_trajectory_options;
+    if (bag_index < configuration_basenames.size()) {
+      std::tie(std::ignore, current_trajectory_options) = LoadOptions(
+          FLAGS_configuration_directory, configuration_basenames.at(bag_index));
+    } else {
+      current_trajectory_options = bag_trajectory_options.at(0);
+    }
+    bag_trajectory_options.push_back(current_trajectory_options);
+  }
+  CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
 
   // 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
@@ -88,9 +118,13 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
   tf2_ros::Buffer tf_buffer;
 
   std::vector<geometry_msgs::TransformStamped> urdf_transforms;
-  if (!FLAGS_urdf_filename.empty()) {
+  for (const std::string& urdf_filename :
-    urdf_transforms =
+       cartographer_ros::SplitString(FLAGS_urdf_filenames, ',')) {
-        ReadStaticTransformsFromUrdf(FLAGS_urdf_filename, &tf_buffer);
+    const auto current_urdf_transforms =
+        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
+    urdf_transforms.insert(urdf_transforms.end(),
+                           current_urdf_transforms.begin(),
+                           current_urdf_transforms.end());
   }
 
   tf_buffer.setUsingDedicatedThread(true);
@@ -102,13 +136,6 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     node.LoadMap(FLAGS_pbstream_filename);
   }
 
-  std::unordered_set<std::string> expected_sensor_ids;
-  for (const std::string& topic :
-       node.ComputeDefaultTopics(trajectory_options)) {
-    CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
-              .second);
-  }
-
   ::ros::Publisher tf_publisher =
       node.node_handle()->advertise<tf2_msgs::TFMessage>(
           kTfTopic, kLatestOnlyPublisherQueueSize);
@@ -133,101 +160,155 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
       },
       false /* oneshot */, false /* autostart */);
 
-  for (const std::string& bag_filename : bag_filenames) {
+  // Colon-delimited string lists of sensor topics for each bag.
+  std::vector<std::string> bag_sensor_topics_strings;
+
+  if (FLAGS_sensor_topics.empty()) {
+    // Use default topic names for all bags, denoted by an empty sensor
+    // topic list string for each bag.
+    bag_sensor_topics_strings =
+        std::vector<std::string>(bag_filenames.size(), std::string());
+  } else {
+    bag_sensor_topics_strings =
+        cartographer_ros::SplitString(FLAGS_sensor_topics, ',');
+    if (bag_sensor_topics_strings.size() == 1) {
+      // Use the single specified topic list string for all bags.
+      bag_sensor_topics_strings.insert(bag_sensor_topics_strings.end(),
+                                       bag_filenames.size() - 1,
+                                       bag_sensor_topics_strings.at(0));
+    }
+  }
+  CHECK_EQ(bag_sensor_topics_strings.size(), bag_filenames.size());
+
+  std::vector<std::unordered_set<std::string>> bag_sensor_topics;
+  std::unordered_map<int, int> bag_index_to_trajectory_id;
+  PlayableBagMultiplexer playable_bag_multiplexer;
+  for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
+       ++current_bag_index) {
+    const std::string& bag_filename = bag_filenames.at(current_bag_index);
     if (!::ros::ok()) {
-      break;
+      return;
     }
+    std::vector<std::string> unresolved_current_bag_sensor_topics;
+    if (bag_sensor_topics_strings.at(current_bag_index).empty()) {
+      // Empty topic list string provided for this trajectory,
+      // use default topics.
+      const auto default_topics = node.ComputeDefaultTopics(
+          bag_trajectory_options.at(current_bag_index));
+      unresolved_current_bag_sensor_topics = std::vector<std::string>(
+          default_topics.begin(), default_topics.end());
+    } else {
+      unresolved_current_bag_sensor_topics = cartographer_ros::SplitString(
+          bag_sensor_topics_strings.at(current_bag_index), ':');
+    }
+    std::unordered_set<std::string> current_bag_sensor_topics;
+    for (const auto& topic : unresolved_current_bag_sensor_topics) {
+      CHECK(current_bag_sensor_topics
+                .insert(node.node_handle()->resolveName(topic))
+                .second);
+    }
+    bag_sensor_topics.push_back(current_bag_sensor_topics);
 
-    const int trajectory_id =
+    playable_bag_multiplexer.AddPlayableBag(PlayableBag(
-        node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options);
+        bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
+        // PlayableBag::FilteringEarlyMessageHandler is used to get an early
+        // peek at the tf messages in the bag and insert them into 'tf_buffer'.
+        // When a message is retrieved by GetNextMessage() further below,
+        // we will have already inserted further 'kDelay' seconds worth of
+        // transforms into 'tf_buffer' via this lambda.
+        [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
+          if (msg.isType<tf2_msgs::TFMessage>()) {
+            if (FLAGS_use_bag_transforms) {
+              const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
+              tf_publisher.publish(tf_message);
 
-    rosbag::Bag bag;
+              for (const auto& transform : tf_message->transforms) {
-    bag.open(bag_filename, rosbag::bagmode::Read);
+                try {
-    rosbag::View view(bag);
+                  // We need to keep 'tf_buffer' small because it becomes very
-    const ::ros::Time begin_time = view.getBeginTime();
+                  // inefficient otherwise. We make sure that tf_messages are
-    const double duration_in_seconds = (view.getEndTime() - begin_time).toSec();
+                  // published before any data messages, so that tf lookups
-
+                  // always work.
-    // We need to keep 'tf_buffer' small because it becomes very inefficient
+                  tf_buffer.setTransform(transform, "unused_authority",
-    // otherwise. We make sure that tf_messages are published before any data
+                                         msg.getTopic() == kTfStaticTopic);
-    // messages, so that tf lookups always work.
+                } catch (const tf2::TransformException& ex) {
-    std::deque<rosbag::MessageInstance> delayed_messages;
+                  LOG(WARNING) << ex.what();
-    // We publish tf messages one second earlier than other messages. Under
+                }
-    // the assumption of higher frequency tf this should ensure that tf can
+              }
-    // always interpolate.
+            }
-    const ::ros::Duration kDelay(1.);
+            // Tell 'PlayableBag' to filter the tf message since there is no
-    for (const rosbag::MessageInstance& msg : view) {
+            // further use for it.
-      if (!::ros::ok()) {
+            return false;
-        break;
+          } else {
-      }
+            return true;
-
-      if (FLAGS_use_bag_transforms && msg.isType<tf2_msgs::TFMessage>()) {
-        auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
-        tf_publisher.publish(tf_message);
-
-        for (const auto& transform : tf_message->transforms) {
-          try {
-            tf_buffer.setTransform(transform, "unused_authority",
-                                   msg.getTopic() == kTfStaticTopic);
-          } catch (const tf2::TransformException& ex) {
-            LOG(WARNING) << ex.what();
           }
-        }
+        }));
-      }
+  }
+  CHECK_EQ(bag_sensor_topics.size(), bag_filenames.size());
 
-      while (!delayed_messages.empty() &&
+  while (playable_bag_multiplexer.IsMessageAvailable()) {
-             delayed_messages.front().getTime() < msg.getTime() - kDelay) {
+    const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
-        const rosbag::MessageInstance& delayed_msg = delayed_messages.front();
+    const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
-        const std::string topic = node.node_handle()->resolveName(
+    const int bag_index = std::get<1>(next_msg_tuple);
-            delayed_msg.getTopic(), false /* resolve */);
+    const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);
-        if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
-          node.HandleLaserScanMessage(
-              trajectory_id, topic,
-              delayed_msg.instantiate<sensor_msgs::LaserScan>());
-        }
-        if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
-          node.HandleMultiEchoLaserScanMessage(
-              trajectory_id, topic,
-              delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
-        }
-        if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
-          node.HandlePointCloud2Message(
-              trajectory_id, topic,
-              delayed_msg.instantiate<sensor_msgs::PointCloud2>());
-        }
-        if (delayed_msg.isType<sensor_msgs::Imu>()) {
-          node.HandleImuMessage(trajectory_id, topic,
-                                delayed_msg.instantiate<sensor_msgs::Imu>());
-        }
-        if (delayed_msg.isType<nav_msgs::Odometry>()) {
-          node.HandleOdometryMessage(
-              trajectory_id, topic,
-              delayed_msg.instantiate<nav_msgs::Odometry>());
-        }
-        if (delayed_msg.isType<sensor_msgs::NavSatFix>()) {
-          node.HandleNavSatFixMessage(
-              trajectory_id, topic,
-              delayed_msg.instantiate<sensor_msgs::NavSatFix>());
-        }
-        clock.clock = delayed_msg.getTime();
-        clock_publisher.publish(clock);
 
-        LOG_EVERY_N(INFO, 100000)
+    int trajectory_id;
-            << "Processed " << (delayed_msg.getTime() - begin_time).toSec()
+    // Lazily add trajectories only when the first message arrives in order
-            << " of " << duration_in_seconds << " bag time seconds...";
+    // to avoid blocking the sensor queue.
-
+    if (bag_index_to_trajectory_id.count(bag_index) == 0) {
-        delayed_messages.pop_front();
+      trajectory_id =
-      }
+          node.AddOfflineTrajectory(bag_sensor_topics.at(bag_index),
-
+                                    bag_trajectory_options.at(bag_index));
-      const std::string topic =
+      CHECK(bag_index_to_trajectory_id
-          node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
+                .emplace(std::piecewise_construct,
-      if (expected_sensor_ids.count(topic) == 0) {
+                         std::forward_as_tuple(bag_index),
-        continue;
+                         std::forward_as_tuple(trajectory_id))
-      }
+                .second);
-      delayed_messages.push_back(msg);
+      LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
+                << bag_filenames.at(bag_index);
+    } else {
+      trajectory_id = bag_index_to_trajectory_id.at(bag_index);
     }
 
-    bag.close();
+    if (!::ros::ok()) {
-    node.FinishTrajectory(trajectory_id);
+      return;
+    }
+    const std::string topic =
+        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
+    if (bag_sensor_topics.at(bag_index).count(topic) == 0) {
+      continue;
+    }
+
+    if (msg.isType<sensor_msgs::LaserScan>()) {
+      node.HandleLaserScanMessage(trajectory_id, topic,
+                                  msg.instantiate<sensor_msgs::LaserScan>());
+    }
+    if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
+      node.HandleMultiEchoLaserScanMessage(
+          trajectory_id, topic,
+          msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
+    }
+    if (msg.isType<sensor_msgs::PointCloud2>()) {
+      node.HandlePointCloud2Message(
+          trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
+    }
+    if (msg.isType<sensor_msgs::Imu>()) {
+      node.HandleImuMessage(trajectory_id, topic,
+                            msg.instantiate<sensor_msgs::Imu>());
+    }
+    if (msg.isType<nav_msgs::Odometry>()) {
+      node.HandleOdometryMessage(trajectory_id, topic,
+                                 msg.instantiate<nav_msgs::Odometry>());
+    }
+    if (msg.isType<sensor_msgs::NavSatFix>()) {
+      node.HandleNavSatFixMessage(trajectory_id, topic,
+                                  msg.instantiate<sensor_msgs::NavSatFix>());
+    }
+    clock.clock = msg.getTime();
+    clock_publisher.publish(clock);
+
+    if (is_last_message_in_bag) {
+      node.FinishTrajectory(trajectory_id);
+    }
   }
 
   // Ensure the clock is republished after the bag has been finished, during the

cartographer_ros/cartographer_ros/playable_bag.cc

@@ -0,0 +1,129 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_ros/playable_bag.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer_ros/node_constants.h"
+#include "glog/logging.h"
+#include "tf2_msgs/TFMessage.h"
+
+namespace cartographer_ros {
+
+PlayableBag::PlayableBag(
+    const std::string& bag_filename, const int bag_id,
+    const ros::Time start_time, const ros::Time end_time,
+    const ros::Duration buffer_delay,
+    FilteringEarlyMessageHandler filtering_early_message_handler)
+    : bag_(cartographer::common::make_unique<rosbag::Bag>(
+          bag_filename, rosbag::bagmode::Read)),
+      view_(cartographer::common::make_unique<rosbag::View>(*bag_, start_time,
+                                                            end_time)),
+      view_iterator_(view_->begin()),
+      finished_(false),
+      bag_id_(bag_id),
+      bag_filename_(bag_filename),
+      duration_in_seconds_(
+          (view_->getEndTime() - view_->getBeginTime()).toSec()),
+      log_counter_(0),
+      buffer_delay_(buffer_delay),
+      filtering_early_message_handler_(
+          std::move(filtering_early_message_handler)) {
+  AdvanceUntilMessageAvailable();
+}
+
+ros::Time PlayableBag::PeekMessageTime() {
+  CHECK(IsMessageAvailable());
+  return buffered_messages_.front().getTime();
+}
+
+std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() {
+  return std::make_tuple(view_->getBeginTime(), view_->getEndTime());
+}
+
+rosbag::MessageInstance PlayableBag::GetNextMessage() {
+  CHECK(IsMessageAvailable());
+  const rosbag::MessageInstance msg = buffered_messages_.front();
+  buffered_messages_.pop_front();
+  AdvanceUntilMessageAvailable();
+  if ((log_counter_++ % 10000) == 0) {
+    LOG(INFO) << "Processed " << (msg.getTime() - view_->getBeginTime()).toSec()
+              << " of " << duration_in_seconds_ << " seconds of bag "
+              << bag_filename_;
+  }
+  return msg;
+}
+
+bool PlayableBag::IsMessageAvailable() {
+  return !buffered_messages_.empty() &&
+         (buffered_messages_.front().getTime() <
+          buffered_messages_.back().getTime() - buffer_delay_);
+}
+
+int PlayableBag::bag_id() { return bag_id_; }
+
+void PlayableBag::AdvanceOneMessage() {
+  CHECK(!finished_);
+  if (view_iterator_ == view_->end()) {
+    finished_ = true;
+    return;
+  }
+  rosbag::MessageInstance& msg = *view_iterator_;
+  if (!filtering_early_message_handler_ ||
+      filtering_early_message_handler_(msg)) {
+    buffered_messages_.push_back(msg);
+  }
+  ++view_iterator_;
+}
+
+void PlayableBag::AdvanceUntilMessageAvailable() {
+  if (finished_) {
+    return;
+  }
+  do {
+    AdvanceOneMessage();
+  } while (!finished_ && !IsMessageAvailable());
+}
+
+void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
+  playable_bags_.push_back(std::move(playable_bag));
+  CHECK(playable_bags_.back().IsMessageAvailable());
+  next_message_queue_.emplace(
+      BagMessageItem{playable_bags_.back().PeekMessageTime(),
+                     static_cast<int>(playable_bags_.size() - 1)});
+}
+
+bool PlayableBagMultiplexer::IsMessageAvailable() {
+  return !next_message_queue_.empty();
+}
+
+std::tuple<rosbag::MessageInstance, int, bool>
+PlayableBagMultiplexer::GetNextMessage() {
+  CHECK(IsMessageAvailable());
+  const int current_bag_index = next_message_queue_.top().bag_index;
+  PlayableBag& current_bag = playable_bags_.at(current_bag_index);
+  rosbag::MessageInstance msg = current_bag.GetNextMessage();
+  CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp);
+  next_message_queue_.pop();
+  if (current_bag.IsMessageAvailable()) {
+    next_message_queue_.emplace(
+        BagMessageItem{current_bag.PeekMessageTime(), current_bag_index});
+  }
+  return std::make_tuple(std::move(msg), current_bag.bag_id(),
+                         !current_bag.IsMessageAvailable());
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/playable_bag.h

@@ -0,0 +1,96 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_ROS_PLAYABLE_BAG_H_
+#define CARTOGRAPHER_ROS_PLAYABLE_BAG_H_
+#include <functional>
+#include <queue>
+
+#include "rosbag/bag.h"
+#include "rosbag/view.h"
+#include "tf2_ros/buffer.h"
+
+namespace cartographer_ros {
+
+class PlayableBag {
+ public:
+  // Handles messages early, i.e. when they are about to enter the buffer.
+  // Returns a boolean indicating whether the message should enter the buffer.
+  using FilteringEarlyMessageHandler =
+      std::function<bool /* forward_message_to_buffer */ (
+          const rosbag::MessageInstance&)>;
+
+  PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time,
+              ros::Time end_time, ros::Duration buffer_delay,
+              FilteringEarlyMessageHandler filtering_early_message_handler);
+
+  ros::Time PeekMessageTime();
+  rosbag::MessageInstance GetNextMessage();
+  bool IsMessageAvailable();
+  std::tuple<ros::Time, ros::Time> GetBeginEndTime();
+
+  int bag_id();
+
+ private:
+  void AdvanceOneMessage();
+  void AdvanceUntilMessageAvailable();
+
+  std::unique_ptr<rosbag::Bag> bag_;
+  std::unique_ptr<rosbag::View> view_;
+  rosbag::View::const_iterator view_iterator_;
+  bool finished_;
+  const int bag_id_;
+  const std::string bag_filename_;
+  const double duration_in_seconds_;
+  int log_counter_;
+  std::deque<rosbag::MessageInstance> buffered_messages_;
+  const ::ros::Duration buffer_delay_;
+  FilteringEarlyMessageHandler filtering_early_message_handler_;
+};
+
+class PlayableBagMultiplexer {
+ public:
+  void AddPlayableBag(PlayableBag playable_bag);
+
+  // Returns the next message from the multiplexed (merge-sorted) message
+  // stream, along with the bag id corresponding to the message, and whether
+  // this was the last message in that bag.
+  std::tuple<rosbag::MessageInstance, int /* bag_id */,
+             bool /* is_last_message_in_bag */>
+  GetNextMessage();
+
+  bool IsMessageAvailable();
+
+ private:
+  struct BagMessageItem {
+    ros::Time message_timestamp;
+    int bag_index;
+    struct TimestampIsGreater {
+      bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
+        return l.message_timestamp > r.message_timestamp;
+      }
+    };
+  };
+
+  std::vector<PlayableBag> playable_bags_;
+  std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
+                      BagMessageItem::TimestampIsGreater>
+      next_message_queue_;
+};
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_PLAYABLE_BAG_H_

cartographer_ros/launch/offline_backpack_2d.launch

@@ -30,8 +30,8 @@
       required="$(arg no_rviz)"
       type="cartographer_offline_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
-          -configuration_basename backpack_2d.lua
+          -configuration_basenames backpack_2d.lua
-          -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
+          -urdf_filenames $(find cartographer_ros)/urdf/backpack_2d.urdf
           -bag_filenames $(arg bag_filenames)"
       output="screen">
     <remap from="echoes" to="horizontal_laser_2d" />

cartographer_ros/launch/offline_backpack_3d.launch

@@ -23,8 +23,8 @@
   <node name="cartographer_offline_node" pkg="cartographer_ros"
       type="cartographer_offline_node" args="
           -configuration_directory $(find cartographer_ros)/configuration_files
-          -configuration_basename backpack_3d.lua
+          -configuration_basenames backpack_3d.lua
-          -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
+          -urdf_filenames $(find cartographer_ros)/urdf/backpack_3d.urdf
           -bag_filenames $(arg bag_filenames)"
       output="screen">
     <remap from="points2_1" to="horizontal_laser_3d" />