LoLGMDExtricateEMD.C

Go to the documentation of this file.
00001 /**
00002    \file  Robots/LoBot/control/LoLGMDExtricateEMD.C
00003    \brief This file defines the non-inline member functions of the
00004    lobot::LGMDExtricateEMD 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/control/LoLGMDExtricateEMD.C $
00039 // $Id: LoLGMDExtricateEMD.C 13840 2010-08-27 22:28:12Z mviswana $
00040 //
00041 
00042 //------------------------------ HEADERS --------------------------------
00043 
00044 // lobot headers
00045 #include "Robots/LoBot/control/LoLGMDExtricateEMD.H"
00046 #include "Robots/LoBot/control/LoMetrics.H"
00047 #include "Robots/LoBot/control/LoTurnArbiter.H"
00048 #include "Robots/LoBot/control/LoSpinArbiter.H"
00049 #include "Robots/LoBot/control/LoSpeedArbiter.H"
00050 
00051 #include "Robots/LoBot/LoApp.H"
00052 #include "Robots/LoBot/slam/LoMap.H"
00053 #include "Robots/LoBot/config/LoConfigHelpers.H"
00054 #include "Robots/LoBot/thread/LoUpdateLock.H"
00055 
00056 #include "Robots/LoBot/misc/LoExcept.H"
00057 #include "Robots/LoBot/misc/LoRegistry.H"
00058 #include "Robots/LoBot/misc/singleton.hh"
00059 
00060 #include "Robots/LoBot/util/LoGL.H"
00061 #include "Robots/LoBot/util/LoMath.H"
00062 
00063 // OpenGL headers
00064 #ifdef INVT_HAVE_LIBGL
00065 #include <GL/gl.h>
00066 #endif
00067 
00068 // Standard C++ headers
00069 #include <iomanip>
00070 #include <sstream>
00071 #include <algorithm>
00072 #include <memory>
00073 
00074 //----------------------------- NAMESPACE -------------------------------
00075 
00076 namespace lobot {
00077 
00078 //-------------------------- KNOB TWIDDLING -----------------------------
00079 
00080 namespace {
00081 
00082 // Retrieve settings from extricate section of config file
00083 template<typename T>
00084 inline T conf(const std::string& key, const T& default_value)
00085 {
00086    return get_conf<T>(LOBE_LGMD_EXTRICATE_EMD, key, default_value) ;
00087 }
00088 
00089 /// This local class encapsulates various parameters that can be used
00090 /// to tweak different aspects of the lgmd_extricate_emd behaviour.
00091 class Params : public singleton<Params> {
00092    /// This behaviour's extrication commands will kick in only when the
00093    /// magnitude of the spiking vector computed by the EMD array exceeds
00094    /// the threshold specified by this setting.
00095    float m_threshold ;
00096 
00097    /// Users may specify what speed they would like the extrication to
00098    /// occur.
00099    ///
00100    /// CAUTION: It would be unwise to make this speed too high.
00101    float m_extricate_speed ;
00102 
00103    /// In case the RPM sensor is configured to be off, we will need to
00104    /// specify the extricate "speed" in terms of PWM values as well.
00105    ///
00106    /// NOTE: All speed related behaviours should specify both a speed in
00107    /// m/s and a PWM value.
00108    ///
00109    /// CAUTION: For the extricate behaviour, it would be unwise to make
00110    /// the extrication PWM too high.
00111    int m_extricate_pwm ;
00112 
00113    /// Usually, steering control is effected using the turn arbiter,
00114    /// which veers the robot towards different direction while it moves.
00115    /// However, the lgmd_extricate behaviour also supports spin-style
00116    /// steering, i.e., momentarily stopping the robot and then turning it
00117    /// cw/ccw in-place. This flag turns on spin-style steering. By
00118    /// default, the behaviour uses the normal steering mode.
00119    bool m_spin_style_steering ;
00120 
00121    /// The number of milliseconds between successive iterations of this
00122    /// behaviour.
00123    ///
00124    /// WARNING: The ability to change a behaviour's update frequency is a
00125    /// very powerful feature whose misuse or abuse can wreak havoc! Be
00126    /// sure to use reasonable values for this setting.
00127    int m_update_delay ;
00128 
00129    /// The location and size (within the Robolocust main window) of the
00130    /// lgmd_extricate_emd behaviour's visualization.
00131    Drawable::Geometry m_geometry ;
00132 
00133    /// Private constructor because this is a singleton.
00134    Params() ;
00135 
00136    // Boilerplate code to make generic singleton design pattern work
00137    friend class singleton<Params> ;
00138 
00139 public:
00140    /// Accessing the various parameters.
00141    //@{
00142    static float threshold()          {return instance().m_threshold ;}
00143    static float extricate_speed()    {return instance().m_extricate_speed    ;}
00144    static int   extricate_pwm()      {return instance().m_extricate_pwm      ;}
00145    static bool  spin_style_steering(){return instance().m_spin_style_steering;}
00146    static int   update_delay()       {return instance().m_update_delay       ;}
00147    static Drawable::Geometry geometry() {return instance().m_geometry        ;}
00148    //@}
00149 } ;
00150 
00151 // Parameters initialization
00152 Params::Params()
00153    : m_threshold(conf("threshold", 1000.0f)),
00154      m_extricate_speed(clamp(conf("extricate_speed", 0.15f), 0.1f, 0.75f)),
00155      m_extricate_pwm(clamp(conf("extricate_pwm", 25), 10, 50)),
00156      m_spin_style_steering(conf("spin_style_steering", false)),
00157      m_update_delay(clamp(conf("update_delay", 150), 1, 1000)),
00158      m_geometry(conf<std::string>("geometry", "0 0 10 10"))
00159 {}
00160 
00161 } // end of local anonymous namespace encapsulating above helpers
00162 
00163 //-------------------------- INITIALIZATION -----------------------------
00164 
00165 LGMDExtricateEMD::LGMDExtricateEMD()
00166    : base(Params::update_delay(), LOBE_LGMD_EXTRICATE_EMD, Params::geometry())
00167 {
00168    start(LOBE_LGMD_EXTRICATE_EMD) ;
00169 }
00170 
00171 void LGMDExtricateEMD::pre_run()
00172 {
00173    if (! App::robot())
00174       throw behavior_error(MOTOR_SYSTEM_MISSING) ;
00175 
00176    UpdateLock::begin_read() ;
00177       const App::LocustModels& L = App::locusts() ;
00178       const int N = L.size() - 1 ;
00179       if (N < 1) {
00180          UpdateLock::end_read() ;
00181          throw behavior_error(NOT_ENOUGH_LOCUSTS) ;
00182       }
00183 
00184       m_emds.reserve(N) ;
00185       for (int i = 0; i < N; ++i) {
00186          float direction = (L[i]->direction() + L[i + 1]->direction())/2.0f ;
00187          m_emds.push_back(new LMD(L[i], L[i + 1], direction)) ;
00188       }
00189    UpdateLock::end_read() ;
00190 }
00191 
00192 LGMDExtricateEMD::Command::Command()
00193    : drive(0), turn(0)
00194 {}
00195 
00196 //---------------------- THE BEHAVIOUR'S ACTION -------------------------
00197 
00198 // This version of the extricate behaviour feeds the LGMD spikes of all
00199 // the locusts into an array of elementary motion detectors and issues
00200 // appropriate turn and drive commands based on the output of the EMD
00201 // array.
00202 void LGMDExtricateEMD::action()
00203 {
00204    // Use EMD array to determine the direction in which maximal spiking
00205    // activity is currently ocurring.
00206    UpdateLock::begin_read() ;
00207       const int N = m_emds.size() ;
00208       Vector emd ;
00209       for (int i = 0; i < N; ++i)
00210          emd += m_emds[i]->update() ;
00211    UpdateLock::end_read() ;
00212 
00213    // Issue motor commands based on EMD vector's direction and magnitude
00214    Command C ;
00215    if (magnitude(emd) >= Params::threshold())
00216    {
00217       const int T = random(TurnArbiter::turn_step(), TurnArbiter::turn_max());
00218       int s = 0 ; // in-place turn amount when spin-style steering is in effect
00219       switch (octant(direction(emd)))
00220       {
00221          case 1: // obstacle in front and on the left
00222             C.drive = -1 ; C.turn =  T ; s = -T ;
00223             break ;
00224          case 2: // obstacle on the left
00225          case 3: // obstacle on the left
00226          case 4: // obstacle behind and on the left
00227             C.drive =  1 ; C.turn = -T ; s = -T ;
00228             break ;
00229          case 5: // obstacle behind and on the right
00230          case 6: // obstacle on the right
00231          case 7: // obstacle on the right
00232             C.drive =  1 ; C.turn =  T ; s = T ;
00233             break ;
00234          case 8: // obstacle in front and on the right
00235             C.drive = -1 ; C.turn = -T ; s = T ;
00236             break ;
00237          default: // hunh?!? octant() shouldn't return anything outside [1,8]
00238             throw misc_error(LOGIC_ERROR) ;
00239       }
00240 
00241       if (Params::spin_style_steering())
00242          SpinArbiter::instance().vote(base::name, new SpinArbiter::Vote(s)) ;
00243       else
00244       {
00245          SpeedArbiter::instance().vote(base::name,
00246             new SpeedArbiter::Vote(C.drive * Params::extricate_speed(),
00247                                    C.drive * Params::extricate_pwm())) ;
00248          TurnArbiter::instance().vote(base::name,
00249             new TurnArbiter::Vote(turn_vote_centered_at(C.turn))) ;
00250       }
00251 
00252       Metrics::Log log ;
00253       log << std::setw(Metrics::opw()) << std::left << base::name ;
00254       Map* M = App::map() ;
00255       if (M)
00256          log << M->current_pose() ;
00257    }
00258 
00259    // Record stuff for visualization
00260    viz_lock() ;
00261       m_emd_vector = emd ;
00262       m_cmd = C ;
00263    viz_unlock() ;
00264 }
00265 
00266 //--------------------------- VISUALIZATION -----------------------------
00267 
00268 #ifdef INVT_HAVE_LIBGL
00269 
00270 // Helper function to convert the magnitude of a vector into a string,
00271 // prefixing the supplied single character label to the mag string.
00272 static std::string mag(char label, const Vector& v)
00273 {
00274    using namespace std ;
00275 
00276    std::ostringstream v_str ;
00277    v_str << label   << ": " << fixed << setprecision(1) << magnitude(v) ;
00278    return v_str.str() ;
00279 }
00280 
00281 // The visualization callback
00282 void LGMDExtricateEMD::render_me()
00283 {
00284    // Make local copies so that extricate thread isn't held up waiting
00285    // for visualization thread to complete.
00286    viz_lock() ;
00287       Command C = m_cmd ;
00288       Vector  E = m_emd_vector ;
00289    viz_unlock() ;
00290 
00291    // Draw principal axes and diagonals marking the octants (to make it
00292    // easier to understand what's going on with the EMD vector and turn
00293    // command).
00294    unit_view_volume() ;
00295    glColor3f(1, 1, 1) ;
00296    glBegin(GL_LINES) ;
00297       glVertex2i(0, -1) ;
00298       glVertex2i(0,  1) ;
00299       glVertex2i(-1, 0) ;
00300       glVertex2i( 1, 0) ;
00301    glEnd() ;
00302 
00303    glPushAttrib(GL_LINE_BIT) ;
00304       glEnable(GL_LINE_STIPPLE) ;
00305       glLineStipple(1, 0x0F0F) ;
00306       glBegin(GL_LINES) ;
00307          glVertex2i(-1, +1) ;
00308          glVertex2i(+1, -1) ;
00309 
00310          glVertex2i(+1, +1) ;
00311          glVertex2i(-1, -1) ;
00312       glEnd() ;
00313    glPopAttrib() ;
00314 
00315    // Render the extrication decision's visualization
00316    if (C.drive == 0 && C.turn == 0)
00317       ; // no extrication took place
00318    else
00319    {
00320       glColor3f(1, 0, 0) ;
00321       draw_vector(normalized(E)) ;
00322 
00323       glColor3f(0, 1, 0) ;
00324       draw_vector(Vector(0.75f * C.drive * cos(C.turn), 0.75f * sin(C.turn))) ;
00325    }
00326 
00327    // Label the visualization so that it is easy to tell which behaviour
00328    // is being visualized. Also show the magnitudes of the forces.
00329    restore_view_volume() ;
00330    text_view_volume() ;
00331       glColor3f(0, 1, 1) ;
00332       draw_label(3, 12, "LGMD Ext. EMD") ;
00333 
00334       glColor3f(0.15f, 0.75f, 0.85f) ;
00335       draw_label(3, m_geometry.height -  4, mag('E', E).c_str()) ;
00336    restore_view_volume() ;
00337 }
00338 
00339 #endif
00340 
00341 //----------------------------- CLEAN-UP --------------------------------
00342 
00343 LGMDExtricateEMD::~LGMDExtricateEMD()
00344 {
00345    purge_container(m_emds) ;
00346 }
00347 
00348 //-----------------------------------------------------------------------
00349 
00350 } // end of namespace encapsulating this file's definitions
00351 
00352 /* So things look consistent in everyone's emacs... */
00353 /* Local Variables: */
00354 /* indent-tabs-mode: nil */
00355 /* End: */
Generated on Sun May 8 08:41:22 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3