From 63eaf6d257c15695a5d3cfd555f27d123811ec41 Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Fri, 18 Jan 2019 13:10:40 +0100
Subject: [PATCH] Publish one last progress message when PlayableBag is
 finished. (#1160)

Without this, it might look like the processing hangs.
---
 cartographer_ros/cartographer_ros/playable_bag.cc | 11 ++++++++---
 cartographer_ros/cartographer_ros/playable_bag.h  |  2 ++
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/playable_bag.cc b/cartographer_ros/cartographer_ros/playable_bag.cc
index 350ee1b..f305ad8 100644
--- a/cartographer_ros/cartographer_ros/playable_bag.cc
+++ b/cartographer_ros/cartographer_ros/playable_bag.cc
@@ -140,10 +140,15 @@ PlayableBagMultiplexer::GetNextMessage() {
   PlayableBag& current_bag = playable_bags_.at(current_bag_index);
   cartographer_ros_msgs::BagfileProgress progress;
   rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress);
-  if (ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >=
-          ros::Duration(progress_pub_interval_) &&
-      bag_progress_pub_.getNumSubscribers() > 0) {
+  const bool publish_progress =
+      current_bag.finished() ||
+      ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >=
+          ros::Duration(progress_pub_interval_);
+  if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) {
     progress.total_bagfiles = playable_bags_.size();
+    if (current_bag.finished()) {
+      progress.processed_seconds = current_bag.duration_in_seconds();
+    }
     bag_progress_pub_.publish(progress);
     bag_progress_time_map_[current_bag.bag_id()] = ros::Time::now();
   }
diff --git a/cartographer_ros/cartographer_ros/playable_bag.h b/cartographer_ros/cartographer_ros/playable_bag.h
index e051a69..06c6086 100644
--- a/cartographer_ros/cartographer_ros/playable_bag.h
+++ b/cartographer_ros/cartographer_ros/playable_bag.h
@@ -47,6 +47,8 @@ class PlayableBag {
 
   int bag_id() const;
   std::set<std::string> topics() const { return topics_; }
+  double duration_in_seconds() const { return duration_in_seconds_; }
+  bool finished() const { return finished_; }
 
  private:
   void AdvanceOneMessage();