Fawkes API Fawkes Development Version
filter.cpp
1
2/***************************************************************************
3 * filter.cpp - Laser data filter interface
4 *
5 * Created: Fri Oct 10 17:12:29 2008
6 * Copyright 2006-2011 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 "filter.h"
23
24#include <core/exception.h>
25#include <utils/time/time.h>
26
27#include <cstdlib>
28#include <cstring>
29#include <limits>
30
31/** @class LaserDataFilter "filter.h"
32 * Laser data filter.
33 * With this interface laser filter are described. These filters take laser
34 * readings as input, mangle them and return a new array of filtered laser data.
35 * @author Tim Niemueller
36 *
37 * @fn void LaserDataFilter::filter() = 0
38 * Filter the incoming data.
39 * Function shall filter the data in the "in" member vector and write output
40 * to the "out" member vector.
41 */
42
43/** @var LaserDataFilter::filter_name
44 * Name of the specific filter instance.
45 */
46
47/** @var LaserDataFilter::in
48 * Vector of input arrays.
49 * Each entry in the vector is an array of data_size entries. It depends on
50 * the filter how multiple inputs are processed.
51 */
52
53/** @var LaserDataFilter::out
54 * Vector of output arrays.
55 * Each entry in the vector is an array of data_size entries. It depends on
56 * the filter how multiple outputs are generated.
57 */
58
59/** @var LaserDataFilter::in_data_size
60 * Number of entries in input arrays.
61 */
62
63/** @var LaserDataFilter::out_data_size
64 * Number of entries in output arrays.
65 */
66
67/** @class LaserDataFilter::Buffer "filter.h"
68 * Laser data buffer.
69 * A buffer comprises the value array and a reference frame ID.
70 */
71
72/** Constructor.
73 * @param filter_name name of this filter instance
74 * @param in_data_size number of entries input value arrays
75 * @param in vector of input arrays
76 * @param out_size number of value arrays to generate in out vector
77 */
78LaserDataFilter::LaserDataFilter(const std::string & filter_name,
79 unsigned int in_data_size,
80 const std::vector<Buffer *> &in,
81 unsigned int out_size)
82: filter_name(filter_name),
83 out_data_size(in_data_size), // yes, in_data_size!
84 in_data_size(in_data_size),
85 in(in)
86{
87 if (out_size > 0)
88 out.resize(out_size);
89 for (unsigned int i = 0; i < out_size; ++i) {
90 out[i] = new Buffer(out_data_size);
91 }
92
93 own_in_ = false;
94 own_out_ = true;
95}
96
97/** Virtual empty destructor. */
99{
100 if (own_in_) {
101 for (unsigned int i = 0; i < in.size(); ++i) {
102 delete in[i];
103 }
104 }
105 if (own_out_) {
106 for (unsigned int i = 0; i < out.size(); ++i) {
107 delete out[i];
108 }
109 }
110}
111
112/** Get filtered data array
113 * @return a Buffer with an array of the same size as the last array
114 * given to filter() or NULL if filter() was never called.
115 */
116std::vector<LaserDataFilter::Buffer *> &
118{
119 return out;
120}
121
122/** Set filtered data array
123 * @param out vector of output values. The vector is only accepted if it has
124 * the same size as the current one. The filter will now longer assume
125 * ownership of the arrays in the vector. Either free the memory or call
126 * set_array_ownership().
127 */
128void
129LaserDataFilter::set_out_vector(std::vector<Buffer *> &out)
130{
131 if (this->out.size() != out.size()) {
132 throw fawkes::Exception("Filter out vector size mismatch: %zu vs. %zu",
133 this->out.size(),
134 out.size());
135 }
136
137 if (own_out_) {
138 for (unsigned int i = 0; i < this->out.size(); ++i) {
139 delete this->out[i];
140 }
141 }
142 this->out.clear();
143
144 this->out = out;
145 own_out_ = false;
146}
147
148/** Resize output arrays.
149 * A side effect is that the output array size will be owned afterwards.
150 * Call this method only in constructors! Note that the output arrays are
151 * only recreated if own by the filter. If you passed an out vector you have
152 * to make sure the contained arrays fit (before calling set_out_vector()!).
153 * @param data_size number of entries in output arrays.
154 */
155void
157{
158 if (out_data_size != data_size) {
159 if (own_out_) {
160 for (unsigned int i = 0; i < out.size(); ++i) {
161 out[i]->resize(data_size);
162 }
163 }
164 }
165
166 out_data_size = data_size;
167}
168
169/** Get size of filtered data array
170 * @return size of filtered data array or 0 if filter() was never called.
171 */
172unsigned int
174{
175 return out_data_size;
176}
177
178/** Resets all readings in outbuf to NaN.
179 * @param outbuf array of out_data_size
180 */
181void
183{
184 for (unsigned int i = 0; i < out_data_size; ++i) {
185 outbuf->values[i] = std::numeric_limits<float>::quiet_NaN();
186 }
187}
188
189/** Copies the readings from inbuf to outbuf.
190 * Requires out_data_size to be equal to in_data_size.
191 * @param inbuf array of in_data_size (= out_data_size) readings
192 * @param outbuf array of out_data_size (= in_data_size) readings
193 */
194void
196 const LaserDataFilter::Buffer *inbuf)
197{
199 throw fawkes::Exception("copy_to_outbuf() requires equal "
200 "input and output data size");
201 }
202 memcpy(outbuf->values, inbuf->values, sizeof(float) * out_data_size);
203}
204
205/** Set input/output array ownership.
206 * Owned arrays will be freed on destruction or when setting new arrays.
207 * @param own_in true to assign ownership of input arrays, false otherwise
208 * @param own_out true to assign ownership of output arrays, false otherwise
209 */
210void
211LaserDataFilter::set_array_ownership(bool own_in, bool own_out)
212{
213 own_in_ = own_in;
214 own_out_ = own_out;
215}
216
217/** Constructor.
218 * @param num_values if not zero allocates the values arrays with the
219 * given number of elements
220 */
221LaserDataFilter::Buffer::Buffer(size_t num_values) : values(NULL), num_values_(num_values)
222{
223 if (num_values_ > 0) {
224 values = (float *)malloc(num_values_ * sizeof(float));
225 }
226 timestamp = new fawkes::Time(0, 0);
227}
228
229/** Copy constructor.
230 * @param other instance to copy from
231 */
233: values(NULL), timestamp(new fawkes::Time(other.timestamp))
234{
235 num_values_ = other.num_values_;
236 if (num_values_ > 0) {
237 values = (float *)malloc(num_values_ * sizeof(float));
238 memcpy(values, other.values, num_values_ * sizeof(float));
239 }
240}
241
242/** Destructor. */
244{
245 delete timestamp;
246 if (values) {
247 free(values);
248 }
249}
250
251/** Assignment operator.
252 * @param other instance to copy from
253 * @return reference to this instance
254 */
257{
258 resize(other.num_values_);
259 if (num_values_ > 0) {
260 memcpy(values, other.values, num_values_ * sizeof(float));
261 }
262 *timestamp = *other.timestamp;
263
264 return *this;
265}
266
267/** Resize buffer size.
268 * Free data array and create a new one. All values are invalidated.
269 * @param num_values if not zero allocates the values arrays with the
270 */
271void
272LaserDataFilter::Buffer::resize(unsigned int num_values)
273{
274 if (num_values != num_values_) {
275 if (values) {
276 free(values);
277 values = NULL;
278 }
279 num_values_ = num_values;
280 if (num_values_ > 0) {
281 values = (float *)malloc(num_values_ * sizeof(float));
282 }
283 }
284}
Laser data buffer.
Definition: filter.h:36
fawkes::Time * timestamp
timestamp of data
Definition: filter.h:46
void resize(unsigned int num_values)
Resize buffer size.
Definition: filter.cpp:272
Buffer & operator=(const Buffer &other)
Assignment operator.
Definition: filter.cpp:256
Buffer(size_t num_values=0)
Constructor.
Definition: filter.cpp:221
~Buffer()
Destructor.
Definition: filter.cpp:243
float * values
values
Definition: filter.h:45
void set_array_ownership(bool own_in, bool own_out)
Set input/output array ownership.
Definition: filter.cpp:211
virtual std::vector< Buffer * > & get_out_vector()
Get filtered data array.
Definition: filter.cpp:117
virtual unsigned int get_out_data_size()
Get size of filtered data array.
Definition: filter.cpp:173
LaserDataFilter(const std::string &filter_name, unsigned int in_data_size, const std::vector< Buffer * > &in, unsigned int out_size)
Constructor.
Definition: filter.cpp:78
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
void copy_to_outbuf(Buffer *outbuf, const Buffer *inbuf)
Copies the readings from inbuf to outbuf.
Definition: filter.cpp:195
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:88
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
void reset_outbuf(Buffer *b)
Resets all readings in outbuf to NaN.
Definition: filter.cpp:182
virtual ~LaserDataFilter()
Virtual empty destructor.
Definition: filter.cpp:98
virtual void set_out_data_size(unsigned int data_size)
Resize output arrays.
Definition: filter.cpp:156
virtual void set_out_vector(std::vector< Buffer * > &out)
Set filtered data array.
Definition: filter.cpp:129
Base class for exceptions in Fawkes.
Definition: exception.h:36
A class for handling time.
Definition: time.h:93
Fawkes library namespace.