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)
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
00059
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
00069
00070 }
00071 }
00072 } catch(...) {
00073 if(packet!=NULL) {
00074 cur=packet;
00075 closePacket();
00076 }
00077
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
00091
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;
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)
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
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)
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
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226