RawCamBehavior.ccGo to the documentation of this file.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()
00011 : BehaviorBase("RawCamBehavior"), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0)
00012 {}
00013
00014 void
00015 RawCamBehavior::DoStart() {
00016 BehaviorBase::DoStart();
00017 std::vector<std::string> args;
00018 args.push_back("raw");
00019 char port[50];
00020 snprintf(port,50,"%d",config->vision.rawcam_port);
00021 args.push_back(port);
00022 if(config->vision.rawcam_transport==0) {
00023 max_buf=UDP_WIRELESS_BUFFER_SIZE;
00024 visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf);
00025 args.push_back("udp");
00026 } else if(config->vision.rawcam_transport==1) {
00027 max_buf=TCP_WIRELESS_BUFFER_SIZE;
00028 visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf);
00029 wireless->setDaemon(visRaw,true);
00030 args.push_back("tcp");
00031 } else {
00032 serr->printf("ERROR: Invalid Config::vision.rawcam_transport: %d\n",config->vision.rawcam_transport);
00033 return;
00034 }
00035 wireless->listen(visRaw,config->vision.rawcam_port);
00036
00037 Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args);
00038
00039 erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::deactivateETID);
00040 erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID,EventBase::deactivateETID);
00041 erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visGrayscaleJPEGSID,EventBase::deactivateETID);
00042 }
00043
00044 void
00045 RawCamBehavior::DoStop() {
00046 erouter->removeListener(this);
00047 if(wireless->isConnected(visRaw->sock))
00048 sendCloseConnectionPacket();
00049 Controller::closeGUI("RawVisionGUI");
00050
00051
00052
00053 wireless->setDaemon(visRaw,false);
00054 wireless->close(visRaw->sock);
00055 BehaviorBase::DoStop();
00056 }
00057
00058 void
00059 RawCamBehavior::processEvent(const EventBase& e) {
00060 if(!wireless->isConnected(visRaw->sock))
00061 return;
00062 const FilterBankEvent* fbke=dynamic_cast<const FilterBankEvent*>(&e);
00063 ASSERTRET(fbke!=NULL,"unexpected event");
00064 if ((get_time() - lastProcessedTime) < config->vision.rawcam_interval) {
00065 return;
00066 }
00067
00068
00069
00070
00071
00072 if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) {
00073 bool succ=writeColor(*fbke);
00074 ASSERTRET(succ,"serialization failed");
00075 } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00076 bool succ=writeSingleChannel(*fbke);
00077 ASSERTRET(succ,"serialization failed");
00078 } else {
00079 serr->printf("%s: Bad rawcam_encoding setting\n",getName().c_str());
00080 }
00081 }
00082
00083 unsigned int RawCamBehavior::getSourceLayer(unsigned int chan, unsigned int numLayers) {
00084 if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00085 if(config->vision.rawcam_channel!=(int)chan)
00086 return -1U;
00087 return numLayers-1-config->vision.rawcam_y_skip;
00088 }
00089
00090 switch(chan) {
00091 case RawCameraGenerator::CHAN_Y:
00092 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00093 if(config->vision.rawcam_y_skip-config->vision.rawcam_uv_skip == 1)
00094 return numLayers-1-config->vision.rawcam_uv_skip;
00095 }
00096 return numLayers-1-config->vision.rawcam_y_skip;
00097 case RawCameraGenerator::CHAN_U:
00098 case RawCameraGenerator::CHAN_V:
00099 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00100 if(config->vision.rawcam_uv_skip-config->vision.rawcam_y_skip == 1)
00101 return numLayers-1-config->vision.rawcam_y_skip;
00102 }
00103 return numLayers-1-config->vision.rawcam_uv_skip;
00104 default:
00105 return -1U;
00106 }
00107 }
00108 unsigned int RawCamBehavior::getSourceYLayer(unsigned int numLayers) {
00109 return getSourceLayer(RawCameraGenerator::CHAN_Y,numLayers);
00110 }
00111 unsigned int RawCamBehavior::getSourceULayer(unsigned int numLayers) {
00112 return getSourceLayer(RawCameraGenerator::CHAN_U,numLayers);
00113 }
00114 unsigned int RawCamBehavior::getSourceVLayer(unsigned int numLayers) {
00115 return getSourceLayer(RawCameraGenerator::CHAN_V,numLayers);
00116 }
00117
00118 bool
00119 RawCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) {
00120 if(packet!=NULL)
00121 return false;
00122
00123 avail=max_buf-1;
00124 ASSERT(cur==NULL,"cur non-NULL");
00125 cur=NULL;
00126 char * buf=packet=(char*)visRaw->getWriteBuffer(avail);
00127 ASSERTRETVAL(packet!=NULL,"could not get buffer",false);
00128
00129 unsigned int used;
00130 used=LoadSave::encode("TekkotsuImage",buf,avail);
00131 ASSERTRETVAL(used!=0,"ran out of space",false);
00132 avail-=used; buf+=used;
00133 used=LoadSave::encode(config->vision.rawcam_encoding,buf,avail);
00134 ASSERTRETVAL(used!=0,"ran out of space",false);
00135 avail-=used; buf+=used;
00136 used=LoadSave::encode(config->vision.rawcam_compression,buf,avail);
00137 ASSERTRETVAL(used!=0,"ran out of space",false);
00138 avail-=used; buf+=used;
00139
00140 used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail);
00141 ASSERTRETVAL(used!=0,"ran out of space",false);
00142 avail-=used; buf+=used;
00143 used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail);
00144 ASSERTRETVAL(used!=0,"ran out of space",false);
00145 avail-=used; buf+=used;
00146 used=LoadSave::encode(time,buf,avail);
00147 ASSERTRETVAL(used!=0,"ran out of space",false);
00148 avail-=used; buf+=used;
00149 used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail);
00150 ASSERTRETVAL(used!=0,"ran out of space",false);
00151 avail-=used; buf+=used;
00152
00153 cur=buf;
00154 return true;
00155 }
00156
00157 bool
00158 RawCamBehavior::writeColor(const FilterBankEvent& e) {
00159 FilterBankGenerator& fbkgen=*e.getSource();
00160
00161 unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip;
00162 unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_uv_skip;
00163
00164 if(config->vision.rawcam_channel==-1) {
00165 if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE
00166 || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00167 if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00168 if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR)
00169 return true;
00170 unsigned int used=0;
00171 openPacket(fbkgen,e.getTimeStamp(),uv_layer);
00172 ASSERTRETVAL(cur!=NULL,"header failed",false);
00173
00174 used=LoadSave::encode("FbkImage",cur,avail);
00175 ASSERTRETVAL(used!=0,"save blank image failed",false);
00176 avail-=used; cur+=used;
00177 used=LoadSave::encode(0,cur,avail);
00178 ASSERTRETVAL(used!=0,"save blank image failed",false);
00179 avail-=used; cur+=used;
00180 used=LoadSave::encode(0,cur,avail);
00181 ASSERTRETVAL(used!=0,"save blank image failed",false);
00182 avail-=used; cur+=used;
00183 used=LoadSave::encode(-1,cur,avail);
00184 ASSERTRETVAL(used!=0,"save blank image failed",false);
00185 avail-=used; cur+=used;
00186 used=LoadSave::encode(RawCameraGenerator::CHAN_Y,cur,avail);
00187 ASSERTRETVAL(used!=0,"save blank image failed",false);
00188 avail-=used; cur+=used;
00189 used=LoadSave::encode("blank",cur,avail);
00190 ASSERTRETVAL(used!=0,"save blank image failed",false);
00191 avail-=used; cur+=used;
00192
00193 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00194 used=fbkgen.SaveBuffer(cur,avail);
00195 ASSERTRETVAL(used!=0,"save image failed",false);
00196 avail-=used; cur+=used;
00197
00198 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00199 used=fbkgen.SaveBuffer(cur,avail);
00200 ASSERTRETVAL(used!=0,"save image failed",false);
00201 avail-=used; cur+=used;
00202
00203 closePacket();
00204 }
00205 return true;
00206 }
00207
00208 unsigned int big_layer=y_layer;
00209 unsigned int small_layer=uv_layer;
00210 if(y_layer<uv_layer) {
00211 big_layer=uv_layer;
00212 small_layer=y_layer;
00213 }
00214 if(const JPEGGenerator* jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen)) {
00215 if(config->vision.rawcam_compression!=Config::vision_config::COMPRESS_JPEG)
00216 return true;
00217 if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) {
00218 unsigned int used=0;
00219 openPacket(fbkgen,e.getTimeStamp(),big_layer);
00220 ASSERTRETVAL(cur!=NULL,"header failed",false);
00221
00222 fbkgen.selectSaveImage(big_layer,0);
00223 used=fbkgen.SaveBuffer(cur,avail);
00224 ASSERTRETVAL(used!=0,"save image failed",false);
00225 avail-=used; cur+=used;
00226
00227 closePacket();
00228 } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) {
00229 unsigned int used=0;
00230 bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00231 ASSERTRETVAL(cur!=NULL,"header failed",false);
00232
00233 if(big_layer==y_layer) {
00234 fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00235 used=fbkgen.SaveBuffer(cur,avail);
00236 ASSERTRETVAL(used!=0,"save image failed",false);
00237 avail-=used; cur+=used;
00238 } else {
00239 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00240 used=fbkgen.SaveBuffer(cur,avail);
00241 ASSERTRETVAL(used!=0,"save image failed",false);
00242 avail-=used; cur+=used;
00243
00244 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00245 used=fbkgen.SaveBuffer(cur,avail);
00246 ASSERTRETVAL(used!=0,"save image failed",false);
00247 avail-=used; cur+=used;
00248 }
00249
00250 if(!opened)
00251 closePacket();
00252 }
00253 } else {
00254 unsigned int used=0;
00255 bool opened=false;
00256
00257 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) {
00258 opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00259 ASSERTRETVAL(cur!=NULL,"header failed",false);
00260
00261 fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y);
00262 used=fbkgen.SaveBuffer(cur,avail);
00263 ASSERTRETVAL(used!=0,"save image failed",false);
00264 avail-=used; cur+=used;
00265 }
00266
00267 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) {
00268 opened=openPacket(fbkgen,e.getTimeStamp(),big_layer);
00269 ASSERTRETVAL(cur!=NULL,"header failed",false);
00270
00271 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U);
00272 used=fbkgen.SaveBuffer(cur,avail);
00273 ASSERTRETVAL(used!=0,"save image failed",false);
00274 avail-=used; cur+=used;
00275
00276 fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V);
00277 used=fbkgen.SaveBuffer(cur,avail);
00278 ASSERTRETVAL(used!=0,"save image failed",false);
00279 avail-=used; cur+=used;
00280 }
00281
00282 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || !opened)
00283 closePacket();
00284 }
00285
00286 return true;
00287 }
00288
00289 bool
00290 RawCamBehavior::writeSingleChannel(const FilterBankEvent& e) {
00291 FilterBankGenerator& fbkgen=*e.getSource();
00292 if( config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID
00293 || config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID )
00294 {
00295 if(const JPEGGenerator * jgen=dynamic_cast<const JPEGGenerator*>(&fbkgen))
00296 if(jgen->getCurrentSourceFormat()!=JPEGGenerator::SRC_GRAYSCALE)
00297 return true;
00298 unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip;
00299
00300 unsigned int used=0;
00301 openPacket(fbkgen,e.getTimeStamp(),layer);
00302 ASSERTRETVAL(cur!=NULL,"header failed",false);
00303
00304 fbkgen.selectSaveImage(layer,config->vision.rawcam_channel);
00305 used=fbkgen.SaveBuffer(cur,avail);
00306 ASSERTRETVAL(used!=0,"save image failed",false);
00307 avail-=used; cur+=used;
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 ASSERTRETVAL(buf!=NULL,"Could not get buffer for closing packet",false);
00338 unsigned int used=LoadSave::encode(msg,buf,len);
00339 ASSERTRETVAL(used!=0,"Could not write close packet",false);
00340 visRaw->write(used);
00341 return true;
00342 }
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
|