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