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 }