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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1