Hack_Navigation.C

00001 /*!@file Robots2/Beobot2/Hardware/Hack_Navigation.C
00002  hacky HNB basement navigation                                 */
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: Christian Siagian <siagian@usc.edu>
00034 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/Navigation/Hack_Navigation/Hack_Navigation.C
00035 // $ $Id: Hack_Navigation.C 12962 2010-03-06 02:13:53Z irock $
00036 //
00037 //////////////////////////////////////////////////////////////////////////
00038 
00039 #include "Robots/Beobot2/Navigation/Hack_Navigation/Hack_Navigation.H"
00040 #include "Ice/BeobotEvents.ice.H"
00041 
00042 #include "Raster/Raster.H"
00043 #include "Util/sformat.H"
00044 #include "Image/Image.H"
00045 #include "Ice/IceImageUtils.H"
00046 
00047 // ######################################################################
00048 Hack_Navigation::Hack_Navigation(OptionManager& mgr,
00049                const std::string& descrName, const std::string& tagName) :
00050   RobotBrainComponent(mgr, descrName, tagName),
00051   itsTimer(1000000)
00052   //  itsOfs(new OutputFrameSeries(mgr))
00053 {
00054   //  addSubComponent(itsOfs);
00055 
00056 }
00057 
00058 // ######################################################################
00059 Hack_Navigation::~Hack_Navigation()
00060 { }
00061 
00062 // ######################################################################
00063 void Hack_Navigation::start1()
00064 {
00065 }
00066 
00067 // ######################################################################
00068 void Hack_Navigation::registerTopics()
00069 {
00070   // subscribe to all sensor data
00071   this->registerSubscription("LRFMessageTopic");
00072   this->registerPublisher("MotorRequestTopic");
00073 }
00074 
00075 // ######################################################################
00076 void Hack_Navigation::evolve()
00077 { }
00078 
00079 // ######################################################################
00080 Beobot2::MotorCommand Hack_Navigation::computeHack_Navigation()
00081 {
00082 //   static float rotation = 0;
00083 //   static bool dir = false;
00084 
00085 //   if(dir)  rotation+=.01;
00086 //   if(!dir) rotation-=.01;
00087 //   if(rotation > 1 || rotation < -1) dir = !dir;
00088 
00089 //   Beobot2::MotorCommand cmd;
00090 //   cmd.rotation = rotation;
00091 //   cmd.translation = 0;
00092 
00093   // check the front 50 degree devided into equal sized zones
00094   int  rangeStart = -50;  // NOTE: make sure range start is smaller
00095   int  rangeEnd   =  49;  //       than end
00096   uint numZones   =   10;
00097   LINFO("Range to check (%d, %d) in %d zones",
00098         rangeStart, rangeEnd, numZones);
00099 
00100   // regions much like the sonar
00101   std::vector<float> straightAheadAvg(numZones);
00102   int total = rangeEnd - rangeStart;
00103   int startIndex = rangeStart + 141;
00104   for(uint j = 0; j < numZones; j++)
00105   {
00106     int frontStart = startIndex + (total*float(j)/numZones);
00107     int frontEnd   = startIndex + (total*float(j+1)/numZones) - 1;
00108     LINFO("Range to check (%d, %d)", frontStart, frontEnd);
00109     for(int i = frontStart; i <= frontEnd; i++)
00110       {
00111         straightAheadAvg[j] += itsDistances[i];
00112       }
00113     straightAheadAvg[j] /= float(frontEnd - frontStart);
00114     LINFO("straight Ahead Average[%d]: %f mm",
00115           j, straightAheadAvg[j]);
00116   }
00117 
00118   // command to be executed by the BeoPilot
00119   Beobot2::MotorCommand cmd;
00120 
00121   // stop if the average distance in any region is less than 50cm
00122   bool stopNow = false;
00123   for(uint j = 0; j < numZones; j++)
00124     if(straightAheadAvg[j] < 500.0) stopNow = true;
00125   if(stopNow)
00126     {
00127       cmd.rotation = 0;
00128       cmd.translation = 0;
00129       LINFO(" BAM Stop");
00130     }
00131   else
00132     {
00133       // find the difference of distance
00134       // between the left and rightmost
00135       float diff = straightAheadAvg[numZones - 1] - straightAheadAvg[0];
00136 
00137       float rot = diff/3000.0;
00138       if(rot >  1.0) rot =  1.0;
00139       if(rot < -1.0) rot = -1.0;
00140       cmd.rotation =  rot;
00141       LINFO("diff: %f -> rot: %f ", diff, rot);
00142 
00143       cmd.translation = 1.0;
00144     }
00145 
00146   return cmd;
00147 }
00148 
00149 // ######################################################################
00150 void Hack_Navigation::updateMessage
00151 (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00152 {
00153   // LRF message
00154   if(eMsg->ice_isA("::BeobotEvents::LRFMessage"))
00155   {
00156     // we got LRF data
00157     BeobotEvents::LRFMessagePtr lrfMsg =
00158       BeobotEvents::LRFMessagePtr::dynamicCast(eMsg);
00159 
00160     itsDistances = lrfMsg->distances;
00161     itsAngles    = lrfMsg->angles;
00162 
00163     // compute navigation
00164     Beobot2::MotorCommand cmd = computeHack_Navigation();
00165 
00166     // send to BeoPilot
00167                 updateMotor(cmd.translation,cmd.rotation);
00168   }
00169 }
00170 
00171 // ######################################################################
00172 void Hack_Navigation::updateMotor(double tran,double rot)
00173 {
00174     BeobotEvents::MotorRequestPtr msg = new BeobotEvents::MotorRequest;
00175     msg->transVel = tran;
00176     msg->rotVel   = rot;
00177     this->publish("MotorRequestTopic", msg);
00178 
00179 }
00180 // ######################################################################
00181 
00182 /* So things look consistent in everyone's emacs... */
00183 /* Local Variables: */
00184 /* indent-tabs-mode: nil */
00185 /* End: */
Generated on Sun May 8 08:41:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3