LoDangerZone.C

00001 /**
00002    \file  Robots/LoBot/control/LoDangerZone.C
00003    \brief This file defines the non-inline member functions of the
00004    lobot::DangerZone 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/LoDangerZone.C $
00039 // $Id: LoDangerZone.C 13780 2010-08-11 22:07:47Z mviswana $
00040 //
00041 
00042 //------------------------------ HEADERS --------------------------------
00043 
00044 // lobot headers
00045 #include "Robots/LoBot/io/LoDangerZone.H"
00046 
00047 #include "Robots/LoBot/config/LoConfigHelpers.H"
00048 
00049 #include "Robots/LoBot/misc/LoExcept.H"
00050 #include "Robots/LoBot/util/LoString.H"
00051 #include "Robots/LoBot/util/LoMath.H"
00052 
00053 // INVT utilities
00054 #include "Util/log.H"
00055 
00056 // Standard C++ headers
00057 #include <sstream>
00058 #include <algorithm>
00059 
00060 //----------------------------- NAMESPACE -------------------------------
00061 
00062 namespace lobot {
00063 
00064 //--------------------------- LOCAL HELPERS -----------------------------
00065 
00066 // Retrieve settings from danger zone section of config file
00067 template<typename T>
00068 static inline T conf(const std::string& key, const T& default_value)
00069 {
00070    return get_conf<T>("danger_zone", key, default_value) ;
00071 }
00072 
00073 // Return a "raw" danger zone spec z so that it lies within the bounds of
00074 // the LRF's distance range D. If z is negative, the user probably wants
00075 // to disable that particular angular block. So return -1 in that case.
00076 static inline int sanify_dzone(int z, const range<int>& D)
00077 {
00078    return (z <= 0) ? -1 : clamp(z, D) ;
00079 }
00080 
00081 // A comparator for DangerZone::Block based on danger zone settings
00082 static bool dzone_cmp(const DangerZone::Block& a, const DangerZone::Block& b)
00083 {
00084    return a.danger_zone() < b.danger_zone() ;
00085 }
00086 
00087 //------------------------ STATIC DATA MEMBERS --------------------------
00088 
00089 const LaserRangeFinder* DangerZone::m_lrf ;
00090 
00091 //-------------------------- INITIALIZATION -----------------------------
00092 
00093 DangerZone::DangerZone()
00094    : m_max(0), m_lrf_data(0)
00095 {
00096    // LRF better be specified at this point
00097    if (! m_lrf)
00098       throw misc_error(DANGER_ZONE_LRF_NOT_SETUP) ;
00099 
00100    const range<int> A = m_lrf->get_angular_range()  ;
00101    const range<int> D = m_lrf->get_distance_range() ;
00102 
00103    // Get the angular blocks specified in the config file
00104    std::vector<int> B(string_to_vector<int>(
00105       conf<std::string>("angular_blocks", to_string(A)))) ;
00106    int N = B.size() ;
00107    if (N < 2) { // user specified some crap in the config file
00108       B.clear() ;
00109       B.push_back(A.min()) ;
00110       B.push_back(A.max()) ;
00111       N = 2 ;
00112    }
00113 
00114    // Get the danger zones for above blocks from config file
00115    std::vector<int> Z(string_to_vector<int>(
00116       conf<std::string>("danger_zones", dup_string(" 350", N - 1)))) ;
00117 
00118    // Get the individual thresholds for above blocks from config file
00119    std::vector<int> T(string_to_vector<int>(
00120       conf<std::string>("thresholds", dup_string(" 10", N - 1)))) ;
00121 
00122    // Store the above settings in internal list for later use
00123    m_blocks.reserve(N - 1) ;
00124    for (int i = 0, j = 1; j < N - 1; ++i, ++j)
00125       if (A.in(B[i]) && A.in(B[j]))
00126          m_blocks.push_back(Block(B[i], B[j] - 1,
00127                                   sanify_dzone(Z[i], D), T[i])) ;
00128 
00129    int penultimate_start = clamp(B[N - 2], A.min(), A.max() - 1) ;
00130    m_blocks.push_back(Block(penultimate_start,
00131                             clamp(B[N - 1], penultimate_start + 1, A.max()),
00132                             sanify_dzone(Z[N - 2], D),
00133                             T[N - 2])) ;
00134 
00135    // Find the max configured danger zone
00136    Blocks::const_iterator max =
00137       std::max_element(m_blocks.begin(), m_blocks.end(), dzone_cmp) ;
00138    m_max = max->danger_zone() ;
00139 }
00140 
00141 // DangerZone::Block constructors
00142 DangerZone::Block::Block(const range<int>& x, int z, int t)
00143    : m_extents(x),
00144      m_danger_zone(z),
00145      m_threshold(clamp(t, 0, m_extents.size()))
00146 {
00147    m_danger_zone_readings.reserve(m_extents.size()) ;
00148 }
00149 
00150 DangerZone::Block::Block(int start, int end, int z, int t)
00151    : m_extents(start, end),
00152      m_danger_zone(z),
00153      m_threshold(clamp(t, 0, m_extents.size()))
00154 {
00155    m_danger_zone_readings.reserve(m_extents.size()) ;
00156 }
00157 
00158 //--------------------- UPDATING THE DANGER ZONE ------------------------
00159 
00160 DangerZone::Block::update::update(const LRFData& d)
00161    : lrf(d)
00162 {}
00163 
00164 void DangerZone::Block::update::operator()(const DangerZone::Block& B) const
00165 {
00166    Block& block = const_cast<Block&>(B) ;
00167 
00168    block.clear() ;
00169    for (int angle = block.start(); angle <= block.end(); ++angle)
00170    {
00171       int distance = lrf[angle] ;
00172       if (distance < 0) // bad reading
00173          continue ;
00174       if (distance <= block.danger_zone())
00175          block.add(Reading(angle, distance)) ;
00176    }
00177    //block.dump("action_lrf_update") ;
00178 }
00179 
00180 void DangerZone::update()
00181 {
00182    DangerZone& Z = instance() ;
00183 
00184    delete Z.m_lrf_data ;
00185    Z.m_lrf_data = new LRFData(m_lrf) ;
00186    std::for_each(Z.m_blocks.begin(), Z.m_blocks.end(),
00187                  Block::update(*Z.m_lrf_data));
00188 }
00189 
00190 //------------------------ DANGER ZONE QUERIES --------------------------
00191 
00192 bool DangerZone::penetrated()
00193 {
00194    const DangerZone& Z = instance() ;
00195    const int N = Z.m_blocks.size()  ;
00196    for  (int i = 0; i < N; ++i)
00197       if (Z.m_blocks[i].penetrated())
00198          return true ;
00199    return false ;
00200 }
00201 
00202 //----------------------------- CLEAN-UP --------------------------------
00203 
00204 DangerZone::~DangerZone()
00205 {
00206    delete m_lrf_data ;
00207 }
00208 
00209 //--------------------------- DEBUG SUPPORT -----------------------------
00210 
00211 void DangerZone::Block::dump(const std::string& caller) const
00212 {
00213    std::ostringstream str ;
00214 
00215    str << "from " << caller << '\n' ;
00216 
00217    str << "\n\tExtents: ["  << m_extents.min() << ' '
00218                             << m_extents.max() << "]" ;
00219    str << "\tDanger Zone: " << m_danger_zone ;
00220    str << "\tThreshold: "   << m_threshold ;
00221 
00222    str << "\n\tDanger zone readings: " ;
00223    const Readings& R = m_danger_zone_readings ; // alias to cut down on typing
00224    for (Readings::const_iterator it = R.begin(); it != R.end(); ++it)
00225       str << '(' << it->angle() << ' ' << it->distance() << ") " ;
00226    str << '\n' ;
00227 
00228    LERROR("%s", str.str().c_str()) ;
00229 }
00230 
00231 //-----------------------------------------------------------------------
00232 
00233 } // end of namespace encapsulating this file's definitions
00234 
00235 /* So things look consistent in everyone's emacs... */
00236 /* Local Variables: */
00237 /* indent-tabs-mode: nil */
00238 /* End: */
Generated on Sun May 8 08:41:30 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3