Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/diagnostics #135

Merged
merged 12 commits into from
Jan 28, 2025
Prev Previous commit
Next Next commit
Monitor MCU Status message frequency
hilary-luo committed Jan 23, 2025
commit 016dea094e9e4179170ada885689888dbcd3896f
Original file line number Diff line number Diff line change
@@ -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_;
8 changes: 8 additions & 0 deletions clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp
Original file line number Diff line number Diff line change
@@ -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_);