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
51void
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}
Laser data filter.
Definition: filter.h:33
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
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 filter()
Filter the incoming data.
Definition: max_circle.cpp:52
LaserMaxCircleDataFilter(const std::string &filter_name, float radius, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: max_circle.cpp:42