Fawkes API Fawkes Development Version
circle_sector.cpp
1
2/***************************************************************************
3 * circle_sector.cpp - Filter laser data for circle sector
4 *
5 * Created: Sat Feb 19 00:28:41 2011
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 "circle_sector.h"
24
25#include <core/exception.h>
26#include <utils/math/angle.h>
27#include <utils/time/time.h>
28
29#include <algorithm>
30#include <cstring>
31
32using namespace fawkes;
33
34/** @class LaserCircleSectorDataFilter "circle.h"
35 * Erase beams outside specified circle sector.
36 * Only data inside the specified circle sector is copied, all other data is
37 * set to zero.
38 * @author Tim Niemueller
39 */
40
41/** Constructor.
42 * @param filter_name name of this filter instance
43 * @param from start angle (index in data)
44 * @param to end angle (index in data)
45 * @param in_data_size number of entries in value arrays
46 * @param in vector of input arrays
47 */
49 unsigned int from,
50 unsigned int to,
51 unsigned int in_data_size,
52 std::vector<LaserDataFilter::Buffer *> &in)
53: LaserDataFilter(filter_name, in_data_size, in, in.size())
54{
55 from_ = from;
56 to_ = to;
57}
58
59void
61{
62 const unsigned int vecsize = std::min(in.size(), out.size());
63 const unsigned int arrsize = std::min(in_data_size, out_data_size);
64 for (unsigned int a = 0; a < vecsize; ++a) {
65 reset_outbuf(out[a]);
66 out[a]->frame = in[a]->frame;
67 out[a]->timestamp->set_time(in[a]->timestamp);
68
69 float *inbuf = in[a]->values;
70 float *outbuf = out[a]->values;
71
72 if (from_ > to_) {
73 for (unsigned int i = from_; i < arrsize; ++i) {
74 outbuf[i] = inbuf[i];
75 }
76 for (unsigned int i = 0; i <= std::min(to_, arrsize - 1); ++i) {
77 outbuf[i] = inbuf[i];
78 }
79 } else {
80 for (unsigned int i = from_; i <= std::min(to_, arrsize - 1); ++i) {
81 outbuf[i] = inbuf[i];
82 }
83 }
84 }
85}
LaserCircleSectorDataFilter(const std::string &filter_name, unsigned int from, unsigned int to, unsigned int data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
void filter()
Filter the incoming data.
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 reset_outbuf(Buffer *b)
Resets all readings in outbuf to NaN.
Definition: filter.cpp:182
Fawkes library namespace.