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-2011 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 <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 * @param in_data_size number of entries input value arrays 00052 * @param in vector of input arrays 00053 */ 00054 LaserDeadSpotsDataFilter::LaserDeadSpotsDataFilter(fawkes::Configuration *config, 00055 fawkes::Logger *logger, 00056 std::string prefix, 00057 unsigned int in_data_size, 00058 std::vector<LaserDataFilter::Buffer *> &in) 00059 : LaserDataFilter(in_data_size, in, in.size()) 00060 { 00061 __logger = logger; 00062 00063 regex_t pathre; 00064 int error = 0; 00065 if ((error = regcomp(&pathre, (prefix + "\\([^/]\\+\\)/\\(start\\|end\\)").c_str(), 0)) != 0) { 00066 size_t errsize = regerror(error, &pathre, NULL, 0); 00067 char tmp[errsize]; 00068 regerror(error, &pathre, tmp, errsize); 00069 regfree(&pathre); 00070 throw Exception("Failed to compile regular expression: %s", tmp); 00071 } 00072 00073 regmatch_t matches[2]; 00074 00075 std::list<std::string> entries; 00076 00077 Configuration::ValueIterator *vit = config->search(prefix.c_str()); 00078 while (vit->next()) { 00079 const char *path = vit->path(); 00080 if (regexec(&pathre, path, 2, matches, 0) == 0) { 00081 unsigned int match1_length = matches[1].rm_eo - matches[1].rm_so; 00082 00083 char entry[match1_length + 1]; entry[match1_length] = 0; 00084 strncpy(entry, &(path[matches[1].rm_so]), match1_length); 00085 entries.push_back(entry); 00086 } 00087 } 00088 delete vit; 00089 entries.sort(); 00090 entries.unique(); 00091 00092 __dead_spots = new unsigned int[entries.size() * 2]; 00093 00094 for (std::list<std::string>::iterator i = entries.begin(); i != entries.end(); ++i) { 00095 std::string path = prefix + *i + "/"; 00096 float start = config->get_float((path + "start").c_str()); 00097 float end = config->get_float((path + "end").c_str()); 00098 00099 __logger->log_debug("LaserDeadSpotsDataFilter", "Adding dead range [%3.3f, %3.3f] (%s)", 00100 start, end, i->c_str()); 00101 __cfg_dead_spots.push_back(std::make_pair(start, end)); 00102 } 00103 00104 __num_spots = __cfg_dead_spots.size(); 00105 00106 if (__num_spots == 0) { 00107 throw Exception("Dead spots filter enabled but no calibration data exists. Run fflaser_deadspots."); 00108 } 00109 00110 calc_spots(); 00111 } 00112 00113 LaserDeadSpotsDataFilter::~LaserDeadSpotsDataFilter() 00114 { 00115 delete __dead_spots; 00116 } 00117 00118 00119 void 00120 LaserDeadSpotsDataFilter::set_out_vector(std::vector<LaserDataFilter::Buffer *> &out) 00121 { 00122 LaserDataFilter::set_out_vector(out); 00123 calc_spots(); 00124 } 00125 00126 void 00127 LaserDeadSpotsDataFilter::calc_spots() 00128 { 00129 if (in_data_size != out_data_size) { 00130 throw Exception("Dead spots filter requires equal input and output data size"); 00131 } 00132 00133 // need to calculate new beam ranges and allocate different memory segment 00134 float angle_factor = 360.0 / in_data_size; 00135 for (unsigned int i = 0; i < __num_spots; ++i) { 00136 __dead_spots[i * 2 ] = 00137 std::min(in_data_size - 1, 00138 (unsigned int)ceilf(__cfg_dead_spots[i].first / angle_factor)); 00139 __dead_spots[i * 2 + 1] = 00140 std::min(in_data_size - 1, 00141 (unsigned int)ceilf(__cfg_dead_spots[i].second / angle_factor)); 00142 } 00143 } 00144 00145 void 00146 LaserDeadSpotsDataFilter::filter() 00147 { 00148 const unsigned int vecsize = std::min(in.size(), out.size()); 00149 for (unsigned int a = 0; a < vecsize; ++a) { 00150 out[a]->frame = in[a]->frame; 00151 float *inbuf = in[a]->values; 00152 float *outbuf = out[a]->values; 00153 00154 unsigned int start = 0; 00155 for (unsigned int i = 0; i < __num_spots; ++i) { 00156 const unsigned int spot_start = __dead_spots[i * 2 ]; 00157 const unsigned int spot_end = __dead_spots[i * 2 + 1]; 00158 for (unsigned int j = start; j < spot_start; ++j) { 00159 outbuf[j] = inbuf[j]; 00160 } 00161 for (unsigned int j = spot_start; j <= spot_end; ++j) { 00162 outbuf[j] = 0.0; 00163 } 00164 start = spot_end + 1; 00165 } 00166 for (unsigned int j = start; j < in_data_size; ++j) { 00167 outbuf[j] = inbuf[j]; 00168 } 00169 } 00170 }