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 <cstring>
00028 #include <cstdlib>
00029 
00030 using namespace fawkes;
00031 
00032 /** @class LaserAcquisitionThread "acquisition_thread.h"
00033  * Laser acqusition thread.
00034  * Interface for different laser types.
00035  * @author Tim Niemueller
00036  *
00037  * @fn void LaserAcquisitionThread::pre_init(fawkes::Configuration *config, fawkes::Logger *logger) = 0;
00038  * Pre initialization.
00039  * This method is called by the sensor thread for pre-initialization. After this
00040  * method has been executed the methods get_distances_data_size() and
00041  * get_echo_data_size() must return valid data.
00042  * @param config configuration
00043  * @param logger logger instance
00044  */
00045 
00046 /** @var fawkes::Mutex * LaserAcquisitionThread::_data_mutex
00047  * Lock while writing to distances or echoes array or marking new data
00048  */
00049 
00050 /** @var bool LaserAcquisitionThread::_new_data
00051  * Set to true in your loop if new data is available. Set to false automatically
00052  * in get_distance_data() and get_echoes_data().
00053  */
00054 
00055 /** @var float * LaserAcquisitionThread::_distances
00056  * Allocate a float array and copy your distance values measured in meters here.
00057  */
00058 
00059 /** @var float * LaserAcquisitionThread::_echoes
00060  * Allocate a float array and copy your echo values here.
00061  */
00062 
00063 /** @var unsigned int LaserAcquisitionThread::_distances_size
00064  * Assign this the size of the _distances array
00065  */
00066 
00067 /** @var unsigned int LaserAcquisitionThread::_echoes_size
00068  * Assign this the size of the _echoes array
00069  */
00070 
00071 
00072 /** Constructor.
00073  * @param thread_name name of the thread, be descriptive
00074  */
00075 LaserAcquisitionThread::LaserAcquisitionThread(const char *thread_name)
00076   : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
00077 {
00078   _data_mutex = new Mutex();
00079   _new_data   = false;
00080   _distances = NULL;
00081   _echoes = NULL;
00082   _distances_size = 0;
00083   _echoes_size = 0;
00084 }
00085 
00086 LaserAcquisitionThread::~LaserAcquisitionThread()
00087 {
00088   delete _data_mutex;
00089 }
00090 
00091 
00092 /** Lock data if fresh.
00093  * If new data has been received since get_distance_data() or get_echo_data()
00094  * was called last the data is locked, no new data can arrive until you call
00095  * unlock(), otherwise the lock is immediately released after checking.
00096  * @return true if the lock was acquired and there is new data, false otherwise
00097  */
00098 bool
00099 LaserAcquisitionThread::lock_if_new_data()
00100 {
00101   _data_mutex->lock();
00102   if (_new_data) {
00103     return true;
00104   } else {
00105     _data_mutex->unlock();
00106     return false;
00107   }
00108 }
00109 
00110 
00111 /** Unlock data, */
00112 void
00113 LaserAcquisitionThread::unlock()
00114 {
00115   _data_mutex->unlock();
00116 }
00117 
00118 
00119 /** Get distance data.
00120  * @return Float array with distance values
00121  */
00122 const float *
00123 LaserAcquisitionThread::get_distance_data()
00124 {
00125   _new_data = false;
00126   return _distances;
00127 }
00128 
00129 
00130 /** Get echo data.
00131  * @return Float array with echo values
00132  */
00133 const float *
00134 LaserAcquisitionThread::get_echo_data()
00135 {
00136   _new_data = false;
00137   return _echoes;
00138 }
00139 
00140 
00141 /** Get distance data size.
00142  * @return size of data float array
00143  */
00144 unsigned int
00145 LaserAcquisitionThread::get_distance_data_size()
00146 {
00147   return _distances_size;
00148 }
00149 
00150 
00151 /** Get echo data size.
00152  * @return size of data float array
00153  */
00154 unsigned int
00155 LaserAcquisitionThread::get_echo_data_size()
00156 {
00157   return _echoes_size;
00158 }
00159 
00160 
00161 /** Allocate distances array.
00162  * Call this from a laser acqusition thread implementation to properly
00163  * initialize the distances array.
00164  * @param num_distances number of distances to allocate the array for
00165  */
00166 void
00167 LaserAcquisitionThread::alloc_distances(unsigned int num_distances)
00168 {
00169   if (_distances)  free(_distances);
00170 
00171   _distances_size = num_distances;
00172   _distances      = (float *)malloc(sizeof(float) * _distances_size);
00173   memset(_distances, 0, sizeof(float) * _distances_size);
00174 }
00175 
00176 
00177 /** Allocate echoes array.
00178  * Call this from a laser acqusition thread implementation to properly
00179  * initialize the echoes array.
00180  * @param num_echoes number of echoes to allocate the array for
00181  */
00182 void
00183 LaserAcquisitionThread::alloc_echoes(unsigned int num_echoes)
00184 {
00185   if (_echoes)  free(_echoes);
00186 
00187   _echoes_size = num_echoes;
00188   _echoes      = (float *)malloc(sizeof(float) * _echoes_size);
00189   memset(_echoes, 0, sizeof(float) * _echoes_size);
00190 }