Fawkes API Fawkes Development Version
1080to360.cpp
1
2/***************************************************************************
3 * 1080to360.cpp - Laser data data filter to downsample 1080 to 360 values
4 *
5 * Created: Mon Jun 01 16:11:39 2015
6 * Copyright 2006-2015 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 "1080to360.h"
23
24#include <core/exception.h>
25#include <utils/math/angle.h>
26#include <utils/time/time.h>
27
28#include <cstdlib>
29
30/** @class Laser1080to360DataFilter "1080to360.h"
31 * Downsample filter from 1080 to 360 values.
32 * @author Tim Niemueller
33 */
34
35/** Constructor.
36 * @param filter_name name of this filter instance
37 * @param average if true, beams will be averaged by left and right neighbours,
38 * otherwise every second beam will be used
39 * @param in_data_size number of entries input value arrays
40 * @param in vector of input arrays
41 */
43 bool average,
44 unsigned int in_data_size,
45 std::vector<LaserDataFilter::Buffer *> &in)
46: LaserDataFilter(filter_name, in_data_size, in, in.size())
47{
48 if (in_data_size != 1080) {
49 throw fawkes::Exception("1080to360 filter needs input array size of "
50 "1080 entries");
51 }
53 average_ = average;
54}
55
56void
58{
59 const unsigned int vecsize = std::min(in.size(), out.size());
60 for (unsigned int a = 0; a < vecsize; ++a) {
61 out[a]->frame = in[a]->frame;
62 out[a]->timestamp->set_time(in[a]->timestamp);
63 float *inbuf = in[a]->values;
64 float *outbuf = out[a]->values;
65
66 if (average_) {
67 for (unsigned int i = 0; i < 360; ++i) {
68 outbuf[i] = (inbuf[i * 3] + inbuf[i * 2 + 1] + inbuf[i * 2 + 2]) / 2.0;
69 }
70 } else {
71 for (unsigned int i = 0; i < 360; ++i) {
72 outbuf[i] = inbuf[i * 3 + 1];
73 }
74 }
75 }
76}
Laser1080to360DataFilter(const std::string &filter_name, bool average, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: 1080to360.cpp:42
void filter()
Filter the incoming data.
Definition: 1080to360.cpp:57
Laser data filter.
Definition: filter.h:33
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
virtual void set_out_data_size(unsigned int data_size)
Resize output arrays.
Definition: filter.cpp:156
Base class for exceptions in Fawkes.
Definition: exception.h:36