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
30using 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;
87 _echoes_size = 0;
88}
89
90LaserAcquisitionThread::~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 */
102bool
104{
105 _data_mutex->lock();
106 if (_new_data) {
107 return true;
108 } else {
110 return false;
111 }
112}
113
114/** Unlock data, */
115void
117{
119}
120
121/** Get distance data.
122 * @return Float array with distance values
123 */
124const float *
126{
127 _new_data = false;
128 return _distances;
129}
130
131/** Get echo data.
132 * @return Float array with echo values
133 */
134const float *
136{
137 _new_data = false;
138 return _echoes;
139}
140
141/** Get distance data size.
142 * @return size of data float array
143 */
144unsigned int
146{
147 return _distances_size;
148}
149
150/** Get echo data size.
151 * @return size of data float array
152 */
153unsigned int
155{
156 return _echoes_size;
157}
158
159/** Get timestamp of data
160 * @return most recent data time
161 */
162const 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 */
173void
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 */
189void
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. */
201void
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;
213}
214
215/** Reset all distance values to NaN. */
216void
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.