Added MCU alerts to diagnostics (#248)

* Added MCU alerts to diagnostics

* Linting

Author: roni-kreinin

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 <map>
+#include <string>
 #include <vector>
 
 #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<std::string, std::vector<std::string>> 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
   //--------------------------------------------------------

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<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_mcu_alerts_;
   rclcpp::Subscription<clearpath_platform_msgs::msg::Power>::SharedPtr sub_mcu_power_;
   rclcpp::Subscription<BatteryState>::SharedPtr sub_bms_state_;
   rclcpp::Subscription<clearpath_platform_msgs::msg::StopStatus>::SharedPtr sub_stop_status_;

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<std_msgs::msg::String>(
+        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<FrequencyStatus>(
       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<std::string> Vector of split string
+ */
+static std::vector<std::string> split(std::string s, std::string delimiter)
+{
+  size_t pos_start = 0, pos_end, delim_len = delimiter.length();
+  std::string token;
+  std::vector<std::string> 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<std::string> 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
  */