Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

RawCamBehavior.cc

Go to the documentation of this file.
00001 #include "RawCamBehavior.h"
00002 #include "Wireless/Wireless.h"
00003 #include "Events/EventRouter.h"
00004 #include "Vision/RawCameraGenerator.h"
00005 #include "Vision/JPEGGenerator.h"
00006 #include "Events/FilterBankEvent.h"
00007 #include "Behaviors/Controller.h"
00008 #include "Shared/ProjectInterface.h"
00009 
00010 RawCamBehavior* RawCamBehavior::theOne=NULL;
00011 
00012 RawCamBehavior::RawCamBehavior()
00013   : CameraStreamBehavior("RawCamBehavior",visRaw), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00014 {
00015   ASSERT(theOne==NULL,"there was already a RawCamBehavior running!");
00016   theOne=this;
00017 }
00018 
00019 void
00020 RawCamBehavior::DoStart() {
00021   BehaviorBase::DoStart();
00022   setupServer();
00023   erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::deactivateETID);
00024   erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID,EventBase::deactivateETID);
00025   erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visGrayscaleJPEGSID,EventBase::deactivateETID);
00026 }
00027 
00028 void
00029 RawCamBehavior::DoStop() {
00030   erouter->removeListener(this);
00031   closeServer();
00032   BehaviorBase::DoStop();
00033 }
00034 
00035 void
00036 RawCamBehavior::processEvent(const EventBase& e) {
00037   if(!wireless->isConnected(visRaw->sock))
00038     return;
00039   if(config->vision.rawcam.transport==0 && visRaw->getTransport()==Socket::SOCK_STREAM
00040      || config->vision.rawcam.transport==1 && visRaw->getTransport()==Socket::SOCK_DGRAM) {
00041     //reset the socket
00042     closeServer();
00043     setupServer();
00044     return;
00045   }
00046   try {
00047     const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00048     if(fbke==NULL) {
00049       CameraStreamBehavior::processEvent(e);
00050       return;
00051     }
00052     if ((get_time() - lastProcessedTime) < config->vision.rawcam.interval) {// not enough time has gone by
00053       return;
00054     }
00055     /* // turning these off enables individual channel compression
00056       if(config->vision.rawcam.compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()!=EventBase::visRawCameraEGID)
00057       return;
00058       if(config->vision.rawcam.compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()!=EventBase::visJPEGEGID)
00059       return; */
00060     if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) {
00061       if(!writeColor(*fbke)) {
00062         if(packet) { //packet was opened, need to close it
00063           cur=packet; // don't send anything
00064           closePacket();
00065         }
00066         //error message should already be printed in writeColor
00067         //ASSERTRET(false,"serialization failed");
00068       }
00069     } else if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) {
00070       if(!writeSingleChannel(*fbke)) {
00071         if(packet) { //packet was opened, need to close it
00072           cur=packet; // don't send anything
00073           closePacket();
00074         }
00075         //error message should already be printed in writeSingleChannel
00076         //ASSERTRET(false,"serialization failed");
00077       }
00078     } else {
00079       serr->printf("%s: Bad rawcam.encoding setting\n",getName().c_str());
00080     }
00081   } catch(...) {
00082     if(packet) { //packet was opened, need to close it
00083       cur=packet; // don't send anything
00084       closePacket();
00085     }
00086     // typically this is a per-frame recurring error, so let's just stop now
00087     serr->printf("%s: exception generated during image serialization, stopping stream.\n",getName().c_str());
00088     DoStop();
00089     throw;
00090   }
00091 }
00092 
00093 unsigned int RawCamBehavior::getSourceLayer(unsigned int chan, unsigned int numLayers) {
00094   if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) {
00095     if(config->vision.rawcam.channel!=(int)chan)
00096       return -1U;
00097     return numLayers-1-config->vision.rawcam.y_skip;
00098   }
00099   // must be full-color
00100   switch(chan) {
00101   case RawCameraGenerator::CHAN_Y:
00102     if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00103       if(config->vision.rawcam.y_skip-config->vision.rawcam.uv_skip == 1)
00104         return numLayers-1-config->vision.rawcam.uv_skip;
00105     }
00106     return numLayers-1-config->vision.rawcam.y_skip;
00107   case RawCameraGenerator::CHAN_U:
00108   case RawCameraGenerator::CHAN_V:
00109     if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00110       if(config->vision.rawcam.uv_skip-config->vision.rawcam.y_skip == 1)
00111         return numLayers-1-config->vision.rawcam.y_skip;
00112     }
00113     return numLayers-1-config->vision.rawcam.uv_skip;
00114   default: // other channels, i.e. Y-derivatives
00115     return -1U;
00116   }
00117 }
00118 unsigned int RawCamBehavior::getSourceYLayer(unsigned int numLayers) {
00119   return getSourceLayer(RawCameraGenerator::CHAN_Y,numLayers);
00120 }
00121 unsigned int RawCamBehavior::getSourceULayer(unsigned int numLayers) {
00122   return getSourceLayer(RawCameraGenerator::CHAN_U,numLayers);
00123 }
00124 unsigned int RawCamBehavior::getSourceVLayer(unsigned int numLayers) {
00125   return getSourceLayer(RawCameraGenerator::CHAN_V,numLayers);
00126 }
00127 
00128 void
00129 RawCamBehavior::closeServer() {
00130   if(wireless->isConnected(visRaw->sock))
00131     sendCloseConnectionPacket();
00132   Controller::closeGUI("RawVisionGUI");
00133   
00134   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00135   // it will try to listen again even though we explicitly closed the server socket...
00136   wireless->setDaemon(visRaw,false);
00137   wireless->close(visRaw->sock);
00138 }
00139 
00140 void
00141 RawCamBehavior::setupServer() {
00142   std::vector<std::string> args;
00143   args.push_back("raw");
00144   char port[50];
00145   snprintf(port,50,"%d",*config->vision.rawcam.port);
00146   args.push_back(port);
00147   if(config->vision.rawcam.transport==0) {
00148     max_buf=UDP_WIRELESS_BUFFER_SIZE;
00149     visRaw=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf);
00150     args.push_back("udp");
00151   } else if(config->vision.rawcam.transport==1) {
00152     max_buf=TCP_WIRELESS_BUFFER_SIZE;
00153     visRaw=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf);
00154     args.push_back("tcp");
00155   } else {
00156     serr->printf("ERROR: Invalid Config::vision.rawcam.transport: %d\n",*config->vision.rawcam.transport);
00157     return;
00158   }
00159   wireless->setDaemon(visRaw,true);
00160   wireless->setReceiver(visRaw,networkCallback);
00161   wireless->listen(visRaw,config->vision.rawcam.port);
00162   
00163   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",*config->vision.rawcam.port,args);
00164 }
00165 
00166 bool
00167 RawCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00168   if(packet!=NULL)
00169     return false;
00170 
00171   avail=max_buf-1; //not sure why -1, but Alok had it, so i will too
00172   ASSERT(cur==NULL,"cur non-NULL");
00173   cur=NULL;
00174   char * buf=packet=(char*)visRaw->getWriteBuffer(avail);
00175   ASSERT(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)");
00176   if(packet==NULL)
00177     return false;
00178   
00179   if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00180   if(!LoadSave::encodeInc(*config->vision.rawcam.encoding,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00181   if(!LoadSave::encodeInc(*config->vision.rawcam.compression,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00182 
00183   if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00184   if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00185   if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00186   if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00187 
00188   cur=buf;
00189   return true;
00190 }
00191 
00192 bool
00193 RawCamBehavior::writeColor(const FilterBankEvent& e) {
00194   FilterBankGenerator& fbkgen=*e.getSource();
00195 
00196   unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip;
00197   unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.uv_skip;
00198 
00199   if(config->vision.rawcam.channel==-1) {
00200     if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE
00201        || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00202       if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00203         if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR)
00204           return true;
00205       openPacket(fbkgen,e.getTimeStamp(),uv_layer);
00206       if(cur==NULL) //error should have been displayed by openPacket
00207         return false;
00208       
00209       if(!LoadSave::encodeInc("FbkImage",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00210       if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00211       if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00212       if(!LoadSave::encodeInc(-1,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00213       if(!LoadSave::encodeInc(RawCameraGenerator::CHAN_Y,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00214       if(!LoadSave::encodeInc("blank",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00215 
00216       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00217       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00218       
00219       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00220       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00221 
00222       closePacket();
00223     }
00224     return true;
00225   }
00226 
00227   unsigned int big_layer=y_layer;
00228   unsigned int small_layer=uv_layer;
00229   if(y_layer<uv_layer) { 
00230     big_layer=uv_layer;
00231     small_layer=y_layer;
00232   }
00233   if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen)) {
00234     if(config->vision.rawcam.compression!=Config::vision_config::RawCamConfig::COMPRESS_JPEG)
00235       return true;
00236     if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) {
00237       openPacket(fbkgen,e.getTimeStamp(),big_layer);
00238       if(cur==NULL) //error should have been displayed by openPacket
00239         return false;
00240       
00241       fbkgen.selectSaveImage(big_layer,0);
00242       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00243       
00244       closePacket();
00245     } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) {
00246       bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00247       if(cur==NULL) //error should have been displayed by openPacket
00248         return false;
00249       
00250       if(big_layer==y_layer) {
00251         fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00252         if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00253       } else {
00254         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00255         if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00256         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00257         if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00258       }
00259       
00260       if(!opened)
00261         closePacket();
00262     }
00263   } else {
00264     bool opened=false;
00265     
00266     if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) {
00267       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00268       if(cur==NULL) //error should have been displayed by openPacket
00269         return false;
00270       fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00271       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00272     }
00273     
00274     if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) {
00275       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00276       if(cur==NULL) //error should have been displayed by openPacket
00277         return false;
00278       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00279       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00280       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00281       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00282     }
00283     
00284     if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || !opened)
00285       closePacket();
00286   }
00287   
00288   return true;
00289 }
00290 
00291 bool
00292 RawCamBehavior::writeSingleChannel(const FilterBankEvent& e) {
00293   FilterBankGenerator& fbkgen=*e.getSource();
00294   if( config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID
00295       || config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID )
00296     {
00297       if(const JPEGGenerator * jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00298         if(jgen->getCurrentSourceFormat()!=JPEGGenerator::SRC_GRAYSCALE)
00299           return true;
00300       unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip;
00301       
00302       openPacket(fbkgen,e.getTimeStamp(),layer);
00303       if(cur==NULL) //error should have been displayed by openPacket
00304         return false;
00305       
00306       fbkgen.selectSaveImage(layer,config->vision.rawcam.channel);
00307       if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00308       
00309       closePacket();
00310     }
00311   return true;
00312 }
00313 
00314 //#include "Shared/WorldState.h"
00315 
00316 void
00317 RawCamBehavior::closePacket() {
00318   if(packet==NULL)
00319     return;
00320   visRaw->write(cur-packet);
00321   /*  cout << "used=" << (cur-packet) << "\tavail==" << avail;
00322   if(RobotName == ERS7Info::TargetName)
00323     cout << "\tmax bandwidth=" << (cur-packet)/1024.f*30 << "KB/sec" << endl;
00324   else
00325     cout << "\tmax bandwidth=" << (cur-packet)/1024.f*24 << "KB/sec" << endl;
00326   */
00327   packet=cur=NULL;
00328   avail=0;
00329   lastProcessedTime = get_time();
00330 }
00331 
00332 bool
00333 RawCamBehavior::sendCloseConnectionPacket() {
00334   char msg[]="CloseConnection";
00335   unsigned int len=strlen(msg)+LoadSave::stringpad;
00336   char * buf = (char*)visRaw->getWriteBuffer(len);
00337   if(buf==NULL) {
00338     std::cerr << "Could not get buffer for closing packet" << std::endl;
00339     return false;
00340   }
00341   unsigned int used=LoadSave::encode(msg,buf,len);
00342   if(used==0)
00343     std::cerr << "Could not write close packet" << std::endl;
00344   visRaw->write(used);
00345   return true;
00346 }
00347 
00348 
00349 /*! @file
00350  * @brief Implements RawCamBehavior, which forwards images from camera over wireless
00351  * @author ejt (Creator)
00352  *
00353  * $Author: ejt $
00354  * $Name: tekkotsu-4_0 $
00355  * $Revision: 1.37 $
00356  * $State: Exp $
00357  * $Date: 2007/06/26 04:27:44 $
00358  */
00359 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4