00001 00034 #ifndef JACKAL_BASE_JACKAL_DIAGNOSTIC_UPDATER_H 00035 #define JACKAL_BASE_JACKAL_DIAGNOSTIC_UPDATER_H 00036 00037 #include <string> 00038 00039 #include "diagnostic_updater/diagnostic_updater.h" 00040 #include "diagnostic_updater/publisher.h" 00041 #include "jackal_msgs/Status.h" 00042 #include "nmea_msgs/Sentence.h" 00043 #include "ros/ros.h" 00044 #include "sensor_msgs/Imu.h" 00045 #include "std_msgs/Bool.h" 00046 00047 namespace jackal_base 00048 { 00049 00050 class JackalDiagnosticUpdater : private diagnostic_updater::Updater 00051 { 00052 public: 00053 JackalDiagnosticUpdater(); 00054 00055 void generalDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00056 void batteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00057 void voltageDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00058 void currentDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00059 void powerDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00060 00061 void statusCallback(const jackal_msgs::Status::ConstPtr& status); 00062 void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); 00063 void navsatCallback(const nmea_msgs::Sentence::ConstPtr& msg); 00064 void wirelessMonitorCallback(const ros::TimerEvent& te); 00065 00066 private: 00067 ros::NodeHandle nh_; 00068 ros::Timer timer_; 00069 ros::Subscriber status_sub_; 00070 jackal_msgs::Status::ConstPtr last_status_; 00071 00072 double expected_imu_frequency_; 00073 diagnostic_updater::TopicDiagnostic* imu_diagnostic_; 00074 ros::Subscriber imu_sub_; 00075 00076 double expected_navsat_frequency_; 00077 std::string navsat_frequency_sentence_; 00078 diagnostic_updater::TopicDiagnostic* navsat_diagnostic_; 00079 ros::Subscriber navsat_sub_; 00080 00081 std::string wireless_interface_; 00082 ros::Timer wireless_monitor_timer_; 00083 ros::Publisher wifi_connected_pub_; 00084 }; 00085 00086 } // namespace jackal_base 00087 00088 #endif // JACKAL_BASE_JACKAL_DIAGNOSTIC_UPDATER_H