BeoLRF.C

00001 /*!@file Robots2/Beobot2/Hardware/BeoLRF.C Ice Module for an LRF        */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/Hardware/BeoLRF.C
00034 // $ $Id: BeoLRF.C 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 //////////////////////////////////////////////////////////////////////////
00037 
00038 #include "Robots/Beobot2/Hardware/BeoLRF.H"
00039 #include "Ice/BeobotEvents.ice.H"
00040 
00041 // ######################################################################
00042 BeoLRF::BeoLRF(OptionManager& mgr,
00043                const std::string& descrName, const std::string& tagName) :
00044   RobotBrainComponent(mgr, descrName, tagName),
00045   itsLRF(new lobot::LaserRangeFinder()),
00046   itsOfs(new OutputFrameSeries(mgr)),
00047   itsTimer(1000000),
00048   itsDrawImage(true),
00049   itsCurrMessageID(0)
00050 {
00051   addSubComponent(itsOfs);
00052   itsTimer.reset();
00053 }
00054 
00055 // ######################################################################
00056 void BeoLRF::setDrawImage(bool drawImage)
00057 {
00058   itsDrawImage = drawImage;
00059 }
00060 
00061 // ######################################################################
00062 BeoLRF::~BeoLRF()
00063 { }
00064 
00065 // ######################################################################
00066 void BeoLRF::registerTopics()
00067 {
00068   this->registerPublisher("LRFMessageTopic");
00069 }
00070 
00071 // ######################################################################
00072 void BeoLRF::evolve()
00073 {
00074   Image<int> dists;
00075   Image<int>::iterator aptr, stop;
00076   int dist; int  min,max; int angle;
00077 
00078   itsLRF->update();
00079   dists = itsLRF->get_distances();
00080   aptr = dists.beginw();
00081   stop = dists.endw();
00082   LDEBUG("dims= %d", dists.getDims().w());
00083 
00084   // some scaling
00085   getMinMax(dists, min, max);
00086   if (max == min) max = min + 1;
00087 
00088   // LRFMessage
00089   BeobotEvents::LRFMessagePtr msg = new BeobotEvents::LRFMessage;
00090 
00091   msg->RequestID = itsCurrMessageID;
00092 
00093   msg->distances.resize(dists.getDims().w());
00094   msg->angles.resize(dists.getDims().w());
00095 
00096   angle = -141;  int count = 0;
00097   Image<PixRGB<byte> > dispImg(512,512,ZEROS);
00098   while(aptr!=stop)
00099     {
00100       dist = *aptr++;
00101       msg->distances[count] = dist;
00102       msg->angles   [count] = angle;
00103 
00104       // note drawing takes up a lot of time
00105       if(itsDrawImage)
00106         {
00107           float rad = dist; rad = ((rad - min)/(max-min))*256;
00108           if (rad < 0) rad = 1.0;
00109 
00110           Point2D<int> pt;
00111           pt.i = 256 - int(rad*sin((double)angle*M_PI/180.0));
00112           pt.j = 256 - int(rad*cos((double)angle*M_PI/180.0));
00113 
00114           drawCircle(dispImg, pt, 2, PixRGB<byte>(255,0,0));
00115 
00116           if((count >= (-50 + 141)) && (count <= (49 + 141)))
00117             drawLine(dispImg, Point2D<int>(256,256),pt,PixRGB<byte>(0,0,255));
00118           else
00119             drawLine(dispImg, Point2D<int>(256,256),pt,PixRGB<byte>(0,255,0));
00120         }
00121 
00122       LDEBUG("[%4d] <%4d>: %13d mm", count, angle, dist);
00123       angle++; count++;
00124     }
00125   if(itsDrawImage)
00126     itsOfs->writeRGB(dispImg,"Output",FrameInfo("output",SRC_POS));
00127 
00128   LINFO("[%6d]Publishing LRFMessage  time: %f",
00129         itsCurrMessageID, itsTimer.get()/1000.0F);
00130   publish("LRFMessageTopic", msg);
00131   itsTimer.reset();
00132   itsCurrMessageID++;
00133 }
00134 
00135 // ######################################################################
00136 void BeoLRF::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00137     const Ice::Current&)
00138 {
00139 }
00140 
00141 
00142 // ######################################################################
00143 /* So things look consistent in everyone's emacs... */
00144 /* Local Variables: */
00145 /* indent-tabs-mode: nil */
00146 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3