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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4