factor better

Author: turniphead

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -47,6 +47,27 @@ void signalHandler(int signum)
   exit(signum);
 }
 
+void update_msg_stats(diagnostic_msgs::DiagnosticStatus &stat, 
+                      std::string stat_name, 
+                      std::chrono::duration<double> min_stat, 
+                      std::chrono::duration<double> max_stat, 
+                      std::chrono::duration<double> total_stat,
+                      long num_loops)
+{
+  diagnostic_msgs::KeyValue min_stat_kv;
+  min_stat_kv.key = "min " + stat_name + " (s)";
+  min_stat_kv.value = std::to_string(min_stat.count());
+  stat.values.push_back(min_stat_kv);
+  diagnostic_msgs::KeyValue max_stat_kv;
+  max_stat_kv.key = "max " + stat_name + " (s)";
+  max_stat_kv.value = std::to_string(max_stat.count());
+  stat.values.push_back(max_stat_kv);
+  diagnostic_msgs::KeyValue avg_stat_kv;
+  avg_stat_kv.key = "avg " + stat_name + " (s)";
+  avg_stat_kv.value = std::to_string(total_stat.count() / num_loops);
+  stat.values.push_back(avg_stat_kv);
+}
+
 int main(int argc, char** argv)
 {
   // Set up ROS.
@@ -225,44 +246,50 @@ int main(int argc, char** argv)
       avg_period_kv.value = std::to_string(period_totals.toSec() / debug_loops);
       robot_status.values.push_back(avg_period_kv);
 
-      diagnostic_msgs::KeyValue min_read_kv;
-      min_read_kv.key = "min read (s)";
-      min_read_kv.value = std::to_string(min_read.count());
-      robot_status.values.push_back(min_read_kv);
-      diagnostic_msgs::KeyValue max_read_kv;
-      max_read_kv.key = "max read (s)";
-      max_read_kv.value = std::to_string(max_read.count());
-      robot_status.values.push_back(max_read_kv);
-      diagnostic_msgs::KeyValue avg_read_kv;
-      avg_read_kv.key = "avg read (s)";
-      avg_read_kv.value = std::to_string(read_totals.count() / debug_loops);
-      robot_status.values.push_back(avg_read_kv);
-
-      diagnostic_msgs::KeyValue min_cm_update_kv;
-      min_cm_update_kv.key = "min cm_update (s)";
-      min_cm_update_kv.value = std::to_string(min_cm_update.count());
-      robot_status.values.push_back(min_cm_update_kv);
-      diagnostic_msgs::KeyValue max_cm_update_kv;
-      max_cm_update_kv.key = "max cm_update (s)";
-      max_cm_update_kv.value = std::to_string(max_cm_update.count());
-      robot_status.values.push_back(max_cm_update_kv);
-      diagnostic_msgs::KeyValue avg_cm_update_kv;
-      avg_cm_update_kv.key = "avg cm_update (s)";
-      avg_cm_update_kv.value = std::to_string(cm_update_totals.count() / debug_loops);
-      robot_status.values.push_back(avg_cm_update_kv);
-
-      diagnostic_msgs::KeyValue min_write_kv;
-      min_write_kv.key = "min write (s)";
-      min_write_kv.value = std::to_string(min_write.count());
-      robot_status.values.push_back(min_write_kv);
-      diagnostic_msgs::KeyValue max_write_kv;
-      max_write_kv.key = "max write (s)";
-      max_write_kv.value = std::to_string(max_write.count());
-      robot_status.values.push_back(max_write_kv);
-      diagnostic_msgs::KeyValue avg_write_kv;
-      avg_write_kv.key = "avg write (s)";
-      avg_write_kv.value = std::to_string(write_totals.count() / debug_loops);
-      robot_status.values.push_back(avg_write_kv);
+
+
+      update_msg_stats(robot_status, "read", min_read, max_read, read_totals, debug_loops);
+      update_msg_stats(robot_status, "cm_update", min_cm_update, max_cm_update, cm_update_totals, debug_loops);
+      update_msg_stats(robot_status, "write", min_write, max_write, write_totals, debug_loops);
+
+      // diagnostic_msgs::KeyValue min_read_kv;
+      // min_read_kv.key = "min read (s)";
+      // min_read_kv.value = std::to_string(min_read.count());
+      // robot_status.values.push_back(min_read_kv);
+      // diagnostic_msgs::KeyValue max_read_kv;
+      // max_read_kv.key = "max read (s)";
+      // max_read_kv.value = std::to_string(max_read.count());
+      // robot_status.values.push_back(max_read_kv);
+      // diagnostic_msgs::KeyValue avg_read_kv;
+      // avg_read_kv.key = "avg read (s)";
+      // avg_read_kv.value = std::to_string(read_totals.count() / debug_loops);
+      // robot_status.values.push_back(avg_read_kv);
+
+      // diagnostic_msgs::KeyValue min_cm_update_kv;
+      // min_cm_update_kv.key = "min cm_update (s)";
+      // min_cm_update_kv.value = std::to_string(min_cm_update.count());
+      // robot_status.values.push_back(min_cm_update_kv);
+      // diagnostic_msgs::KeyValue max_cm_update_kv;
+      // max_cm_update_kv.key = "max cm_update (s)";
+      // max_cm_update_kv.value = std::to_string(max_cm_update.count());
+      // robot_status.values.push_back(max_cm_update_kv);
+      // diagnostic_msgs::KeyValue avg_cm_update_kv;
+      // avg_cm_update_kv.key = "avg cm_update (s)";
+      // avg_cm_update_kv.value = std::to_string(cm_update_totals.count() / debug_loops);
+      // robot_status.values.push_back(avg_cm_update_kv);
+
+      // diagnostic_msgs::KeyValue min_write_kv;
+      // min_write_kv.key = "min write (s)";
+      // min_write_kv.value = std::to_string(min_write.count());
+      // robot_status.values.push_back(min_write_kv);
+      // diagnostic_msgs::KeyValue max_write_kv;
+      // max_write_kv.key = "max write (s)";
+      // max_write_kv.value = std::to_string(max_write.count());
+      // robot_status.values.push_back(max_write_kv);
+      // diagnostic_msgs::KeyValue avg_write_kv;
+      // avg_write_kv.key = "avg write (s)";
+      // avg_write_kv.value = std::to_string(write_totals.count() / debug_loops);
+      // robot_status.values.push_back(avg_write_kv);
 
       dia_array.status.push_back(robot_status);
       diagnostic_pub.publish(dia_array);