00001 00032 #ifndef HUSKY_BASE_HUSKY_DIAGNOSTICS_H 00033 #define HUSKY_BASE_HUSKY_DIAGNOSTICS_H 00034 00035 #include "ros/ros.h" 00036 #include "diagnostic_updater/diagnostic_updater.h" 00037 #include "husky_base/horizon_legacy_wrapper.h" 00038 #include "husky_msgs/HuskyStatus.h" 00039 00040 namespace husky_base 00041 { 00042 00043 class HuskySoftwareDiagnosticTask : 00044 public diagnostic_updater::DiagnosticTask 00045 { 00046 public: 00047 explicit HuskySoftwareDiagnosticTask(husky_msgs::HuskyStatus &msg, double target_control_freq); 00048 00049 void run(diagnostic_updater::DiagnosticStatusWrapper &stat); 00050 00051 void updateControlFrequency(double frequency); 00052 00053 private: 00054 void reset(); 00055 00056 double control_freq_, target_control_freq_; 00057 husky_msgs::HuskyStatus &msg_; 00058 }; 00059 00060 template<typename T> 00061 class HuskyHardwareDiagnosticTask : 00062 public diagnostic_updater::DiagnosticTask 00063 { 00064 public: 00065 explicit HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); 00066 00067 void run(diagnostic_updater::DiagnosticStatusWrapper &stat) 00068 { 00069 typename horizon_legacy::Channel<T>::Ptr latest = horizon_legacy::Channel<T>::requestData(1.0); 00070 if (latest) 00071 { 00072 update(stat, latest); 00073 } 00074 } 00075 00076 void update(diagnostic_updater::DiagnosticStatusWrapper &stat, typename horizon_legacy::Channel<T>::Ptr &status); 00077 00078 private: 00079 husky_msgs::HuskyStatus &msg_; 00080 }; 00081 00082 template<> 00083 HuskyHardwareDiagnosticTask<clearpath::DataSystemStatus>::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); 00084 00085 template<> 00086 HuskyHardwareDiagnosticTask<clearpath::DataPowerSystem>::HuskyHardwareDiagnosticTask(husky_msgs::HuskyStatus &msg); 00087 00088 template<> 00089 HuskyHardwareDiagnosticTask<clearpath::DataSafetySystemStatus>::HuskyHardwareDiagnosticTask( 00090 husky_msgs::HuskyStatus &msg); 00091 00092 template<> 00093 void HuskyHardwareDiagnosticTask<clearpath::DataSystemStatus>::update( 00094 diagnostic_updater::DiagnosticStatusWrapper &stat, 00095 horizon_legacy::Channel<clearpath::DataSystemStatus>::Ptr &status); 00096 00097 template<> 00098 void HuskyHardwareDiagnosticTask<clearpath::DataPowerSystem>::update( 00099 diagnostic_updater::DiagnosticStatusWrapper &stat, 00100 horizon_legacy::Channel<clearpath::DataPowerSystem>::Ptr &status); 00101 00102 template<> 00103 void HuskyHardwareDiagnosticTask<clearpath::DataSafetySystemStatus>::update( 00104 diagnostic_updater::DiagnosticStatusWrapper &stat, 00105 horizon_legacy::Channel<clearpath::DataSafetySystemStatus>::Ptr &status); 00106 00107 } // namespace husky_base 00108 #endif // HUSKY_BASE_HUSKY_DIAGNOSTICS_H