AgentManagerA.C

00001 /*!@file SeaBee/AgentManager.C
00002  for AUVSI2007 manage agents in COM_A                                   */
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // /////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Micheal Montalbo <montalbo@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/AgentManagerA.C $
00035 // $Id: AgentManagerA.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 #define NUM_STORED_FRAMES    20
00038 
00039 #include "AgentManager.H"
00040 
00041 // ######################################################################
00042 // function for each of the separately threaded agents
00043 // which calls their schedulers
00044 void* runDownwardVisionAgent(void* a)
00045 {
00046 
00047   AgentManager* am  = (AgentManager *)a;
00048   rutz::shared_ptr<DownwardVisionAgent> dv = am->getDownwardVisionAgent();
00049 
00050   dv->run();
00051 
00052   return NULL;
00053 }
00054 
00055 // ######################################################################
00056 void* runSonarListenAgent(void* a)
00057 {
00058   AgentManager* am  = (AgentManager *)a;
00059   rutz::shared_ptr<SonarListenAgent> sl = am->getSonarListenAgent();
00060 
00061   sl->run();
00062   return NULL;
00063 }
00064 
00065 // ######################################################################
00066 // function for each of the separately threaded agents
00067 // which calls their schedulers
00068 void* runForwardVisionAgent(void* a)
00069 {
00070   AgentManager* am  = (AgentManager *)a;
00071   rutz::shared_ptr<ForwardVisionAgent> fv = am->getForwardVisionAgent();
00072 
00073   fv->run();
00074 
00075   return NULL;
00076 }
00077 
00078 // ######################################################################
00079 void* runCaptainAgent(void* a)
00080 {
00081   AgentManager* am  = (AgentManager *)a;
00082   rutz::shared_ptr<CaptainAgent> c = am->getCaptainAgent();
00083 
00084   c->run();
00085   return NULL;
00086 }
00087 
00088 // ######################################################################
00089 void* runMovementAgent(void* a)
00090 {
00091   AgentManager* am  = (AgentManager *)a;
00092   rutz::shared_ptr<MovementAgent> ma = am->getMovementAgent();
00093 
00094   ma->run();
00095 
00096   return NULL;
00097 }
00098 
00099 // ######################################################################
00100 AgentManager::AgentManager(nub::soft_ref<SubController> subController,
00101                              nub::soft_ref<EnvVisualCortex> evc,
00102                              ModelManager& mgr,
00103                              const std::string& descrName,
00104                              const std::string& tagName)
00105   :
00106   ModelComponent(mgr, descrName, tagName)
00107 {
00108 
00109 //   nub::soft_ref<SubController> (new SubController(mgr,
00110 //                                                               "Motor",
00111 //                                                              "Primitive"));
00112   itsSubController = subController;
00113   itsEVC = evc;
00114   //  mgr.addSubComponent(itsSubController);
00115 
00116   rutz::shared_ptr<AgentManager> ama(this);
00117 
00118   //init itsSensorResults
00119   initSensorResults();
00120 
00121   // create the agents
00122   itsDownwardVisionAgent.reset(new DownwardVisionAgent("DownwardVisionAgent",ama));
00123   itsSonarListenAgent.reset(new SonarListenAgent("SonarListenAgent",ama));
00124   itsForwardVisionAgent.reset(new ForwardVisionAgent("ForwardVisionAgent",ama));
00125   itsCaptainAgent.reset(new CaptainAgent("CaptainAgen
00126 t",ama));
00127   itsMovementAgent.reset(new MovementAgent(itsSubController,
00128                                            ama,
00129                                            "MovementAgent"));
00130 
00131   // store created agents in vector
00132   itsSubmarineAgents.push_back(itsDownwardVisionAgent);
00133   itsSubmarineAgents.push_back(itsSonarListenAgent);
00134   itsSubmarineAgents.push_back(itsForwardVisionAgent);
00135   itsSubmarineAgents.push_back(itsCaptainAgent);
00136   itsSubmarineAgents.push_back(itsMovementAgent);
00137 
00138   // connect the agents properly
00139   itsForwardVisionAgent->setCaptainAgent(itsCaptainAgent);
00140   itsForwardVisionAgent->setVisualCortex(itsEVC);
00141   itsCaptainAgent->setMovementAgent(itsMovementAgent);
00142   itsCaptainAgent->setForwardVisionAgent(itsForwardVisionAgent);
00143   itsMovementAgent->setCaptainAgent(itsCaptainAgent);
00144 
00145 
00146   pthread_mutex_init(&itsDisplayLock, NULL);
00147   pthread_mutex_init(&itsCurrentImageLock, NULL);
00148   pthread_mutex_init(&itsCommandsLock, NULL);
00149   pthread_mutex_init(&itsSensorResultsLock, NULL);
00150 
00151 
00152   itsTimer.reset(new Timer(1000000));
00153   itsFrameDuration.resize(NUM_STORED_FRAMES);
00154 
00155   // create threads for the agents
00156   pthread_create
00157     (&itsDownwardVisionAgentThread, NULL, runDownwardVisionAgent,
00158      (void *)this);
00159 
00160   pthread_create
00161     (&itsSonarListenAgentThread, NULL, runSonarListenAgent,
00162      (void *)this);
00163 
00164   pthread_create
00165     (&itsForwardVisionAgentThread, NULL, runForwardVisionAgent,
00166      (void *)this);
00167 
00168   pthread_create
00169     (&itsCaptainAgentThread, NULL, runCaptainAgent,
00170      (void *)this);
00171 
00172   pthread_create
00173     (&itsMovementAgentThread, NULL, runMovementAgent,
00174      (void *)this);
00175 }
00176 
00177 
00178 // ######################################################################
00179 AgentManager::~AgentManager()
00180 { }
00181 
00182 // ######################################################################
00183 void AgentManager::startRun()
00184 {
00185   // call prefrontal cortex to start
00186   itsCaptainAgent->start();
00187 }
00188 
00189 // ######################################################################
00190 void AgentManager::setCurrentImage
00191 (Image<PixRGB<byte> > image, uint fNum)
00192 {
00193   // set the current image
00194   pthread_mutex_lock(&itsCurrentImageLock);
00195   itsCurrentImage = image;
00196   itsFrameNumber = fNum;
00197   pthread_mutex_unlock(&itsCurrentImageLock);
00198 
00199   // compute and show framerate over the last NAVG frames:
00200   itsFrameDuration[fNum % NUM_STORED_FRAMES] = itsTimer->get();
00201   itsTimer->reset();
00202 
00203   uint nFrames = NUM_STORED_FRAMES;
00204   if (nFrames < NUM_STORED_FRAMES) nFrames = fNum;
00205   uint64 avg = 0ULL;
00206   for(uint i = 0; i < nFrames; i ++) avg += itsFrameDuration[i];
00207   float frate = 1000000.0F / float(avg) * float(NUM_STORED_FRAMES);
00208 
00209   std::string ntext(sformat("%6d: %6.3f fps -> %8.3f ms/fr",
00210                             fNum,frate, 1000.0/frate));
00211   //  writeText(image, Point2D<int>(0,0), ntext.c_str());
00212 
00213   //if((fNum % 30) == 0)
00214   //  drawImage(image,Point2D<int>(0,0));
00215   //  LINFO("%s",ntext.c_str());
00216 }
00217 
00218 // ######################################################################
00219 void AgentManager::pushCommand(CommandType cmdType,
00220                                 rutz::shared_ptr<Mission> mission)
00221 {
00222   rutz::shared_ptr<AgentManagerCommand> cmd(new AgentManagerCommand());
00223   cmd->itsCommandType = cmdType;
00224   cmd->itsMission = *mission;
00225 
00226   pthread_mutex_lock(&itsCommandsLock);
00227   itsCommands.push_back(cmd);
00228   pthread_mutex_unlock(&itsCommandsLock);
00229 }
00230 
00231 // ######################################################################
00232 uint AgentManager::getNumCommands()
00233 {
00234   return itsCommands.size();
00235 }
00236 
00237 // ######################################################################
00238 rutz::shared_ptr<AgentManagerCommand> AgentManager::popCommand()
00239 {
00240   rutz::shared_ptr<AgentManagerCommand> amc = itsCommands.front();
00241   itsCommands.pop_front();
00242   return amc;
00243 }
00244 
00245 // ######################################################################
00246 void AgentManager::updateAgentsMission(Mission theMission)
00247 {
00248   uint size = itsSubmarineAgents.size();
00249 
00250   // iterate through all agent manager's agents
00251   for(uint i = 0; i < size; i++)
00252     {
00253       if(itsSubmarineAgents.at(i) != itsCaptainAgent)
00254         {
00255           // for each non-Captain agent, update mission
00256           (itsSubmarineAgents.at(i))->msgUpdateMission(theMission);
00257         }
00258     }
00259 }
00260 
00261 // ######################################################################
00262 rutz::shared_ptr<DownwardVisionAgent> AgentManagerB::getDownwardVisionAgent()
00263 {
00264   return itsDownwardVisionAgent;
00265 }
00266 
00267 // ######################################################################
00268 rutz::shared_ptr<SonarListenAgent> AgentManagerB::getSonarListenAgent()
00269 {
00270   return itsSonarListenAgent;
00271 }
00272 
00273 // ######################################################################
00274 rutz::shared_ptr<ForwardVisionAgent>
00275 AgentManager::getForwardVisionAgent()
00276 {
00277   return itsForwardVisionAgent;
00278 }
00279 
00280 // ######################################################################
00281 rutz::shared_ptr<CaptainAgent>
00282 AgentManager::getCaptainAgent()
00283 {
00284   return itsCaptainAgent;
00285 }
00286 
00287 // ######################################################################
00288 rutz::shared_ptr<MovementAgent>
00289 AgentManager::getMovementAgent()
00290 {
00291   return itsMovementAgent;
00292 }
00293 
00294 // ######################################################################
00295 // void AgentManager::drawImage(Image<PixRGB<byte> > ima, Point2D<int> point)
00296 // {
00297 //   pthread_mutex_lock(&itsDisplayLock);
00298 //   inplacePaste(itsDisplayImage, ima, point);
00299 //   //  itsWindow->drawImage(itsDisplayImage,0,0);
00300 //   pthread_mutex_unlock(&itsDisplayLock);
00301 
00302 // }
00303 
00304 // ######################################################################
00305 void AgentManager::initSensorResults()
00306 {
00307   rutz::shared_ptr<SensorResult> buoy(new SensorResult(SensorResult::BUOY));
00308   rutz::shared_ptr<SensorResult> pipe(new SensorResult(SensorResult::PIPE));
00309   rutz::shared_ptr<SensorResult> bin(new SensorResult(SensorResult::BIN));
00310   rutz::shared_ptr<SensorResult> cross(new SensorResult(SensorResult::CROSS));
00311   rutz::shared_ptr<SensorResult> pinger(new SensorResult(SensorResult::PINGER));
00312   rutz::shared_ptr<SensorResult> saliency(new SensorResult(SensorResult::SALIENCY));
00313   rutz::shared_ptr<SensorResult> stereo(new SensorResult(SensorResult::STEREO));
00314 
00315   itsSensorResults.push_back(buoy);
00316   itsSensorResults.push_back(pipe);
00317   itsSensorResults.push_back(bin);
00318   itsSensorResults.push_back(cross);
00319   itsSensorResults.push_back(pinger);
00320   itsSensorResults.push_back(saliency);
00321   itsSensorResults.push_back(stereo);
00322 
00323   for(itsSensorResultsItr = itsSensorResults.begin();
00324       itsSensorResultsItr != itsSensorResults.end();
00325       itsSensorResultsItr++)
00326     {
00327       rutz::shared_ptr<SensorResult> r = *itsSensorResultsItr;
00328       r->startTimer();
00329     }
00330 
00331 }
00332 
00333 // ######################################################################
00334 bool AgentManager::updateSensorResult
00335 (rutz::shared_ptr<SensorResult> sensorResult)
00336 {
00337   bool retVal = false;
00338 
00339   pthread_mutex_lock(&itsSensorResultsLock);
00340   SensorResult::SensorResultType type = sensorResult->getType();
00341 
00342   for(itsSensorResultsItr = itsSensorResults.begin();
00343       itsSensorResultsItr != itsSensorResults.end();
00344       itsSensorResultsItr++)
00345     {
00346       rutz::shared_ptr<SensorResult>
00347         currentResult = *(itsSensorResultsItr);
00348 
00349       if(currentResult->getType() == type)
00350         {
00351           currentResult->copySensorResult(*sensorResult);
00352           retVal = true;
00353         }
00354     }
00355 
00356   pthread_mutex_unlock(&itsSensorResultsLock);
00357 
00358   return retVal;
00359 }
00360 
00361 // ######################################################################
00362 rutz::shared_ptr<SensorResult> AgentManager::getSensorResult
00363 (SensorResult::SensorResultType type)
00364 {
00365   pthread_mutex_lock(&itsSensorResultsLock);
00366 
00367   for(itsSensorResultsItr = itsSensorResults.begin();
00368       itsSensorResultsItr != itsSensorResults.end();
00369       itsSensorResultsItr++)
00370     {
00371       rutz::shared_ptr<SensorResult>
00372         currentResult = *(itsSensorResultsItr);
00373 
00374       if(currentResult->getType() == type)
00375         {
00376           pthread_mutex_unlock(&itsSensorResultsLock);
00377           return currentResult;
00378         }
00379     }
00380 
00381   LINFO("Requested SensorResult type not found");
00382   rutz::shared_ptr<SensorResult> notFound(new SensorResult());
00383 
00384   pthread_mutex_unlock(&itsSensorResultsLock);
00385   return notFound;
00386 }
00387 
00388 // ######################################################################
00389 /* So things look consistent in everyone's emacs... */
00390 /* Local Variables: */
00391 /* indent-tabs-mode: nil */
00392 /* End: */
Generated on Sun May 8 08:40:16 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3