add last_diagnostic_duration

Author: turniphead

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -200,6 +200,8 @@ int main(int argc, char** argv)
   msgStats pub_temp_stats;
   reset_msg_stats(pub_temp_stats);
 
+  std::chrono::duration<double> last_diagnostics_duration = std::chrono::duration<double>::zero();
+
   long debug_loops = 0;
 
   // Debug timing printout every 5 seconds
@@ -276,6 +278,11 @@ int main(int argc, char** argv)
       push_msg_stats(robot_status, "read: pub_robot", pub_robot_stats, debug_loops);
       push_msg_stats(robot_status, "read: pub_temp", pub_temp_stats, debug_loops);
 
+      diagnostic_msgs::KeyValue diagnostic_duration_kv;
+      diagnostic_duration_kv.key = "Last diagnostic duration (s)";
+      diagnostic_duration_kv.value = std::to_string(last_diagnostics_duration.count());
+      robot_status.values.push_back(diagnostic_duration_kv);
+
       dia_array.status.push_back(robot_status);
       diagnostic_pub.publish(dia_array);
 
@@ -292,14 +299,15 @@ int main(int argc, char** argv)
       reset_msg_stats(pub_temp_stats);
       debug_loops = 0;
       debug_timing_start = debug_timing_now;
+      last_diagnostics_duration = debug_timing_now - std::chrono::steady_clock::now();
     }
 
     // if (!control_rate.sleep())
-    if (period.toSec() > expected_cycle_time)
-    {
+    // if (period.toSec() > expected_cycle_time)
+    // {
       // ROS_WARN_STREAM("Could not keep cycle rate of " << expected_cycle_time * 1000 << "ms");
       // ROS_WARN_STREAM("Actual cycle time:" << period.toNSec() / 1000000.0 << "ms");
-    }
+    // }
   }
 
   spinner.stop();