00001 #include "CameraBehavior.h"
00002 #include "Events/EventRouter.h"
00003 #include "Shared/ERS210Info.h"
00004 #include "Shared/ERS220Info.h"
00005 #include "Shared/ERS7Info.h"
00006 #include "Wireless/Socket.h"
00007 #include "Shared/WorldState.h"
00008 #include "SoundPlay/SoundManager.h"
00009 #include "Shared/Config.h"
00010 #include "Shared/ProjectInterface.h"
00011
00012 #include "Vision/FilterBankGenerator.h"
00013 #include "Vision/JPEGGenerator.h"
00014
00015 #include <sys/types.h>
00016 #include <sys/stat.h>
00017 #include <unistd.h>
00018 #include <dirent.h>
00019
00020 void
00021 CameraBehavior::DoStart() {
00022 BehaviorBase::DoStart();
00023 if(state->robotDesign&WorldState::ERS210Mask) {
00024 camera_click.setSourceID(ERS210Info::HeadFrButOffset);
00025 } else if(state->robotDesign&WorldState::ERS220Mask) {
00026 camera_click.setSourceID(ERS220Info::HeadFrButOffset);
00027 } else if(state->robotDesign&WorldState::ERS7Mask) {
00028 camera_click.setSourceID(ERS7Info::HeadButOffset);
00029 }
00030 initIndex();
00031 sndman->LoadFile("camera.wav");
00032 erouter->addListener(this,camera_click);
00033 }
00034
00035 void CameraBehavior::DoStop() {
00036 erouter->forgetListener(this);
00037 sndman->ReleaseFile("camera.wav");
00038 BehaviorBase::DoStop();
00039 }
00040
00041
00042
00043
00044
00045
00046 void
00047 CameraBehavior::processEvent(const EventBase& e) {
00048 if(e.shorterThan(camera_click))
00049 return;
00050 if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE) {
00051
00052 FilterBankGenerator * gen=ProjectInterface::defRawCameraGenerator;
00053
00054
00055 FILE * f=openNextFile(".raw");
00056 if(f==NULL)
00057 return;
00058
00059
00060 if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) {
00061 for(unsigned int chan=0; chan<3; chan++) {
00062 gen->selectSaveImage(ProjectInterface::doubleLayer,chan);
00063 unsigned int len=gen->SaveFile(f);
00064 if(len==0) {
00065 serr->printf("Error saving file\n");
00066 sndman->PlayFile(config->controller.error_snd);
00067 return;
00068 }
00069 }
00070 } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00071 gen->selectSaveImage(ProjectInterface::doubleLayer,config->vision.rawcam_channel);
00072 unsigned int len=gen->SaveFile(f);
00073 if(len==0) {
00074 serr->printf("Error saving file\n");
00075 sndman->PlayFile(config->controller.error_snd);
00076 return;
00077 }
00078 }
00079
00080
00081 fclose(f);
00082
00083 } else if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) {
00084
00085 JPEGGenerator * jpeg=NULL;
00086 unsigned int chan=0;
00087 if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR)
00088 jpeg=dynamic_cast<JPEGGenerator*>(ProjectInterface::defColorJPEGGenerator);
00089 else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) {
00090 jpeg=dynamic_cast<JPEGGenerator*>(ProjectInterface::defGrayscaleJPEGGenerator);
00091 chan=config->vision.rawcam_channel;
00092 }
00093 if(jpeg!=NULL) {
00094 unsigned int tmp_q=jpeg->getQuality();
00095 jpeg->setQuality(92);
00096
00097
00098 FILE * f=openNextFile(".jpg");
00099 if(f==NULL)
00100 return;
00101
00102
00103 unsigned char * imgbuf=jpeg->getImage(ProjectInterface::doubleLayer,chan);
00104 unsigned int writ=fwrite(imgbuf,jpeg->getImageSize(ProjectInterface::doubleLayer,chan),1,f);
00105 if(writ==0) {
00106 serr->printf("Error saving file\n");
00107 sndman->PlayFile(config->controller.error_snd);
00108 return;
00109 }
00110
00111
00112 fclose(f);
00113
00114 jpeg->setQuality(tmp_q);
00115 }
00116 }
00117 sout->printf("done\n");
00118 }
00119
00120 FILE *
00121 CameraBehavior::openNextFile(const std::string& ext) {
00122 FILE * f=fopen(getNextName(ext).c_str(),"w+");
00123 if(f==NULL) {
00124 serr->printf("Error opening file\n");
00125 sndman->PlayFile(config->controller.error_snd);
00126 return NULL;
00127 }
00128 sndman->PlayFile("camera.wav");
00129 return f;
00130 }
00131
00132
00133 std::string
00134 CameraBehavior::getNextName(const std::string& ext) {
00135 char tmp[100];
00136 snprintf(tmp,100,"/ms/data/img%05d%s",index++,ext.c_str());
00137 sout->printf("Saving %s...",tmp);
00138 return tmp;
00139 }
00140
00141 void
00142 CameraBehavior::initIndex() {
00143 std::string path="/ms/data/";
00144 DIR* dir=opendir(path.c_str());
00145 if(dir==NULL) {
00146 serr->printf("bad path: %s\n",path.c_str());
00147 return;
00148 }
00149 struct dirent * ent=readdir(dir);
00150 while(ent!=NULL) {
00151 struct stat s;
00152 std::string fullpath=path+ent->d_name;
00153 int err=stat(fullpath.c_str(),&s);
00154 if(err!=0) {
00155 serr->printf("File disappeared: %s\n");
00156 return;
00157 }
00158 if((s.st_mode&S_IFDIR)==0 && strncasecmp(ent->d_name,"IMG",3)==0) {
00159 unsigned int cur=atoi(&ent->d_name[3]);
00160 if(cur>index)
00161 index=cur;
00162 }
00163 ent=readdir(dir);
00164 }
00165 closedir(dir);
00166 index++;
00167 sout->printf("The next saved image will go to %simg%05d\n",path.c_str(),index);
00168 }
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181