23 #include "deadspots.h"
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>
33 #include <sys/types.h>
36 using namespace fawkes;
57 unsigned int in_data_size,
58 std::vector<LaserDataFilter::Buffer *> &in)
65 if ((error = regcomp(&pathre, (prefix +
"\\([^/]\\+\\)/\\(start\\|end\\)").c_str(), 0)) != 0) {
66 size_t errsize = regerror(error, &pathre, NULL, 0);
68 regerror(error, &pathre, tmp, errsize);
70 throw Exception(
"Failed to compile regular expression: %s", tmp);
73 regmatch_t matches[2];
75 std::list<std::string> entries;
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;
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);
92 __dead_spots =
new unsigned int[entries.size() * 2];
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());
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));
104 __num_spots = __cfg_dead_spots.size();
106 if (__num_spots == 0) {
107 throw Exception(
"Dead spots filter enabled but no calibration data exists. Run fflaser_deadspots.");
113 LaserDeadSpotsDataFilter::~LaserDeadSpotsDataFilter()
120 LaserDeadSpotsDataFilter::set_out_vector(std::vector<LaserDataFilter::Buffer *> &out)
127 LaserDeadSpotsDataFilter::calc_spots()
130 throw Exception(
"Dead spots filter requires equal input and output data size");
135 for (
unsigned int i = 0; i < __num_spots; ++i) {
136 __dead_spots[i * 2 ] =
138 (
unsigned int)ceilf(__cfg_dead_spots[i].first / angle_factor));
139 __dead_spots[i * 2 + 1] =
141 (
unsigned int)ceilf(__cfg_dead_spots[i].second / angle_factor));
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;
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];
161 for (
unsigned int j = spot_start; j <= spot_end; ++j) {
164 start = spot_end + 1;
167 outbuf[j] = inbuf[j];