diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp index 015a9c1..aa92200 100644 --- 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 mcu_freq_status_; rclcpp::Subscription::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 index a804d91..489ed16 100755 --- 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::FrequencyStatusParam(&mcu_status_rate, &mcu_status_rate, 0.1, 5)); + // subscribe to MCU status sub_mcu_status_ = this->create_subscription( @@ -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_);