00001 #include "RawCamBehavior.h"
00002 #include "Wireless/Wireless.h"
00003 #include "Events/EventRouter.h"
00004 #include "Vision/RawCameraGenerator.h"
00005 #include "Vision/JPEGGenerator.h"
00006 #include "Events/FilterBankEvent.h"
00007 #include "Behaviors/Controller.h"
00008 #include "Shared/ProjectInterface.h"
00009
00010 RawCamBehavior* RawCamBehavior::theOne=NULL;
00011
00012 RawCamBehavior::RawCamBehavior()
00013 : CameraStreamBehavior("RawCamBehavior",visRaw), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00014 {
00015 ASSERT(theOne==NULL,"there was already a RawCamBehavior running!");
00016 theOne=this;
00017 }
00018
00019 void
00020 RawCamBehavior::DoStart() {
00021 BehaviorBase::DoStart();
00022 setupServer();
00023 erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::deactivateETID);
00024 erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID,EventBase::deactivateETID);
00025 erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visGrayscaleJPEGSID,EventBase::deactivateETID);
00026 }
00027
00028 void
00029 RawCamBehavior::DoStop() {
00030 erouter->removeListener(this);
00031 closeServer();
00032 BehaviorBase::DoStop();
00033 }
00034
00035 void
00036 RawCamBehavior::processEvent(const EventBase& e) {
00037 if(!wireless->isConnected(visRaw->sock))
00038 return;
00039 if(config->vision.rawcam.transport==0 && visRaw->getTransport()==Socket::SOCK_STREAM
00040 || config->vision.rawcam.transport==1 && visRaw->getTransport()==Socket::SOCK_DGRAM) {
00041
00042 closeServer();
00043 setupServer();
00044 return;
00045 }
00046 try {
00047 const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00048 if(fbke==NULL) {
00049 CameraStreamBehavior::processEvent(e);
00050 return;
00051 }
00052 if ((get_time() - lastProcessedTime) < config->vision.rawcam.interval) {
00053 return;
00054 }
00055
00056
00057
00058
00059
00060 if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) {
00061 if(!writeColor(*fbke)) {
00062 if(packet) {
00063 cur=packet;
00064 closePacket();
00065 }
00066
00067
00068 }
00069 } else if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) {
00070 if(!writeSingleChannel(*fbke)) {
00071 if(packet) {
00072 cur=packet;
00073 closePacket();
00074 }
00075
00076
00077 }
00078 } else {
00079 serr->printf("%s: Bad rawcam.encoding setting\n",getName().c_str());
00080 }
00081 } catch(...) {
00082 if(packet) {
00083 cur=packet;
00084 closePacket();
00085 }
00086
00087 serr->printf("%s: exception generated during image serialization, stopping stream.\n",getName().c_str());
00088 DoStop();
00089 throw;
00090 }
00091 }
00092
00093 unsigned int RawCamBehavior::getSourceLayer(unsigned int chan, unsigned int numLayers) {
00094 if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) {
00095 if(config->vision.rawcam.channel!=(int)chan)
00096 return -1U;
00097 return numLayers-1-config->vision.rawcam.y_skip;
00098 }
00099
00100 switch(chan) {
00101 case RawCameraGenerator::CHAN_Y:
00102 if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00103 if(config->vision.rawcam.y_skip-config->vision.rawcam.uv_skip == 1)
00104 return numLayers-1-config->vision.rawcam.uv_skip;
00105 }
00106 return numLayers-1-config->vision.rawcam.y_skip;
00107 case RawCameraGenerator::CHAN_U:
00108 case RawCameraGenerator::CHAN_V:
00109 if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00110 if(config->vision.rawcam.uv_skip-config->vision.rawcam.y_skip == 1)
00111 return numLayers-1-config->vision.rawcam.y_skip;
00112 }
00113 return numLayers-1-config->vision.rawcam.uv_skip;
00114 default:
00115 return -1U;
00116 }
00117 }
00118 unsigned int RawCamBehavior::getSourceYLayer(unsigned int numLayers) {
00119 return getSourceLayer(RawCameraGenerator::CHAN_Y,numLayers);
00120 }
00121 unsigned int RawCamBehavior::getSourceULayer(unsigned int numLayers) {
00122 return getSourceLayer(RawCameraGenerator::CHAN_U,numLayers);
00123 }
00124 unsigned int RawCamBehavior::getSourceVLayer(unsigned int numLayers) {
00125 return getSourceLayer(RawCameraGenerator::CHAN_V,numLayers);
00126 }
00127
00128 void
00129 RawCamBehavior::closeServer() {
00130 if(wireless->isConnected(visRaw->sock))
00131 sendCloseConnectionPacket();
00132 Controller::closeGUI("RawVisionGUI");
00133
00134
00135
00136 wireless->setDaemon(visRaw,false);
00137 wireless->close(visRaw->sock);
00138 }
00139
00140 void
00141 RawCamBehavior::setupServer() {
00142 std::vector<std::string> args;
00143 args.push_back("raw");
00144 char port[50];
00145 snprintf(port,50,"%d",*config->vision.rawcam.port);
00146 args.push_back(port);
00147 if(config->vision.rawcam.transport==0) {
00148 max_buf=UDP_WIRELESS_BUFFER_SIZE;
00149 visRaw=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf);
00150 args.push_back("udp");
00151 } else if(config->vision.rawcam.transport==1) {
00152 max_buf=TCP_WIRELESS_BUFFER_SIZE;
00153 visRaw=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf);
00154 args.push_back("tcp");
00155 } else {
00156 serr->printf("ERROR: Invalid Config::vision.rawcam.transport: %d\n",*config->vision.rawcam.transport);
00157 return;
00158 }
00159 wireless->setDaemon(visRaw,true);
00160 wireless->setReceiver(visRaw,networkCallback);
00161 wireless->listen(visRaw,config->vision.rawcam.port);
00162
00163 Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",*config->vision.rawcam.port,args);
00164 }
00165
00166 bool
00167 RawCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00168 if(packet!=NULL)
00169 return false;
00170
00171 avail=max_buf-1;
00172 ASSERT(cur==NULL,"cur non-NULL");
00173 cur=NULL;
00174 char * buf=packet=(char*)visRaw->getWriteBuffer(avail);
00175 ASSERT(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)");
00176 if(packet==NULL)
00177 return false;
00178
00179 if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00180 if(!LoadSave::encodeInc(*config->vision.rawcam.encoding,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00181 if(!LoadSave::encodeInc(*config->vision.rawcam.compression,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00182
00183 if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00184 if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00185 if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00186 if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00187
00188 cur=buf;
00189 return true;
00190 }
00191
00192 bool
00193 RawCamBehavior::writeColor(const FilterBankEvent& e) {
00194 FilterBankGenerator& fbkgen=*e.getSource();
00195
00196 unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip;
00197 unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.uv_skip;
00198
00199 if(config->vision.rawcam.channel==-1) {
00200 if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE
00201 || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) {
00202 if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00203 if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR)
00204 return true;
00205 openPacket(fbkgen,e.getTimeStamp(),uv_layer);
00206 if(cur==NULL)
00207 return false;
00208
00209 if(!LoadSave::encodeInc("FbkImage",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00210 if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00211 if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00212 if(!LoadSave::encodeInc(-1,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00213 if(!LoadSave::encodeInc(RawCameraGenerator::CHAN_Y,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00214 if(!LoadSave::encodeInc("blank",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;;
00215
00216 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00217 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00218
00219 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00220 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00221
00222 closePacket();
00223 }
00224 return true;
00225 }
00226
00227 unsigned int big_layer=y_layer;
00228 unsigned int small_layer=uv_layer;
00229 if(y_layer<uv_layer) {
00230 big_layer=uv_layer;
00231 small_layer=y_layer;
00232 }
00233 if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen)) {
00234 if(config->vision.rawcam.compression!=Config::vision_config::RawCamConfig::COMPRESS_JPEG)
00235 return true;
00236 if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) {
00237 openPacket(fbkgen,e.getTimeStamp(),big_layer);
00238 if(cur==NULL)
00239 return false;
00240
00241 fbkgen.selectSaveImage(big_layer,0);
00242 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00243
00244 closePacket();
00245 } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) {
00246 bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00247 if(cur==NULL)
00248 return false;
00249
00250 if(big_layer==y_layer) {
00251 fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00252 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00253 } else {
00254 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00255 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00256 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00257 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00258 }
00259
00260 if(!opened)
00261 closePacket();
00262 }
00263 } else {
00264 bool opened=false;
00265
00266 if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) {
00267 opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00268 if(cur==NULL)
00269 return false;
00270 fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00271 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00272 }
00273
00274 if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) {
00275 opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00276 if(cur==NULL)
00277 return false;
00278 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00279 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00280 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00281 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00282 }
00283
00284 if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || !opened)
00285 closePacket();
00286 }
00287
00288 return true;
00289 }
00290
00291 bool
00292 RawCamBehavior::writeSingleChannel(const FilterBankEvent& e) {
00293 FilterBankGenerator& fbkgen=*e.getSource();
00294 if( config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID
00295 || config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID )
00296 {
00297 if(const JPEGGenerator * jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00298 if(jgen->getCurrentSourceFormat()!=JPEGGenerator::SRC_GRAYSCALE)
00299 return true;
00300 unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip;
00301
00302 openPacket(fbkgen,e.getTimeStamp(),layer);
00303 if(cur==NULL)
00304 return false;
00305
00306 fbkgen.selectSaveImage(layer,config->vision.rawcam.channel);
00307 if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false;
00308
00309 closePacket();
00310 }
00311 return true;
00312 }
00313
00314
00315
00316 void
00317 RawCamBehavior::closePacket() {
00318 if(packet==NULL)
00319 return;
00320 visRaw->write(cur-packet);
00321
00322
00323
00324
00325
00326
00327 packet=cur=NULL;
00328 avail=0;
00329 lastProcessedTime = get_time();
00330 }
00331
00332 bool
00333 RawCamBehavior::sendCloseConnectionPacket() {
00334 char msg[]="CloseConnection";
00335 unsigned int len=strlen(msg)+LoadSave::stringpad;
00336 char * buf = (char*)visRaw->getWriteBuffer(len);
00337 if(buf==NULL) {
00338 std::cerr << "Could not get buffer for closing packet" << std::endl;
00339 return false;
00340 }
00341 unsigned int used=LoadSave::encode(msg,buf,len);
00342 if(used==0)
00343 std::cerr << "Could not write close packet" << std::endl;
00344 visRaw->write(used);
00345 return true;
00346 }
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359