Fawkes API  Fawkes Development Version
acquisition_thread.cpp
00001 
00002 /***************************************************************************
00003  *  acqusition_thread.cpp - Thread that retrieves the laser data
00004  *
00005  *  Created: Wed Oct 08 13:42:32 2008
00006  *  Copyright  2008  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "acquisition_thread.h"
00024 
00025 #include <core/threading/mutex.h>
00026 
00027 #include <limits>
00028 #include <cstring>
00029 #include <cstdlib>
00030 
00031 using namespace fawkes;
00032 
00033 /** @class LaserAcquisitionThread "acquisition_thread.h"
00034  * Laser acqusition thread.
00035  * Interface for different laser types.
00036  * @author Tim Niemueller
00037  *
00038  * @fn void LaserAcquisitionThread::pre_init(fawkes::Configuration *config, fawkes::Logger *logger) = 0;
00039  * Pre initialization.
00040  * This method is called by the sensor thread for pre-initialization. After this
00041  * method has been executed the methods get_distances_data_size() and
00042  * get_echo_data_size() must return valid data.
00043  * @param config configuration
00044  * @param logger logger instance
00045  */
00046 
00047 /** @var fawkes::Mutex * LaserAcquisitionThread::_data_mutex
00048  * Lock while writing to distances or echoes array or marking new data
00049  */
00050 
00051 /** @var bool LaserAcquisitionThread::_new_data
00052  * Set to true in your loop if new data is available. Set to false automatically
00053  * in get_distance_data() and get_echoes_data().
00054  */
00055 
00056 /** @var float * LaserAcquisitionThread::_distances
00057  * Allocate a float array and copy your distance values measured in meters here.
00058  */
00059 
00060 /** @var float * LaserAcquisitionThread::_echoes
00061  * Allocate a float array and copy your echo values here.
00062  */
00063 
00064 /** @var unsigned int LaserAcquisitionThread::_distances_size
00065  * Assign this the size of the _distances array
00066  */
00067 
00068 /** @var unsigned int LaserAcquisitionThread::_echoes_size
00069  * Assign this the size of the _echoes array
00070  */
00071 
00072 
00073 /** Constructor.
00074  * @param thread_name name of the thread, be descriptive
00075  */
00076 LaserAcquisitionThread::LaserAcquisitionThread(const char *thread_name)
00077   : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
00078 {
00079   _data_mutex = new Mutex();
00080   _new_data   = false;
00081   _distances = NULL;
00082   _echoes = NULL;
00083   _distances_size = 0;
00084   _echoes_size = 0;
00085 }
00086 
00087 LaserAcquisitionThread::~LaserAcquisitionThread()
00088 {
00089   delete _data_mutex;
00090 }
00091 
00092 
00093 /** Lock data if fresh.
00094  * If new data has been received since get_distance_data() or get_echo_data()
00095  * was called last the data is locked, no new data can arrive until you call
00096  * unlock(), otherwise the lock is immediately released after checking.
00097  * @return true if the lock was acquired and there is new data, false otherwise
00098  */
00099 bool
00100 LaserAcquisitionThread::lock_if_new_data()
00101 {
00102   _data_mutex->lock();
00103   if (_new_data) {
00104     return true;
00105   } else {
00106     _data_mutex->unlock();
00107     return false;
00108   }
00109 }
00110 
00111 
00112 /** Unlock data, */
00113 void
00114 LaserAcquisitionThread::unlock()
00115 {
00116   _data_mutex->unlock();
00117 }
00118 
00119 
00120 /** Get distance data.
00121  * @return Float array with distance values
00122  */
00123 const float *
00124 LaserAcquisitionThread::get_distance_data()
00125 {
00126   _new_data = false;
00127   return _distances;
00128 }
00129 
00130 
00131 /** Get echo data.
00132  * @return Float array with echo values
00133  */
00134 const float *
00135 LaserAcquisitionThread::get_echo_data()
00136 {
00137   _new_data = false;
00138   return _echoes;
00139 }
00140 
00141 
00142 /** Get distance data size.
00143  * @return size of data float array
00144  */
00145 unsigned int
00146 LaserAcquisitionThread::get_distance_data_size()
00147 {
00148   return _distances_size;
00149 }
00150 
00151 
00152 /** Get echo data size.
00153  * @return size of data float array
00154  */
00155 unsigned int
00156 LaserAcquisitionThread::get_echo_data_size()
00157 {
00158   return _echoes_size;
00159 }
00160 
00161 
00162 /** Allocate distances array.
00163  * Call this from a laser acqusition thread implementation to properly
00164  * initialize the distances array.
00165  * @param num_distances number of distances to allocate the array for
00166  */
00167 void
00168 LaserAcquisitionThread::alloc_distances(unsigned int num_distances)
00169 {
00170   if (_distances)  free(_distances);
00171 
00172   _distances_size = num_distances;
00173   _distances      = (float *)malloc(sizeof(float) * _distances_size);
00174   memset(_distances,
00175          std::numeric_limits<float>::quiet_NaN(),
00176          sizeof(float) * _distances_size);
00177 }
00178 
00179 
00180 /** Allocate echoes array.
00181  * Call this from a laser acqusition thread implementation to properly
00182  * initialize the echoes array.
00183  * @param num_echoes number of echoes to allocate the array for
00184  */
00185 void
00186 LaserAcquisitionThread::alloc_echoes(unsigned int num_echoes)
00187 {
00188   if (_echoes)  free(_echoes);
00189 
00190   _echoes_size = num_echoes;
00191   _echoes      = (float *)malloc(sizeof(float) * _echoes_size);
00192   memset(_echoes, 0, sizeof(float) * _echoes_size);
00193 }