From 5f2dff9df06e14c78c17df17e8c9c85d34a51fc8 Mon Sep 17 00:00:00 2001
From: Alireza <alireza.mixedreality@gmail.com>
Date: Tue, 9 Oct 2018 15:26:48 +0200
Subject: [PATCH] fix: Use an explicit message_counter instead of using
 `std::distance` (#1045)

As @ojura reported and explained in #940, there is a critical issue with the new changes introduced in the mentioned PR, which significantly slows down the offline node.
- The reason of this problem was that In order to count the number of processed messages the `std::distance` function was used which is computationally expensive(O(n)).
- Instead, the former `log_counter_` and now `message_counter_` class variable which was used to print a message every X seconds is also employed to count the number of the processed (and also skipped) messages.
---
 cartographer_ros/cartographer_ros/playable_bag.cc | 8 ++++----
 cartographer_ros/cartographer_ros/playable_bag.h  | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc
index 813e3ad..350ee1b 100644
--- a/cartographer_ros/cartographer_ros/playable_bag.cc
+++ b/cartographer_ros/cartographer_ros/playable_bag.cc
@@ -36,7 +36,7 @@ PlayableBag::PlayableBag(
       bag_filename_(bag_filename),
       duration_in_seconds_(
           (view_->getEndTime() - view_->getBeginTime()).toSec()),
-      log_counter_(0),
+      message_counter_(0),
       buffer_delay_(buffer_delay),
       filtering_early_message_handler_(
           std::move(filtering_early_message_handler)) {
@@ -62,7 +62,7 @@ rosbag::MessageInstance PlayableBag::GetNextMessage(
   buffered_messages_.pop_front();
   AdvanceUntilMessageAvailable();
   double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec();
-  if ((log_counter_++ % 10000) == 0) {
+  if ((message_counter_ % 10000) == 0) {
     LOG(INFO) << "Processed " << processed_seconds << " of "
               << duration_in_seconds_ << " seconds of bag " << bag_filename_;
   }
@@ -71,8 +71,7 @@ rosbag::MessageInstance PlayableBag::GetNextMessage(
     progress->current_bagfile_name = bag_filename_;
     progress->current_bagfile_id = bag_id_;
     progress->total_messages = view_->size();
-    progress->processed_messages =
-        std::distance(view_->begin(), view_iterator_);
+    progress->processed_messages = message_counter_;
     progress->total_seconds = duration_in_seconds_;
     progress->processed_seconds = processed_seconds;
   }
@@ -100,6 +99,7 @@ void PlayableBag::AdvanceOneMessage() {
     buffered_messages_.push_back(msg);
   }
   ++view_iterator_;
+  ++message_counter_;
 }
 
 void PlayableBag::AdvanceUntilMessageAvailable() {
diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h
index afb5fdc..e051a69 100644
--- a/cartographer_ros/cartographer_ros/playable_bag.h
+++ b/cartographer_ros/cartographer_ros/playable_bag.h
@@ -59,7 +59,7 @@ class PlayableBag {
   const int bag_id_;
   const std::string bag_filename_;
   const double duration_in_seconds_;
-  int log_counter_;
+  int message_counter_;
   std::deque<rosbag::MessageInstance> buffered_messages_;
   const ::ros::Duration buffer_delay_;
   FilteringEarlyMessageHandler filtering_early_message_handler_;