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