23 #include "min_merge.h" 25 #include <core/exception.h> 26 #include <utils/time/time.h> 27 #include <logging/logger.h> 45 unsigned int in_data_size,
46 std::vector<LaserDataFilter::Buffer *> &in)
49 timestamp_selection_method_(TIMESTAMP_LATEST)
65 std::vector<LaserDataFilter::Buffer *> &
in,
67 unsigned int timestamp_index)
70 timestamp_selection_method_(timestamp_selection_method),
71 timestamp_index_(timestamp_index)
73 if (timestamp_index_ >= in.size()) {
74 throw fawkes::Exception(
"min_merge timestamp index larger than number of input buffers");
82 const unsigned int vecsize =
in.size();
83 if (vecsize == 0)
return;
85 if (ignored_.size() !=
in.size()) ignored_.resize(
in.size(),
false);
87 out[0]->frame =
in[0]->frame;
91 for (
unsigned int a = 0; a < vecsize; ++a) {
92 if (
in[a]->frame.empty()) {
100 in[a]->name.c_str(),
in[a]->frame.c_str());
103 if (first == -1) first = a;
112 float *outbuf =
out[0]->values;
114 for (
unsigned int a = first + 1; a < vecsize; ++a) {
115 if (ignored_[a])
continue;
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)",
121 in[a]->name.c_str(),
in[a]->frame.c_str());
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];
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;
139 out[0]->timestamp->set_time(first_time);
141 out[0]->timestamp->set_time(
in[timestamp_index_]->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;
150 out[0]->timestamp->set_time(latest);
std::vector< Buffer * > out
Vector of output arrays.
A class for handling time.
Base class for exceptions in Fawkes.
virtual void filter()
Filter the incoming data.
void copy_to_outbuf(Buffer *outbuf, const Buffer *inbuf)
Copies the readings from inbuf to outbuf.
TimestampSelectionMethod
Timestamp selection method.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
use a specific index in the input buffer list
const std::string filter_name
Name of the specific filter instance.
LaserMinMergeDataFilter(const std::string filter_name, fawkes::Logger *logger, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer *> &in)
Constructor.
use the first (oldest) of all timestamps
unsigned int out_data_size
Number of entries in output arrays.
std::vector< Buffer * > in
Vector of input arrays.
unsigned int in_data_size
Number of entries in input arrays.