Fawkes API Fawkes Development Version
deadspots.cpp
1
2/***************************************************************************
3 * deadspots.cpp - Laser data dead spots filter
4 *
5 * Created: Wed Jun 24 22:42:51 2009
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 "deadspots.h"
24
25#include <config/config.h>
26#include <core/exception.h>
27#include <core/macros.h>
28#include <logging/logger.h>
29#include <sys/types.h>
30#include <utils/math/angle.h>
31#include <utils/time/time.h>
32
33#include <cstdlib>
34#include <cstring>
35#include <regex.h>
36
37using namespace fawkes;
38
39/** @class LaserDeadSpotsDataFilter "filters/deadspots.h"
40 * Erase dead spots (i.e. mounting rods in the laser range) from laser data.
41 * This filter reads a number of values stored in /hardware/laser/deadspots, where
42 * each dead spot must contain two entries, a start and an end in degrees. Each
43 * entry is stored as submembers of the given tree, for example as
44 * /hardware/laser/deadspots/0/start and /hardware/laser/deadspots/0/end.
45 * @author Tim Niemueller
46 */
47
48/** Constructor.
49 * @param filter_name name of this filter instance
50 * @param config configuration instance
51 * @param logger logger for informational output
52 * @param prefix configuration prefix where to log for config information
53 * @param in_data_size number of entries input value arrays
54 * @param in vector of input arrays
55 */
58 fawkes::Logger * logger,
59 const std::string & prefix,
60 unsigned int in_data_size,
61 std::vector<LaserDataFilter::Buffer *> &in)
62: LaserDataFilter(filter_name, in_data_size, in, in.size())
63{
64 logger_ = logger;
65
66 regex_t pathre;
67 int error = 0;
68 if ((error = regcomp(&pathre, (prefix + "\\([^/]\\+\\)/\\(start\\|end\\)").c_str(), 0)) != 0) {
69 size_t errsize = regerror(error, &pathre, NULL, 0);
70 char tmp[errsize];
71 regerror(error, &pathre, tmp, errsize);
72 regfree(&pathre);
73 throw Exception("Failed to compile regular expression: %s", tmp);
74 }
75
76 regmatch_t matches[2];
77
78 std::list<std::string> entries;
79
80 Configuration::ValueIterator *vit = config->search(prefix.c_str());
81 while (vit->next()) {
82 const char *path = vit->path();
83 if (regexec(&pathre, path, 2, matches, 0) == 0) {
84 size_t match1_length = matches[1].rm_eo - matches[1].rm_so;
85
86 char entry[match1_length + 1];
87 entry[match1_length] = 0;
88 strncpy(entry, &(path[matches[1].rm_so]), match1_length);
89 entries.push_back(entry);
90 }
91 }
92 delete vit;
93 entries.sort();
94 entries.unique();
95
96 dead_spots_size_ = entries.size() * 2;
97 dead_spots_ = new unsigned int[dead_spots_size_];
98
99 for (std::list<std::string>::iterator i = entries.begin(); i != entries.end(); ++i) {
100 std::string path = prefix + *i + "/";
101 float start = config->get_float((path + "start").c_str());
102 float end = config->get_float((path + "end").c_str());
103
104 logger_->log_debug(
105 "LaserDeadSpotsDataFilter", "Adding dead range [%3.3f, %3.3f] (%s)", start, end, i->c_str());
106 cfg_dead_spots_.push_back(std::make_pair(start, end));
107 }
108
109 num_spots_ = cfg_dead_spots_.size();
110
111 if (num_spots_ == 0) {
112 throw Exception(
113 "Dead spots filter enabled but no calibration data exists. Run fflaser_deadspots.");
114 }
115
116 calc_spots();
117}
118
119/** Constructor.
120 * @param other instance to copy from
121 */
123: LaserDataFilter(other.filter_name, other.in_data_size, other.in, other.in.size()),
124 logger_(other.logger_),
125 num_spots_(other.num_spots_),
126 dead_spots_size_(other.dead_spots_size_),
127 cfg_dead_spots_(other.cfg_dead_spots_)
128{
129 dead_spots_ = new unsigned int[dead_spots_size_];
130 for (unsigned int i = 0; i < dead_spots_size_; ++i) {
131 dead_spots_[i] = other.dead_spots_[i];
132 }
133}
134
135LaserDeadSpotsDataFilter::~LaserDeadSpotsDataFilter()
136{
137 delete[] dead_spots_;
138}
139
140/** Assignment operator
141 * @param other instance to copy from
142 * @return reference to this instance
143 */
146{
147 if (&other == this)
148 return *this;
149
150 delete[] dead_spots_;
151
152 filter_name = other.filter_name;
154 in = other.in;
155 logger_ = other.logger_;
156
157 cfg_dead_spots_ = other.cfg_dead_spots_;
158 num_spots_ = other.num_spots_;
159 dead_spots_size_ = other.dead_spots_size_;
160 dead_spots_ = new unsigned int[dead_spots_size_];
161 for (unsigned int i = 0; i < dead_spots_size_; ++i) {
162 dead_spots_[i] = other.dead_spots_[i];
163 }
164
165 return *this;
166}
167
168void
169LaserDeadSpotsDataFilter::set_out_vector(std::vector<LaserDataFilter::Buffer *> &out)
170{
172 calc_spots();
173}
174
175void
176LaserDeadSpotsDataFilter::calc_spots()
177{
179 throw Exception("Dead spots filter requires equal input and output data size");
180 }
181
182 // need to calculate new beam ranges and allocate different memory segment
183 float angle_factor = 360.0 / in_data_size;
184 for (unsigned int i = 0; i < num_spots_; ++i) {
185 dead_spots_[i * 2] =
186 std::min(in_data_size - 1, (unsigned int)ceilf(cfg_dead_spots_[i].first / angle_factor));
187 dead_spots_[i * 2 + 1] =
188 std::min(in_data_size - 1, (unsigned int)ceilf(cfg_dead_spots_[i].second / angle_factor));
189 }
190}
191
192void
194{
195 const unsigned int vecsize = std::min(in.size(), out.size());
196 for (unsigned int a = 0; a < vecsize; ++a) {
197 out[a]->frame = in[a]->frame;
198 out[a]->timestamp->set_time(in[a]->timestamp);
199 float *inbuf = in[a]->values;
200 float *outbuf = out[a]->values;
201
202 unsigned int start = 0;
203 for (unsigned int i = 0; i < num_spots_; ++i) {
204 const unsigned int spot_start = dead_spots_[i * 2];
205 const unsigned int spot_end = dead_spots_[i * 2 + 1];
206 for (unsigned int j = start; j < spot_start; ++j) {
207 outbuf[j] = inbuf[j];
208 }
209 for (unsigned int j = spot_start; j <= spot_end; ++j) {
210 outbuf[j] = 0.0;
211 }
212 start = spot_end + 1;
213 }
214 for (unsigned int j = start; j < in_data_size; ++j) {
215 outbuf[j] = inbuf[j];
216 }
217 }
218}
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::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
virtual void set_out_vector(std::vector< Buffer * > &out)
Set filtered data array.
Definition: filter.cpp:129
Erase dead spots (i.e.
Definition: deadspots.h:38
void filter()
Filter the incoming data.
Definition: deadspots.cpp:193
LaserDeadSpotsDataFilter(const std::string &filter_name, fawkes::Configuration *config, fawkes::Logger *logger, const std::string &prefix, unsigned int data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: deadspots.cpp:56
LaserDeadSpotsDataFilter & operator=(const LaserDeadSpotsDataFilter &other)
Assignment operator.
Definition: deadspots.cpp:145
Iterator interface to iterate over config values.
Definition: config.h:75
virtual const char * path() const =0
Path of value.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Fawkes library namespace.