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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:15 2005 by Doxygen 1.4.0