Tekkotsu 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::theOne=NULL;
00011 
00012 SegCamBehavior::SegCamBehavior()
00013   : CameraStreamBehavior("SegCamBehavior",visRLE), visRLE(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00014 {
00015   ASSERT(theOne==NULL,"there was already a SegCamBehavior running!");
00016   theOne=this;
00017 }
00018 
00019 void
00020 SegCamBehavior::DoStart() {
00021   BehaviorBase::DoStart();
00022   setupServer();
00023   erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID,EventBase::deactivateETID);
00024   erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID,EventBase::deactivateETID);
00025 }
00026 
00027 void
00028 SegCamBehavior::DoStop() {
00029   erouter->removeListener(this);
00030   closeServer();
00031   BehaviorBase::DoStop();
00032 }
00033 
00034 void
00035 SegCamBehavior::processEvent(const EventBase& e) {
00036   if(!wireless->isConnected(visRLE->sock))
00037     return;
00038   if(config->vision.segcam.transport==0 && visRLE->getTransport()==Socket::SOCK_STREAM
00039      || config->vision.segcam.transport==1 && visRLE->getTransport()==Socket::SOCK_DGRAM) {
00040     closeServer();
00041     setupServer();
00042     return;
00043   }
00044   try {
00045     const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00046     if(fbke==NULL) {
00047       CameraStreamBehavior::processEvent(e);
00048       return;
00049     }
00050     if ((get_time() - lastProcessedTime) < config->vision.segcam.interval) // not enough time has gone by
00051       return;
00052     if(config->vision.segcam.compression==Config::vision_config::SegCamConfig::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) {
00053       if(!writeSeg(*fbke)) {
00054         if(packet!=NULL) {
00055           cur=packet;
00056           closePacket();
00057         }
00058         //error message should already have been reported
00059         //ASSERTRET(succ,"serialization failed");
00060       }
00061     }
00062     if(config->vision.segcam.compression==Config::vision_config::SegCamConfig::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) {
00063       if(!writeRLE(*fbke)) {
00064         if(packet!=NULL) {
00065           cur=packet;
00066           closePacket();
00067         }
00068         //error message should already have been reported
00069         //ASSERTRET(succ,"serialization failed");
00070       }
00071     }
00072   } catch(...) {
00073     if(packet!=NULL) {
00074       cur=packet;
00075       closePacket();
00076     }
00077     // typically this is a per-frame recurring error, so let's just stop now
00078     serr->printf("%s: exception generated during image serialization, stopping stream.\n",getName().c_str());
00079     DoStop();
00080     throw;
00081   }
00082 }
00083 
00084 void
00085 SegCamBehavior::closeServer() {
00086   if(wireless->isConnected(visRLE->sock))
00087     sendCloseConnectionPacket();
00088   Controller::closeGUI("SegVisionGUI");
00089   
00090   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00091   // it will try to listen again even though we explicitly closed the server socket...
00092   wireless->setDaemon(visRLE,false);
00093   wireless->close(visRLE->sock);
00094 }
00095 
00096 void
00097 SegCamBehavior::setupServer() {
00098   std::vector<std::string> args;
00099   args.push_back("rle");
00100   char port[50];
00101   snprintf(port,50,"%d",*config->vision.segcam.port);
00102   args.push_back(port);
00103   if(config->vision.segcam.transport==0) {
00104     max_buf=UDP_WIRELESS_BUFFER_SIZE;
00105     visRLE=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf);
00106     args.push_back("udp");
00107   } else if(config->vision.segcam.transport==1) {
00108     max_buf=TCP_WIRELESS_BUFFER_SIZE;
00109     visRLE=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf);
00110     args.push_back("tcp");
00111   } else {
00112     serr->printf("ERROR: Invalid Config::vision.segcam.transport: %d\n",*config->vision.segcam.transport);
00113     return;
00114   }
00115   wireless->setDaemon(visRLE,true);
00116   wireless->setReceiver(visRLE,networkCallback);
00117   wireless->listen(visRLE,config->vision.segcam.port);
00118   
00119   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.segcam.port,args);
00120 }
00121 
00122 bool
00123 SegCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00124   if(packet!=NULL)
00125     return false;
00126 
00127   avail=max_buf-1; //not sure why -1, but Alok had it, so i will too
00128   ASSERT(cur==NULL,"cur non-NULL");
00129   cur=NULL;
00130   char * buf=packet=(char*)visRLE->getWriteBuffer(avail);
00131   ASSERT(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)");
00132   if(packet==NULL)
00133     return false;
00134   
00135   if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00136   if(!LoadSave::encodeInc(Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00137   if(!LoadSave::encodeInc(Config::vision_config::SegCamConfig::COMPRESS_RLE,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00138 
00139   if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00140   if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00141   if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00142   if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00143 
00144   cur=buf;
00145   return true;
00146 }
00147 
00148 bool
00149 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00150   FilterBankGenerator& fbkgen=*e.getSource();
00151 
00152   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam.skip;
00153   openPacket(fbkgen,e.getTimeStamp(),layer);
00154   if(cur==NULL) //error should have been displayed by openPacket
00155     return false;
00156   
00157   RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00158   ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00159 
00160   rle->selectSaveImage(layer,config->vision.segcam.channel);
00161   if(!LoadSave::checkInc(rle->saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam.transport to TCP and reopen seg cam")) return false;
00162   
00163   // send out the color map ourselves (since RLE compression doesn't have a concept of color)
00164   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00165   ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00166   if(!seg->encodeColorsInc(cur,avail)) return false;
00167 
00168   closePacket();
00169 
00170   return true;
00171 }
00172 
00173 bool
00174 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00175   FilterBankGenerator& fbkgen=*e.getSource();
00176 
00177   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam.skip;
00178   openPacket(fbkgen,e.getTimeStamp(),layer);
00179   if(cur==NULL) //error should have been displayed by openPacket
00180     return false;
00181   
00182   fbkgen.selectSaveImage(layer,config->vision.segcam.channel);
00183   if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam.transport to TCP and reopen seg cam")) return false;
00184   
00185   closePacket();
00186 
00187   return true;
00188 }
00189 
00190 void
00191 SegCamBehavior::closePacket() {
00192   if(packet==NULL)
00193     return;
00194   visRLE->write(cur-packet);
00195   packet=cur=NULL;
00196   avail=0;
00197   lastProcessedTime = get_time();
00198 }
00199 
00200 bool
00201 SegCamBehavior::sendCloseConnectionPacket() {
00202   char msg[]="CloseConnection";
00203   unsigned int len=strlen(msg)+LoadSave::stringpad;
00204   char * buf = (char*)visRLE->getWriteBuffer(len);
00205   if(buf==NULL) {
00206     std::cerr << "Could not get buffer for closing packet" << std::endl;
00207     return false;
00208   }
00209   unsigned int used=LoadSave::encode(msg,buf,len);
00210   if(used==0)
00211     std::cerr << "Could not write close packet" << std::endl;
00212   visRLE->write(used);
00213   return true;
00214 }
00215 
00216 /*! @file
00217  * @brief Implements SegCamBehavior, which forwards segmented images from camera over wireless
00218  * @author ejt (Creator)
00219  *
00220  * $Author: ejt $
00221  * $Name: tekkotsu-4_0 $
00222  * $Revision: 1.29 $
00223  * $State: Exp $
00224  * $Date: 2007/05/21 20:51:20 $
00225  */
00226 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4