Fawkes API
Fawkes Development Version
|
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 }