diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp index ecd16849..186badb3 100644 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_labels.hpp @@ -27,6 +27,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #define CLEARPATH_DIAGNOSTIC_LABELS_HPP #include +#include #include #include "sensor_msgs/msg/battery_state.hpp" @@ -64,6 +65,59 @@ class DiagnosticLabels { // Genric Robot inline static const std::string GENERIC = "generic"; + //-------------------------------------------------------- + // Labels for Firmware Alert Codes + // Format is {alert_code, {"Alert Title", "Troubleshooting/Description"}} + //-------------------------------------------------------- + inline static const std::map> FIRMWARE_ALERTS = { + {"E110", {"Main Contactor Error!", ""}}, + {"E113", {"Battery Out of Range!", ""}}, + {"E116", {"E-Stop Contactor Error!", ""}}, + {"E117", {"Brake Contactor Error!", ""}}, + {"E120", {"Motor 1 Voltage Range Error!", ""}}, + {"E121", {"Motor 2 Voltage Range Error!", ""}}, + {"E122", {"Motor 3 Voltage Range Error!", ""}}, + {"E123", {"Motor 4 Voltage Range Error!", ""}}, + {"E124", + {"Motor Voltage Range Error!", "This is a general error for a voltage issue on any motor."}}, + {"E140", {"User Power Contactor Error!", ""}}, + {"E150", {"24V Aux Power Fail!", ""}}, + {"E151", {"12V Aux Power Fail!", ""}}, + {"E152", {"12V1 Sys Power Fail!", ""}}, + {"E153", {"12V2 Sys Power Fail!", ""}}, + {"E154", {"12V A User Power Fail!", ""}}, + {"E155", {"12V B User Power Fail!", ""}}, + {"E156", {"VBAT User Power Fail!", ""}}, + {"E157", {"24V User Power Fail!", ""}}, + {"E160", + {"Power Supply Failure!", + "This is a general error which is set for any of the power fail errors."}}, + {"E170", {"VBAT User Power Fuse Tripped!", ""}}, + {"E171", {"24V User Power Fuse Tripped!", ""}}, + {"E172", {"12V A User Power Fuse Tripped!", ""}}, + {"E173", {"12V B User Power Fuse Tripped!", ""}}, + {"E174", {"Expansion Power Fuse Tripped!", ""}}, + {"E810", {"Fan 1 Below Minimum Speed!", ""}}, + {"E811", {"Fan 2 Below Minimum Speed!", ""}}, + {"E812", {"Fan 3 Below Minimum Speed!", ""}}, + {"E813", {"Fan 4 Below Minimum Speed!", ""}}, + {"E814", {"Fan 5 Below Minimum Speed!", ""}}, + {"E815", {"Fan 6 Below Minimum Speed!", ""}}, + {"E816", {"Fan 7 Below Minimum Speed!", ""}}, + {"E817", {"Fan 8 Below Minimum Speed!", ""}}, + {"E820", {"Fan Below Minimum Speed!", "This is a general error for any fan."}}, + {"E830", {"Fan 1 Above Maximum Speed!", ""}}, + {"E831", {"Fan 2 Above Maximum Speed!", ""}}, + {"E832", {"Fan 3 Above Maximum Speed!", ""}}, + {"E833", {"Fan 4 Above Maximum Speed!", ""}}, + {"E834", {"Fan 5 Above Maximum Speed!", ""}}, + {"E835", {"Fan 6 Above Maximum Speed!", ""}}, + {"E836", {"Fan 7 Above Maximum Speed!", ""}}, + {"E837", {"Fan 8 Above Maximum Speed!", ""}}, + {"E840", {"Fan Above Maximum Speed!", "This is a general error for any fan."}}, + }; + + //-------------------------------------------------------- // Labels for clearpath_platform_msgs::msg::Power //-------------------------------------------------------- diff --git a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp index dd825a63..ea33f7c2 100644 --- a/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp +++ b/clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp @@ -40,6 +40,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/string.hpp" #include "clearpath_diagnostics/clearpath_diagnostic_labels.hpp" #include "clearpath_platform_msgs/msg/power.hpp" @@ -63,6 +64,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Callbacks void mcu_status_callback(const clearpath_platform_msgs::msg::Status & msg); + void mcu_alerts_callback(const std_msgs::msg::String & msg); void mcu_power_callback(const clearpath_platform_msgs::msg::Power & msg); void bms_state_callback(const BatteryState & msg); void stop_status_callback(const clearpath_platform_msgs::msg::StopStatus & msg); @@ -71,6 +73,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Diagnostic Tasks void firmware_diagnostic(DiagnosticStatusWrapper & stat); void mcu_status_diagnostic(DiagnosticStatusWrapper & stat); + void firmware_alerts_diagnostic(DiagnosticStatusWrapper & stat); void mcu_power_diagnostic(DiagnosticStatusWrapper & stat); void bms_state_diagnostic(DiagnosticStatusWrapper & stat); void stop_status_diagnostic(DiagnosticStatusWrapper & stat); @@ -94,6 +97,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Topic names and rates std::string mcu_status_topic_; + std::string mcu_alerts_topic_; std::string mcu_power_topic_; std::string bms_state_topic_; std::string stop_status_topic_; @@ -112,6 +116,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Message Data std::string mcu_firmware_version_; clearpath_platform_msgs::msg::Status mcu_status_msg_; + std_msgs::msg::String mcu_alerts_msg_; clearpath_platform_msgs::msg::Power mcu_power_msg_; BatteryState bms_state_msg_; clearpath_platform_msgs::msg::StopStatus stop_status_msg_; @@ -126,6 +131,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node // Subscriptions rclcpp::Subscription::SharedPtr sub_mcu_status_; + rclcpp::Subscription::SharedPtr sub_mcu_alerts_; rclcpp::Subscription::SharedPtr sub_mcu_power_; rclcpp::Subscription::SharedPtr sub_bms_state_; rclcpp::Subscription::SharedPtr sub_stop_status_; diff --git a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp index d4ba5510..6f888cba 100755 --- a/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp +++ b/clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp @@ -53,6 +53,9 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater() // Get optional parameters from the config mcu_status_topic_ = get_string_param("mcu_status_topic"); mcu_status_topic_ = (mcu_status_topic_ == UNKNOWN) ? "platform/mcu/status" : mcu_status_topic_; + mcu_alerts_topic_ = get_string_param("mcu_alerts_topic"); + mcu_alerts_topic_ = (mcu_alerts_topic_ == + UNKNOWN) ? "platform/mcu/status/alerts" : mcu_alerts_topic_; mcu_power_topic_ = get_string_param("mcu_power_topic"); mcu_power_topic_ = (mcu_power_topic_ == UNKNOWN) ? "platform/mcu/status/power" : mcu_power_topic_; bms_state_topic_ = get_string_param("bms_state_topic"); @@ -100,12 +103,20 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater() rclcpp::SensorDataQoS(), std::bind(&ClearpathDiagnosticUpdater::mcu_status_callback, this, std::placeholders::_1)); + sub_mcu_alerts_ = + this->create_subscription( + mcu_alerts_topic_, + rclcpp::SensorDataQoS(), + std::bind(&ClearpathDiagnosticUpdater::mcu_alerts_callback, this, std::placeholders::_1)); + // Create MCU Frequency Status tracking objects mcu_status_freq_status_ = std::make_shared( FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, mcu_status_tolerance_, 10)); // Add diagnostic tasks for MCU data updater_.add("MCU Status", this, &ClearpathDiagnosticUpdater::mcu_status_diagnostic); + updater_.add("MCU Firmware Alerts", this, + &ClearpathDiagnosticUpdater::firmware_alerts_diagnostic); updater_.add("MCU Firmware Version", this, &ClearpathDiagnosticUpdater::firmware_diagnostic); RCLCPP_INFO(this->get_logger(), "MCU diagnostics started."); } @@ -228,6 +239,71 @@ void ClearpathDiagnosticUpdater::firmware_diagnostic(DiagnosticStatusWrapper & s stat.add("Firmware Version on MCU", mcu_firmware_version_); } +/** + * @brief Helper function to split a string into a vector of strings by delimiter + * + * @param s String to split + * @param delimiter Delimiter to split with + * @return std::vector Vector of split string + */ +static std::vector split(std::string s, std::string delimiter) +{ + size_t pos_start = 0, pos_end, delim_len = delimiter.length(); + std::string token; + std::vector res; + + while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) { + token = s.substr(pos_start, pos_end - pos_start); + pos_start = pos_end + delim_len; + res.push_back(token); + } + + res.push_back(s.substr(pos_start)); + return res; +} + +/** + * @brief Report Firmware Alerts to diagnostics + */ +void ClearpathDiagnosticUpdater::firmware_alerts_diagnostic(DiagnosticStatusWrapper & stat) +{ + if (mcu_alerts_msg_.data.empty() || mcu_alerts_msg_.data == "None") { + stat.summary(DiagnosticStatus::OK, "No firmware alerts reported"); + } else { + unsigned char diagnostic_status = DiagnosticStatus::OK; + std::vector alerts = split(mcu_alerts_msg_.data, ","); + + for (const auto & a : alerts) { + std::string alert_title = "Firmware Alert " + a; + std::string alert_message; + unsigned char alert_status = DiagnosticStatus::OK; + try { + alert_message = DiagnosticLabels::FIRMWARE_ALERTS.at(a)[0]; + // Add the troubleshooting message if it exists + if (DiagnosticLabels::FIRMWARE_ALERTS.at(a)[1].size() > 1) { + alert_message += ": " + DiagnosticLabels::FIRMWARE_ALERTS.at(a)[1]; + } + } catch (const std::out_of_range &) { + alert_message = "Unknown firmware alert code"; + } + stat.add(alert_title, alert_message); + + // Warning for W alerts, Error for E alerts + if (a.find('W') != std::string::npos) { + alert_status = DiagnosticStatus::WARN; + } else if (a.find('E') != std::string::npos) { + alert_status = DiagnosticStatus::ERROR; + } + + if (alert_status > diagnostic_status) { + diagnostic_status = alert_status; + } + } + + stat.summary(diagnostic_status, "Firmware alerts reported"); + } +} + /** * @brief Save data from MCU Status messages */ @@ -239,6 +315,15 @@ void ClearpathDiagnosticUpdater::mcu_status_callback( mcu_status_freq_status_->tick(); } +/** + * @brief Save data from MCU Alerts messages + */ +void ClearpathDiagnosticUpdater::mcu_alerts_callback( + const std_msgs::msg::String & msg) +{ + mcu_alerts_msg_ = msg; +} + /** * @brief Report MCU Status message information to diagnostics */