Fix/expected diag rates

clearpath_diagnostics/config/diagnostic_aggregator.yaml

@@ -20,10 +20,6 @@ diagnostic_aggregator:
           path: E-stop Status
           expected: [ 'clearpath_diagnostic_updater: E-stop Status' ]
           contains: [ 'E-stop' ]
-        lighting:
-          type: diagnostic_aggregator/GenericAnalyzer
-          path: Lighting
-          contains: [ 'Light' ]
         drive:
           type: diagnostic_aggregator/GenericAnalyzer
           path: Drive System
@@ -40,14 +36,6 @@ diagnostic_aggregator:
             'twist_mux',
             'joy_node'
           ]
-        odometry:
-          type: diagnostic_aggregator/GenericAnalyzer
-          path: Odometry
-          contains: [ 'odometry', 'ekf_node' ]
-        networking:
-          type: diagnostic_aggregator/GenericAnalyzer
-          path: Networking
-          contains: [ 'Wi-Fi' ]
     # sensors:
     #   type: diagnostic_aggregator/AnalyzerGroup
     #   path: Sensors

clearpath_diagnostics/include/clearpath_diagnostics/clearpath_diagnostic_updater.hpp

@@ -74,6 +74,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
   void mcu_power_diagnostic(DiagnosticStatusWrapper & stat);
   void bms_state_diagnostic(DiagnosticStatusWrapper & stat);
   void stop_status_diagnostic(DiagnosticStatusWrapper & stat);
+  void estop_diagnostic(DiagnosticStatusWrapper & stat);
 
   // Get parameters from config
   std::string get_string_param(std::string param_name, bool mandatory = false);
@@ -98,9 +99,15 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
   std::string stop_status_topic_;
   std::string estop_topic_;
   double mcu_status_rate_;
+  double mcu_status_tolerance_;
   double mcu_power_rate_;
+  double mcu_power_tolerance_;
   double bms_state_rate_;
+  double bms_state_tolerance_;
   double stop_status_rate_;
+  double stop_status_tolerance_;
+  double estop_rate_;
+  double estop_tolerance_;
 
   // Message Data
   std::string mcu_firmware_version_;
@@ -115,6 +122,7 @@ class ClearpathDiagnosticUpdater : public rclcpp::Node
   std::shared_ptr<FrequencyStatus> mcu_power_freq_status_;
   std::shared_ptr<FrequencyStatus> bms_state_freq_status_;
   std::shared_ptr<FrequencyStatus> stop_status_freq_status_;
+  std::shared_ptr<FrequencyStatus> estop_freq_status_;
 
   // Subscriptions
   rclcpp::Subscription<clearpath_platform_msgs::msg::Status>::SharedPtr sub_mcu_status_;

clearpath_diagnostics/src/clearpath_diagnostic_updater.cpp

@@ -64,12 +64,24 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
   estop_topic_ = (estop_topic_ == UNKNOWN) ? "platform/emergency_stop" : estop_topic_;
   mcu_status_rate_ = get_double_param("mcu_status_rate");
   mcu_status_rate_ = (std::isnan(mcu_status_rate_)) ? 1.0 : mcu_status_rate_;
+  mcu_status_tolerance_ = get_double_param("mcu_status_tolerance");
+  mcu_status_tolerance_ = (std::isnan(mcu_status_tolerance_)) ? 0.15 : mcu_status_tolerance_;
   mcu_power_rate_ = get_double_param("mcu_power_rate");
   mcu_power_rate_ = (std::isnan(mcu_power_rate_)) ? 10.0 : mcu_power_rate_;
+  mcu_power_tolerance_ = get_double_param("mcu_power_tolerance");
+  mcu_power_tolerance_ = (std::isnan(mcu_power_tolerance_)) ? 0.15 : mcu_power_tolerance_;
   bms_state_rate_ = get_double_param("bms_state_rate");
-  bms_state_rate_ = (std::isnan(bms_state_rate_)) ? 1.5 : bms_state_rate_;
+  bms_state_rate_ = (std::isnan(bms_state_rate_)) ? 10.0 : bms_state_rate_;
+  bms_state_tolerance_ = get_double_param("bms_state_tolerance");
+  bms_state_tolerance_ = (std::isnan(bms_state_tolerance_)) ? 0.15 : bms_state_tolerance_;
   stop_status_rate_ = get_double_param("stop_status_rate");
   stop_status_rate_ = (std::isnan(stop_status_rate_)) ? 1.0 : stop_status_rate_;
+  stop_status_tolerance_ = get_double_param("stop_status_tolerance");
+  stop_status_tolerance_ = (std::isnan(stop_status_tolerance_)) ? 0.15 : stop_status_tolerance_;
+  estop_rate_ = get_double_param("estop_rate");
+  estop_rate_ = (std::isnan(estop_rate_)) ? 1.0 : estop_rate_;
+  estop_tolerance_ = get_double_param("estop_tolerance");
+  estop_tolerance_ = (std::isnan(estop_tolerance_)) ? 0.15 : estop_tolerance_;
 
   // Initialize variables that are populated in callbacks
   mcu_firmware_version_ = UNKNOWN;
