Fawkes API  Fawkes Development Version
deadspots.cpp
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 }