Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
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
*/
76
LaserAcquisitionThread::LaserAcquisitionThread
(
const
char
*thread_name)
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
100
LaserAcquisitionThread::lock_if_new_data
()
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
114
LaserAcquisitionThread::unlock
()
115
{
116
_data_mutex
->
unlock
();
117
}
118
119
120
/** Get distance data.
121
* @return Float array with distance values
122
*/
123
const
float
*
124
LaserAcquisitionThread::get_distance_data
()
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
*
135
LaserAcquisitionThread::get_echo_data
()
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
146
LaserAcquisitionThread::get_distance_data_size
()
147
{
148
return
_distances_size
;
149
}
150
151
152
/** Get echo data size.
153
* @return size of data float array
154
*/
155
unsigned
int
156
LaserAcquisitionThread::get_echo_data_size
()
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
}
src
plugins
laser
acquisition_thread.cpp
Generated by
1.8.3.1