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-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 }