Fawkes API  Fawkes Development Version
max_circle.cpp
1 
2 /***************************************************************************
3  * max_circle.cpp - Laser data circle data filter (example)
4  *
5  * Created: Fri Oct 10 17:16:57 2008
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "max_circle.h"
24 
25 #include <utils/math/angle.h>
26 #include <utils/time/time.h>
27 
28 #include <cstdlib>
29 
30 /** @class LaserMaxCircleDataFilter "circle.h"
31  * Cut of laser data at max distance.
32  * All beams longer than a given radius are cut of at the maximum length.
33  * @author Tim Niemueller
34  */
35 
36 /** Constructor.
37  * @param filter_name name of this filter instance
38  * @param radius radius of cut-off circle in meters
39  * @param in_data_size number of entries input value arrays
40  * @param in vector of input arrays
41  */
43  float radius,
44  unsigned int in_data_size,
45  std::vector<LaserDataFilter::Buffer *> &in)
46 : LaserDataFilter(filter_name, in_data_size, in, in.size())
47 {
48  radius_ = radius;
49 }
50 
51 void
53 {
54  const unsigned int vecsize = std::min(in.size(), out.size());
55  const unsigned int arrsize = std::min(in_data_size, out_data_size);
56  for (unsigned int a = 0; a < vecsize; ++a) {
57  out[a]->frame = in[a]->frame;
58  out[a]->timestamp->set_time(in[a]->timestamp);
59  float *inbuf = in[a]->values;
60  float *outbuf = out[a]->values;
61  for (unsigned int i = 0; i < arrsize; ++i) {
62  if (inbuf[i] > radius_) {
63  outbuf[i] = radius_;
64  } else {
65  outbuf[i] = inbuf[i];
66  }
67  }
68  }
69 }
LaserDataFilter::in_data_size
unsigned int in_data_size
Definition: filter.h:88
LaserDataFilter::in
std::vector< Buffer * > in
Definition: filter.h:89
LaserDataFilter::out_data_size
unsigned int out_data_size
Definition: filter.h:87
LaserMaxCircleDataFilter::LaserMaxCircleDataFilter
LaserMaxCircleDataFilter(const std::string &filter_name, float radius, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: max_circle.cpp:42
LaserDataFilter
Definition: filter.h:32
LaserDataFilter::out
std::vector< Buffer * > out
Definition: filter.h:90
LaserMaxCircleDataFilter::filter
void filter()
Definition: max_circle.cpp:52