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  */
44 LaserMinMergeDataFilter::LaserMinMergeDataFilter(const std::string &filter_name,
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 
81 void
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 }
LaserDataFilter::copy_to_outbuf
void copy_to_outbuf(Buffer *outbuf, const Buffer *inbuf)
Copies the readings from inbuf to outbuf.
Definition: filter.cpp:195
LaserDataFilter::filter_name
std::string filter_name
Name of the specific filter instance.
Definition: filter.h:86
LaserMinMergeDataFilter::TIMESTAMP_INDEX
@ TIMESTAMP_INDEX
use a specific index in the input buffer list
Definition: min_merge.h:41
LaserDataFilter::in
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
LaserMinMergeDataFilter::TimestampSelectionMethod
TimestampSelectionMethod
Timestamp selection method.
Definition: min_merge.h:38
LaserMinMergeDataFilter::filter
virtual void filter()
Filter the incoming data.
Definition: min_merge.cpp:82
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
LaserDataFilter::out_data_size
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
LaserMinMergeDataFilter::TIMESTAMP_FIRST
@ TIMESTAMP_FIRST
use the first (oldest) of all timestamps
Definition: min_merge.h:40
LaserMinMergeDataFilter::LaserMinMergeDataFilter
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
fawkes::Time
A class for handling time.
Definition: time.h:93
LaserDataFilter
Laser data filter.
Definition: filter.h:33
LaserDataFilter::out
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36