obstacle-avoidance.C

Go to the documentation of this file.
00001 /*!@file RCBot/obstacle-avoidance.C avoide obstacles by determining
00002   the total right and left motion, and moving to the lowest motion.
00003   That is, move, where the space is free                                */
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: Lior Elazary <lelazary@yahoo.com>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/RCBot/obstacle-avoidance.C $
00036 // $Id: obstacle-avoidance.C 9412 2008-03-10 23:10:15Z farhan $
00037 //
00038 
00039 #include "Component/ModelManager.H"
00040 #include "Component/OptionManager.H"
00041 #include "Controllers/PID.H"
00042 #include "Devices/FrameGrabberConfigurator.H"
00043 #include "Devices/sc8000.H"
00044 #include "GUI/XWindow.H"
00045 #include "Image/ColorOps.H"
00046 #include "Image/CutPaste.H"
00047 #include "Image/DrawOps.H"
00048 #include "Image/Image.H"
00049 #include "Image/MathOps.H"
00050 #include "Image/Pixels.H"
00051 #include "Image/ShapeOps.H"
00052 #include "RCBot/Motion/MotionEnergy.H"
00053 #include "Raster/Raster.H"
00054 #include "Transport/FrameIstream.H"
00055 #include "Util/Timer.H"
00056 #include "Util/Types.H"
00057 #include "Util/log.H"
00058 #include <math.h>
00059 
00060 #define USE_V4L
00061 
00062 XWindow *window;
00063 
00064 // ######################################################################
00065 int main(const int argc, const char **argv)
00066 {
00067   MYLOGVERB = LOG_INFO;  // suppress debug messages
00068 
00069   // Instantiate a ModelManager:
00070   ModelManager manager("Obstacle Avoidance");
00071 
00072 #ifdef USE_V4L
00073   nub::soft_ref<FrameGrabberConfigurator>
00074     gbc(new FrameGrabberConfigurator(manager));
00075   manager.addSubComponent(gbc);
00076 
00077   nub::soft_ref<SC8000>
00078     sc8000(new SC8000(manager));
00079   manager.addSubComponent(sc8000);
00080 
00081   //calibrate the servos
00082   sc8000->calibrate(1, 13650, 10800, 16000);
00083   sc8000->calibrate(3, 14000, 12000, 16000);
00084 
00085 #else
00086   // Instantiate our various ModelComponents:
00087   nub::soft_ref<InputFrameSeries>
00088     ifs(new InputFrameSeries(manager));
00089   manager.addSubComponent(ifs);
00090 #endif
00091 
00092   // Parse command-line:
00093   if (manager.parseCommandLine((const int)argc, (const char**)argv,
00094                                "<image>", 0, 1) == false)
00095     return(1);
00096 
00097   SimTime stime = SimTime::ZERO(); Point2D<int> winner(-1, -1);
00098 
00099 #ifdef USE_V4L
00100   // do post-command-line configs:
00101   nub::soft_ref<FrameIstream> gb = gbc->getFrameGrabber();
00102   if (gb.get() == NULL)
00103     LFATAL("You need to select a frame grabber type via the "
00104            "--fg-type=XX command-line option for this program "
00105            "to be useful -- ABORT");
00106   // get the frame grabber to start streaming:
00107   gb->startStream();
00108 #else
00109   // do post-command-line configs:
00110   FrameState is = ifs->update(stime);
00111 #endif
00112 
00113   manager.start();
00114 
00115   Timer timer(1000000);  timer.reset(); // reset the timer
00116   int frame = 0;
00117 
00118   int iw = gb->getWidth(), ih = gb->getHeight();
00119   // create the window
00120   window = new XWindow(Dims(iw*2+2, ih), -1, -1, "Output");
00121 
00122   // create the motion pyramid
00123   MotionEnergyPyrBuilder<byte> motionPyr(Gaussian5);
00124 
00125   int stop = 0;
00126   while(1)
00127   {
00128 #ifdef USE_V4L
00129     Image< PixRGB<byte> > input = gb->readRGB();
00130 #else
00131     const FrameState is = ifs->update(stime);
00132     if (is == FRAME_COMPLETE) break; // done receiving frames
00133     Image< PixRGB<byte> > input = ifs->readRGB();
00134     // empty image signifies end-of-stream
00135     if (!input.initialized()) break;
00136 #endif
00137 
00138     Image<byte> lum = luminance(input);
00139     motionPyr.updateMotion(lum, 1);
00140     ImageSet<float> vMotionPyr = motionPyr.buildVerticalMotion();
00141     //ImageSet<float> hMotionPyr = motionPyr.buildHorizontalMotion();
00142     Image<float> motion = vMotionPyr[0];
00143 
00144     float motionLeft = 0, motionRight = 0;
00145     Image<float>::iterator motionPtr = motion.beginw();
00146     Image<float>::const_iterator motionPtrStop = motion.end();
00147 
00148     int inx = 0;
00149     while (motionPtr != motionPtrStop) {
00150       int y = inx / motion.getWidth();
00151       int x = inx - (y*motion.getWidth());
00152 
00153       if (y > 1){
00154         if (x < (motion.getWidth()/2)){
00155           motionLeft  += fabs(*motionPtr);
00156         } else {
00157           motionRight += fabs(*motionPtr);
00158         }
00159       }
00160 
00161       motionPtr++;
00162       inx++;
00163     }
00164 
00165     double val = motionRight + motionLeft;
00166 
00167     LINFO("Right %0.4f Left %0.4f Total %0.4f",        motionRight, motionLeft, val);
00168 
00169     if (val > 20) {  // Danger, Will Robinson! Danger!
00170       if (motionLeft > motionRight) {
00171         drawLine(lum, Point2D<int>(64,64), Point2D<int>(64+30,64-30), (byte)0, 2);
00172         sc8000->move(1, -1);
00173       } else {
00174         drawLine(lum, Point2D<int>(64,64), Point2D<int>(64-30,64-30), (byte)0, 2);
00175         sc8000->move(1, 1);
00176       }
00177     } else {
00178       sc8000->move(1, 0);
00179     }
00180 
00181     if (val > 4000 || stop) {
00182       LINFO("\n\nSTOP STOP STOP STOP \n");
00183       sc8000->move(3, 0);
00184       sleep(2);
00185     } else {
00186       sc8000->move(3, -0.270);
00187     }
00188 
00189     inplaceNormalize(motion, 0.0F, 255.0F);
00190 
00191     window->drawImage(lum);
00192     window->drawImage((Image<byte>)motion, lum.getWidth()+2, 0);
00193 
00194     frame++;
00195     stime += SimTime::SECS(0.1);
00196   }
00197 
00198   LINFO("Time taken %g\n", timer.getSecs());
00199 
00200   // stop all our ModelComponents
00201   manager.stop();
00202   return 0;
00203 }
00204 
00205 // ######################################################################
00206 /* So things look consistent in everyone's emacs... */
00207 /* Local Variables: */
00208 /* indent-tabs-mode: nil */
00209 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3