Tekkotsu: RawCamBehavior.cc Source 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()==SocketNS::SOCK_STREAM
00040      || config->vision.rawcam_transport==1 && visRaw->getTransport()==SocketNS::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::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::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::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::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::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(SocketNS::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(SocketNS::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   ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false);
00176   
00177   if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00178   if(!LoadSave::encodeInc(config->vision.rawcam_encoding,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00179   if(!LoadSave::encodeInc(config->vision.rawcam_compression,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00180 
00181   if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00182   if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00183   if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00184   if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00185 
00186   cur=buf;
00187   return true;
00188 }
00189 
00190 bool
00191 RawCamBehavior::writeColor(const FilterBankEvent& e) {
00192   FilterBankGenerator& fbkgen=*e.getSource();
00193 
00194   unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip;
00195   unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_uv_skip;
00196 
00197   if(config->vision.rawcam_channel==-1) {
00198     if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE
00199        || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00200       if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00201         if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR)
00202           return true;
00203       openPacket(fbkgen,e.getTimeStamp(),uv_layer);
00204       if(cur==NULL) //error should have been displayed by openPacket
00205         return false;
00206       
00207       if(!LoadSave::encodeInc("FbkImage",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00208       if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00209       if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00210       if(!LoadSave::encodeInc(-1,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00211       if(!LoadSave::encodeInc(RawCameraGenerator::CHAN_Y,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00212       if(!LoadSave::encodeInc("blank",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00213 
00214       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00215       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;
00216       
00217       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00218       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;
00219 
00220       closePacket();
00221     }
00222     return true;
00223   }
00224 
00225   unsigned int big_layer=y_layer;
00226   unsigned int small_layer=uv_layer;
00227   if(y_layer<uv_layer) { 
00228     big_layer=uv_layer;
00229     small_layer=y_layer;
00230   }
00231   if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen)) {
00232     if(config->vision.rawcam_compression!=Config::vision_config::COMPRESS_JPEG)
00233       return true;
00234     if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) {
00235       openPacket(fbkgen,e.getTimeStamp(),big_layer);
00236       if(cur==NULL) //error should have been displayed by openPacket
00237         return false;
00238       
00239       fbkgen.selectSaveImage(big_layer,0);
00240       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;
00241       
00242       closePacket();
00243     } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) {
00244       bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00245       if(cur==NULL) //error should have been displayed by openPacket
00246         return false;
00247       
00248       if(big_layer==y_layer) {
00249         fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00250         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;
00251       } else {
00252         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00253         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;
00254         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
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       }
00257       
00258       if(!opened)
00259         closePacket();
00260     }
00261   } else {
00262     bool opened=false;
00263     
00264     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) {
00265       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00266       if(cur==NULL) //error should have been displayed by openPacket
00267         return false;
00268       fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00269       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;
00270     }
00271     
00272     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) {
00273       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00274       if(cur==NULL) //error should have been displayed by openPacket
00275         return false;
00276       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00277       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;
00278       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
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     }
00281     
00282     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || !opened)
00283       closePacket();
00284   }
00285   
00286   return true;
00287 }
00288 
00289 bool
00290 RawCamBehavior::writeSingleChannel(const FilterBankEvent& e) {
00291   FilterBankGenerator& fbkgen=*e.getSource();
00292   if( config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID
00293       || config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID )
00294     {
00295       if(const JPEGGenerator * jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00296         if(jgen->getCurrentSourceFormat()!=JPEGGenerator::SRC_GRAYSCALE)
00297           return true;
00298       unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip;
00299       
00300       openPacket(fbkgen,e.getTimeStamp(),layer);
00301       if(cur==NULL) //error should have been displayed by openPacket
00302         return false;
00303       
00304       fbkgen.selectSaveImage(layer,config->vision.rawcam_channel);
00305       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;
00306       
00307       closePacket();
00308     }
00309   return true;
00310 }
00311 
00312 //#include "Shared/WorldState.h"
00313 
00314 void
00315 RawCamBehavior::closePacket() {
00316   if(packet==NULL)
00317     return;
00318   visRaw->write(cur-packet);
00319   /*  cout << "used=" << (cur-packet) << "\tavail==" << avail;
00320   if(state->robotDesign & WorldState::ERS7Mask)
00321     cout << "\tmax bandwidth=" << (cur-packet)/1024.f*30 << "KB/sec" << endl;
00322   else
00323     cout << "\tmax bandwidth=" << (cur-packet)/1024.f*24 << "KB/sec" << endl;
00324   */
00325   packet=cur=NULL;
00326   avail=0;
00327   lastProcessedTime = get_time();
00328 }
00329 
00330 bool
00331 RawCamBehavior::sendCloseConnectionPacket() {
00332   char msg[]="CloseConnection";
00333   unsigned int len=strlen(msg)+LoadSave::stringpad;
00334   char * buf = (char*)visRaw->getWriteBuffer(len);
00335   if(buf==NULL) {
00336     std::cerr << "Could not get buffer for closing packet" << std::endl;
00337     return false;
00338   }
00339   unsigned int used=LoadSave::encode(msg,buf,len);
00340   if(used==0)
00341     std::cerr << "Could not write close packet" << std::endl;
00342   visRaw->write(used);
00343   return true;
00344 }
00345 
00346 
00347 /*! @file
00348  * @brief Implements RawCamBehavior, which forwards images from camera over wireless
00349  * @author ejt (Creator)
00350  *
00351  * $Author: ejt $
00352  * $Name: tekkotsu-3_0 $
00353  * $Revision: 1.33 $
00354  * $State: Exp $
00355  * $Date: 2006/09/14 18:57:03 $
00356  */
00357