00001 #include "ImageStreamDriver.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/MarkScope.h"
00004 #include "Shared/RobotInfo.h"
00005 #include "Shared/Config.h"
00006 #include "Shared/ImageUtil.h"
00007 #include "Shared/debuget.h"
00008
00009 using namespace std;
00010
00011 const char * ImageStreamDriver::formatNames[NUM_FORMATS+1] = { "yuv", "png", "jpeg", "tekkotsu", "" };
00012
00013
00014 INSTANTIATE_NAMEDENUMERATION_STATICS(ImageStreamDriver::format_t);
00015
00016 const std::string ImageStreamDriver::autoRegisterImageStreamDriver = DeviceDriver::getRegistry().registerType<ImageStreamDriver>("ImageStream");
00017
00018 unsigned int ImageStreamDriver::nextTimestamp() {
00019 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00020 if(comm==NULL || !comm->isReadable())
00021 return -1U;
00022 return get_time();
00023 }
00024
00025 unsigned int ImageStreamDriver::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00026 payload=NULL; payloadSize=0;
00027 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00028 if(comm==NULL || !comm->isReadable())
00029 return frameNumber;
00030
00031
00032
00033
00034
00035
00036 unsigned int layer=0;
00037 unsigned int width=CameraResolutionX;
00038 unsigned int height=CameraResolutionY;
00039 unsigned int components=3;
00040 {
00041 MarkScope autolock(*comm);
00042
00043 std::istream is(&comm->getReadStreambuf());
00044 switch(*format) {
00045 case FORMAT_YUV: {
00046 unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00047 buffer.resize(remain);
00048 char * buf=&buffer.front();
00049 if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00050 if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00051 if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00052 if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00053 is.read(buf,remain);
00054 ++frameNumber;
00055 } break;
00056
00057 case FORMAT_PNG: {
00058 size_t w,h,c;
00059 bool isFirst=(img==NULL);
00060 if(!image_util::decodePNG(is,w,h,c,img,imgSize)) {
00061 cerr << "ImageStreamDriver PNG decompression failed" << endl;
00062 return frameNumber;
00063 }
00064 width=w;
00065 height=h;
00066 components=c;
00067
00068 ASSERT(imgSize==width*height*components,"image size doesn't match expected size");
00069 unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00070 buffer.resize(remain);
00071 char * buf=&buffer.front();
00072 if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00073 if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00074 if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00075 if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00076 if(isFirst) {
00077 memcpy(buf,img,imgSize);
00078 delete [] img;
00079 img=buf;
00080 }
00081
00082 ++frameNumber;
00083 } break;
00084
00085 case FORMAT_JPEG: {
00086 size_t w,h,c;
00087 bool isFirst=(img==NULL);
00088 if(!image_util::decodeJPEG(is,w,h,c,img,imgSize)) {
00089 cerr << "ImageStreamDriver JPEG decompression failed" << endl;
00090 return frameNumber;
00091 }
00092 width=w;
00093 height=h;
00094 components=c;
00095
00096 ASSERT(imgSize==width*height*components,"image size doesn't match expected size");
00097 unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00098 buffer.resize(remain);
00099 char * buf=&buffer.front();
00100 if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00101 if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00102 if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00103 if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00104 if(isFirst) {
00105 memcpy(buf,img,imgSize);
00106 delete [] img;
00107 img=buf;
00108 }
00109
00110 ++frameNumber;
00111 } break;
00112
00113 case FORMAT_TEKKOTSU: {
00114
00115
00116
00117
00118 char tmp[256];
00119 unsigned int len;
00120 string fmt;
00121 is.read(tmp,LoadSave::getSerializedSize(len));
00122 LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00123 if(len!=13) {
00124 cerr << "Expecting Tekkotsu image format, but image header doesn't match! (len==" << len << ")" << endl;
00125 return frameNumber;
00126 }
00127 is.read(tmp,len+1);
00128 if(strncmp(tmp,"TekkotsuImage",len+1)!=0) {
00129 tmp[len+2]='\0';
00130 cerr << "Expecting Tekkotsu image format, but image header doesn't match! (" << tmp << ")" << endl;
00131 return frameNumber;
00132 }
00133
00134 int encoding;
00135 is.read(tmp,LoadSave::getSerializedSize(encoding));
00136 LoadSave::decode(encoding,tmp,LoadSave::getSerializedSize(encoding));
00137 if(encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL)
00138 components=1;
00139 int compression;
00140 is.read(tmp,LoadSave::getSerializedSize(compression));
00141 LoadSave::decode(compression,tmp,LoadSave::getSerializedSize(compression));
00142 is.read(tmp,LoadSave::getSerializedSize(width));
00143 LoadSave::decode(width,tmp,LoadSave::getSerializedSize(width));
00144 is.read(tmp,LoadSave::getSerializedSize(height));
00145 LoadSave::decode(height,tmp,LoadSave::getSerializedSize(height));
00146 unsigned int remote_timestamp;
00147 is.read(tmp,LoadSave::getSerializedSize(remote_timestamp));
00148 LoadSave::decode(remote_timestamp,tmp,LoadSave::getSerializedSize(remote_timestamp));
00149 unsigned int fnum;
00150 is.read(tmp,LoadSave::getSerializedSize(fnum));
00151 LoadSave::decode(fnum,tmp,LoadSave::getSerializedSize(fnum));
00152
00153
00154 unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00155 buffer.resize(remain);
00156 char * buf=&buffer.front();
00157 if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00158 if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00159 if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00160 if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00161
00162 for(unsigned int i=0; i<components; ++i) {
00163
00164 is.read(tmp,LoadSave::getSerializedSize(len));
00165 LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00166 if(len!=8) {
00167 cerr << "Expecting FbkImage image format, but header doesn't match! (len==" << len << ")" << endl;
00168 return frameNumber;
00169 }
00170 is.read(tmp,len+1);
00171 if(strncmp(tmp,"FbkImage",len+1)!=0) {
00172 tmp[len+2]='\0';
00173 cerr << "Expecting FbkImage image format, but image header doesn't match! (" << tmp << ")" << endl;
00174 return frameNumber;
00175 }
00176 unsigned int lwidth;
00177 is.read(tmp,LoadSave::getSerializedSize(lwidth));
00178 LoadSave::decode(lwidth,tmp,LoadSave::getSerializedSize(lwidth));
00179 unsigned int lheight;
00180 is.read(tmp,LoadSave::getSerializedSize(lheight));
00181 LoadSave::decode(lheight,tmp,LoadSave::getSerializedSize(lheight));
00182 unsigned int lyr;
00183 is.read(tmp,LoadSave::getSerializedSize(lyr));
00184 LoadSave::decode(lyr,tmp,LoadSave::getSerializedSize(lyr));
00185 unsigned int chan_id;
00186 is.read(tmp,LoadSave::getSerializedSize(chan_id));
00187 LoadSave::decode(chan_id,tmp,LoadSave::getSerializedSize(chan_id));
00188
00189
00190 is.read(tmp,LoadSave::getSerializedSize(len));
00191 LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00192 is.read(tmp,len+1);
00193 if(strcmp(tmp,"blank")==0) {
00194 int useChan=(components==1)?i:chan_id;
00195 int off=useChan;
00196 for(unsigned int y=0; y<height; y++) {
00197 for(unsigned int x=0; x<width; x++) {
00198 buf[off]=128;
00199 off+=components;
00200 }
00201 }
00202 } else if(strcmp(tmp,"RawImage")==0) {
00203 int useChan=(components==1)?i:chan_id;
00204 vector<char> chan(lwidth*lheight);
00205 is.read(&chan.front(),chan.size());
00206 copyImage(buf,width,height,components,&chan.front(),lwidth,lheight,useChan);
00207
00208 } else if(strcmp(tmp,"JPEGGrayscale")==0) {
00209 int useChan=(components==1)?i:chan_id;
00210 is.read(tmp,LoadSave::getSerializedSize(len));
00211 LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00212 vector<char> jpeg(len);
00213 is.read(&jpeg.front(),jpeg.size());
00214 size_t jwidth,jheight,jcomp;
00215 image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp);
00216 if(jcomp!=1) {
00217 cerr << "Got color image where grayscale was expected" << endl;
00218 return frameNumber;
00219 }
00220 vector<char> chan(jwidth*jheight);
00221 size_t tsize=chan.size();
00222 char* tchan=&chan.front();
00223 if(!image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp, tchan, tsize)) {
00224 cerr << "JPEG decompression failed" << endl;
00225 return frameNumber;
00226 }
00227 copyImage(buf,width,height,components,&chan.front(),lwidth,lheight,useChan);
00228
00229 } else if(strcmp(tmp,"JPEGColor")==0) {
00230 is.read(tmp,LoadSave::getSerializedSize(len));
00231 LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00232 vector<char> jpeg(len);
00233 is.read(&jpeg.front(),jpeg.size());
00234 size_t jwidth,jheight,jcomp;
00235 size_t tsize=remain;
00236 if(!image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp, buf, tsize)) {
00237 cerr << "JPEG decompression failed" << endl;
00238 return frameNumber;
00239 }
00240 i=components;
00241
00242 } else {
00243 cerr << "Unknown image generator " << tmp << endl;
00244 return frameNumber;
00245 }
00246 }
00247
00248 if(!is)
00249 return frameNumber;
00250 frameNumber=fnum;
00251 } break;
00252 }
00253 }
00254 payload = &buffer.front();
00255 payloadSize = buffer.size();
00256 timestamp=get_time();
00257 name=instanceName;
00258 return frameNumber;
00259 }
00260
00261 void ImageStreamDriver::setDataSourceThread(LoadDataThread* th) {
00262 if(thread==NULL && th!=NULL) {
00263 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00264 if(comm!=NULL)
00265 comm->open();
00266 commName.addPrimitiveListener(this);
00267 }
00268 if(thread!=NULL && th==NULL) {
00269 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00270 if(comm!=NULL)
00271 comm->close();
00272 commName.removePrimitiveListener(this);
00273 }
00274 DataSource::setDataSourceThread(th);
00275 }
00276
00277 void ImageStreamDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00278 if(&pl==&commName) {
00279
00280
00281 CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00282 if(comm!=NULL)
00283 comm->close();
00284 comm = CommPort::getRegistry().getInstance(commName);
00285 if(comm!=NULL)
00286 comm->open();
00287 } else if(&pl==&format) {
00288 CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00289 if(comm!=NULL) {
00290 MarkScope autolock(*comm);
00291 delete [] img;
00292 img=NULL;
00293 imgSize=0;
00294 } else {
00295 delete [] img;
00296 img=NULL;
00297 imgSize=0;
00298 }
00299 } else {
00300 std::cerr << "Unhandled value change in " << getClassName() << ": " << pl.get() << std::endl;
00301 }
00302 }
00303
00304 void ImageStreamDriver::copyImage(char * buf, unsigned int width, unsigned int height, unsigned int channels, const char * chan, unsigned int lwidth, unsigned int lheight, unsigned int c) {
00305 if(lwidth==width && lheight==height) {
00306
00307 for(unsigned int y=0;y<height;y++) {
00308 unsigned int datarowstart=y*width*channels+c;
00309 unsigned int tmprowstart=y*lwidth;
00310 for(unsigned int x=0;x<width;x++)
00311 buf[datarowstart+x*channels]=chan[tmprowstart+x];
00312 }
00313 } else {
00314
00315
00316
00317
00318
00319
00320
00321
00322 float xsc=(lwidth-1)/(float)(width-1);
00323 float ysc=(lheight-1)/(float)(height-1);
00324 for(unsigned int y=0;y<height-1;y++) {
00325 unsigned int datarowstart=y*width*channels+c;
00326 float ty=y*ysc;
00327 unsigned int ly=(int)ty;
00328 float fy=ty-ly;
00329 unsigned int tmprowstart=ly*lwidth;
00330 for(unsigned int x=0;x<width-1;x++) {
00331 float tx=x*xsc;
00332 unsigned int lx=(int)tx;
00333 float fx=tx-lx;
00334
00335 float lv=((int)chan[tmprowstart+lx]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1]&0xFF)*fx;
00336 float uv=((int)chan[tmprowstart+lx+lwidth]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1+lwidth]&0xFF)*fx;
00337 buf[datarowstart+x*channels]=(char)(lv*(1-fy)+uv*fy);
00338 }
00339 buf[datarowstart+(width-1)*channels]=chan[tmprowstart+lwidth-1];
00340 }
00341 unsigned int datarowstart=width*(height-1)*channels+c;
00342 unsigned int tmprowstart=lwidth*(lheight-1);
00343 for(unsigned int x=0;x<width-1;x++) {
00344 float tx=x*xsc;
00345 unsigned int lx=(int)tx;
00346 float fx=tx-lx;
00347 buf[datarowstart+x*channels]=(char)(((int)chan[tmprowstart+lx]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1]&0xFF)*fx);
00348 }
00349 buf[datarowstart+(width-1)*channels]=chan[tmprowstart+lwidth-1];
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399 }
00400 }
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411