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 <cstdlib>
27 #include <cstring>
28 #include <limits>
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 /** Constructor.
76  * @param thread_name name of the thread, be descriptive
77  */
79 : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
80 {
81  _data_mutex = new Mutex();
82  _timestamp = new Time();
83  _new_data = false;
84  _distances = NULL;
85  _echoes = NULL;
86  _distances_size = 0;
87  _echoes_size = 0;
88 }
89 
90 LaserAcquisitionThread::~LaserAcquisitionThread()
91 {
92  delete _data_mutex;
93  delete _timestamp;
94 }
95 
96 /** Lock data if fresh.
97  * If new data has been received since get_distance_data() or get_echo_data()
98  * was called last the data is locked, no new data can arrive until you call
99  * unlock(), otherwise the lock is immediately released after checking.
100  * @return true if the lock was acquired and there is new data, false otherwise
101  */
102 bool
104 {
105  _data_mutex->lock();
106  if (_new_data) {
107  return true;
108  } else {
109  _data_mutex->unlock();
110  return false;
111  }
112 }
113 
114 /** Unlock data, */
115 void
117 {
118  _data_mutex->unlock();
119 }
120 
121 /** Get distance data.
122  * @return Float array with distance values
123  */
124 const float *
126 {
127  _new_data = false;
128  return _distances;
129 }
130 
131 /** Get echo data.
132  * @return Float array with echo values
133  */
134 const float *
136 {
137  _new_data = false;
138  return _echoes;
139 }
140 
141 /** Get distance data size.
142  * @return size of data float array
143  */
144 unsigned int
146 {
147  return _distances_size;
148 }
149 
150 /** Get echo data size.
151  * @return size of data float array
152  */
153 unsigned int
155 {
156  return _echoes_size;
157 }
158 
159 /** Get timestamp of data
160  * @return most recent data time
161  */
162 const fawkes::Time *
164 {
165  return _timestamp;
166 }
167 
168 /** Allocate distances array.
169  * Call this from a laser acqusition thread implementation to properly
170  * initialize the distances array.
171  * @param num_distances number of distances to allocate the array for
172  */
173 void
174 LaserAcquisitionThread::alloc_distances(unsigned int num_distances)
175 {
176  if (_distances)
177  free(_distances);
178 
179  _distances_size = num_distances;
180  _distances = (float *)malloc(sizeof(float) * _distances_size);
181  std::fill_n(_distances, _distances_size, std::numeric_limits<float>::quiet_NaN());
182 }
183 
184 /** Allocate echoes array.
185  * Call this from a laser acqusition thread implementation to properly
186  * initialize the echoes array.
187  * @param num_echoes number of echoes to allocate the array for
188  */
189 void
190 LaserAcquisitionThread::alloc_echoes(unsigned int num_echoes)
191 {
192  if (_echoes)
193  free(_echoes);
194 
195  _echoes_size = num_echoes;
196  _echoes = (float *)malloc(sizeof(float) * _echoes_size);
197  memset(_echoes, 0, sizeof(float) * _echoes_size);
198 }
199 
200 /** Reset all distance values to NaN. */
201 void
203 {
204  _data_mutex->lock();
205  if (!_distances)
206  return;
207 
208  for (size_t i = 0; i < _distances_size; ++i) {
209  _distances[i] = std::numeric_limits<float>::quiet_NaN();
210  }
211  _new_data = true;
212  _data_mutex->unlock();
213 }
214 
215 /** Reset all distance values to NaN. */
216 void
218 {
219  if (!_echoes)
220  return;
221 
222  for (size_t i = 0; i < _echoes_size; ++i) {
223  _echoes[i] = std::numeric_limits<float>::quiet_NaN();
224  }
225 }
const float * get_echo_data()
Get echo data.
unsigned int get_echo_data_size()
Get echo data size.
const fawkes::Time * get_timestamp()
Get timestamp of data.
LaserAcquisitionThread(const char *thread_name)
Constructor.
void alloc_echoes(unsigned int num_echoes)
Allocate echoes array.
unsigned int _echoes_size
Assign this the size of the _echoes array.
const float * get_distance_data()
Get distance data.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
bool _new_data
Set to true in your loop if new data is available.
void alloc_distances(unsigned int num_distances)
Allocate distances array.
void reset_distances()
Reset all distance values to NaN.
void unlock()
Unlock data,.
unsigned int _distances_size
Assign this the size of the _distances array.
float * _echoes
Allocate a float array and copy your echo values here.
unsigned int get_distance_data_size()
Get distance data size.
fawkes::Time * _timestamp
Time when the most recent data was received.
bool lock_if_new_data()
Lock data if fresh.
void reset_echoes()
Reset all distance values to NaN.
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Fawkes library namespace.