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.regioncam.transport==0 && visRegion->getTransport()==Socket::SOCK_STREAM
00034 || config->vision.regioncam.transport==1 && visRegion->getTransport()==Socket::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
00054
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.regioncam.port);
00065 args.push_back(port);
00066 if(config->vision.regioncam.transport==0) {
00067 max_buf=UDP_WIRELESS_BUFFER_SIZE;
00068 visRegion=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf);
00069 args.push_back("udp");
00070 } else if(config->vision.regioncam.transport==1) {
00071 max_buf=TCP_WIRELESS_BUFFER_SIZE;
00072 visRegion=wireless->socket(Socket::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.regioncam.transport);
00077 return;
00078 }
00079 wireless->listen(visRegion,config->vision.regioncam.port);
00080 Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RegionVisionGUI",config->vision.regioncam.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;
00089 ASSERT(cur==NULL,"cur non-NULL");
00090 cur=NULL;
00091 char * buf=packet=(char*)visRegion->getWriteBuffer(avail);
00092 ASSERT(packet!=NULL,"could not get buffer");
00093 if(packet==NULL)
00094 return false;
00095
00096 if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00097 if(!LoadSave::encodeInc(Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00098 if(!LoadSave::encodeInc(Config::vision_config::SegCamConfig::COMPRESS_RLE,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00099
00100 if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00101 if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00102 if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00103 if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00104
00105 cur=buf;
00106 return true;
00107 }
00108
00109 bool
00110 RegionCamBehavior::writeRegions(const FilterBankEvent& e) {
00111 FilterBankGenerator& fbkgen=*e.getSource();
00112
00113 unsigned int layer=fbkgen.getNumLayers()-1-config->vision.regioncam.skip;
00114 openPacket(fbkgen,e.getTimeStamp(),layer);
00115 ASSERTRETVAL(cur!=NULL,"header failed",false);
00116
00117 RegionGenerator * regGen = dynamic_cast<RegionGenerator*>(&fbkgen);
00118 ASSERTRETVAL(regGen!=NULL,"fbkgen isn't an RegionGenerator",false);
00119
00120 regGen->selectSaveImage(layer,config->vision.segcam.channel);
00121 if(!LoadSave::checkInc(regGen->saveBuffer(cur,avail),cur,avail,"save region image failed")) return false;
00122
00123 const SegmentedColorGenerator * seg = dynamic_cast<const SegmentedColorGenerator*>((regGen->getSourceGenerator())->getSourceGenerator());
00124 ASSERTRETVAL(seg!=NULL,"The source of RegionGenerator's source is not a SegmentedColorGenerator - how do i know what the colors are?",false);
00125 if(!seg->encodeColorsInc(cur,avail)) return false;
00126
00127 closePacket();
00128
00129 return true;
00130 }
00131
00132
00133 void
00134 RegionCamBehavior::closePacket() {
00135 if(packet==NULL)
00136 return;
00137 visRegion->write(cur-packet);
00138 packet=cur=NULL;
00139 avail=0;
00140 }
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153