LoTrack.C

Go to the documentation of this file.
00001 /**
00002    \file  Robots/LoBot/control/LoTrack.C
00003    \brief This file defines the non-inline member functions of the
00004    lobot::Track 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/LoTrack.C $
00039 // $Id: LoTrack.C 13619 2010-06-25 01:59:32Z mviswana $
00040 //
00041 
00042 //------------------------------ HEADERS --------------------------------
00043 
00044 // lobot headers
00045 #include "Robots/LoBot/control/LoTrack.H"
00046 #include "Robots/LoBot/control/LoMetrics.H"
00047 
00048 #include "Robots/LoBot/LoApp.H"
00049 #include "Robots/LoBot/slam/LoMap.H"
00050 #include "Robots/LoBot/slam/LoSlamParams.H"
00051 #include "Robots/LoBot/io/LoRobot.H"
00052 
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 <algorithm>
00071 #include <vector>
00072 #include <iterator>
00073 
00074 //----------------------------- NAMESPACE -------------------------------
00075 
00076 namespace lobot {
00077 
00078 //-------------------------- KNOB TWIDDLING -----------------------------
00079 
00080 namespace {
00081 
00082 // Retrieve settings from track 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_TRACK, key, default_value) ;
00087 }
00088 
00089 /// This local class encapsulates various parameters that can be used to
00090 /// tweak different aspects of the track behaviour.
00091 class Params : public singleton<Params> {
00092    /// How much of the pose history should we draw?
00093    int m_history_size ;
00094 
00095    /// The number of milliseconds between successive iterations of this
00096    /// behaviour.
00097    ///
00098    /// WARNING: The ability to change a behaviour's update frequency is a
00099    /// very powerful feature whose misuse or abuse can wreak havoc! Be
00100    /// sure to use reasonable values for this setting.
00101    int m_update_delay ;
00102 
00103    /// Private constructor because this is a singleton.
00104    Params() ;
00105    friend class singleton<Params> ;
00106 
00107 public:
00108    /// Accessing the various parameters
00109    //@{
00110    static int history_size() {return instance().m_history_size ;}
00111    static int update_delay() {return instance().m_update_delay ;}
00112    //@}
00113 } ;
00114 
00115 // Parameter initialization
00116 Params::Params()
00117    : m_history_size(visualize(LOBE_TRACK)
00118                        ? clamp(conf("history_size", 25), 3, 100) : 0),
00119      m_update_delay(clamp(conf("update_delay", 1000), 500, 2500))
00120 {}
00121 
00122 } // end of local anonymous namespace encapsulating above helpers
00123 
00124 //-------------------------- INITIALIZATION -----------------------------
00125 
00126 Track::Track()
00127    : base(Params::update_delay())
00128 {
00129    start(LOBE_TRACK) ;
00130 }
00131 
00132 void Track::pre_run()
00133 {
00134    if (! App::robot())
00135       throw behavior_error(MOTOR_SYSTEM_MISSING) ;
00136 
00137    Map* map = App::map() ;
00138    if (! map)
00139       throw behavior_error(MAPPING_DISABLED) ;
00140    map->add_pose_hook(Map::PoseHook(add_pose,
00141                                     reinterpret_cast<unsigned long>(this))) ;
00142    if (visualize(LOBE_TRACK))
00143       map->add_hook(RenderHook(
00144          render_history, reinterpret_cast<unsigned long>(this))) ;
00145 }
00146 
00147 //---------------------- THE BEHAVIOUR'S ACTION -------------------------
00148 
00149 // Add latest pose to pose history for visualization. Also send latest
00150 // pose to metrics log.
00151 //
00152 // NOTE: This function is executed by the thread that performs the pose
00153 // update (e.g., the survey behaviour).
00154 void Track::add_pose(const Pose& p, unsigned long client_data)
00155 {
00156    using namespace std ;
00157    Metrics::Log() << setw(Metrics::opw()) << left << "tracking pose " << p ;
00158 
00159    Track* T = reinterpret_cast<Track*>(client_data) ;
00160    T->viz_lock() ;
00161       T->m_history.push_front(p) ;
00162       if (static_cast<int>(T->m_history.size()) > Params::history_size())
00163          T->m_history.pop_back() ;
00164    T->viz_unlock() ;
00165 }
00166 
00167 // All of the action takes place in add_pose(), viz., metrics logging,
00168 // and render_history(). But we still use this method to periodically log
00169 // the robot's current speed.
00170 void Track::action()
00171 {
00172    UpdateLock::begin_read() ;
00173       float speed = App::robot()->current_speed() ;
00174    UpdateLock::end_read() ;
00175 
00176    using namespace std ;
00177    Metrics::Log() << setw(Metrics::opw()) << left << "tracking speed "
00178                   << setprecision(3)  << fixed << speed << " m/s" ;
00179 }
00180 
00181 //--------------------------- VISUALIZATION -----------------------------
00182 
00183 #ifdef INVT_HAVE_LIBGL
00184 
00185 // Helper to generate vertices for the trail joining successive poses.
00186 //
00187 // NOTE: Since the GL coordinate system is rotated ccw by 90 degrees to
00188 // make the robot's notion of "up" coincide with ours, we have to swap
00189 // the x and y coordinates of all vertices.
00190 static void render_pose(const Pose& p)
00191 {
00192    glVertex2f(p.y(), p.x()) ;
00193 }
00194 
00195 // Callback for rendering the pose history on the Robolocust map
00196 void Track::render_history(unsigned long client_data)
00197 {
00198    // Copy history to avoid holding up pose updating threads
00199    Track* track = reinterpret_cast<Track*>(client_data) ;
00200    track->viz_lock() ;
00201       std::vector<Pose> history(track->m_history.begin(),
00202                                 track->m_history.end()) ;
00203    track->viz_unlock() ;
00204    if (history.empty())
00205       return ;
00206 
00207    // Setup 2D "view volume" to match real/physical coordinate system
00208    // except that the whole thing is rotated 90 degrees ccw so that our
00209    // notion of "up" matches that of the robot's.
00210    float L, R, B, T ; // map extents
00211    SlamParams::map_extents(&L, &R, &B, &T) ;
00212    track->setup_view_volume(T, B, L, R) ;
00213 
00214    // Now we're ready to draw the pose history...
00215    glPushAttrib(GL_POINT_BIT | GL_LINE_BIT) ;
00216    glEnable(GL_LINE_STIPPLE) ;
00217    glLineStipple(1, 0xAAAA) ;
00218    glColor3f(0, 0, 0) ;
00219    glBegin(GL_LINE_STRIP) ;
00220       std::for_each(history.begin(), history.end(), render_pose) ;
00221    glEnd() ;
00222    glPopAttrib() ;
00223 
00224    // Reset GL transformations so next drawable won't get screwed
00225    track->restore_view_volume() ;
00226 }
00227 
00228 #endif
00229 
00230 //----------------------------- CLEAN-UP --------------------------------
00231 
00232 Track::~Track(){}
00233 
00234 //-----------------------------------------------------------------------
00235 
00236 } // end of namespace encapsulating this file's definitions
00237 
00238 /* So things look consistent in everyone's emacs... */
00239 /* Local Variables: */
00240 /* indent-tabs-mode: nil */
00241 /* End: */
Generated on Sun May 8 08:41:23 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3