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

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