LoLaserRangeFinder.C

Go to the documentation of this file.
00001 /**
00002    \file  Robots/LoBot/io/LoLaserRangeFinder.C
00003    \brief This file defines the non-inline member functions of the
00004    lobot::LaserRangeFinder class.
00005 */
00006 
00007 // //////////////////////////////////////////////////////////////////// //
00008 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   //
00009 // by the University of Southern California (USC) and the iLab at USC.  //
00010 // See http://iLab.usc.edu for information about this project.          //
00011 // //////////////////////////////////////////////////////////////////// //
00012 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00013 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00014 // in Visual Environments, and Applications'' by Christof Koch and      //
00015 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00016 // pending; application number 09/912,225 filed July 23, 2001; see      //
00017 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00018 // //////////////////////////////////////////////////////////////////// //
00019 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00022 // redistribute it and/or modify it under the terms of the GNU General  //
00023 // Public License as published by the Free Software Foundation; either  //
00024 // version 2 of the License, or (at your option) any later version.     //
00025 //                                                                      //
00026 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00027 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00028 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00029 // PURPOSE.  See the GNU General Public License for more details.       //
00030 //                                                                      //
00031 // You should have received a copy of the GNU General Public License    //
00032 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00033 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00034 // Boston, MA 02111-1307 USA.                                           //
00035 // //////////////////////////////////////////////////////////////////// //
00036 //
00037 // Primary maintainer for this file: mviswana usc edu
00038 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/LoBot/io/LoLaserRangeFinder.C $
00039 // $Id: LoLaserRangeFinder.C 12863 2010-02-19 20:43:51Z mviswana $
00040 //
00041 
00042 //------------------------------ HEADERS --------------------------------
00043 
00044 // lobot headers
00045 #include "Robots/LoBot/io/LoLaserRangeFinder.H"
00046 #include "Robots/LoBot/misc/LoExcept.H"
00047 
00048 // Standard C++ headers
00049 #include <algorithm>
00050 
00051 #include <ctime>
00052 #include <cstdlib>
00053 
00054 //---------------------- ALTERNATIVE DEFINITION -------------------------
00055 
00056 // Un/comment following line when laser range finder is un/available
00057 //#define LOBOT_LRF_DEVMODE 1
00058 
00059 // A convenient dummy during development when the laser range finder may
00060 // not always be at hand for testing purposes.
00061 #ifdef LOBOT_LRF_DEVMODE
00062 
00063 namespace lobot {
00064 
00065 // Constructor
00066 LaserRangeFinder::LaserRangeFinder(const std::string&, int)
00067    : m_bufsiz(0), m_retsiz(0), m_buffer(0),
00068      m_angle_range(-119, 135),
00069      m_distance_range(60, 5600),
00070      m_distances(new int[m_angle_range.size()])
00071 {}
00072 
00073 // A quick function object to generate random integers in the given range
00074 class rand_int {
00075    int min, max ;
00076 public:
00077    rand_int(int i = 0, int x = 100) : min(i - 1), max(x + 1) {
00078       std::srand(std::time(0)) ;
00079    }
00080    int operator()() const {
00081       const float n = static_cast<float>(std::rand())/RAND_MAX ;
00082       return static_cast<int>(min + n * (max - min)) ;
00083    }
00084 } ;
00085 
00086 // Dummy API
00087 void LaserRangeFinder::update()
00088 {
00089    std::generate_n(m_distances, m_angle_range.size(),
00090                    rand_int(m_distance_range.min(), m_distance_range.max())) ;
00091 }
00092 
00093 int LaserRangeFinder::get_distance(int angle) const
00094 {
00095    if (angle < m_angle_range.min() || angle > m_angle_range.max())
00096       return -1 ;
00097    return m_distances[angle - m_angle_range.min()] ;
00098 }
00099 
00100 Image<int> LaserRangeFinder::get_distances() const
00101 {
00102    Image<int> D(m_angle_range.size(), 1, ZEROS) ;
00103    std::copy(m_distances, m_distances + m_angle_range.size(), D.beginw()) ;
00104    return D ;
00105 }
00106 
00107 float LaserRangeFinder::average_distance(int min_angle, int max_angle) const
00108 {
00109    const int a = std::max(min_angle - m_angle_range.min(), 0) ;
00110    const int b = std::min(max_angle - m_angle_range.min(),
00111                           m_angle_range.size() - 1) ;
00112    if (a == b) // special case for single angle
00113       return m_distances[a] ;
00114 
00115    float sum = 0 ; int n = 0 ;
00116    for (int i = a; i <= b; ++i)
00117       if (m_distances[i] > 0) {
00118          sum += m_distances[i] ;
00119          ++n ;
00120       }
00121    return (n > 0) ? sum/n : -1 ;
00122 }
00123 
00124 int LaserRangeFinder::max_reading(int min_angle, int max_angle) const
00125 {
00126    const int a = std::max(min_angle - m_angle_range.min(), 0) ;
00127    const int b = std::min(max_angle - m_angle_range.min(),
00128                           m_angle_range.size() - 1) ;
00129    if (a == b) // special case for single angle
00130       return m_distances[a] ;
00131    return *(std::max_element(m_distances + a, m_distances + b + 1)) ;
00132 }
00133 
00134 // Destructor
00135 LaserRangeFinder::~LaserRangeFinder()
00136 {
00137    delete[] m_distances ;
00138 }
00139 
00140 }
00141 
00142 #else // not in development mode ==> LRF available ==> use the real thing
00143 
00144 //---------------------- ALTERNATIVE DEFINITION -------------------------
00145 
00146 // In case liburg is missing
00147 #ifndef INVT_HAVE_LIBURG
00148 
00149 namespace lobot {
00150 
00151 // Constructor
00152 LaserRangeFinder::LaserRangeFinder(const std::string&, int)
00153    : m_handle(0), m_bufsiz(0), m_retsiz(0), m_buffer(0),
00154      m_angle_range(0, 0), m_distance_range(-1, -1), m_distances(0)
00155 {
00156    throw missing_libs(MISSING_LIBURG) ;
00157 }
00158 
00159 // Empty API
00160 void       LaserRangeFinder::update(){}
00161 int        LaserRangeFinder::get_distance(int) const {return -1 ;}
00162 Image<int> LaserRangeFinder::get_distances()   const {return Image<int>() ;}
00163 float      LaserRangeFinder::average_distance(int, int) const {return -1  ;}
00164 int        LaserRangeFinder::max_reading(int, int)      const {return -1  ;}
00165 
00166 // Destructor
00167 LaserRangeFinder::~LaserRangeFinder(){}
00168 
00169 } // end of namespace encapsulating above empty definition
00170 
00171 #else // liburg available ==> the real McCoy
00172 
00173 //-------------------------- FULL DEFINITION ----------------------------
00174 
00175 namespace lobot {
00176 
00177 //-------------------------- INITIALIZATION -----------------------------
00178 
00179 LaserRangeFinder::LaserRangeFinder(const std::string& device, int baud_rate)
00180    : m_bufsiz(0), m_retsiz(0), m_buffer(0),
00181      m_angle_range(0, 0), m_distance_range(-1, -1), m_distances(0)
00182 {
00183    if (urg_connect(& m_handle, device.c_str(), baud_rate) < 0)
00184       throw lrf_error(LRF_CONNECTION_FAILURE) ;
00185 
00186    m_bufsiz = urg_getDataMax(& m_handle) ;
00187    m_buffer = new long[m_bufsiz] ;
00188    std::fill_n(m_buffer, m_bufsiz, 0) ;
00189 
00190    m_angle_range.reset(urg_index2deg(& m_handle, 0),
00191                        urg_index2deg(& m_handle, m_bufsiz - 1)) ;
00192    m_distances = new int[m_angle_range.size()] ;
00193    std::fill_n(m_distances, m_angle_range.size(), 0) ;
00194 
00195    m_distance_range.reset(static_cast<int>(urg_getDistanceMin(& m_handle)),
00196                           static_cast<int>(urg_getDistanceMax(& m_handle))) ;
00197 }
00198 
00199 //---------------------- DISTANCE DATA RETRIEVAL ------------------------
00200 
00201 // Buffer latest measurements from device
00202 void LaserRangeFinder::update()
00203 {
00204    if (urg_requestData(& m_handle, URG_GD, URG_FIRST, URG_LAST) < 0)
00205       throw lrf_error(LRF_DATA_RETRIEVAL_FAILURE) ;
00206 
00207    m_retsiz = urg_receiveData(& m_handle, m_buffer, m_bufsiz) ;
00208    if (m_retsiz < 0)
00209       throw lrf_error(LRF_DATA_RETRIEVAL_FAILURE) ;
00210 
00211    const int m = m_angle_range.min() ;
00212    const int M = m_angle_range.max() ;
00213    for  (int angle = m, i = 0; angle <= M; ++angle, ++i)
00214    {
00215       long d = m_buffer[urg_deg2index(const_cast<urg_t*>(& m_handle), angle)] ;
00216       if (d < m_distance_range.min() || d > m_distance_range.max())
00217          d = -1 ;
00218       m_distances[i] = static_cast<int>(d) ;
00219    }
00220 }
00221 
00222 // Return measurement corresponding to given angle
00223 int LaserRangeFinder::get_distance(int angle) const
00224 {
00225    if (angle < m_angle_range.min() || angle > m_angle_range.max())
00226       return -1 ;
00227    return m_distances[angle - m_angle_range.min()] ;
00228 }
00229 
00230 // Return all measurements packed together in an Image<int>
00231 Image<int> LaserRangeFinder::get_distances() const
00232 {
00233    Image<int> D(m_angle_range.size(), 1, NO_INIT) ;
00234    std::copy(m_distances, m_distances + m_angle_range.size(), D.beginw()) ;
00235    return D ;
00236 }
00237 
00238 // Return average distance over the given range of angles
00239 float LaserRangeFinder::average_distance(int min_angle, int max_angle) const
00240 {
00241    const int a = std::max(min_angle - m_angle_range.min(), 0) ;
00242    const int b = std::min(max_angle - m_angle_range.min(),
00243                           m_angle_range.size() - 1) ;
00244    if (a == b) // special case for single angle
00245       return m_distances[a] ;
00246 
00247    float sum = 0 ; int n = 0 ;
00248    for (int i = a; i <= b; ++i)
00249       if (m_distances[i] > 0) {
00250          sum += m_distances[i] ;
00251          ++n ;
00252       }
00253    return (n > 0) ? sum/n : -1 ;
00254 }
00255 
00256 // Return maximum distance reading in given angular range
00257 int LaserRangeFinder::max_reading(int min_angle, int max_angle) const
00258 {
00259    const int a = std::max(min_angle - m_angle_range.min(), 0) ;
00260    const int b = std::min(max_angle - m_angle_range.min(),
00261                           m_angle_range.size() - 1) ;
00262    if (a == b) // special case for single angle
00263       return m_distances[a] ;
00264    return *(std::max_element(m_distances + a, m_distances + b + 1)) ;
00265 }
00266 
00267 //----------------------------- CLEAN-UP --------------------------------
00268 
00269 LaserRangeFinder::~LaserRangeFinder()
00270 {
00271    urg_disconnect(& m_handle) ;
00272    delete[] m_distances ;
00273    delete[] m_buffer ;
00274 }
00275 
00276 //-----------------------------------------------------------------------
00277 
00278 } // end of namespace encapsulating this file's definitions
00279 
00280 #endif // #ifndef INVT_HAVE_LIBURG
00281 #endif // #ifdef  LOBOT_LRF_DEVMODE
00282 
00283 /* So things look consistent in everyone's emacs... */
00284 /* Local Variables: */
00285 /* indent-tabs-mode: nil */
00286 /* End: */
Generated on Sun May 8 08:41:30 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3