Homepage
Demos
Overview
Downloads
Dev. Resources
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("SegCamBehavior"), visRLE(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00012 {
00013 }
00014 
00015 void
00016 SegCamBehavior::DoStart() {
00017   BehaviorBase::DoStart();
00018   
00019   std::vector<std::string> args;
00020   args.push_back("rle");
00021   char port[50];
00022   snprintf(port,50,"%d",config->vision.rle_port);
00023   args.push_back(port);
00024   if(config->vision.rle_transport==0) {
00025     max_buf=UDP_WIRELESS_BUFFER_SIZE;
00026     visRLE=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf);
00027     args.push_back("udp");
00028   } else if(config->vision.rle_transport==1) {
00029     max_buf=TCP_WIRELESS_BUFFER_SIZE;
00030     visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf);
00031     wireless->setDaemon(visRLE,true);
00032     args.push_back("tcp");
00033   } else {
00034     serr->printf("ERROR: Invalid Config::vision.rle_transport: %d\n",config->vision.rle_transport);
00035     return;
00036   }
00037   wireless->listen(visRLE,config->vision.rle_port);
00038 
00039   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.rle_port,args);
00040 
00041   erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID,EventBase::deactivateETID);
00042   erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID,EventBase::deactivateETID);
00043 }
00044 
00045 void
00046 SegCamBehavior::DoStop() {
00047   erouter->removeListener(this);
00048   if(wireless->isConnected(visRLE->sock))
00049     sendCloseConnectionPacket();
00050   Controller::closeGUI("SegVisionGUI");
00051 
00052   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00053   // it will try to listen again even though we explicitly closed the server socket...
00054   wireless->setDaemon(visRLE,false);
00055   wireless->close(visRLE->sock);
00056   BehaviorBase::DoStop();
00057 }
00058 
00059 void
00060 SegCamBehavior::processEvent(const EventBase& e) {
00061   if(!wireless->isConnected(visRLE->sock))
00062     return;
00063   const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00064   ASSERTRET(fbke!=NULL,"unexpected event");
00065   if ((get_time() - lastProcessedTime) < config->vision.rle_interval) // not enough time has gone by
00066     return;
00067   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) {
00068     bool succ=writeSeg(*fbke);
00069     ASSERTRET(succ,"serialization failed");
00070   }
00071   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) {
00072     bool succ=writeRLE(*fbke);
00073     ASSERTRET(succ,"serialization failed");
00074   }
00075 }
00076 
00077 bool
00078 SegCamBehavior::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*)visRLE->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_config::ENCODE_SINGLE_CHANNEL,buf,avail);
00093   ASSERTRETVAL(used!=0,"ran out of space",false);
00094   avail-=used; buf+=used;
00095   used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,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 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00118   FilterBankGenerator& fbkgen=*e.getSource();
00119 
00120   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00121   unsigned int used=0;
00122   openPacket(fbkgen,e.getTimeStamp(),layer);
00123   ASSERTRETVAL(cur!=NULL,"header failed",false);
00124   
00125   RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00126   ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00127 
00128   rle->selectSaveImage(layer,config->vision.rlecam_channel);
00129   used=rle->SaveBuffer(cur,avail);
00130   ASSERTRETVAL(used!=0,"save rle image failed",false);
00131   avail-=used; cur+=used;
00132   
00133   // send out the color map ourselves (since RLE compression doesn't have a concept of color)
00134   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00135   ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00136   if(0==(used=seg->encodeColors(cur,avail))) return false;
00137   avail-=used; cur+=used;
00138 
00139   closePacket();
00140 
00141   return true;
00142 }
00143 
00144 bool
00145 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00146   FilterBankGenerator& fbkgen=*e.getSource();
00147 
00148   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00149   unsigned int used=0;
00150   openPacket(fbkgen,e.getTimeStamp(),layer);
00151   ASSERTRETVAL(cur!=NULL,"header failed",false);
00152   
00153   fbkgen.selectSaveImage(layer,config->vision.rlecam_channel);
00154   used=fbkgen.SaveBuffer(cur,avail);
00155   ASSERTRETVAL(used!=0,"save seg image failed",false);
00156   avail-=used; cur+=used;
00157   
00158   closePacket();
00159 
00160   return true;
00161 }
00162 
00163 void
00164 SegCamBehavior::closePacket() {
00165   if(packet==NULL)
00166     return;
00167   visRLE->write(cur-packet);
00168   packet=cur=NULL;
00169   avail=0;
00170   lastProcessedTime = get_time();
00171 }
00172 
00173 bool
00174 SegCamBehavior::sendCloseConnectionPacket() {
00175   char msg[]="CloseConnection";
00176   unsigned int len=strlen(msg)+LoadSave::stringpad;
00177   char * buf = (char*)visRLE->getWriteBuffer(len);
00178   ASSERTRETVAL(buf!=NULL,"Could not get buffer for closing packet",false);
00179   unsigned int used=LoadSave::encode(msg,buf,len);
00180   ASSERTRETVAL(used!=0,"Could not write close packet",false);
00181   visRLE->write(used);
00182   return true;
00183 }
00184 
00185 /*! @file
00186  * @brief Implements SegCamBehavior, which forwards segmented images from camera over wireless
00187  * @author ejt (Creator)
00188  *
00189  * $Author: ejt $
00190  * $Name: tekkotsu-2_4_1 $
00191  * $Revision: 1.14 $
00192  * $State: Exp $
00193  * $Date: 2005/07/29 18:35:04 $
00194  */
00195 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4