Fawkes API Fawkes Development Version
min_merge.cpp
1
2/***************************************************************************
3 * min_merge.cpp - Laser min merge data filter
4 *
5 * Created: Wed Mar 16 21:46:36 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 "min_merge.h"
24
25#include <core/exception.h>
26#include <logging/logger.h>
27#include <utils/time/time.h>
28
29#include <cstring>
30
31/** @class LaserMinMergeDataFilter "min_merge.h"
32 * Merge multiple laser data arrays into one.
33 * For each value in the output array takes the minimum value of all input
34 * arrays.
35 * @author Tim Niemueller
36 */
37
38/** Constructor.
39 * @param filter_name name of this filter instance
40 * @param logger logger
41 * @param in_data_size number of entries input value arrays
42 * @param in vector of input arrays
43 */
45 fawkes::Logger * logger,
46 unsigned int in_data_size,
47 std::vector<LaserDataFilter::Buffer *> &in)
48: LaserDataFilter(filter_name, in_data_size, in, 1),
49 logger(logger),
50 timestamp_selection_method_(TIMESTAMP_LATEST),
51 timestamp_index_(0)
52{
53}
54
55/** Constructor.
56 * @param filter_name name of this filter instance
57 * @param logger logger
58 * @param in_data_size number of entries input value arrays
59 * @param in vector of input arrays
60 * @param timestamp_selection_method method to use for timestamp selection
61 * @param timestamp_index if timestamp selection method is TIMESTAMP_INDEX this
62 * is the index of the input buffer to choose the timestamp from
63 */
65 const std::string & filter_name,
66 fawkes::Logger * logger,
67 unsigned int in_data_size,
68 std::vector<LaserDataFilter::Buffer *> &in,
69 TimestampSelectionMethod timestamp_selection_method,
70 unsigned int timestamp_index)
71: LaserDataFilter(filter_name, in_data_size, in, 1),
72 logger(logger),
73 timestamp_selection_method_(timestamp_selection_method),
74 timestamp_index_(timestamp_index)
75{
76 if (timestamp_index_ >= in.size()) {
77 throw fawkes::Exception("min_merge timestamp index larger than number of input buffers");
78 }
79}
80
81void
83{
84 const unsigned int vecsize = in.size();
85 if (vecsize == 0)
86 return;
87
88 if (ignored_.size() != in.size())
89 ignored_.resize(in.size(), false);
90
91 out[0]->frame = in[0]->frame;
92
93 int first = -1;
94
95 for (unsigned int a = 0; a < vecsize; ++a) {
96 if (in[a]->frame.empty()) {
97 if (!ignored_[a]) {
98 logger->log_warn(filter_name.c_str(),
99 "input buffer %s has no frame, ignoring",
100 in[a]->name.c_str());
101 }
102 ignored_[a] = true;
103 } else {
104 if (ignored_[a]) {
105 logger->log_warn(filter_name.c_str(),
106 "input buffer %s has recovered, frame %s",
107 in[a]->name.c_str(),
108 in[a]->frame.c_str());
109 }
110 ignored_[a] = false;
111 if (first == -1)
112 first = a;
113 }
114 }
115
116 if (first == -1) {
117 throw fawkes::Exception("MinMerge[%s] has no valid input", filter_name.c_str());
118 }
119
120 copy_to_outbuf(out[0], in[first]);
121 float *outbuf = out[0]->values;
122
123 for (unsigned int a = first + 1; a < vecsize; ++a) {
124 if (ignored_[a])
125 continue;
126
127 if (in[a]->frame != out[0]->frame) {
128 throw fawkes::Exception("MinMerge[%s] frame mismatch: two frames with different frame IDs "
129 "(output has %s but input buffer %s has %s)",
130 filter_name.c_str(),
131 out[0]->frame.c_str(),
132 in[a]->name.c_str(),
133 in[a]->frame.c_str());
134 }
135 float *inbuf = in[a]->values;
136 for (unsigned int i = 0; i < (const unsigned int)out_data_size; ++i) {
137 if ((outbuf[i] == 0)
138 || ((inbuf[i] != 0)
139 && (!std::isfinite(outbuf[i])
140 || (std::isfinite(inbuf[1]) && (inbuf[i] < outbuf[i]))))) {
141 outbuf[i] = inbuf[i];
142 }
143 }
144 }
145
146 if (timestamp_selection_method_ == TIMESTAMP_FIRST) {
147 fawkes::Time first_time(in[first]->timestamp);
148 for (unsigned int a = first + 1; a < vecsize; ++a) {
149 if (ignored_[a])
150 continue;
151 if (*in[a]->timestamp < first_time) {
152 first_time = in[a]->timestamp;
153 }
154 }
155 out[0]->timestamp->set_time(first_time);
156 } else if (timestamp_selection_method_ == TIMESTAMP_INDEX) {
157 out[0]->timestamp->set_time(in[timestamp_index_]->timestamp);
158 } else { // TIMESTAMP_LATEST
159 fawkes::Time latest(in[first]->timestamp);
160 for (unsigned int a = first + 1; a < vecsize; ++a) {
161 if (ignored_[a])
162 continue;
163 if (*in[a]->timestamp > latest) {
164 latest = in[a]->timestamp;
165 }
166 }
167 out[0]->timestamp->set_time(latest);
168 }
169}
Laser data filter.
Definition: filter.h:33
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
void copy_to_outbuf(Buffer *outbuf, const Buffer *inbuf)
Copies the readings from inbuf to outbuf.
Definition: filter.cpp:195
std::string filter_name
Name of the specific filter instance.
Definition: filter.h:86
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
TimestampSelectionMethod
Timestamp selection method.
Definition: min_merge.h:38
@ TIMESTAMP_FIRST
use the first (oldest) of all timestamps
Definition: min_merge.h:40
@ TIMESTAMP_INDEX
use a specific index in the input buffer list
Definition: min_merge.h:41
LaserMinMergeDataFilter(const std::string &filter_name, fawkes::Logger *logger, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: min_merge.cpp:44
virtual void filter()
Filter the incoming data.
Definition: min_merge.cpp:82
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
A class for handling time.
Definition: time.h:93