More robust fps estimation.

StefanFabian · Jul 30, 2021 · d9e04ab · d9e04ab
1 parent 7da46ce
commit d9e04ab
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/include/qml_ros_plugin/helpers/rolling_average.h b/include/qml_ros_plugin/helpers/rolling_average.h
@@ -22,10 +22,10 @@ class RollingAverage
   void add( const T &value )
   {
     if (count_values_ == COUNT) sum_ -= values_[index_];
+    else ++count_values_;
     values_[index_] = value;
     sum_ += value;
     if ( ++index_ == COUNT ) index_ = 0;
-    if (count_values_ != COUNT) ++count_values_;
   }
 
   T value() const { return count_values_ == 0 ? 0 : sum_ / count_values_; }

diff --git a/src/image_transport_manager.cpp b/src/image_transport_manager.cpp
@@ -297,13 +297,13 @@ Q_OBJECT
     network_latency_average_.add( network_latency );
     int processing_latency = static_cast<int>((ros::Time::now() - received).toSec() * 1000);
     processing_latency_average_.add( processing_latency );
-    if ( base_interval > 0 ) framerate_average_.add( 1000.0 / base_interval );
+    image_interval_average_.add(base_interval);
     for ( const auto &sub : subscribers )
     {
       if ( sub == nullptr || !sub->callback ) continue;
       sub->network_latency = network_latency_average_;
       sub->processing_latency = processing_latency_average_;
-      sub->framerate_ = std::round(framerate_average_ * 10) / 10;
+      sub->framerate_ = std::round(1000.0 / image_interval_average_ * 10) / 10;
       sub->callback( frame );
     }
   }
@@ -338,7 +338,7 @@ Q_OBJECT
   std::vector<ImageTransportSubscriptionHandle *> subscriptions_;
   std::vector<std::weak_ptr<ImageTransportSubscriptionHandle>> subscription_handles_;
   QList<QVideoFrame::PixelFormat> supported_formats_;
-  RollingAverage<double, 10> framerate_average_;
+  RollingAverage<int, 10> image_interval_average_;
   RollingAverage<int, 10> network_latency_average_;
   RollingAverage<int, 10> processing_latency_average_;
   ros::Time last_received_stamp_;