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