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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1