LoggerModule.C

00001 #include "Robots/SeaBeeIII/LoggerModule.H"
00002 
00003 #include "Component/ModelParam.H"
00004 #include "Component/ModelOptionDef.H"
00005 
00006 #ifndef LOGGERMODULE_C
00007 #define LOGGERMODULE_C
00008 
00009 const ModelOptionCateg MOC_SeaBeeIIILogger = {
00010     MOC_SORTPRI_3, "SeaBeeIII Logger Related Options" };
00011 
00012 const ModelOptionDef OPT_LogPath =
00013 { MODOPT_ARG(string), "LogPath", &MOC_SeaBeeIIILogger, OPTEXP_CORE,
00014   "Path where the logger should write fwd/dwn retina images and sensor readings",
00015    "log-path", '\0', "<string>", "/home/uscr/log/test" };
00016 
00017 // ######################################################################
00018 LoggerModule::LoggerModule(int id, OptionManager& mgr,
00019                 const std::string& descrName, const std::string& tagName) :
00020   RobotBrainComponent(mgr, descrName, tagName),
00021   itsLogPath(&OPT_LogPath, this, 0),
00022   itsFwdFrameCount(0),
00023   itsLastFwdFrameCount(0),
00024   itsDwnFrameCount(0),
00025   itsLastDwnFrameCount(0),
00026   itsImgWriter(),
00027   itsLogOpen(false)
00028 {
00029 }
00030 // ######################################################################
00031 LoggerModule::~LoggerModule()
00032 {
00033   itsLogFile.close();
00034 }
00035 
00036 void LoggerModule::registerTopics()
00037 {
00038   registerSubscription("BeeStemMessageTopic");
00039   registerSubscription("RetinaMessageTopic");
00040 }
00041 
00042 void LoggerModule::evolve()
00043 {
00044   itsDataMutex.lock();
00045 
00046   if(!itsLogOpen)
00047     {
00048       itsLogOpen = true;
00049 
00050       char* lstr = new char[100];
00051       sprintf(lstr,"%s/log.txt",itsLogPath.getVal().c_str());
00052       itsLogFile.open(lstr, std::ios::out);
00053     }
00054 
00055   char* str = new char[100];
00056   sprintf(str,"%s/fwd_img/FwdImg_%06d.pnm",itsLogPath.getVal().c_str(),itsFwdFrameCount);
00057 
00058   if(itsLastFwdFrameCount != itsFwdFrameCount)
00059     {
00060       itsLastFwdFrameCount = itsFwdFrameCount;
00061       itsImgWriter.writeRGB(itsCurrentFwdImg,str);
00062     }
00063 
00064   str = new char[100];
00065   sprintf(str,"%s/dwn_img/DwnImg_%06d.pnm",itsLogPath.getVal().c_str(),itsDwnFrameCount);
00066 
00067   if(itsLastDwnFrameCount != itsDwnFrameCount)
00068     {
00069       itsLastDwnFrameCount = itsDwnFrameCount;
00070       itsImgWriter.writeRGB(itsCurrentDwnImg,str);
00071     }
00072 
00073   delete[] str;
00074 
00075   itsLogFile<<itsFwdFrameCount<<" "<<itsCurrentHeading<<" "
00076             <<itsCurrentIntPressure<<" "<<itsCurrentExtPressure<<std::endl;
00077   itsDataMutex.unlock();
00078 
00079   usleep(40);
00080 }
00081 
00082 // ######################################################################
00083 void LoggerModule::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00084                 const Ice::Current&)
00085 {
00086   if(eMsg->ice_isA("::RobotSimEvents::BeeStemMessage"))
00087     {
00088 
00089       RobotSimEvents::BeeStemMessagePtr msg = RobotSimEvents::BeeStemMessagePtr::dynamicCast(eMsg);
00090       itsDataMutex.lock();
00091 
00092       itsCurrentHeading = msg->compassHeading;
00093       itsCurrentExtPressure= msg->externalPressure;
00094       itsCurrentIntPressure = msg->internalPressure;
00095 
00096       itsDataMutex.unlock();
00097     }
00098   else if(eMsg->ice_isA("::RobotSimEvents::RetinaMessage"))
00099     {
00100       RobotSimEvents::RetinaMessagePtr msg = RobotSimEvents::RetinaMessagePtr::dynamicCast(eMsg);
00101       itsDataMutex.lock();
00102 
00103       if(msg->cameraID == "FwdCamera")
00104         {
00105           itsCurrentFwdImg = Ice2Image<PixRGB<byte> >(msg->img);
00106           itsFwdFrameCount++;
00107         }
00108       else
00109         {
00110           itsCurrentDwnImg = Ice2Image<PixRGB<byte> >(msg->img);
00111           itsDwnFrameCount++;
00112         }
00113 
00114       itsDataMutex.unlock();
00115 
00116     }
00117 }
00118 
00119 #endif
Generated on Sun May 8 08:41:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3