BeoSubDepthSensor.C

Go to the documentation of this file.
00001 /*!@file BeoSub/BeoSubDepthSensor.C class for interfacing with a depth sensor */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2003   //
00005 // by the 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: Laurent Itti <itti@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/BeoSub/BeoSubDepthSensor.C $
00035 // $Id: BeoSubDepthSensor.C 4679 2005-06-24 04:59:53Z rjpeters $
00036 //
00037 
00038 #include "BeoSub/BeoSubDepthSensor.H"
00039 #include <cstdio>
00040 #include <sstream>
00041 #include <string>
00042 #include <unistd.h>
00043 
00044 void *Sensor_run(void *c);
00045 // ######################################################################
00046 void *Sensor_run(void *c)
00047 {
00048   BeoSubDepthSensor *d = (BeoSubDepthSensor *)c;
00049   d->run();
00050   return NULL;
00051 }
00052 
00053 // ######################################################################
00054 BeoSubDepthSensor::BeoSubDepthSensor(OptionManager& mgr,
00055                                      const std::string& descrName,
00056                                      const std::string& tagName,
00057                                      const char *dev) :
00058   ModelComponent(mgr, descrName, tagName),
00059   serial(new Serial(mgr)),
00060   keepgoing(true), depth(0.0f)
00061 {
00062   serial->configure(dev, 9600, "8N1");
00063   addSubComponent(serial);
00064   pthread_mutex_init(&lock, NULL);
00065 }
00066 
00067 // ######################################################################
00068 void BeoSubDepthSensor::start2()
00069 { pthread_create(&runner, NULL, &Sensor_run, (void *)this); }
00070 
00071 // ######################################################################
00072 void BeoSubDepthSensor::stop1()
00073 {
00074   keepgoing = false;
00075   usleep(300000); // make sure thread has exited
00076 }
00077 
00078 // ######################################################################
00079 BeoSubDepthSensor::~BeoSubDepthSensor()
00080 { pthread_mutex_destroy(&lock); }
00081 
00082 // ######################################################################
00083 void BeoSubDepthSensor::run()
00084 {
00085   unsigned char c = 255;
00086   while(keepgoing)
00087     {
00088       // skip to next CR:
00089       while(c != '\r') c = serial->read();
00090 
00091       // get data until next CR:
00092       std::string str;
00093       while( (c = serial->read() ) != '\r')
00094         if (isdigit(c) || c == '.' || c == '-') str += c;
00095         else str += ' ';
00096 
00097       // convert to floats:
00098       std::stringstream strs(str);
00099       int b1, b2, b3; strs >> b1 >> b2 >> b3;
00100       float d = float(b1) + (float(b2) + float(b3) / 256.0F) / 256.0F;
00101 
00102       pthread_mutex_lock(&lock);
00103       depth = d;
00104       pthread_mutex_unlock(&lock);
00105     }
00106 
00107   pthread_exit(0);
00108 }
00109 
00110 // ######################################################################
00111 float BeoSubDepthSensor::get()
00112 {
00113   volatile float val;
00114   pthread_mutex_lock(&lock);
00115   val = depth;
00116   pthread_mutex_unlock(&lock);
00117   return val;
00118 }
00119 
00120 // ######################################################################
00121 /* So things look consistent in everyone's emacs... */
00122 /* Local Variables: */
00123 /* indent-tabs-mode: nil */
00124 /* End: */
Generated on Sun May 8 08:40:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3