Homepage Demos Overview Downloads Tutorials Reference
Credits

SegCamBehavior.cc

Go to the documentation of this file.
00001 #include "SegCamBehavior.h"
00002 #include "Wireless/Wireless.h"
00003 #include "Events/EventRouter.h"
00004 #include "Events/FilterBankEvent.h"
00005 #include "Behaviors/Controller.h"
00006 #include "Shared/ProjectInterface.h"
00007 #include "Vision/SegmentedColorGenerator.h"
00008 #include "Vision/RLEGenerator.h"
00009 
00010 SegCamBehavior::SegCamBehavior()
00011   : BehaviorBase(), visRLE(NULL), packet(NULL), cur(NULL), avail(0)
00012 {
00013 }
00014 
00015 void
00016 SegCamBehavior::DoStart() {
00017   BehaviorBase::DoStart();
00018   visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, WIRELESS_BUFFER_SIZE);
00019   wireless->setDaemon(visRLE,true);
00020   wireless->listen(visRLE,config->vision.rle_port);
00021 
00022   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.rle_port);
00023 
00024   erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID);
00025   erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID);
00026 }
00027 
00028 void
00029 SegCamBehavior::DoStop() {
00030   erouter->forgetListener(this);
00031   Controller::closeGUI("SegVisionGUI");
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(visRLE,false);
00036   wireless->close(visRLE->sock);
00037   BehaviorBase::DoStop();
00038 }
00039 
00040 void
00041 SegCamBehavior::processEvent(const EventBase& e) {
00042   if(!wireless->isConnected(visRLE->sock))
00043     return;
00044   const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00045   ASSERTRET(fbke!=NULL,"unexpected event");
00046   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) {
00047     bool succ=writeSeg(*fbke);
00048     ASSERTRET(succ,"serialization failed");
00049   }
00050   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) {
00051     bool succ=writeRLE(*fbke);
00052     ASSERTRET(succ,"serialization failed");
00053   }
00054 }
00055 
00056 bool
00057 SegCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00058   if(packet!=NULL)
00059     return false;
00060 
00061   avail=WIRELESS_BUFFER_SIZE-1; //not sure why -1, but Alok had it, so i will too
00062   ASSERT(cur==NULL,"cur non-NULL");
00063   cur=NULL;
00064   char * buf=packet=(char*)visRLE->getWriteBuffer(avail);
00065   ASSERTRETVAL(packet!=NULL,"could not get buffer",false);
00066   
00067   unsigned int used;
00068   used=LoadSave::encode("TekkotsuImage",buf,avail);
00069   ASSERTRETVAL(used!=0,"ran out of space",false);
00070   avail-=used; buf+=used;
00071   used=LoadSave::encode(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail);
00072   ASSERTRETVAL(used!=0,"ran out of space",false);
00073   avail-=used; buf+=used;
00074   used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,buf,avail);
00075   ASSERTRETVAL(used!=0,"ran out of space",false);
00076   avail-=used; buf+=used;
00077 
00078   used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail);
00079   ASSERTRETVAL(used!=0,"ran out of space",false);
00080   avail-=used; buf+=used;
00081   used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail);
00082   ASSERTRETVAL(used!=0,"ran out of space",false);
00083   avail-=used; buf+=used;
00084   used=LoadSave::encode(time,buf,avail);
00085   ASSERTRETVAL(used!=0,"ran out of space",false);
00086   avail-=used; buf+=used;
00087   used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail);
00088   ASSERTRETVAL(used!=0,"ran out of space",false);
00089   avail-=used; buf+=used;
00090 
00091   cur=buf;
00092   return true;
00093 }
00094 
00095 bool
00096 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00097   FilterBankGenerator& fbkgen=*e.getSource();
00098 
00099   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00100   unsigned int used=0;
00101   openPacket(fbkgen,e.getTimeStamp(),layer);
00102   ASSERTRETVAL(cur!=NULL,"header failed",false);
00103   
00104   RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00105   ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00106 
00107   rle->selectSaveImage(layer,config->vision.rlecam_channel);
00108   used=rle->SaveBuffer(cur,avail);
00109   ASSERTRETVAL(used!=0,"save rle image failed",false);
00110   avail-=used; cur+=used;
00111   
00112   // send out the color map ourselves (since RLE compression doesn't have a concept of color)
00113   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00114   ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00115   if(0==(used=seg->encodeColors(cur,avail))) return false;
00116   avail-=used; cur+=used;
00117 
00118   closePacket();
00119 
00120   return true;
00121 }
00122 
00123 bool
00124 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00125   FilterBankGenerator& fbkgen=*e.getSource();
00126 
00127   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00128   unsigned int used=0;
00129   openPacket(fbkgen,e.getTimeStamp(),layer);
00130   ASSERTRETVAL(cur!=NULL,"header failed",false);
00131   
00132   fbkgen.selectSaveImage(layer,config->vision.rlecam_channel);
00133   used=fbkgen.SaveBuffer(cur,avail);
00134   ASSERTRETVAL(used!=0,"save seg image failed",false);
00135   avail-=used; cur+=used;
00136   
00137   closePacket();
00138 
00139   return true;
00140 }
00141 
00142 void
00143 SegCamBehavior::closePacket() {
00144   if(packet==NULL)
00145     return;
00146   visRLE->write(cur-packet);
00147   packet=cur=NULL;
00148   avail=0;
00149 }
00150 
00151 
00152 /*! @file
00153  * @brief Implements SegCamBehavior, which forwards segmented images from camera over wireless
00154  * @author ejt (Creator)
00155  *
00156  * $Author: ejt $
00157  * $Name: tekkotsu-2_1 $
00158  * $Revision: 1.6 $
00159  * $State: Exp $
00160  * $Date: 2004/02/05 19:11:26 $
00161  */
00162 

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