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