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()
00011   : BehaviorBase("RawCamBehavior"), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00012 {}
00013 
00014 void
00015 RawCamBehavior::DoStart() {
00016   BehaviorBase::DoStart();
00017   std::vector<std::string> args;
00018   args.push_back("raw");
00019   char port[50];
00020   snprintf(port,50,"%d",config->vision.rawcam_port);
00021   args.push_back(port);
00022   if(config->vision.rawcam_transport==0) {
00023     max_buf=UDP_WIRELESS_BUFFER_SIZE;
00024     visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf);
00025     args.push_back("udp");
00026   } else if(config->vision.rawcam_transport==1) {
00027     max_buf=TCP_WIRELESS_BUFFER_SIZE;
00028     visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf);
00029     wireless->setDaemon(visRaw,true);
00030     args.push_back("tcp");
00031   } else {
00032     serr->printf("ERROR: Invalid Config::vision.rawcam_transport: %d\n",config->vision.rawcam_transport);
00033     return;
00034   }
00035   wireless->listen(visRaw,config->vision.rawcam_port);
00036   
00037   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args);
00038 
00039   erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::deactivateETID);
00040   erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID,EventBase::deactivateETID);
00041   erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visGrayscaleJPEGSID,EventBase::deactivateETID);
00042 }
00043 
00044 void
00045 RawCamBehavior::DoStop() {
00046   erouter->removeListener(this);
00047   if(wireless->isConnected(visRaw->sock))
00048     sendCloseConnectionPacket();
00049   Controller::closeGUI("RawVisionGUI");
00050 
00051   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00052   // it will try to listen again even though we explicitly closed the server socket...
00053   wireless->setDaemon(visRaw,false);
00054   wireless->close(visRaw->sock);
00055   BehaviorBase::DoStop();
00056 }
00057 
00058 void
00059 RawCamBehavior::processEvent(const EventBase& e) {
00060   if(!wireless->isConnected(visRaw->sock))
00061     return;
00062   const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00063   ASSERTRET(fbke!=NULL,"unexpected event");
00064   if ((get_time() - lastProcessedTime) < config->vision.rawcam_interval) {// not enough time has gone by
00065     return;
00066   }
00067   /* // turning these off enables individual channel compression
00068     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()!=EventBase::visRawCameraEGID)
00069     return;
00070     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()!=EventBase::visJPEGEGID)
00071     return; */
00072   if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) {
00073     bool succ=writeColor(*fbke);
00074     ASSERTRET(succ,"serialization failed");
00075   } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00076     bool succ=writeSingleChannel(*fbke);
00077     ASSERTRET(succ,"serialization failed");
00078   } else {
00079     serr->printf("%s: Bad rawcam_encoding setting\n",getName().c_str());
00080   }
00081 }
00082 
00083 unsigned int RawCamBehavior::getSourceLayer(unsigned int chan, unsigned int numLayers) {
00084   if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00085     if(config->vision.rawcam_channel!=(int)chan)
00086       return -1U;
00087     return numLayers-1-config->vision.rawcam_y_skip;
00088   }
00089   // must be full-color
00090   switch(chan) {
00091   case RawCameraGenerator::CHAN_Y:
00092     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00093       if(config->vision.rawcam_y_skip-config->vision.rawcam_uv_skip == 1)
00094         return numLayers-1-config->vision.rawcam_uv_skip;
00095     }
00096     return numLayers-1-config->vision.rawcam_y_skip;
00097   case RawCameraGenerator::CHAN_U:
00098   case RawCameraGenerator::CHAN_V:
00099     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00100       if(config->vision.rawcam_uv_skip-config->vision.rawcam_y_skip == 1)
00101         return numLayers-1-config->vision.rawcam_y_skip;
00102     }
00103     return numLayers-1-config->vision.rawcam_uv_skip;
00104   default: // other channels, i.e. Y-derivatives
00105     return -1U;
00106   }
00107 }
00108 unsigned int RawCamBehavior::getSourceYLayer(unsigned int numLayers) {
00109   return getSourceLayer(RawCameraGenerator::CHAN_Y,numLayers);
00110 }
00111 unsigned int RawCamBehavior::getSourceULayer(unsigned int numLayers) {
00112   return getSourceLayer(RawCameraGenerator::CHAN_U,numLayers);
00113 }
00114 unsigned int RawCamBehavior::getSourceVLayer(unsigned int numLayers) {
00115   return getSourceLayer(RawCameraGenerator::CHAN_V,numLayers);
00116 }
00117 
00118 bool
00119 RawCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00120   if(packet!=NULL)
00121     return false;
00122 
00123   avail=max_buf-1; //not sure why -1, but Alok had it, so i will too
00124   ASSERT(cur==NULL,"cur non-NULL");
00125   cur=NULL;
00126   char * buf=packet=(char*)visRaw->getWriteBuffer(avail);
00127   ASSERTRETVAL(packet!=NULL,"could not get buffer",false);
00128   
00129   unsigned int used;
00130   used=LoadSave::encode("TekkotsuImage",buf,avail);
00131   ASSERTRETVAL(used!=0,"ran out of space",false);
00132   avail-=used; buf+=used;
00133   used=LoadSave::encode(config->vision.rawcam_encoding,buf,avail);
00134   ASSERTRETVAL(used!=0,"ran out of space",false);
00135   avail-=used; buf+=used;
00136   used=LoadSave::encode(config->vision.rawcam_compression,buf,avail);
00137   ASSERTRETVAL(used!=0,"ran out of space",false);
00138   avail-=used; buf+=used;
00139 
00140   used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail);
00141   ASSERTRETVAL(used!=0,"ran out of space",false);
00142   avail-=used; buf+=used;
00143   used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail);
00144   ASSERTRETVAL(used!=0,"ran out of space",false);
00145   avail-=used; buf+=used;
00146   used=LoadSave::encode(time,buf,avail);
00147   ASSERTRETVAL(used!=0,"ran out of space",false);
00148   avail-=used; buf+=used;
00149   used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail);
00150   ASSERTRETVAL(used!=0,"ran out of space",false);
00151   avail-=used; buf+=used;
00152 
00153   cur=buf;
00154   return true;
00155 }
00156 
00157 bool
00158 RawCamBehavior::writeColor(const FilterBankEvent& e) {
00159   FilterBankGenerator& fbkgen=*e.getSource();
00160 
00161   unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip;
00162   unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_uv_skip;
00163 
00164   if(config->vision.rawcam_channel==-1) {
00165     if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE
00166        || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00167       if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00168         if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR)
00169           return true;
00170       unsigned int used=0;
00171       openPacket(fbkgen,e.getTimeStamp(),uv_layer);
00172       ASSERTRETVAL(cur!=NULL,"header failed",false);
00173       
00174       used=LoadSave::encode("FbkImage",cur,avail);
00175       ASSERTRETVAL(used!=0,"save blank image failed",false);
00176       avail-=used; cur+=used;
00177       used=LoadSave::encode(0,cur,avail);
00178       ASSERTRETVAL(used!=0,"save blank image failed",false);
00179       avail-=used; cur+=used;
00180       used=LoadSave::encode(0,cur,avail);
00181       ASSERTRETVAL(used!=0,"save blank image failed",false);
00182       avail-=used; cur+=used;
00183       used=LoadSave::encode(-1,cur,avail);
00184       ASSERTRETVAL(used!=0,"save blank image failed",false);
00185       avail-=used; cur+=used;
00186       used=LoadSave::encode(RawCameraGenerator::CHAN_Y,cur,avail);
00187       ASSERTRETVAL(used!=0,"save blank image failed",false);
00188       avail-=used; cur+=used;
00189       used=LoadSave::encode("blank",cur,avail);
00190       ASSERTRETVAL(used!=0,"save blank image failed",false);
00191       avail-=used; cur+=used;
00192 
00193       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00194       used=fbkgen.SaveBuffer(cur,avail);
00195       ASSERTRETVAL(used!=0,"save image failed",false);
00196       avail-=used; cur+=used;
00197       
00198       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00199       used=fbkgen.SaveBuffer(cur,avail);
00200       ASSERTRETVAL(used!=0,"save image failed",false);
00201       avail-=used; cur+=used;
00202 
00203       closePacket();
00204     }
00205     return true;
00206   }
00207 
00208   unsigned int big_layer=y_layer;
00209   unsigned int small_layer=uv_layer;
00210   if(y_layer<uv_layer) { 
00211     big_layer=uv_layer;
00212     small_layer=y_layer;
00213   }
00214   if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen)) {
00215     if(config->vision.rawcam_compression!=Config::vision_config::COMPRESS_JPEG)
00216       return true;
00217     if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) {
00218       unsigned int used=0;
00219       openPacket(fbkgen,e.getTimeStamp(),big_layer);
00220       ASSERTRETVAL(cur!=NULL,"header failed",false);
00221       
00222       fbkgen.selectSaveImage(big_layer,0);
00223       used=fbkgen.SaveBuffer(cur,avail);
00224       ASSERTRETVAL(used!=0,"save image failed",false);
00225       avail-=used; cur+=used;
00226       
00227       closePacket();
00228     } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) {
00229       unsigned int used=0;
00230       bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00231       ASSERTRETVAL(cur!=NULL,"header failed",false);
00232       
00233       if(big_layer==y_layer) {
00234         fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00235         used=fbkgen.SaveBuffer(cur,avail);
00236         ASSERTRETVAL(used!=0,"save image failed",false);
00237         avail-=used; cur+=used;
00238       } else {
00239         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00240         used=fbkgen.SaveBuffer(cur,avail);
00241         ASSERTRETVAL(used!=0,"save image failed",false);
00242         avail-=used; cur+=used;
00243       
00244         fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00245         used=fbkgen.SaveBuffer(cur,avail);
00246         ASSERTRETVAL(used!=0,"save image failed",false);
00247         avail-=used; cur+=used;
00248       }
00249       
00250       if(!opened)
00251         closePacket();
00252     }
00253   } else {
00254     unsigned int used=0;
00255     bool opened=false;
00256     
00257     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) {
00258       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00259       ASSERTRETVAL(cur!=NULL,"header failed",false);
00260       
00261       fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00262       used=fbkgen.SaveBuffer(cur,avail);
00263       ASSERTRETVAL(used!=0,"save image failed",false);
00264       avail-=used; cur+=used;
00265     }
00266     
00267     if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) {
00268       opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00269       ASSERTRETVAL(cur!=NULL,"header failed",false);
00270       
00271       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00272       used=fbkgen.SaveBuffer(cur,avail);
00273       ASSERTRETVAL(used!=0,"save image failed",false);
00274       avail-=used; cur+=used;
00275       
00276       fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00277       used=fbkgen.SaveBuffer(cur,avail);
00278       ASSERTRETVAL(used!=0,"save image failed",false);
00279       avail-=used; cur+=used;
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       unsigned int used=0;
00301       openPacket(fbkgen,e.getTimeStamp(),layer);
00302       ASSERTRETVAL(cur!=NULL,"header failed",false);
00303       
00304       fbkgen.selectSaveImage(layer,config->vision.rawcam_channel);
00305       used=fbkgen.SaveBuffer(cur,avail);
00306       ASSERTRETVAL(used!=0,"save image failed",false);
00307       avail-=used; cur+=used;
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(state->robotDesign & WorldState::ERS7Mask)
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   ASSERTRETVAL(buf!=NULL,"Could not get buffer for closing packet",false);
00338   unsigned int used=LoadSave::encode(msg,buf,len);
00339   ASSERTRETVAL(used!=0,"Could not write close packet",false);
00340   visRaw->write(used);
00341   return true;
00342 }
00343 
00344 
00345 /*! @file
00346  * @brief Implements RawCamBehavior, which forwards images from camera over wireless
00347  * @author ejt (Creator)
00348  *
00349  * $Author: ejt $
00350  * $Name: tekkotsu-2_4_1 $
00351  * $Revision: 1.21 $
00352  * $State: Exp $
00353  * $Date: 2005/07/29 18:35:05 $
00354  */
00355 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4