deadspots.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "deadspots.h"
00024
00025 #include <core/exception.h>
00026 #include <core/macros.h>
00027 #include <utils/math/angle.h>
00028 #include <utils/logging/logger.h>
00029 #include <config/config.h>
00030
00031 #include <cstdlib>
00032 #include <cstring>
00033 #include <sys/types.h>
00034 #include <regex.h>
00035
00036 using namespace fawkes;
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 LaserDeadSpotsDataFilter::LaserDeadSpotsDataFilter(fawkes::Configuration *config,
00053 fawkes::Logger *logger,
00054 std::string prefix)
00055 {
00056 __logger = logger;
00057
00058 regex_t pathre;
00059 int error = 0;
00060 if ((error = regcomp(&pathre, (prefix + "\\([^/]\\+\\)/\\(start\\|end\\)").c_str(), 0)) != 0) {
00061 size_t errsize = regerror(error, &pathre, NULL, 0);
00062 char tmp[errsize];
00063 regerror(error, &pathre, tmp, errsize);
00064 regfree(&pathre);
00065 throw Exception("Failed to compile regular expression: %s", tmp);
00066 }
00067
00068 regmatch_t matches[2];
00069
00070 std::list<std::string> entries;
00071
00072 Configuration::ValueIterator *vit = config->search(prefix.c_str());
00073 while (vit->next()) {
00074 const char *path = vit->path();
00075 if (regexec(&pathre, path, 2, matches, 0) == 0) {
00076 unsigned int match1_length = matches[1].rm_eo - matches[1].rm_so;
00077
00078 char entry[match1_length + 1]; entry[match1_length] = 0;
00079 strncpy(entry, &(path[matches[1].rm_so]), match1_length);
00080 entries.push_back(entry);
00081 }
00082 }
00083 delete vit;
00084 entries.sort();
00085 entries.unique();
00086
00087 __dead_spots = new unsigned int[entries.size() * 2];
00088
00089 for (std::list<std::string>::iterator i = entries.begin(); i != entries.end(); ++i) {
00090 std::string path = prefix + *i + "/";
00091 float start = config->get_float((path + "start").c_str());
00092 float end = config->get_float((path + "end").c_str());
00093
00094 __logger->log_debug("LaserDeadSpotsDataFilter", "Adding dead range [%3.3f, %3.3f] (%s)",
00095 start, end, i->c_str());
00096 __cfg_dead_spots.push_back(std::make_pair(start, end));
00097 }
00098
00099 __num_spots = __cfg_dead_spots.size();
00100
00101 if (__num_spots == 0) {
00102 throw Exception("Dead spots filter enabled but no calibration data exists. Run fflaser_deadspots.");
00103 }
00104 }
00105
00106 LaserDeadSpotsDataFilter::~LaserDeadSpotsDataFilter()
00107 {
00108 delete __dead_spots;
00109 }
00110
00111
00112 void
00113 LaserDeadSpotsDataFilter::filter(const float *data, unsigned int data_size)
00114 {
00115 if (unlikely(_filtered_data_size != data_size)) {
00116
00117 float angle_factor = 360.0 / data_size;
00118 for (unsigned int i = 0; i < __num_spots; ++i) {
00119 __dead_spots[i * 2 ] = std::min(data_size - 1, (unsigned int)ceilf(__cfg_dead_spots[i].first / angle_factor));
00120 __dead_spots[i * 2 + 1] = std::min(data_size - 1, (unsigned int)ceilf(__cfg_dead_spots[i].second / angle_factor));
00121 }
00122 if (_filtered_data) free(_filtered_data);
00123 _filtered_data = (float *)malloc(sizeof(float) * data_size);
00124 _filtered_data_size = data_size;
00125 }
00126
00127 unsigned int start = 0;
00128 for (unsigned int i = 0; i < __num_spots; ++i) {
00129 const unsigned int spot_start = __dead_spots[i * 2 ];
00130 const unsigned int spot_end = __dead_spots[i * 2 + 1];
00131 for (unsigned int j = start; j < spot_start; ++j) {
00132 _filtered_data[j] = data[j];
00133 }
00134 for (unsigned int j = spot_start; j <= spot_end; ++j) {
00135 _filtered_data[j] = 0.0;
00136 }
00137 start = spot_end + 1;
00138 }
00139 for (unsigned int j = start; j < data_size; ++j) {
00140 _filtered_data[j] = data[j];
00141 }
00142 }