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