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  */
78 LaserDataFilter::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  */
116 std::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  */
128 void
129 LaserDataFilter::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  */
155 void
156 LaserDataFilter::set_out_data_size(unsigned int data_size)
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  */
172 unsigned 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  */
181 void
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  */
194 void
196  const LaserDataFilter::Buffer *inbuf)
197 {
198  if (in_data_size != out_data_size) {
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  */
210 void
211 LaserDataFilter::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  */
221 LaserDataFilter::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  */
271 void
272 LaserDataFilter::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.