@@ -90,7 +102,7 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
 
     // Create MCU Frequency Status tracking objects
     mcu_status_freq_status_ = std::make_shared<FrequencyStatus>(
-      FrequencyStatusParam(&mcu_status_rate_, &mcu_status_rate_, 0.1, 10));
+      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);
@@ -123,17 +135,23 @@ ClearpathDiagnosticUpdater::ClearpathDiagnosticUpdater()
 
   // Create Frequency Status tracking objects
   mcu_power_freq_status_ = std::make_shared<FrequencyStatus>(
-    FrequencyStatusParam(&mcu_power_rate_, &mcu_power_rate_, 0.1, 5));
+    FrequencyStatusParam(&mcu_power_rate_, &mcu_power_rate_, mcu_power_tolerance_, 10));
   bms_state_freq_status_ = std::make_shared<FrequencyStatus>(
-    FrequencyStatusParam(&bms_state_rate_, &bms_state_rate_, 0.35, 10));
+    FrequencyStatusParam(&bms_state_rate_, &bms_state_rate_, bms_state_tolerance_, 10));
   stop_status_freq_status_ = std::make_shared<FrequencyStatus>(
-    FrequencyStatusParam(&stop_status_rate_, &stop_status_rate_, 0.1, 10));
+    FrequencyStatusParam(&stop_status_rate_, &stop_status_rate_, stop_status_tolerance_, 10));
+  estop_freq_status_ = std::make_shared<FrequencyStatus>(
+    FrequencyStatusParam(&estop_rate_, &estop_rate_, estop_tolerance_, 10));
 
   // Add diagnostic tasks
   updater_.add("Power Status", this, &ClearpathDiagnosticUpdater::mcu_power_diagnostic);
   updater_.add("Battery Management System", this,
     &ClearpathDiagnosticUpdater::bms_state_diagnostic);
-  updater_.add("E-stop Status", this, &ClearpathDiagnosticUpdater::stop_status_diagnostic);
+  if (stop_status_rate_ == 0.0) {
+    updater_.add("E-stop Status", this, &ClearpathDiagnosticUpdater::estop_diagnostic);
+  } else {
+    updater_.add("E-stop Status", this, &ClearpathDiagnosticUpdater::stop_status_diagnostic);
+  }
 
   setup_topic_rate_diagnostics();
 }
