diagnostic.cpp
Go to the documentation of this file.
00001 #include "corobot_diagnostics/diagnostic.h"
00002 
00003 Diagnostic::Diagnostic()
00004 {
00005     errorIndex = -1;
00006 }
00007 
00008 Diagnostic::~Diagnostic()
00009 {
00010 }
00011 
00012 void Diagnostic::timerCallback(const ros::TimerEvent&)
00013 // timer callback that takes care of displaying the next message on the lcd
00014 {
00015     if (errorList.size() > 0)
00016     {
00017         if ((++errorIndex) >= errorList.size()) 
00018          {
00019           //Put the index back to the 1st message if we last displayed the last message
00020             errorIndex = 0;
00021          }
00022          
00023      //   lcd.write(errorList.at(errorIndex));
00024     }
00025    // else
00026     //    lcd.write(std::string("Everything is OK"));
00027 }
00028 
00029 void Diagnostic::newError(const std_msgs::String &message)
00030 // Add a new Error message to be displayed. This message goes at the back of a list and will be displayed for a while every couple of seconds, depending on the number of messages in the list
00031 {
00032     // Check if we already have that error message in our current list
00033     bool equal = false;
00034     for (unsigned int l = 0; l < errorList.size(); l++)
00035     {
00036         if (errorList.at(l).compare(message.data) == 0)
00037         {
00038                 equal = true;
00039             }
00040         }
00041         // If we don't, we add it
00042         if(equal == false)
00043         {
00044                 errorList.push_back(message.data);
00045         }
00046    
00047 }
00048 
00049 void Diagnostic::removeError(const std_msgs::String &message)
00050 //Remove an error message from the List. The message won't be displayed on the LCD anymore
00051 {
00052     // We remove the message from the list, if it exists
00053     for (unsigned int l = 0; l < errorList.size(); ++l)
00054     {
00055         if (errorList.at(l).compare(message.data) == 0)
00056         {
00057                 errorList.erase(errorList.begin() + l);
00058                 break;
00059             }
00060         }
00061 }
00062 
00063 int main(int argc, char* argv[])
00064 {
00065     Diagnostic diagnostic;
00066         ros::init(argc, argv, "diagnosticLCD");
00067         ros::NodeHandle n;
00068         //Create the timer that will display the messages every 5s
00069         ros::Timer timer = n.createTimer(ros::Duration(4), &Diagnostic::timerCallback, &diagnostic);
00070         
00071         // Subscribe to the topics
00072         ros::Subscriber newError= n.subscribe("new_diagnostic_error", 1000, &Diagnostic::newError, &diagnostic); 
00073         ros::Subscriber removeError= n.subscribe("remove_diagnostic_error", 1000, &Diagnostic::removeError, &diagnostic);
00074         
00075         ros::spin();
00076         
00077         return 0;
00078         
00079 }


corobot_diagnostics
Author(s):
autogenerated on Wed Aug 26 2015 11:09:34