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