@@ -284,12 +302,16 @@ void ClearpathDiagnosticUpdater::mcu_power_diagnostic(DiagnosticStatusWrapper &
     }
 
     try {
-      for (unsigned i = 0; i < mcu_power_msg_.measured_voltages.size(); i++) {
+      unsigned int count_v = std::min(mcu_power_msg_.measured_voltages.size(),
+                             DiagnosticLabels::MEASURED_VOLTAGES.at(platform_model_).size());
+      for (unsigned i = 0; i < count_v; i++) {
         std::string name = "Measured Voltage: " +
           DiagnosticLabels::MEASURED_VOLTAGES.at(platform_model_)[i] + " (V)";
         stat.add(name, mcu_power_msg_.measured_voltages[i]);
       }
-      for (unsigned i = 0; i < mcu_power_msg_.measured_currents.size(); i++) {
+      unsigned int count_c = std::min(mcu_power_msg_.measured_currents.size(),
+                             DiagnosticLabels::MEASURED_CURRENTS.at(platform_model_).size());
+      for (unsigned i = 0; i < count_c; i++) {
         std::string name = "Measured Current: " +
           DiagnosticLabels::MEASURED_CURRENTS.at(platform_model_)[i] + " (A)";
         stat.add(name, mcu_power_msg_.measured_currents[i]);
@@ -408,6 +430,24 @@ void ClearpathDiagnosticUpdater::estop_callback(
   const std_msgs::msg::Bool & msg)
 {
   estop_msg_ = msg;
+  estop_freq_status_->tick();
+}
+
+/**
+ * @brief Report E-stop message information to diagnostics - only used if no MCU
+ */
+void ClearpathDiagnosticUpdater::estop_diagnostic(DiagnosticStatusWrapper & stat)
+{
+  estop_freq_status_->run(stat);
+
+  if (stat.level != diagnostic_updater::DiagnosticStatusWrapper::ERROR) {
+    // if status messages are being received then add the message details
+    stat.add("E-stop Triggered", (estop_msg_.data ? "True" : "False"));
+
+    if (estop_msg_.data) {
+      stat.mergeSummary(DiagnosticStatus::WARN, "E-stopped");
+    }
+  }
 }
 
 /**

clearpath_generator_common/clearpath_generator_common/param/platform.py

@@ -36,6 +36,7 @@
 from clearpath_config.clearpath_config import ClearpathConfig
 from clearpath_config.common.types.platform import Platform
 from clearpath_config.common.utils.dictionary import merge_dict, replace_dict_items
+from clearpath_config.platform.battery import BatteryConfig
 from clearpath_config.sensors.types.cameras import BaseCamera, IntelRealsense
 from clearpath_config.sensors.types.gps import BaseGPS
 from clearpath_config.sensors.types.imu import BaseIMU, PhidgetsSpatial
@@ -243,6 +244,25 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                     }
                 })
 
+            # Add Lighting for every platform except A200 and J100
+            if self.clearpath_config.get_platform_model() not in (Platform.A200, Platform.J100):
+                self.param_file.update({
+                    self.DIAGNOSTIC_AGGREGATOR_NODE: {
+                        'platform': {
+                            'analyzers': {
+                                'lighting': {
+                                    'type': 'diagnostic_aggregator/GenericAnalyzer',
+                                    'path': 'Lighting',
+                                    'expected': [
+                                        'lighting_node: Light Status'
+                                    ],
+                                    'contains': ['Light']
+                                }
+                            }
+                        }
+                    }
+                })
+
             # Add cooling for A300 only for now
             if self.clearpath_config.get_platform_model() == Platform.A300:
                 self.param_file.update({
@@ -265,6 +285,9 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                         'platform': {
                             'analyzers': {
                                 'odometry': {
+                                    'type': 'diagnostic_aggregator/GenericAnalyzer',
+                                    'path': 'Odometry',
+                                    'contains': ['odometry', 'ekf_node'],
                                     'expected': [
                                         'ekf_node: Filter diagnostic updater',
                                         'ekf_node: odometry/filtered topic status',
@@ -275,6 +298,22 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                     }
                 })
 
+            if self.clearpath_config.platform.enable_wireless_watcher:
+                self.param_file.update({
+                    self.DIAGNOSTIC_AGGREGATOR_NODE: {
+                        'platform': {
+                            'analyzers': {
+                                'networking': {
+                                    'type': 'diagnostic_aggregator/GenericAnalyzer',
+                                    'path': 'Networking',
+                                    'contains': ['Wi-Fi'],
+                                    'expected': ['wireless_watcher: Wi-Fi Monitor']
+                                }
+                            }
+                        }
+                    }
+                })
+
             sensor_analyzers = {}
 
             # List all topics to be monitored from each launched sensor
@@ -373,14 +412,51 @@ def generate_parameters(self, use_sim_time: bool = False) -> None:
                     print(f'\033[93mWarning: ros-{ROS_DISTRO}-clearpath-firmware'
                           ' package not found\033[0m')
 
+            # Set expected BMS rate based on the platform battery model
+            bms_state_rate = 10.0
+            bms_state_tolerance = 0.15
+            if (self.clearpath_config.platform.battery.model in [BatteryConfig.S_24V20_U1]):
+                bms_state_rate = 1.5
+                bms_state_tolerance = 0.35
+            elif (self.clearpath_config.platform.battery.model in
+                    [BatteryConfig.VALENCE_U24_12XP, BatteryConfig.VALENCE_U27_12XP]):
+                bms_state_rate = 3.0
+                bms_state_tolerance = 0.25
+            elif (self.clearpath_config.platform.battery.model in [BatteryConfig.NEC_ALM12V35]):
+                bms_state_rate = 1.0
+                bms_state_tolerance = 0.25
+            elif (platform_model == Platform.A200):
+                bms_state_rate = 1.8
+                bms_state_tolerance = 0.25
+
             self.param_file.update({
                 self.DIAGNOSTIC_UPDATER_NODE: {
                     'ros_distro': ROS_DISTRO,
                     'latest_apt_firmware_version': latest_apt_firmware_version,
-                    'installed_apt_firmware_version': installed_apt_firmware_version
+                    'installed_apt_firmware_version': installed_apt_firmware_version,
+                    'bms_state_rate': bms_state_rate,
+                    'bms_state_tolerance': bms_state_tolerance
                 }
             })
 
+            # Additional considerations for A200 platform
+            if platform_model == Platform.A200:
+                self.param_file.update({
+                    self.DIAGNOSTIC_UPDATER_NODE: {
+                        'stop_status_rate': 0.0,  # Disable stop status diagnostic for A200
+                        'mcu_power_rate': 1.8,
+                        'mcu_power_tolerance': 0.25,
+                        'estop_rate': 1.8,
+                        'estop_tolerance': 0.25
+                    }
+                })
+            elif platform_model == Platform.W200:
+                self.param_file.update({
+                    self.DIAGNOSTIC_UPDATER_NODE: {
+                        'stop_status_rate': 0.0,  # Disable stop status diagnostic for W200
+                    }
+                })
+
             # List all topics to be monitored from each launched sensor
             for sensor in self.clearpath_config.sensors.get_all_sensors():