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