Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * deadspots.cpp - Laser data dead spots filter 00004 * 00005 * Created: Wed Jun 24 22:42:51 2009 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 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 /** @class LaserDeadSpotsDataFilter "filters/deadspots.h" 00039 * Erase dead spots (i.e. mounting rods in the laser range) from laser data. 00040 * This filter reads a number of values stored in /hardware/laser/deadspots, where 00041 * each dead spot must contain two entries, a start and an end in degrees. Each 00042 * entry is stored as submembers of the given tree, for example as 00043 * /hardware/laser/deadspots/0/start and /hardware/laser/deadspots/0/end. 00044 * @author Tim Niemueller 00045 */ 00046 00047 /** Constructor. 00048 * @param config configuration instance 00049 * @param logger logger for informational output 00050 * @param prefix configuration prefix where to log for config information 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 // need to calculate new beam ranges and allocate different memory segment 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 }