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 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "acquisition_thread.h"
24 
25 #include <core/threading/mutex.h>
26 
27 #include <limits>
28 #include <cstring>
29 #include <cstdlib>
30 
31 using namespace fawkes;
32 
33 /** @class LaserAcquisitionThread "acquisition_thread.h"
34  * Laser acqusition thread.
35  * Interface for different laser types.
36  * @author Tim Niemueller
37  *
38  * @fn void LaserAcquisitionThread::pre_init(fawkes::Configuration *config, fawkes::Logger *logger) = 0;
39  * Pre initialization.
40  * This method is called by the sensor thread for pre-initialization. After this
41  * method has been executed the methods get_distances_data_size() and
42  * get_echo_data_size() must return valid data.
43  * @param config configuration
44  * @param logger logger instance
45  */
46 
47 /** @var fawkes::Mutex * LaserAcquisitionThread::_data_mutex
48  * Lock while writing to distances or echoes array or marking new data
49  */
50 
51 /** @var bool LaserAcquisitionThread::_new_data
52  * Set to true in your loop if new data is available. Set to false automatically
53  * in get_distance_data() and get_echoes_data().
54  */
55 
56 /** @var float * LaserAcquisitionThread::_distances
57  * Allocate a float array and copy your distance values measured in meters here.
58  */
59 
60 /** @var float * LaserAcquisitionThread::_echoes
61  * Allocate a float array and copy your echo values here.
62  */
63 
64 /** @var unsigned int LaserAcquisitionThread::_distances_size
65  * Assign this the size of the _distances array
66  */
67 
68 /** @var unsigned int LaserAcquisitionThread::_echoes_size
69  * Assign this the size of the _echoes array
70  */
71 
72 
73 /** Constructor.
74  * @param thread_name name of the thread, be descriptive
75  */
77  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
78 {
79  _data_mutex = new Mutex();
80  _new_data = false;
81  _distances = NULL;
82  _echoes = NULL;
83  _distances_size = 0;
84  _echoes_size = 0;
85 }
86 
87 LaserAcquisitionThread::~LaserAcquisitionThread()
88 {
89  delete _data_mutex;
90 }
91 
92 
93 /** Lock data if fresh.
94  * If new data has been received since get_distance_data() or get_echo_data()
95  * was called last the data is locked, no new data can arrive until you call
96  * unlock(), otherwise the lock is immediately released after checking.
97  * @return true if the lock was acquired and there is new data, false otherwise
98  */
99 bool
101 {
102  _data_mutex->lock();
103  if (_new_data) {
104  return true;
105  } else {
106  _data_mutex->unlock();
107  return false;
108  }
109 }
110 
111 
112 /** Unlock data, */
113 void
115 {
116  _data_mutex->unlock();
117 }
118 
119 
120 /** Get distance data.
121  * @return Float array with distance values
122  */
123 const float *
125 {
126  _new_data = false;
127  return _distances;
128 }
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 
142 /** Get distance data size.
143  * @return size of data float array
144  */
145 unsigned int
147 {
148  return _distances_size;
149 }
150 
151 
152 /** Get echo data size.
153  * @return size of data float array
154  */
155 unsigned int
157 {
158  return _echoes_size;
159 }
160 
161 
162 /** Allocate distances array.
163  * Call this from a laser acqusition thread implementation to properly
164  * initialize the distances array.
165  * @param num_distances number of distances to allocate the array for
166  */
167 void
168 LaserAcquisitionThread::alloc_distances(unsigned int num_distances)
169 {
170  if (_distances) free(_distances);
171 
172  _distances_size = num_distances;
173  _distances = (float *)malloc(sizeof(float) * _distances_size);
174  memset(_distances,
175  std::numeric_limits<float>::quiet_NaN(),
176  sizeof(float) * _distances_size);
177 }
178 
179 
180 /** Allocate echoes array.
181  * Call this from a laser acqusition thread implementation to properly
182  * initialize the echoes array.
183  * @param num_echoes number of echoes to allocate the array for
184  */
185 void
186 LaserAcquisitionThread::alloc_echoes(unsigned int num_echoes)
187 {
188  if (_echoes) free(_echoes);
189 
190  _echoes_size = num_echoes;
191  _echoes = (float *)malloc(sizeof(float) * _echoes_size);
192  memset(_echoes, 0, sizeof(float) * _echoes_size);
193 }
unsigned int get_distance_data_size()
Get distance data size.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
Thread class encapsulation of pthreads.
Definition: thread.h:42
void alloc_distances(unsigned int num_distances)
Allocate distances array.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
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.
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.