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("SegCamBehavior"), visRLE(NULL), packet(NULL), cur(NULL), avail(0), max_buf(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);
00042   erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID);
00043 }
00044 
00045 void
00046 SegCamBehavior::DoStop() {
00047   erouter->removeListener(this);
00048   Controller::closeGUI("SegVisionGUI");
00049 
00050   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00051   // it will try to listen again even though we explicitly closed the server socket...
00052   wireless->setDaemon(visRLE,false);
00053   wireless->close(visRLE->sock);
00054   BehaviorBase::DoStop();
00055 }
00056 
00057 void
00058 SegCamBehavior::processEvent(const EventBase& e) {
00059   if(!wireless->isConnected(visRLE->sock))
00060     return;
00061   const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00062   ASSERTRET(fbke!=NULL,"unexpected event");
00063   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) {
00064     bool succ=writeSeg(*fbke);
00065     ASSERTRET(succ,"serialization failed");
00066   }
00067   if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) {
00068     bool succ=writeRLE(*fbke);
00069     ASSERTRET(succ,"serialization failed");
00070   }
00071 }
00072 
00073 bool
00074 SegCamBehavior::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*)visRLE->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_config::ENCODE_SINGLE_CHANNEL,buf,avail);
00089   ASSERTRETVAL(used!=0,"ran out of space",false);
00090   avail-=used; buf+=used;
00091   used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,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 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00114   FilterBankGenerator& fbkgen=*e.getSource();
00115 
00116   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00117   unsigned int used=0;
00118   openPacket(fbkgen,e.getTimeStamp(),layer);
00119   ASSERTRETVAL(cur!=NULL,"header failed",false);
00120   
00121   RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00122   ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00123 
00124   rle->selectSaveImage(layer,config->vision.rlecam_channel);
00125   used=rle->SaveBuffer(cur,avail);
00126   ASSERTRETVAL(used!=0,"save rle image failed",false);
00127   avail-=used; cur+=used;
00128   
00129   // send out the color map ourselves (since RLE compression doesn't have a concept of color)
00130   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00131   ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00132   if(0==(used=seg->encodeColors(cur,avail))) return false;
00133   avail-=used; cur+=used;
00134 
00135   closePacket();
00136 
00137   return true;
00138 }
00139 
00140 bool
00141 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00142   FilterBankGenerator& fbkgen=*e.getSource();
00143 
00144   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip;
00145   unsigned int used=0;
00146   openPacket(fbkgen,e.getTimeStamp(),layer);
00147   ASSERTRETVAL(cur!=NULL,"header failed",false);
00148   
00149   fbkgen.selectSaveImage(layer,config->vision.rlecam_channel);
00150   used=fbkgen.SaveBuffer(cur,avail);
00151   ASSERTRETVAL(used!=0,"save seg image failed",false);
00152   avail-=used; cur+=used;
00153   
00154   closePacket();
00155 
00156   return true;
00157 }
00158 
00159 void
00160 SegCamBehavior::closePacket() {
00161   if(packet==NULL)
00162     return;
00163   visRLE->write(cur-packet);
00164   packet=cur=NULL;
00165   avail=0;
00166 }
00167 
00168 
00169 /*! @file
00170  * @brief Implements SegCamBehavior, which forwards segmented images from camera over wireless
00171  * @author ejt (Creator)
00172  *
00173  * $Author: ejt $
00174  * $Name: tekkotsu-2_2_1 $
00175  * $Revision: 1.11 $
00176  * $State: Exp $
00177  * $Date: 2004/11/11 01:45:36 $
00178  */
00179 

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1