00001 00024 #ifndef PUMA_MOTOR_DRIVER_DIAGNOSTIC_UPDATER_H 00025 #define PUMA_MOTOR_DRIVER_DIAGNOSTIC_UPDATER_H 00026 00027 #include <string> 00028 00029 #include "ros/ros.h" 00030 #include "std_msgs/Bool.h" 00031 #include "diagnostic_updater/diagnostic_updater.h" 00032 #include "diagnostic_updater/publisher.h" 00033 #include "puma_motor_msgs/MultiStatus.h" 00034 #include "puma_motor_msgs/Status.h" 00035 00036 namespace puma_motor_driver 00037 { 00038 00039 class PumaMotorDriverDiagnosticUpdater : private diagnostic_updater::Updater 00040 { 00041 public: 00042 PumaMotorDriverDiagnosticUpdater(); 00043 00044 void driverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat, int driver); 00045 00046 void statusCallback(const puma_motor_msgs::MultiStatus::ConstPtr& status_msg); 00047 00048 private: 00049 ros::NodeHandle nh_; 00050 ros::Subscriber status_sub_; 00051 puma_motor_msgs::MultiStatus::ConstPtr last_status_; 00052 bool initialized_; 00053 00054 static const char* getFaultString(uint8_t fault); 00055 static const char* getModeString(uint8_t mode); 00056 }; 00057 00058 } // namespace puma_motor_driver 00059 00060 #endif // PUMA_MOTOR_DRIVER_DIAGNOSTIC_UPDATER_H