23 #include "deadspots.h" 25 #include <core/exception.h> 26 #include <core/macros.h> 27 #include <utils/math/angle.h> 28 #include <utils/time/time.h> 29 #include <logging/logger.h> 30 #include <config/config.h> 34 #include <sys/types.h> 60 unsigned int in_data_size,
61 std::vector<LaserDataFilter::Buffer *> &in)
68 if ((error = regcomp(&pathre, (prefix +
"\\([^/]\\+\\)/\\(start\\|end\\)").c_str(), 0)) != 0) {
69 size_t errsize = regerror(error, &pathre, NULL, 0);
71 regerror(error, &pathre, tmp, errsize);
73 throw Exception(
"Failed to compile regular expression: %s", tmp);
76 regmatch_t matches[2];
78 std::list<std::string> entries;
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;
86 char entry[match1_length + 1]; entry[match1_length] = 0;
87 strncpy(entry, &(path[matches[1].rm_so]), match1_length);
88 entries.push_back(entry);
95 __dead_spots =
new unsigned int[entries.size() * 2];
97 for (std::list<std::string>::iterator i = entries.begin(); i != entries.end(); ++i) {
98 std::string path = prefix + *i +
"/";
99 float start = config->
get_float((path +
"start").c_str());
100 float end = config->
get_float((path +
"end").c_str());
102 __logger->
log_debug(
"LaserDeadSpotsDataFilter",
"Adding dead range [%3.3f, %3.3f] (%s)",
103 start, end, i->c_str());
104 __cfg_dead_spots.push_back(std::make_pair(start, end));
107 __num_spots = __cfg_dead_spots.size();
109 if (__num_spots == 0) {
110 throw Exception(
"Dead spots filter enabled but no calibration data exists. Run fflaser_deadspots.");
116 LaserDeadSpotsDataFilter::~LaserDeadSpotsDataFilter()
123 LaserDeadSpotsDataFilter::set_out_vector(std::vector<LaserDataFilter::Buffer *> &
out)
130 LaserDeadSpotsDataFilter::calc_spots()
133 throw Exception(
"Dead spots filter requires equal input and output data size");
138 for (
unsigned int i = 0; i < __num_spots; ++i) {
139 __dead_spots[i * 2 ] =
141 (
unsigned int)ceilf(__cfg_dead_spots[i].first / angle_factor));
142 __dead_spots[i * 2 + 1] =
144 (
unsigned int)ceilf(__cfg_dead_spots[i].second / angle_factor));
151 const unsigned int vecsize = std::min(
in.size(), out.size());
152 for (
unsigned int a = 0; a < vecsize; ++a) {
153 out[a]->frame =
in[a]->frame;
154 out[a]->timestamp->set_time(
in[a]->timestamp);
155 float *inbuf =
in[a]->values;
156 float *outbuf = out[a]->values;
158 unsigned int start = 0;
159 for (
unsigned int i = 0; i < __num_spots; ++i) {
160 const unsigned int spot_start = __dead_spots[i * 2 ];
161 const unsigned int spot_end = __dead_spots[i * 2 + 1];
162 for (
unsigned int j = start; j < spot_start; ++j) {
163 outbuf[j] = inbuf[j];
165 for (
unsigned int j = spot_start; j <= spot_end; ++j) {
168 start = spot_end + 1;
171 outbuf[j] = inbuf[j];
std::vector< Buffer * > out
Vector of output arrays.
Fawkes library namespace.
LaserDeadSpotsDataFilter(const std::string filter_name, fawkes::Configuration *config, fawkes::Logger *logger, std::string prefix, unsigned int data_size, std::vector< LaserDataFilter::Buffer *> &in)
Constructor.
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.
Base class for exceptions in Fawkes.
void filter()
Filter the incoming data.
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.
unsigned int out_data_size
Number of entries in output arrays.
virtual void set_out_vector(std::vector< Buffer *> &out)
Set filtered data array.
Interface for configuration handling.
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.
unsigned int in_data_size
Number of entries in input arrays.