lots of diagnostics

Author: turniphead

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -27,6 +27,10 @@
 #include <ros/ros.h>
 #include <controller_manager/controller_manager.h>
 
+#include <diagnostic_msgs/DiagnosticArray.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
+#include <diagnostic_msgs/KeyValue.h>
+
 #include <csignal>
 #include <ur_robot_driver/ros/hardware_interface.h>
 
@@ -53,6 +57,8 @@ int main(int argc, char** argv)
   ros::NodeHandle nh;
   ros::NodeHandle nh_priv("~");
 
+  ros::Publisher diagnostic_pub = nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
+
   // register signal SIGINT and signal handler
   signal(SIGINT, signalHandler);
 
@@ -129,21 +135,161 @@ 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);
+
+  std::chrono::duration<double> min_read = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> max_read = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> read_totals = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> min_cm_update = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> max_cm_update = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> cm_update_totals = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> min_write = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> max_write = std::chrono::duration<double>::zero();
+  std::chrono::duration<double> write_totals = std::chrono::duration<double>::zero();
+  long debug_loops = 0;
+
+  // Debug timing printout every 5 seconds
+  const std::chrono::seconds debug_timing_period{5};
+  std::chrono::steady_clock::time_point debug_timing_start = std::chrono::steady_clock::now();
+
   // Run as fast as possible
   while (ros::ok())
   {
     // Receive current state from robot
+    const std::chrono::steady_clock::time_point read_start = std::chrono::steady_clock::now();
     g_hw_interface->read(timestamp, period);
+    const std::chrono::duration<double> read_elapsed = std::chrono::steady_clock::now() - read_start;
+    if (min_read == std::chrono::duration<double>::zero() || read_elapsed < min_read) min_read = read_elapsed;
+    if (max_read == std::chrono::duration<double>::zero() || read_elapsed > max_read) max_read = read_elapsed;
+    read_totals += read_elapsed;
 
     // 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());
     stopwatch_last = stopwatch_now;
 
+    const std::chrono::steady_clock::time_point cm_update_start = std::chrono::steady_clock::now();
     cm.update(timestamp, period, g_hw_interface->shouldResetControllers());
+    const std::chrono::duration<double> cm_update_elapsed = std::chrono::steady_clock::now() - cm_update_start;
+    if (min_cm_update == std::chrono::duration<double>::zero() || cm_update_elapsed < min_cm_update) min_cm_update = cm_update_elapsed;
+    if (max_cm_update == std::chrono::duration<double>::zero() || cm_update_elapsed > max_cm_update) max_cm_update = cm_update_elapsed;
+    cm_update_totals += cm_update_elapsed;
 
+    const std::chrono::steady_clock::time_point write_start = std::chrono::steady_clock::now();
     g_hw_interface->write(timestamp, period);
+    const std::chrono::duration<double> write_elapsed = std::chrono::steady_clock::now() - write_start;
+    if (min_write == std::chrono::duration<double>::zero() || write_elapsed < min_write) min_write = write_elapsed;
+    if (max_write == std::chrono::duration<double>::zero() || write_elapsed > max_write) max_write = write_elapsed;
+    write_totals += write_elapsed;
+
+
+
+
+    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
+    const std::chrono::steady_clock::time_point debug_timing_now = std::chrono::steady_clock::now();
+    const std::chrono::duration<double> total_elapsed = debug_timing_now - debug_timing_start;
+    if (total_elapsed > debug_timing_period) {
+      diagnostic_msgs::DiagnosticArray dia_array;
+      diagnostic_msgs::DiagnosticStatus robot_status;
+      robot_status.name = "Driver Timing";
+      robot_status.level = diagnostic_msgs::DiagnosticStatus::OK;
+      robot_status.message = "Everything seems to be ok.";
+
+      diagnostic_msgs::KeyValue loop_durations_last;
+      loop_durations_last.key = "Loop durations last (s)";
+      loop_durations_last.value = std::to_string(total_elapsed.count());
+      robot_status.values.push_back(loop_durations_last);
+      diagnostic_msgs::KeyValue num_loops;
+      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);
+
+      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);
+
+
+      // ROS_INFO_STREAM(
+      //     "Loop durations last " << total_elapsed.count() << "s: "
+      //     "min=" << min_period.toSec() * 1000.0 << "ms, "
+      //     "max=" << max_period.toSec() * 1000.0 << "ms, "
+      //     "avg=" << period_totals.toSec() * 1000.0 / debug_loops << "ms, "
+      //     "avgchrono=" << total_elapsed.count() * 1000.0 / debug_loops << "ms"
+      // );
+      min_period = ros::Duration(0);
+      max_period = ros::Duration(0);
+      period_totals = ros::Duration(0);
+      std::chrono::duration<double> min_read = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> max_read = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> read_totals = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> min_cm_update = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> max_cm_update = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> cm_update_totals = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> min_write = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> max_write = std::chrono::duration<double>::zero();
+      std::chrono::duration<double> write_totals = std::chrono::duration<double>::zero();
+      debug_loops = 0;
+      debug_timing_start = debug_timing_now;
+    }
+
     // if (!control_rate.sleep())
     if (period.toSec() > expected_cycle_time)
     {