Tekkotsu: SegCamBehavior.cc Source 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()==SocketNS::SOCK_STREAM
00039      || config->vision.segcam_transport==1 && visRLE->getTransport()==SocketNS::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::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::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(SocketNS::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(SocketNS::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   ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false);
00132   
00133   if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00134   if(!LoadSave::encodeInc(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00135   if(!LoadSave::encodeInc(Config::vision_config::COMPRESS_RLE,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00136 
00137   if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00138   if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00139   if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00140   if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;;
00141 
00142   cur=buf;
00143   return true;
00144 }
00145 
00146 bool
00147 SegCamBehavior::writeRLE(const FilterBankEvent& e) {
00148   FilterBankGenerator& fbkgen=*e.getSource();
00149 
00150   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip;
00151   openPacket(fbkgen,e.getTimeStamp(),layer);
00152   if(cur==NULL) //error should have been displayed by openPacket
00153     return false;
00154   
00155   RLEGenerator * rle = dynamic_cast<RLEGenerator*>(&fbkgen);
00156   ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false);
00157 
00158   rle->selectSaveImage(layer,config->vision.segcam_channel);
00159   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;
00160   
00161   // send out the color map ourselves (since RLE compression doesn't have a concept of color)
00162   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>(rle->getSourceGenerator());
00163   ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00164   if(!seg->encodeColorsInc(cur,avail)) return false;
00165 
00166   closePacket();
00167 
00168   return true;
00169 }
00170 
00171 bool
00172 SegCamBehavior::writeSeg(const FilterBankEvent& e) {
00173   FilterBankGenerator& fbkgen=*e.getSource();
00174 
00175   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip;
00176   openPacket(fbkgen,e.getTimeStamp(),layer);
00177   if(cur==NULL) //error should have been displayed by openPacket
00178     return false;
00179   
00180   fbkgen.selectSaveImage(layer,config->vision.segcam_channel);
00181   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;
00182   
00183   closePacket();
00184 
00185   return true;
00186 }
00187 
00188 void
00189 SegCamBehavior::closePacket() {
00190   if(packet==NULL)
00191     return;
00192   visRLE->write(cur-packet);
00193   packet=cur=NULL;
00194   avail=0;
00195   lastProcessedTime = get_time();
00196 }
00197 
00198 bool
00199 SegCamBehavior::sendCloseConnectionPacket() {
00200   char msg[]="CloseConnection";
00201   unsigned int len=strlen(msg)+LoadSave::stringpad;
00202   char * buf = (char*)visRLE->getWriteBuffer(len);
00203   if(buf==NULL) {
00204     std::cerr << "Could not get buffer for closing packet" << std::endl;
00205     return false;
00206   }
00207   unsigned int used=LoadSave::encode(msg,buf,len);
00208   if(used==0)
00209     std::cerr << "Could not write close packet" << std::endl;
00210   visRLE->write(used);
00211   return true;
00212 }
00213 
00214 /*! @file
00215  * @brief Implements SegCamBehavior, which forwards segmented images from camera over wireless
00216  * @author ejt (Creator)
00217  *
00218  * $Author: ejt $
00219  * $Name: tekkotsu-3_0 $
00220  * $Revision: 1.26 $
00221  * $State: Exp $
00222  * $Date: 2006/09/14 18:57:03 $
00223  */
00224