Monitor MCU Status message frequency

clearpathrobotics · Jan 23, 2025 · 016dea0 · 016dea0
1 parent 92202ff
commit 016dea0
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp
@@ -61,6 +61,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
   int pcb_temperature_;
   long connection_uptime_;
   long mcu_uptime_;
+  std::shared_ptr<diagnostic_updater::FrequencyStatus> mcu_freq_status_;
   rclcpp::Subscription<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;
 
   diagnostic_updater::Updater updater_;

diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp
@@ -39,8 +39,13 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
 
   // Set Hardware ID as serial number in diagnostics
   updater_.setHardwareID(serial_number_);
+  // Publish MCU Status information as diagnostics
   updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic);
 
+  double mcu_status_rate = 1.0;
+  mcu_freq_status_ = std::make_shared<diagnostic_updater::FrequencyStatus>(
+    diagnostic_updater::FrequencyStatusParam(&mcu_status_rate, &mcu_status_rate, 0.1, 5));
+
   // subscribe to MCU status
   sub_mcu_status_ =
     this->create_subscription<clearpath_platform_msgs::msg::Status>(
@@ -72,11 +77,14 @@ void ClearpathDiagnosticUpdater::mcu_callback(const clearpath_platform_msgs::msg
   connection_uptime_ = msg.connection_uptime.sec;
   mcu_temperature_ = msg.mcu_temperature;
   pcb_temperature_ = msg.pcb_temperature;
+  mcu_freq_status_->tick();
 }
 
 void ClearpathDiagnosticUpdater::mcu_status_diagnostic(
   diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
+  mcu_freq_status_->run(stat);
+
   // add to append key-value pairs to the diagnostic
   stat.add("Firmware Version", firmware_version_);
   stat.add("Hardware ID", mcu_hardware_id_);