WURDE: /home/fwph/code/wurde/modules/visionModule/visionModule.cpp Source File

00001 /*
00002  * visionModule.cpp - Program to capture from firewire cameras and distribute
00003  *                    images to operator units to perform various image processing
00004  *                    tasks. Part of the RoLE Robotics Programming Framework.
00005  *
00006  * Copyright (C) 2006 Frederick Heckel <fwph@cse.wustl.edu>, Washington University
00007  *                                                               in St. Louis
00008  *
00009  */
00010 
00011 #include <Capability.H>
00012 #include <RobotTypes.H>
00013 #include <VisionControl.H>
00014 #include <visionModule.H>
00015 #include <CommsManager.H>
00016 #include <Logger.H>
00017 #include <list>
00018 #include <map>
00019 #include <WURDECam.H>
00020 #include <ImageSource.H>
00021 #include <Time.H>
00022 
00023 using namespace std;
00024 using namespace WURDE;
00025 using namespace WURDEVision;
00026 
00027 int main(int argc, char *argv[]){
00028        
00029        CommsManager myManager("VisionModule");       
00030        
00031        myManager.parseOptions(argc,argv,"c:v:");
00032        g_logdebug << "My name is " << myManager.getName() << endl;
00033        Logger myLogger(myManager.getName()+"Logger");              
00034        VisionControl myControl(myManager.getName()+"Control");
00035        myLogger.setLogLevel(LOG_INFO);
00036        
00037        unsigned int frames=0;
00038        /* find camera and vision files */
00039        string confFile,camFile;
00040 
00041        Time starttime;
00042        
00043        if(g_globalConfiguration.haveOption("c")){
00044               camFile=g_globalConfiguration.getOption("c");
00045               if(camFile[0]!='/'){
00046                      camFile=g_globalConfiguration.getConfigDirectory()+"/"+camFile;
00047               }
00048        }else{
00049               camFile=g_globalConfiguration.getConfigDirectory()+"/"+"camera-config.xml";
00050        }
00051        
00052        if(g_globalConfiguration.haveOption("v")){
00053               confFile=g_globalConfiguration.getOption("v");
00054               if(confFile[0]!='/'){
00055                      confFile=g_globalConfiguration.getConfigDirectory()+"/"+confFile;
00056               }       
00057        }else{
00058                g_logdebug << "Didn't find an override" << endl;
00059               confFile=g_globalConfiguration.getConfigDirectory()+"/vision-config.xml";
00060        }
00061 
00062        g_logdebug << "Reading from vision config file " << confFile << endl;
00063 
00064        /* read the config files, load cameras, load operators */
00065        std::list<VisionOperator *> theOperators;
00066        list<VisionOperator *>::iterator iter;       
00067        vector<CameraConfig> cameraconf=parseCameraFile(camFile);
00068 
00069        std::string strtmp=myManager.getName();
00070        VisionModuleConfig config=parseVisionFile(confFile,strtmp,cameraconf);
00071 
00072        Time mono,stereo;
00073 
00074        g_debug("Done parsing vision config.");
00075 
00076        g_debug("Loading operator plugin libraries.");
00077        loadOperatorMap(config.plugins);
00078 
00079        WURDEImage readImage, stereoImage;
00080 
00081        vector<WURDECamera *> cameras;
00082        std::vector<WURDECamera *>::iterator camiter;
00083        
00084        if(myManager.getState()!=STATE_INFO){
00085               
00086               g_debug("Loading the cameras.");
00087               loadCameras(config, cameras);
00088        
00089               g_debug("Loading the operators.");
00090               loadOperators(config,theOperators,false);
00091 
00092               if(theOperators.size()==0){
00093                      for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00094                             (*camiter)->resetFrame();
00095                             (*camiter)->stopCapture();
00096                             delete (*camiter);
00097                      }
00098                      myManager.cleanUp();
00099                      g_debug("No operators loaded. Shutting down.");
00100                      return 0;
00101               }
00102        }else{
00103               loadOperators(config,theOperators,true);
00104               g_logdebug << "Loaded " << theOperators.size() << " operators." <<endl;
00105        }
00106 
00107        g_logdebug << " Going into Capability registration " << endl;
00108        for(iter=theOperators.begin();iter!=theOperators.end();iter++){        
00109               list<WURDE::Capability * > *objects=(*iter)->getSuppliers();
00110               list<Capability *>::iterator objiter;
00111               cout << " Supplier list " << objects->size() << endl;
00112               g_debug("Starting to register an operator");
00113               
00114               for(objiter=objects->begin();objiter!=objects->end();objiter++){
00115                      g_debug("Registering supplier");
00116                      myManager.registerSupplier(*objiter);
00117               }
00118 
00119               objects=(*iter)->getConsumers();
00120 
00121               for(objiter=objects->begin();objiter!=objects->end();objiter++){
00122                      g_debug("Registering consumer");
00123                      myManager.registerConsumer(*objiter);
00124               }
00125        }
00126 
00127        g_logdebug << "Finished Capability registration.. " << endl;
00128        std::string camname;
00129 
00130        myManager.registerSupplier(&myControl);
00131 
00132        if(myManager.getState()==STATE_INFO){
00133               myManager.runUpdate();
00134               g_loginfo << myManager.getName() << " sent registration information, now cleaning up and shutting down." << endl;
00135               myManager.cleanUp();
00136               for(iter=theOperators.begin();iter!=theOperators.end();iter++){
00137                      delete (*iter);
00138               }       
00139               return 0;
00140        }
00141 
00142        g_debug("Entering Loop");
00143        char buffer[512];
00144 
00145        myManager.setSleep(1.0/40.0);
00146        sprintf(buffer,"Have %d operators and %d cameras",theOperators.size(),cameras.size());
00147        g_debug(buffer);
00148 
00149        starttime.now();
00150 
00151        while(myManager.runUpdate()==STATE_RUN||
00152              myManager.getState()==STATE_IDLE){
00153               sprintf(buffer,"Processing frame %d", frames);
00154               g_debug(buffer);
00155               frames++;
00156               for(iter=theOperators.begin();iter!=theOperators.end();iter++){
00157                      if((*iter)->getState()==STATE_RUN){
00158                             g_debug("Running operator...");
00159                             readImage.second=NULL;
00160                             stereoImage.second=NULL;
00161 
00162                             if(!(*iter)->isStereoModule()){                     
00163                                    //we can run monocular modules so long as we have a camera
00164                                    camname=(*iter)->getCamera0();
00165                                    for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00166                                           if((*camiter)->getName()==camname){
00167                                                   // mono=(*camiter)->getTimestamp();
00168                                                  switch((*iter)->getFormat()){
00169                                                  case FORMAT_CHAR_111_RGB:
00170                                                         readImage=(*camiter)->captureRGB();
00171                                                         break;
00172                                                  case FORMAT_CHAR_111_YUV24:
00173                                                         readImage=(*camiter)->captureYUV24();
00174                                                         break;
00175                                                  case FORMAT_CHAR_11_YUV422:
00176                                                         readImage=(*camiter)->captureYUV422();
00177                                                         break;
00178                                                  case FORMAT_CHAR_1_GRAY:
00179                                                         readImage=(*camiter)->captureGray();
00180                                                         break;
00181                                                  default:
00182                                                         g_logerror << "Unsupported image type requested by operator: " << (*iter)->getFormat() << endl;                                                 
00183                                                  }
00184                                           }
00185                                    }
00186 
00187                                    if(readImage.second){
00188                                           (*iter)->runOperator(readImage);
00189                                    }else{
00190                                            if(g_wurdeCamErrno==ERROR_UNSUPPORTED_IMAGE_TYPE){
00191                                                    g_logerror << "Unsupported image type requested by operator: " << (*iter)->getFormat() << endl;
00192                                            }
00193                                    }            
00194 
00195                             }else{
00196                                    //if we have only one camera, we can't run stereo modules
00197 
00198                                    //we can run monocular modules so long as we have a camera
00199                                    camname=(*iter)->getCamera0();
00200                                    for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00201                                           if((*camiter)->getName()==camname){
00202                                                  switch((*iter)->getFormat()){
00203                                                  case FORMAT_CHAR_111_RGB:
00204                                                         readImage=(*camiter)->captureRGB();
00205                                                         break;
00206                                                  case FORMAT_CHAR_111_YUV24:
00207                                                         readImage=(*camiter)->captureYUV24();
00208                                                         break;
00209                                                  case FORMAT_CHAR_11_YUV422:
00210                                                         readImage=(*camiter)->captureYUV422();
00211                                                         break;
00212                                                  case FORMAT_CHAR_1_GRAY:
00213                                                         readImage=(*camiter)->captureGray();
00214                                                         break;
00215                                                  default:
00216                                                          g_logerror << "Unsupported image type requested by operator: " << (*iter)->getFormat() << "@" << endl;
00217                                                  }
00218                                                  //                                      mono=(*camiter)->getTimestamp();
00219                                           }
00220                                    }
00221                                    if(!readImage.second){
00222                                            if(g_wurdeCamErrno==ERROR_UNSUPPORTED_IMAGE_TYPE){
00223                                                    g_logerror << "Unsupported image type requested by operator from main camera: " << (*iter)->getFormat() << endl;
00224                                            }
00225                                    }
00226                                    
00227                                    camname=(*iter)->getCamera1();
00228                                    g_logdebug << "Starting to search for stereo image" << endl;
00229                                    for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00230                                            g_logdebug << "Searching..."<<endl;
00231                                           if((*camiter)->getName()==camname){
00232                                                   g_logdebug << "Getting stereo image" << endl;
00233                                                  switch((*iter)->getFormat()){
00234                                                  case FORMAT_CHAR_111_RGB:
00235                                                         stereoImage=(*camiter)->captureRGB();
00236                                                         break;
00237                                                  case FORMAT_CHAR_111_YUV24:
00238                                                         stereoImage=(*camiter)->captureYUV24();
00239                                                         break;
00240                                                  case FORMAT_CHAR_11_YUV422:
00241                                                         stereoImage=(*camiter)->captureYUV422();
00242                                                         break;
00243                                                  case FORMAT_CHAR_1_GRAY:
00244                                                         stereoImage=(*camiter)->captureGray();
00245                                                         break;
00246                                                  default:
00247                                                          g_logerror << "Unsupported image type requested by operator: " << (*iter)->getFormat() << "#" << endl;                                                 
00248                                                  }
00249                                                  //                                              stereo=(*camiter)->getTimestamp();
00250                                           }else{
00251                                                   g_logdebug << "Camera " << (*camiter)->getName() << " is not " << camname << endl;
00252                                           }
00253                                    }
00254                                    if(!stereoImage.second){
00255                                            if(g_wurdeCamErrno==ERROR_UNSUPPORTED_IMAGE_TYPE){
00256                                                    g_logerror << "Unsupported image type requested by operator from main camera: " << (*iter)->getFormat() << endl;
00257                                            }
00258                                    }
00259 
00260                                    if(readImage.second&&stereoImage.second){
00261                                           (*iter)->runOperator(readImage,stereoImage);
00262                                    }
00263                             }
00264                                    
00265                      
00266                      }else if((*iter)->getState()==STATE_QUIT){
00267                             // here we should go ahead and remove the object
00268                             g_debug("Operator is in QUIT");
00269                      }else{
00270                             g_debug("Operator state not RUN or QUIT");
00271                      }
00272               }
00273               //             printf("TEST\n");
00274               
00275               
00276               for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00277                      g_debug("Resetting frame");
00278                      (*camiter)->resetFrame();
00279               }
00280 
00281        }
00282        Time endtime;
00283        endtime.now();
00284        //       starttime=endtime-starttime;
00285        g_loginfo << ((float)frames)/(endtime.getSeconds()-starttime.getSeconds()) << " frames per second: " << frames << " over " << endtime.getSeconds()-starttime.getSeconds()<<endl;
00286 
00287        for(iter=theOperators.begin();iter!=theOperators.end();iter++){
00288               (*iter)->quitOperator();
00289        }
00290 
00291        myManager.cleanUp();
00292 
00293        for(iter=theOperators.begin();iter!=theOperators.end();iter++){
00294               delete (*iter);
00295        }       
00296 
00297        for(camiter=cameras.begin();camiter!=cameras.end();camiter++){
00298               (*camiter)->resetFrame();
00299               (*camiter)->stopCapture();
00300               delete (*camiter);
00301        }
00302 
00303 
00304        return 0;
00305 }