Add time skip option for offline node (#680)

Introduces a "skip" option which skips first _t_ seconds.

cartographer_ros/cartographer_ros/offline_node.cc

@@ -60,6 +60,9 @@ DEFINE_bool(load_frozen_state, true,
 DEFINE_bool(keep_running, false,
             "Keep running the offline node after all messages from the bag "
             "have been processed.");
+DEFINE_double(skip_seconds, 0,
+              "Optional amount of seconds to skip from the beginning "
+              "(i.e. when the earliest bag starts.). ");
 
 namespace cartographer_ros {
 
@@ -233,12 +236,21 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
 
   // TODO(gaschler): Warn if resolved topics are not in bags.
   std::unordered_map<int, int> bag_index_to_trajectory_id;
+  const ros::Time begin_time = playable_bag_multiplexer.PeekMessageTime();
   while (playable_bag_multiplexer.IsMessageAvailable()) {
+    if (!::ros::ok()) {
+      return;
+    }
+
     const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
     const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
     const int bag_index = std::get<1>(next_msg_tuple);
     const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);
 
+    if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) {
+      continue;
+    }
+
     int trajectory_id;
     // Lazily add trajectories only when the first message arrives in order
     // to avoid blocking the sensor queue.
@@ -257,9 +269,6 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
       trajectory_id = bag_index_to_trajectory_id.at(bag_index);
     }
 
-    if (!::ros::ok()) {
-      return;
-    }
     const auto bag_topic = std::make_pair(
         bag_index,
         node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));

cartographer_ros/cartographer_ros/playable_bag.cc

@@ -45,12 +45,12 @@ PlayableBag::PlayableBag(
   AdvanceUntilMessageAvailable();
 }
 
-ros::Time PlayableBag::PeekMessageTime() {
+ros::Time PlayableBag::PeekMessageTime() const {
   CHECK(IsMessageAvailable());
   return buffered_messages_.front().getTime();
 }
 
-std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() {
+std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() const {
   return std::make_tuple(view_->getBeginTime(), view_->getEndTime());
 }
 
@@ -67,13 +67,13 @@ rosbag::MessageInstance PlayableBag::GetNextMessage() {
   return msg;
 }
 
-bool PlayableBag::IsMessageAvailable() {
+bool PlayableBag::IsMessageAvailable() const {
   return !buffered_messages_.empty() &&
          (buffered_messages_.front().getTime() <
           buffered_messages_.back().getTime() - buffer_delay_);
 }
 
-int PlayableBag::bag_id() { return bag_id_; }
+int PlayableBag::bag_id() const { return bag_id_; }
 
 void PlayableBag::AdvanceOneMessage() {
   CHECK(!finished_);
@@ -106,7 +106,7 @@ void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
                      static_cast<int>(playable_bags_.size() - 1)});
 }
 
-bool PlayableBagMultiplexer::IsMessageAvailable() {
+bool PlayableBagMultiplexer::IsMessageAvailable() const {
   return !next_message_queue_.empty();
 }
 
@@ -126,4 +126,9 @@ PlayableBagMultiplexer::GetNextMessage() {
                          !current_bag.IsMessageAvailable());
 }
 
+ros::Time PlayableBagMultiplexer::PeekMessageTime() const {
+  CHECK(IsMessageAvailable());
+  return next_message_queue_.top().message_timestamp;
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/playable_bag.h

@@ -37,12 +37,12 @@ class PlayableBag {
               ros::Time end_time, ros::Duration buffer_delay,
               FilteringEarlyMessageHandler filtering_early_message_handler);
 
-  ros::Time PeekMessageTime();
+  ros::Time PeekMessageTime() const;
   rosbag::MessageInstance GetNextMessage();
-  bool IsMessageAvailable();
-  std::tuple<ros::Time, ros::Time> GetBeginEndTime();
+  bool IsMessageAvailable() const;
+  std::tuple<ros::Time, ros::Time> GetBeginEndTime() const;
 
-  int bag_id();
+  int bag_id() const;
 
  private:
   void AdvanceOneMessage();
@@ -72,7 +72,8 @@ class PlayableBagMultiplexer {
              bool /* is_last_message_in_bag */>
   GetNextMessage();
 
-  bool IsMessageAvailable();
+  bool IsMessageAvailable() const;
+  ros::Time PeekMessageTime() const;
 
  private:
   struct BagMessageItem {