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