Fawkes API  Fawkes Development Version
acquisition_thread.cpp
1 
2 /***************************************************************************
3  * acqusition_thread.cpp - Thread that retrieves the laser data
4  *
5  * Created: Wed Oct 08 13:42:32 2008
6  * Copyright 2008-2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "acquisition_thread.h"
23 
24 #include <core/threading/mutex.h>
25 
26 #include <limits>
27 #include <cstring>
28 #include <cstdlib>
29 
30 using namespace fawkes;
31 
32 /** @class LaserAcquisitionThread "acquisition_thread.h"
33  * Laser acqusition thread.
34  * Interface for different laser types.
35  * @author Tim Niemueller
36  *
37  * @fn void LaserAcquisitionThread::pre_init(fawkes::Configuration *config, fawkes::Logger *logger) = 0;
38  * Pre initialization.
39  * This method is called by the sensor thread for pre-initialization. After this
40  * method has been executed the methods get_distances_data_size() and
41  * get_echo_data_size() must return valid data.
42  * @param config configuration
43  * @param logger logger instance
44  */
45 
46 /** @var fawkes::Mutex * LaserAcquisitionThread::_data_mutex
47  * Lock while writing to distances or echoes array or marking new data
48  */
49 
50 /** @var bool LaserAcquisitionThread::_new_data
51  * Set to true in your loop if new data is available. Set to false automatically
52  * in get_distance_data() and get_echoes_data().
53  */
54 
55 /** @var float * LaserAcquisitionThread::_distances
56  * Allocate a float array and copy your distance values measured in meters here.
57  */
58 
59 /** @var float * LaserAcquisitionThread::_echoes
60  * Allocate a float array and copy your echo values here.
61  */
62 
63 /** @var unsigned int LaserAcquisitionThread::_distances_size
64  * Assign this the size of the _distances array
65  */
66 
67 /** @var unsigned int LaserAcquisitionThread::_echoes_size
68  * Assign this the size of the _echoes array
69  */
70 
71 /** @var fawkes::Time * LaserAcquisitionThread::_timestamp
72  * Time when the most recent data was received.
73  */
74 
75 
76 /** Constructor.
77  * @param thread_name name of the thread, be descriptive
78  */
80  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
81 {
82  _data_mutex = new Mutex();
83  _timestamp = new Time();
84  _new_data = false;
85  _distances = NULL;
86  _echoes = NULL;
87  _distances_size = 0;
88  _echoes_size = 0;
89 }
90 
91 LaserAcquisitionThread::~LaserAcquisitionThread()
92 {
93  delete _data_mutex;
94  delete _timestamp;
95 }
96 
97 
98 /** Lock data if fresh.
99  * If new data has been received since get_distance_data() or get_echo_data()
100  * was called last the data is locked, no new data can arrive until you call
101  * unlock(), otherwise the lock is immediately released after checking.
102  * @return true if the lock was acquired and there is new data, false otherwise
103  */
104 bool
106 {
107  _data_mutex->lock();
108  if (_new_data) {
109  return true;
110  } else {
111  _data_mutex->unlock();
112  return false;
113  }
114 }
115 
116 
117 /** Unlock data, */
118 void
120 {
121  _data_mutex->unlock();
122 }
123 
124 
125 /** Get distance data.
126  * @return Float array with distance values
127  */
128 const float *
130 {
131  _new_data = false;
132  return _distances;
133 }
134 
135 
136 /** Get echo data.
137  * @return Float array with echo values
138  */
139 const float *
141 {
142  _new_data = false;
143  return _echoes;
144 }
145 
146 
147 /** Get distance data size.
148  * @return size of data float array
149  */
150 unsigned int
152 {
153  return _distances_size;
154 }
155 
156 
157 /** Get echo data size.
158  * @return size of data float array
159  */
160 unsigned int
162 {
163  return _echoes_size;
164 }
165 
166 
167 /** Get timestamp of data
168  * @return most recent data time
169  */
170 const fawkes::Time *
172 {
173  return _timestamp;
174 }
175 
176 
177 /** Allocate distances array.
178  * Call this from a laser acqusition thread implementation to properly
179  * initialize the distances array.
180  * @param num_distances number of distances to allocate the array for
181  */
182 void
183 LaserAcquisitionThread::alloc_distances(unsigned int num_distances)
184 {
185  if (_distances) free(_distances);
186 
187  _distances_size = num_distances;
188  _distances = (float *)malloc(sizeof(float) * _distances_size);
189  std::fill_n(_distances, _distances_size,
190  std::numeric_limits<float>::quiet_NaN());
191 }
192 
193 
194 /** Allocate echoes array.
195  * Call this from a laser acqusition thread implementation to properly
196  * initialize the echoes array.
197  * @param num_echoes number of echoes to allocate the array for
198  */
199 void
200 LaserAcquisitionThread::alloc_echoes(unsigned int num_echoes)
201 {
202  if (_echoes) free(_echoes);
203 
204  _echoes_size = num_echoes;
205  _echoes = (float *)malloc(sizeof(float) * _echoes_size);
206  memset(_echoes, 0, sizeof(float) * _echoes_size);
207 }
208 
209 
210 /** Reset all distance values to NaN. */
211 void
213 {
214  _data_mutex->lock();
215  if (! _distances) return;
216 
217  for (size_t i = 0; i < _distances_size; ++i) {
218  _distances[i] = std::numeric_limits<float>::quiet_NaN();
219  }
220  _new_data = true;
221  _data_mutex->unlock();
222 }
223 
224 
225 /** Reset all distance values to NaN. */
226 void
228 {
229  if (! _echoes) return;
230 
231  for (size_t i = 0; i < _echoes_size; ++i) {
232  _echoes[i] = std::numeric_limits<float>::quiet_NaN();
233  }
234 }
unsigned int get_distance_data_size()
Get distance data size.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
void reset_distances()
Reset all distance values to NaN.
void alloc_distances(unsigned int num_distances)
Allocate distances array.
fawkes::Time * _timestamp
Time when the most recent data was received.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
void reset_echoes()
Reset all distance values to NaN.
unsigned int get_echo_data_size()
Get echo data size.
unsigned int _distances_size
Assign this the size of the _distances array.
LaserAcquisitionThread(const char *thread_name)
Constructor.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const float * get_distance_data()
Get distance data.
unsigned int _echoes_size
Assign this the size of the _echoes array.
void unlock()
Unlock data,.
bool _new_data
Set to true in your loop if new data is available.
const fawkes::Time * get_timestamp()
Get timestamp of data.
void alloc_echoes(unsigned int num_echoes)
Allocate echoes array.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
Mutex mutual exclusion lock.
Definition: mutex.h:32
bool lock_if_new_data()
Lock data if fresh.
const float * get_echo_data()
Get echo data.
float * _echoes
Allocate a float array and copy your echo values here.