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 #include <core/exception.h>
24 #include <utils/time/time.h>
25 
26 #include <cstring>
27 #include <cstdlib>
28 #include <limits>
29 
30 /** @class LaserDataFilter "filter.h"
31  * Laser data filter.
32  * With this interface laser filter are described. These filters take laser
33  * readings as input, mangle them and return a new array of filtered laser data.
34  * @author Tim Niemueller
35  *
36  * @fn void LaserDataFilter::filter() = 0
37  * Filter the incoming data.
38  * Function shall filter the data in the "in" member vector and write output
39  * to the "out" member vector.
40  */
41 
42 /** @var LaserDataFilter::filter_name
43  * Name of the specific filter instance.
44  */
45 
46 /** @var LaserDataFilter::in
47  * Vector of input arrays.
48  * Each entry in the vector is an array of data_size entries. It depends on
49  * the filter how multiple inputs are processed.
50  */
51 
52 /** @var LaserDataFilter::out
53  * Vector of output arrays.
54  * Each entry in the vector is an array of data_size entries. It depends on
55  * the filter how multiple outputs are generated.
56  */
57 
58 /** @var LaserDataFilter::in_data_size
59  * Number of entries in input arrays.
60  */
61 
62 /** @var LaserDataFilter::out_data_size
63  * Number of entries in output arrays.
64  */
65 
66 /** @class LaserDataFilter::Buffer "filter.h"
67  * Laser data buffer.
68  * A buffer comprises the value array and a reference frame ID.
69  */
70 
71 /** Constructor.
72  * @param filter_name name of this filter instance
73  * @param in_data_size number of entries input value arrays
74  * @param in vector of input arrays
75  * @param out_size number of value arrays to generate in out vector
76  */
77 LaserDataFilter::LaserDataFilter(const std::string filter_name,
78  unsigned int in_data_size,
79  std::vector<Buffer *> &in, unsigned int out_size)
80  : filter_name(filter_name), out_data_size(in_data_size), // yes, in_data_size!
81  in_data_size(in_data_size), in(in)
82 {
83  if (out_size > 0) out.resize(out_size);
84  for (unsigned int i = 0; i < out_size; ++i) {
85  out[i] = new Buffer(out_data_size);
86  }
87 
88  __own_in = false;
89  __own_out = true;
90 }
91 
92 
93 /** Virtual empty destructor. */
95 {
96  if (__own_in) {
97  for (unsigned int i = 0; i < in.size(); ++i) {
98  free(in[i]->values);
99  delete in[i];
100  }
101  }
102  if (__own_out) {
103  for (unsigned int i = 0; i < out.size(); ++i) {
104  free(out[i]->values);
105  delete out[i];
106  }
107  }
108 }
109 
110 
111 /** Get filtered data array
112  * @return a Buffer with an array of the same size as the last array
113  * given to filter() or NULL if filter() was never called.
114  */
115 std::vector<LaserDataFilter::Buffer *> &
117 {
118  return out;
119 }
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(), out.size());
134  }
135 
136  if (__own_out) {
137  for (unsigned int i = 0; i < this->out.size(); ++i) {
138  free(this->out[i]->values);
139  delete this->out[i];
140  }
141  }
142  this->out.clear();
143 
144  this->out = out;
145  __own_out = false;
146 }
147 
148 
149 /** Resize output arrays.
150  * A side effect is that the output array size will be owned afterwards.
151  * Call this method only in constructors! Note that the output arrays are
152  * only recreated if own by the filter. If you passed an out vector you have
153  * to make sure the contained arrays fit (before calling set_out_vector()!).
154  * @param data_size number of entries in output arrays.
155  */
156 void
157 LaserDataFilter::set_out_data_size(unsigned int data_size)
158 {
159  if (out_data_size != data_size) {
160  if (__own_out) {
161  for (unsigned int i = 0; i < out.size(); ++i) {
162  free(out[i]->values);
163  out[i]->values = (float *)malloc(data_size * sizeof(float));
164  }
165  }
166  }
167 
168  out_data_size = data_size;
169 }
170 
171 
172 /** Get size of filtered data array
173  * @return size of filtered data array or 0 if filter() was never called.
174  */
175 unsigned int
177 {
178  return out_data_size;
179 }
180 
181 
182 /** Resets all readings in outbuf to NaN.
183  * @param outbuf array of out_data_size
184  */
185 void
187 {
188  for (unsigned int i = 0; i < out_data_size; ++i) {
189  outbuf->values[i] = std::numeric_limits<float>::quiet_NaN();
190  }
191 }
192 
193 /** Copies the readings from inbuf to outbuf.
194  * Requires out_data_size to be equal to in_data_size.
195  * @param inbuf array of in_data_size (= out_data_size) readings
196  * @param outbuf array of out_data_size (= in_data_size) readings
197  */
198 void
200  const LaserDataFilter::Buffer *inbuf)
201 {
202  if (in_data_size != out_data_size) {
203  throw fawkes::Exception("copy_to_outbuf() requires equal "\
204  "input and output data size");
205  }
206  memcpy(outbuf->values, inbuf->values, sizeof(float) * out_data_size);
207 }
208 
209 
210 /** Set input/output array ownership.
211  * Owned arrays will be freed on destruction or when setting new arrays.
212  * @param own_in true to assign ownership of input arrays, false otherwise
213  * @param own_out true to assign ownership of output arrays, false otherwise
214  */
215 void
216 LaserDataFilter::set_array_ownership(bool own_in, bool own_out)
217 {
218  __own_in = own_in;
219  __own_out = own_out;
220 }
221 
222 
223 /** Constructor.
224  * @param num_values if not zero allocates the values arrays with the
225  * given number of elements
226  */
228 {
229  if (num_values > 0) {
230  values = (float *)malloc(num_values * sizeof(float));
231  }
232  timestamp = new fawkes::Time(0,0);
233 }
234 
235 
236 /** Destructor. */
238 {
239  delete timestamp;
240 }
LaserDataFilter(const std::string filter_name, unsigned int in_data_size, std::vector< Buffer *> &in, unsigned int out_size)
Constructor.
Definition: filter.cpp:77
float * values
values
Definition: filter.h:41
void set_array_ownership(bool own_in, bool own_out)
Set input/output array ownership.
Definition: filter.cpp:216
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:76
A class for handling time.
Definition: time.h:91
~Buffer()
Destructor.
Definition: filter.cpp:237
virtual unsigned int get_out_data_size()
Get size of filtered data array.
Definition: filter.cpp:176
Base class for exceptions in Fawkes.
Definition: exception.h:36
void copy_to_outbuf(Buffer *outbuf, const Buffer *inbuf)
Copies the readings from inbuf to outbuf.
Definition: filter.cpp:199
Laser data buffer.
Definition: filter.h:35
Buffer(size_t num_values=0)
Constructor.
Definition: filter.cpp:227
virtual void set_out_data_size(unsigned int data_size)
Resize output arrays.
Definition: filter.cpp:157
virtual ~LaserDataFilter()
Virtual empty destructor.
Definition: filter.cpp:94
void reset_outbuf(Buffer *b)
Resets all readings in outbuf to NaN.
Definition: filter.cpp:186
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:73
virtual void set_out_vector(std::vector< Buffer *> &out)
Set filtered data array.
Definition: filter.cpp:129
virtual std::vector< Buffer * > & get_out_vector()
Get filtered data array.
Definition: filter.cpp:116
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:75
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:74