Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * filter.cpp - Laser data filter interface 00004 * 00005 * Created: Fri Oct 10 17:12:29 2008 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include "filter.h" 00023 #include <core/exception.h> 00024 00025 #include <cstring> 00026 #include <cstdlib> 00027 00028 /** @class LaserDataFilter "filter.h" 00029 * Laser data filter. 00030 * With this interface laser filter are described. These filters take laser 00031 * readings as input, mangle them and return a new array of filtered laser data. 00032 * @author Tim Niemueller 00033 * 00034 * @fn void LaserDataFilter::filter() = 0 00035 * Filter the incoming data. 00036 * Function shall filter the data in the "in" member vector and write output 00037 * to the "out" member vector. 00038 */ 00039 00040 /** @var LaserDataFilter::in 00041 * Vector of input arrays. 00042 * Each entry in the vector is an array of data_size entries. It depends on 00043 * the filter how multiple inputs are processed. 00044 */ 00045 00046 /** @var LaserDataFilter::out 00047 * Vector of output arrays. 00048 * Each entry in the vector is an array of data_size entries. It depends on 00049 * the filter how multiple outputs are generated. 00050 */ 00051 00052 /** @var LaserDataFilter::in_data_size 00053 * Number of entries in input arrays. 00054 */ 00055 00056 /** @var LaserDataFilter::out_data_size 00057 * Number of entries in output arrays. 00058 */ 00059 00060 /** @class LaserDataFilter::Buffer "filter.h" 00061 * Laser data buffer. 00062 * A buffer comprises the value array and a reference frame ID. 00063 */ 00064 00065 /** Constructor. 00066 * @param in_data_size number of entries input value arrays 00067 * @param in vector of input arrays 00068 * @param out_size number of value arrays to generate in out vector 00069 */ 00070 LaserDataFilter::LaserDataFilter(unsigned int in_data_size, 00071 std::vector<Buffer *> &in, unsigned int out_size) 00072 { 00073 this->in = in; 00074 this->in_data_size = in_data_size; 00075 this->out_data_size = in_data_size; // yes, in_data_size! 00076 00077 if (out_size > 0) out.resize(out_size); 00078 for (unsigned int i = 0; i < out_size; ++i) { 00079 out[i] = new Buffer(out_data_size); 00080 } 00081 00082 __own_in = false; 00083 __own_out = true; 00084 } 00085 00086 00087 /** Virtual empty destructor. */ 00088 LaserDataFilter::~LaserDataFilter() 00089 { 00090 if (__own_in) { 00091 for (unsigned int i = 0; i < in.size(); ++i) { 00092 free(in[i]->values); 00093 delete in[i]; 00094 } 00095 } 00096 if (__own_out) { 00097 for (unsigned int i = 0; i < out.size(); ++i) { 00098 free(out[i]->values); 00099 delete out[i]; 00100 } 00101 } 00102 } 00103 00104 00105 /** Get filtered data array 00106 * @return a Buffer with an array of the same size as the last array 00107 * given to filter() or NULL if filter() was never called. 00108 */ 00109 std::vector<LaserDataFilter::Buffer *> & 00110 LaserDataFilter::get_out_vector() 00111 { 00112 return out; 00113 } 00114 00115 00116 /** Set filtered data array 00117 * @param out vector of output values. The vector is only accepted if it has 00118 * the same size as the current one. The filter will now longer assume 00119 * ownership of the arrays in the vector. Either free the memory or call 00120 * set_array_ownership(). 00121 */ 00122 void 00123 LaserDataFilter::set_out_vector(std::vector<Buffer *> &out) 00124 { 00125 if (this->out.size() != out.size()) { 00126 throw fawkes::Exception("Filter out vector size mismatch: %zu vs. %zu", 00127 this->out.size(), out.size()); 00128 } 00129 00130 if (__own_out) { 00131 for (unsigned int i = 0; i < this->out.size(); ++i) { 00132 free(this->out[i]->values); 00133 delete this->out[i]; 00134 } 00135 } 00136 this->out.clear(); 00137 00138 this->out = out; 00139 __own_out = false; 00140 } 00141 00142 00143 /** Resize output arrays. 00144 * A side effect is that the output array size will be owned afterwards. 00145 * Call this method only in constructors! Note that the output arrays are 00146 * only recreated if own by the filter. If you passed an out vector you have 00147 * to make sure the contained arrays fit (before calling set_out_vector()!). 00148 * @param data_size number of entries in output arrays. 00149 */ 00150 void 00151 LaserDataFilter::set_out_data_size(unsigned int data_size) 00152 { 00153 if (out_data_size != data_size) { 00154 if (__own_out) { 00155 for (unsigned int i = 0; i < out.size(); ++i) { 00156 free(out[i]->values); 00157 out[i]->values = (float *)malloc(data_size * sizeof(float)); 00158 } 00159 } 00160 } 00161 00162 out_data_size = data_size; 00163 } 00164 00165 00166 /** Get size of filtered data array 00167 * @return size of filtered data array or 0 if filter() was never called. 00168 */ 00169 unsigned int 00170 LaserDataFilter::get_out_data_size() 00171 { 00172 return out_data_size; 00173 } 00174 00175 00176 /** Resets all readings in outbuf to 0.0 00177 * @param outbuf array of out_data_size 00178 */ 00179 void 00180 LaserDataFilter::reset_outbuf(Buffer *outbuf) 00181 { 00182 memset(outbuf->values, 0, sizeof(float) * out_data_size); 00183 } 00184 00185 /** Copies the readings from inbuf to outbuf. 00186 * Requires out_data_size to be equal to in_data_size. 00187 * @param inbuf array of in_data_size (= out_data_size) readings 00188 * @param outbuf array of out_data_size (= in_data_size) readings 00189 */ 00190 void 00191 LaserDataFilter::copy_to_outbuf(LaserDataFilter::Buffer *outbuf, 00192 const LaserDataFilter::Buffer *inbuf) 00193 { 00194 if (in_data_size != out_data_size) { 00195 throw fawkes::Exception("copy_to_outbuf() requires equal "\ 00196 "input and output data size"); 00197 } 00198 memcpy(outbuf->values, inbuf->values, sizeof(float) * out_data_size); 00199 } 00200 00201 00202 /** Set input/output array ownership. 00203 * Owned arrays will be freed on destruction or when setting new arrays. 00204 * @param own_in true to assign ownership of input arrays, false otherwise 00205 * @param own_out true to assign ownership of output arrays, false otherwise 00206 */ 00207 void 00208 LaserDataFilter::set_array_ownership(bool own_in, bool own_out) 00209 { 00210 __own_in = own_in; 00211 __own_out = own_out; 00212 } 00213 00214 00215 /** Constructor. 00216 * @param num_values if not zero allocates the values arrays with the 00217 * given number of elements 00218 */ 00219 LaserDataFilter::Buffer::Buffer(size_t num_values) 00220 { 00221 if (num_values > 0) { 00222 values = (float *)malloc(num_values * sizeof(float)); 00223 } 00224 }