refactor period stats

Author: turniphead

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -175,10 +175,8 @@ int main(int argc, char** argv)
 
   double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
 
-  ros::Duration min_period = ros::Duration(0);
-  ros::Duration max_period = ros::Duration(0);
-  ros::Duration period_totals = ros::Duration(0);
-
+  msgStats period_stats;
+  reset_msg_stats(period_stats);
   msgStats read_stats;
   reset_msg_stats(read_stats);
   msgStats cm_update_stats;
@@ -229,7 +227,9 @@ int main(int argc, char** argv)
     // Get current time and elapsed time since last read
     timestamp = ros::Time::now();
     stopwatch_now = std::chrono::steady_clock::now();
-    period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
+    const std::chrono::duration<double> period_chrono = stopwatch_now - stopwatch_last;
+    update_msg_stats(period_chrono, period_stats);
+    period.fromSec(period_chrono.count());
     stopwatch_last = stopwatch_now;
 
     const std::chrono::steady_clock::time_point cm_update_start = std::chrono::steady_clock::now();
@@ -242,9 +242,6 @@ int main(int argc, char** argv)
     const std::chrono::duration<double> write_elapsed = std::chrono::steady_clock::now() - write_start;
     update_msg_stats(write_elapsed, write_stats);
 
-    if (min_period.isZero() || period < min_period) min_period = period;
-    if (max_period.isZero() || period > max_period) max_period = period;
-    period_totals += period;
     ++debug_loops;
 
     // Check if it's time to print
@@ -266,19 +263,8 @@ int main(int argc, char** argv)
       num_loops.key = "Number of loops";
       num_loops.value = std::to_string(debug_loops);
       robot_status.values.push_back(num_loops);
-      diagnostic_msgs::KeyValue min_period_kv;
-      min_period_kv.key = "min loop period (s)";
-      min_period_kv.value = std::to_string(min_period.toSec());
-      robot_status.values.push_back(min_period_kv);
-      diagnostic_msgs::KeyValue max_period_kv;
-      max_period_kv.key = "max loop period (s)";
-      max_period_kv.value = std::to_string(max_period.toSec());
-      robot_status.values.push_back(max_period_kv);
-      diagnostic_msgs::KeyValue avg_period_kv;
-      avg_period_kv.key = "avg loop period (s)";
-      avg_period_kv.value = std::to_string(period_totals.toSec() / debug_loops);
-      robot_status.values.push_back(avg_period_kv);
 
+      push_msg_stats(robot_status, "period loop", period_stats, debug_loops);
       push_msg_stats(robot_status, "read", read_stats, debug_loops);
       push_msg_stats(robot_status, "cm_update", cm_update_stats, debug_loops);
       push_msg_stats(robot_status, "write", write_stats, debug_loops);
@@ -293,9 +279,7 @@ int main(int argc, char** argv)
       dia_array.status.push_back(robot_status);
       diagnostic_pub.publish(dia_array);
 
-      min_period = ros::Duration(0);
-      max_period = ros::Duration(0);
-      period_totals = ros::Duration(0);
+      reset_msg_stats(period_stats);
       reset_msg_stats(read_stats);
       reset_msg_stats(cm_update_stats);
       reset_msg_stats(write_stats);