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 
37 using 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  */
57  fawkes::Configuration *config,
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  unsigned int 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 
135 LaserDeadSpotsDataFilter::~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;
153  in_data_size = other.in_data_size;
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 
168 void
169 LaserDeadSpotsDataFilter::set_out_vector(std::vector<LaserDataFilter::Buffer *> &out)
170 {
172  calc_spots();
173 }
174 
175 void
176 LaserDeadSpotsDataFilter::calc_spots()
177 {
178  if (in_data_size != out_data_size) {
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 
192 void
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 }
LaserDataFilter::filter_name
std::string filter_name
Definition: filter.h:86
LaserDeadSpotsDataFilter::operator=
LaserDeadSpotsDataFilter & operator=(const LaserDeadSpotsDataFilter &other)
Assignment operator.
Definition: deadspots.cpp:145
LaserDataFilter::in_data_size
unsigned int in_data_size
Definition: filter.h:88
LaserDataFilter::in
std::vector< Buffer * > in
Definition: filter.h:89
LaserDeadSpotsDataFilter
Definition: deadspots.h:37
fawkes::Configuration::ValueIterator
Definition: config.h:77
fawkes::Configuration
Definition: config.h:70
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
fawkes::Logger
Definition: logger.h:41
fawkes
LaserDataFilter::out_data_size
unsigned int out_data_size
Definition: filter.h:87
LaserDeadSpotsDataFilter::filter
void filter()
Definition: deadspots.cpp:193
LaserDataFilter::set_out_vector
virtual void set_out_vector(std::vector< Buffer * > &out)
Set filtered data array.
Definition: filter.cpp:129
LaserDataFilter
Definition: filter.h:32
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
LaserDeadSpotsDataFilter::LaserDeadSpotsDataFilter
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
fawkes::Configuration::ValueIterator::path
virtual const char * path() const =0
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
LaserDataFilter::out
std::vector< Buffer * > out
Definition: filter.h:90
fawkes::Configuration::ValueIterator::next
virtual bool next()=0
fawkes::Exception
Definition: exception.h:41