Fawkes API  Fawkes Development Version
filter.cpp
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 }