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