SeaBeeMainB.C

Go to the documentation of this file.
00001 /*!@file SeaBee/SeaBeeMainB.C main 2007 competition code
00002   Run SeaBeeMainA at CPU_A
00003   Run SeaBeeMainB at CPU_B                             */
00004 // //////////////////////////////////////////////////////////////////// //
00005 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00006 // University of Southern California (USC) and the iLab at USC.         //
00007 // See http://iLab.usc.edu for information about this project.          //
00008 // //////////////////////////////////////////////////////////////////// //
00009 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00010 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00011 // in Visual Environments, and Applications'' by Christof Koch and      //
00012 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00013 // pending; application number 09/912,225 filed July 23, 2001; see      //
00014 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00015 // //////////////////////////////////////////////////////////////////// //
00016 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00017 //                                                                      //
00018 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00019 // redistribute it and/or modify it under the terms of the GNU General  //
00020 // Public License as published by the Free Software Foundation; either  //
00021 // version 2 of the License, or (at your option) any later version.     //
00022 //                                                                      //
00023 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00024 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00025 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00026 // PURPOSE.  See the GNU General Public License for more details.       //
00027 //                                                                      //
00028 // You should have received a copy of the GNU General Public License    //
00029 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00030 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00031 // Boston, MA 02111-1307 USA.                                           //
00032 // //////////////////////////////////////////////////////////////////// //
00033 //
00034 // Primary maintainer for this file: Michael Montalbo <montalbo@usc.edu>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/SeaBeeMainB.C $
00036 // $Id: SeaBeeMainB.C 10794 2009-02-08 06:21:09Z itti $
00037 //
00038 //////////////////////////////////////////////////////////////////////////
00039 
00040 #include "Beowulf/Beowulf.H"
00041 #include "Component/ModelManager.H"
00042 #include "Media/FrameSeries.H"
00043 #include "Transport/FrameIstream.H"
00044 #include "Raster/Raster.H"
00045 #include "Media/MediaOpts.H"
00046 #include "GUI/XWinManaged.H"
00047 
00048 #include "Video/VideoFormat.H"
00049 #include "Devices/FrameGrabberConfigurator.H"
00050 #include "Devices/FrameGrabberFactory.H"
00051 
00052 #include "Devices/DeviceOpts.H"
00053 #include "Image/CutPaste.H"
00054 
00055 #include "Globals.H"
00056 #include "AgentManagerB.H"
00057 #include "Mission.H"
00058 #include "SubController.H"
00059 
00060 #include <signal.h>
00061 
00062 #define SIM_MODE false
00063 
00064 volatile bool goforever = false;
00065 uint fNum = 0;
00066 
00067 // gets the messages from COM_A and relay it to the agent manager
00068 void checkInMessages
00069 ( nub::soft_ref<Beowulf> beo,
00070   nub::ref<AgentManagerB> agentManager,
00071   rutz::shared_ptr<XWinManaged> cwin);
00072 
00073 // gets the out messages from the agent manager and send it to COM_A
00074 void checkOutMessages
00075 ( nub::ref<AgentManagerB> agentManager,
00076   nub::soft_ref<Beowulf> beo);
00077 
00078 // unpackage an ocean object to a TCP message to process
00079 void packageSensorResult
00080 ( SensorResult sensorResult,
00081   TCPmessage  &smsg);
00082 
00083 
00084 // ######################################################################
00085 //! Signal handler (e.g., for control-C)
00086 void terminate(int s)
00087 {
00088   LERROR("*** INTERRUPT ***");
00089   goforever = false;
00090   exit(1);
00091 }
00092 
00093 // ######################################################################
00094 // ######################################################################
00095 int main( int argc, const char* argv[] )
00096 {
00097   MYLOGVERB = LOG_INFO;
00098 
00099   // instantiate a model manager:
00100   ModelManager manager("SeaBee 2008 competition slave");
00101 
00102   // Instantiate our various ModelComponents:
00103   nub::soft_ref<Beowulf>
00104     beo(new Beowulf(manager, "Beowulf Slave", "BeowulfSlave", false));
00105   manager.addSubComponent(beo);
00106 
00107 #if SIM_MODE == false
00108 
00109   //initiliaze forward and back bottom cameras
00110   //nub::soft_ref<FrameIstream> gbF(makeV4L2grabber(manager));
00111   nub::soft_ref<FrameIstream> gbB(makeV4L2grabber(manager));
00112 
00113   gbB->setModelParamVal("FrameGrabberDevice",std::string("/dev/video0"));
00114   gbB->setModelParamVal("FrameGrabberChannel",0);
00115   //  gbB->setModelParamVal("InputFrameSource", "V4L2");
00116   //  gbB->setModelParamVal("FrameGrabberMode", VIDFMT_YUYV);
00117   //      gbB->setModelParamVal("FrameGrabberByteSwap", false);
00118   //  gbB->setModelParamVal("FrameGrabberFPS", 30);
00119   manager.addSubComponent(gbB);
00120 #endif
00121 
00122 
00123   // create an agent manager
00124   nub::ref<AgentManagerB> agentManager(new AgentManagerB(manager));
00125   manager.addSubComponent(agentManager);
00126 
00127   manager.exportOptions(MC_RECURSE);
00128 
00129   // Parse command-line:
00130   if (manager.parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
00131 
00132 
00133 //    std::string dims = convertToString(Dims(w, h));
00134 //    LINFO("image size: [%dx%d]", w, h);
00135 //    manager.setOptionValString(&OPT_InputFrameDims, dims);
00136 
00137 //    manager.setModelParamVal("InputFrameDims", Dims(w, h),
00138 //                             MC_RECURSE | MC_IGNORE_MISSING);
00139 
00140   TCPmessage rmsg;     // buffer to receive messages
00141   TCPmessage smsg;     // buffer to send messages
00142   int32 rframe = 0, raction = 0, rnode = -1;
00143 
00144   // catch signals and redirect them to terminate for clean exit:
00145   signal(SIGHUP, terminate); signal(SIGINT, terminate);
00146   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00147   signal(SIGALRM, terminate);
00148 
00149   // let's get all our ModelComponent instances started:
00150   manager.start();
00151 
00152   while(!beo->receive(rnode, rmsg, rframe, raction, 5));
00153   int initval  = uint(rmsg.getElementInt32());
00154   LINFO("Recieved INIT(%d) from COM_A", initval);
00155 
00156   // Get camera image dimensions
00157   uint w, h;
00158 
00159 #if SIM_MODE == false
00160   //    w = ifs->getWidth();
00161   //    h = ifs->getHeight();
00162   w = gbB->getWidth();
00163   h = gbB->getHeight();
00164 #else
00165   w = 320;
00166   h = 240;
00167 #endif
00168 
00169 
00170   rutz::shared_ptr<XWinManaged> cwin
00171     (new XWinManaged(Dims(w,h), 0, 0, "SeaBeeMainB Window"));
00172 
00173   // rutz::shared_ptr<XWinManaged> cwinB
00174   // (new XWinManaged(Dims(2*w,2*h), 2*w, 0, "Downward Vision WindowB"));
00175   //   Image<PixRGB<byte> >dispImageB(2*wB, 2*hB, ZEROS);
00176 
00177   agentManager->setWindow(cwin);
00178   //   agentManager->setWindow(cwinB,dispImageB);
00179 
00180   // send a message of ready to go
00181   smsg.reset(rframe, INIT_DONE);
00182   beo->send(rnode, smsg);
00183 
00184 #if SIM_MODE == false
00185   //start streaming input frames
00186   //ifs->startStream();
00187 
00188   // STR: start streaming
00189   gbB->startStream();
00190 #endif
00191 
00192   goforever = true;
00193 
00194   Image<PixRGB<byte> > img(w,h,ZEROS);
00195 
00196   //  Image<PixRGB<byte> > imgF(w,h,ZEROS);
00197   //  Image<PixRGB<byte> > imgB(w,h,ZEROS);
00198 
00199   while(goforever)
00200     {
00201       // get and store image
00202 #if SIM_MODE == false
00203 
00204       //      ifs->updateNext(); img = ifs->readRGB();
00205       //      if(!img.initialized()) {Raster::waitForKey(); break; }
00206       img = gbB->readRGB();
00207       agentManager->setCurrentImageF(img, fNum);
00208       agentManager->setCurrentImageB(img, fNum);
00209 //       imgF = gbF->readRGB();
00210 
00211 #else
00212       // img = subController->getImage(2);
00213           //imgB = subController->getImage(2);
00214 #endif
00215 
00216       fNum++;
00217 
00218       // check messages recieved from COM_A
00219       checkInMessages(beo, agentManager, cwin);
00220 
00221       // check if there is messages to send to COM_A
00222       checkOutMessages(agentManager, beo);
00223     }
00224 
00225   // we are done
00226   manager.stop();
00227   return 0;
00228 }
00229 
00230 // ######################################################################
00231 void checkInMessages
00232 ( nub::soft_ref<Beowulf> beo,
00233   nub::ref<AgentManagerB> agentManager,
00234   rutz::shared_ptr<XWinManaged> cwin
00235 )
00236 {
00237   int32 rframe = 0, raction = 0, rnode = -1;
00238   TCPmessage rmsg;     // buffer to receive messages
00239 
00240   // get all the messages sent in
00241   if(beo->receive(rnode, rmsg, rframe, raction, 5))
00242     {
00243       //      LINFO("COM_B is recieving a command: %d", raction);
00244 
00245       Image<PixRGB<byte> > img;
00246       // check the purpose of the message
00247       switch(raction)
00248         {
00249         case ABORT:
00250           {
00251             goforever = false; LINFO("Stop SeaBee COM-B");
00252             break;
00253           }
00254         case UPDATE_MISSION:
00255           {
00256             //reconstruct mission object
00257             Mission theMission;
00258             theMission.missionName = Mission::MissionName(rmsg.getElementInt32());
00259             theMission.timeForMission = int(rmsg.getElementInt32());
00260             theMission.missionState = Mission::MissionState(rmsg.getElementInt32());
00261 
00262             //update downward vision agent's mission
00263             agentManager->updateAgentsMission(theMission);
00264             break;
00265           }
00266         case IMAGE_UPDATE:
00267           fNum++;
00268           img = rmsg.getElementColByteIma();
00269           agentManager->setCurrentImageF(img, fNum);
00270           //          cwin->drawImage(img, 0, 0);
00271           break;
00272         default:
00273           LINFO("Unknown purpose");
00274         }
00275     }
00276 }
00277 
00278 // ######################################################################
00279 void checkOutMessages
00280 ( nub::ref<AgentManagerB> agentManager,
00281   nub::soft_ref<Beowulf> beo)
00282 {
00283   int32 rnode = -1;
00284   TCPmessage smsg;     // buffer to send messages
00285 
00286   // while there is a message that needs sending
00287   if(agentManager->getNumResults() > 0)
00288     {
00289       // pop the back of the result vector
00290       SensorResult result = agentManager->popResult();
00291 
00292       // package data from ocean object into TCP message
00293       packageSensorResult(result, smsg);
00294 
00295       // send the message
00296       beo->send(rnode, smsg);
00297     }
00298 }
00299 
00300 // ######################################################################
00301 void packageSensorResult
00302 ( SensorResult sensorResult,
00303   TCPmessage  &smsg)
00304 {
00305   int32 rframe = 0;
00306 
00307   smsg.reset(rframe, SENSOR_RESULT);
00308 
00309   SensorResult::SensorResultType type =
00310     sensorResult.getType();
00311 
00312   smsg.addInt32(type);
00313 
00314   smsg.addInt32(sensorResult.getStatus());
00315 
00316   // package the message
00317   switch(type)
00318     {
00319     case SensorResult::BUOY:
00320       {
00321         Point3D pos = sensorResult.getPosition();
00322         smsg.addInt32(int32(pos.x));
00323         smsg.addInt32(int32(pos.y));
00324         smsg.addInt32(int32(pos.z));
00325         break;
00326       }
00327     case SensorResult::PIPE:
00328       {
00329         Point3D pos = sensorResult.getPosition();
00330         smsg.addInt32(int32(pos.x));
00331         smsg.addInt32(int32(pos.y));
00332         smsg.addInt32(int32(pos.z));
00333         Angle ori = sensorResult.getOrientation();
00334         smsg.addDouble(ori.getVal());
00335         uint fnum = sensorResult.getFrameNum();
00336         smsg.addInt32(int32(fnum));
00337         break;
00338       }
00339     case SensorResult::BIN:
00340       {
00341         Point3D pos = sensorResult.getPosition();
00342         smsg.addInt32(int32(pos.x));
00343         smsg.addInt32(int32(pos.y));
00344         smsg.addInt32(int32(pos.z));
00345         break;
00346       }
00347     case SensorResult::PINGER:
00348       {
00349         Angle ori = sensorResult.getOrientation();
00350         smsg.addDouble(ori.getVal());
00351         break;
00352       }
00353     case SensorResult::SALIENCY:
00354       {
00355         Point3D pos = sensorResult.getPosition();
00356         smsg.addInt32(int32(pos.x));
00357         smsg.addInt32(int32(pos.y));
00358         smsg.addInt32(int32(pos.z));
00359         break;
00360       }
00361       default: LINFO("Unknown sensor result type: %d",type);
00362     }
00363 }
00364 
00365 // ######################################################################
00366 /* So things look consistent in everyone's emacs... */
00367 /* Local Variables: */
00368 /* indent-tabs-mode: nil */
00369 /* End: */
Generated on Sun May 8 08:42:14 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3