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

Tekkotsu v2.1
Generated Tue Mar 16 23:19:15 2004 by Doxygen 1.3.5