Fix TopicStatistics dynamic windowing to adjust evaluation frequency …

…in the right direction

clients/roscpp/include/ros/statistics.h

@@ -67,11 +67,13 @@ class ROSCPP_DECL StatisticsLogger
 
 private:
 
-  // these are hard constrains
+  // Range of window length, in seconds
   int max_window;
   int min_window;
 
-  // these are soft constrains
+  // Range of acceptable messages in window.
+  // Window size will be adjusted if number of observed is
+  // outside this range.
   int max_elements;
   int min_elements;
 

clients/roscpp/src/libros/statistics.cpp

@@ -113,7 +113,8 @@ void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_he
   }
 
   // should publish new statistics?
-  if (stats.last_publish + ros::Duration(1 / pub_frequency_) < received_time)
+  double pub_period = 1.0 / pub_frequency_;
+  if (stats.last_publish + ros::Duration(pub_period) < received_time)
   {
     ros::Time window_start = stats.last_publish;
     stats.last_publish = received_time;
@@ -236,11 +237,11 @@ void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_he
     pub_.publish(msg);
 
     // dynamic window resizing
-    if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_frequency_ * 2 <= max_window)
+    if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_period / 2.0 >= min_window)
     {
       pub_frequency_ *= 2;
     }
-    if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_frequency_ / 2 >= min_window)
+    if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_period * 2.0 <= max_window)
     {
       pub_frequency_ /= 2;
     }

clients/rospy/src/rospy/impl/statistics.py

@@ -68,15 +68,15 @@ def read_parameters(self):
         Fetch window parameters from parameter server
         """
 
-        # Range of window length, in seconds
-        self.min_elements = rospy.get_param("/statistics_window_min_elements", 10)
-        self.max_elements = rospy.get_param("/statistics_window_max_elements", 100)
-
         # Range of acceptable messages in window.
         # Window size will be adjusted if number of observed is
         # outside this range.
-        self.max_window = rospy.get_param("/statistics_window_max_size", 64)
+        self.min_elements = rospy.get_param("/statistics_window_min_elements", 10)
+        self.max_elements = rospy.get_param("/statistics_window_max_elements", 100)
+
+        # Range of window length, in seconds
         self.min_window = rospy.get_param("/statistics_window_min_size", 4)
+        self.max_window = rospy.get_param("/statistics_window_max_size", 64)
 
     def callback(self, msg, publisher, stat_bytes):
         """
@@ -208,9 +208,10 @@ def sendStatistics(self, subscriber_statistics_logger):
         self.pub.publish(msg)
 
         # adjust window, if message count is not appropriate.
-        if len(self.arrival_time_list_) > subscriber_statistics_logger.max_elements and self.pub_frequency.to_sec() * 2 <= subscriber_statistics_logger.max_window:
+        pub_period = 1.0 / self.pub_frequency.to_sec()
+        if len(self.arrival_time_list_) > subscriber_statistics_logger.max_elements and pub_period / 2 >= subscriber_statistics_logger.min_window:
             self.pub_frequency *= 2
-        if len(self.arrival_time_list_) < subscriber_statistics_logger.min_elements and self.pub_frequency.to_sec() / 2 >= subscriber_statistics_logger.min_window:
+        if len(self.arrival_time_list_) < subscriber_statistics_logger.min_elements and pub_period * 2 <= subscriber_statistics_logger.max_window:
             self.pub_frequency /= 2
 
         # clear collected stats, start new window.
@@ -257,7 +258,7 @@ def callback(self, subscriber_statistics_logger, msg, stat_bytes):
             self.last_seq_ = msg.header.seq
 
         # send out statistics with a certain frequency
-        if self.last_pub_time + self.pub_frequency < arrival_time:
+        if self.last_pub_time + rospy.Duration(1.0 / self.pub_frequency.to_sec()) < arrival_time:
             self.last_pub_time = arrival_time
             self.sendStatistics(subscriber_statistics_logger)