Tekkotsu: RegionCamBehavior.cc Source File

00001 #include "RegionCamBehavior.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/RegionGenerator.h"
00009 
00010 RegionCamBehavior::RegionCamBehavior()
00011   : BehaviorBase("RegionCamBehavior"), visRegion(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0)
00012 {
00013 }
00014 
00015 void
00016 RegionCamBehavior::DoStart() {
00017   BehaviorBase::DoStart();
00018   setupServer();
00019   erouter->addListener(this,EventBase::visRegionEGID,ProjectInterface::visRegionSID,EventBase::deactivateETID);
00020 }
00021 
00022 void
00023 RegionCamBehavior::DoStop() {
00024   erouter->removeListener(this);
00025   closeServer();
00026   BehaviorBase::DoStop();
00027 }
00028 
00029 void
00030 RegionCamBehavior::processEvent(const EventBase& e) {
00031   if(!wireless->isConnected(visRegion->sock))
00032     return;
00033   if(config->vision.region_transport==0 && visRegion->getTransport()==SocketNS::SOCK_STREAM
00034      || config->vision.region_transport==1 && visRegion->getTransport()==SocketNS::SOCK_DGRAM) {
00035     closeServer();
00036     setupServer();
00037     return;
00038   }
00039   const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00040   ASSERTRET(fbke!=NULL,"unexpected event");
00041 #if DEBUG
00042   bool succ=writeRegions(*fbke);
00043   ASSERTRET(succ,"serialization failed");
00044 #else
00045   writeRegions(*fbke);
00046 #endif
00047 }
00048 
00049 void
00050 RegionCamBehavior::closeServer() {
00051   Controller::closeGUI("RegionVisionGUI");
00052   
00053   // this could be considered a bug in our wireless - if we don't setDaemon(...,false)
00054   // it will try to listen again even though we explicitly closed the server socket...
00055   wireless->setDaemon(visRegion,false);
00056   wireless->close(visRegion->sock);
00057 }
00058 
00059 void
00060 RegionCamBehavior::setupServer() {
00061   std::vector<std::string> args;
00062   args.push_back("reg"); 
00063   char port[50];
00064   snprintf(port,50,"%d",config->vision.region_port);
00065   args.push_back(port);
00066   if(config->vision.region_transport==0) {
00067     max_buf=UDP_WIRELESS_BUFFER_SIZE;
00068     visRegion=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf);
00069     args.push_back("udp");
00070   } else if(config->vision.region_transport==1) {
00071     max_buf=TCP_WIRELESS_BUFFER_SIZE;
00072     visRegion=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf);
00073     wireless->setDaemon(visRegion,true);
00074     args.push_back("tcp");
00075   } else {
00076     serr->printf("ERROR: Invalid Config::vision.region_transport: %d\n",config->vision.region_transport);
00077     return;
00078   }
00079   wireless->listen(visRegion,config->vision.region_port);
00080   Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RegionVisionGUI",config->vision.region_port,args);
00081 }
00082 
00083 bool
00084 RegionCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00085   if(packet!=NULL)
00086     return false;
00087 
00088   avail=max_buf-1; //not sure why -1, but Alok had it, so i will too
00089   ASSERT(cur==NULL,"cur non-NULL");
00090   cur=NULL;
00091   char * buf=packet=(char*)visRegion->getWriteBuffer(avail);
00092   ASSERTRETVAL(packet!=NULL,"could not get buffer",false);
00093   
00094   if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00095   if(!LoadSave::encodeInc(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00096   if(!LoadSave::encodeInc(Config::vision_config::COMPRESS_RLE,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00097 
00098   if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00099   if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00100   if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00101   if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00102 
00103   cur=buf;
00104   return true;
00105 }
00106 
00107 bool
00108 RegionCamBehavior::writeRegions(const FilterBankEvent& e) {
00109   FilterBankGenerator& fbkgen=*e.getSource();
00110 
00111   unsigned int layer=fbkgen.getNumLayers()-1-config->vision.regioncam_skip;
00112   openPacket(fbkgen,e.getTimeStamp(),layer);
00113   ASSERTRETVAL(cur!=NULL,"header failed",false);
00114   
00115   RegionGenerator * regGen = dynamic_cast<RegionGenerator*>(&fbkgen);
00116   ASSERTRETVAL(regGen!=NULL,"fbkgen isn't an RegionGenerator",false);
00117 
00118   regGen->selectSaveImage(layer,config->vision.segcam_channel);
00119   if(!LoadSave::checkInc(regGen->saveBuffer(cur,avail),cur,avail,"save region image failed")) return false;
00120   
00121   const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>((regGen->getSourceGenerator())->getSourceGenerator()); //Get the source of his source (the SegmentedColorGenerator)
00122   ASSERTRETVAL(seg!=NULL,"The source of RegionGenerator's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00123   if(!seg->encodeColorsInc(cur,avail)) return false;
00124 
00125   closePacket();
00126 
00127   return true;
00128 }
00129 
00130 
00131 void
00132 RegionCamBehavior::closePacket() {
00133   if(packet==NULL)
00134     return;
00135   visRegion->write(cur-packet);
00136   packet=cur=NULL;
00137   avail=0;
00138 }
00139 
00140 
00141 /*! @file
00142  * @brief Implements RegionCamBehavior, which forwards the regions from RegionGenerator over wireless
00143  * @author harm & niels (Creators)
00144  *
00145  * $Author: ejt $
00146  * $Name: tekkotsu-3_0 $
00147  * $Revision: 1.6 $
00148  * $State: Exp $
00149  * $Date: 2006/09/09 04:32:29 $
00150  */
00151