Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * circle.cpp - Laser data circle data filter (example) 00004 * 00005 * Created: Fri Oct 10 17:16:57 2008 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 "circle.h" 00024 00025 #include <utils/math/angle.h> 00026 #include <cstdlib> 00027 00028 /** @class LaserCircleDataFilter "circle.h" 00029 * Demonstration laser data filter. 00030 * Cuts off all beams that are longer than a given radius. 00031 * @author Tim Niemueller 00032 */ 00033 00034 /** Constructor. 00035 * @param radius radius of cut-off circle in meters 00036 */ 00037 LaserCircleDataFilter::LaserCircleDataFilter(float radius) 00038 { 00039 __radius = radius; 00040 } 00041 00042 void 00043 LaserCircleDataFilter::filter(const float *data, unsigned int data_size) 00044 { 00045 if ( _filtered_data_size != data_size ) { 00046 if (_filtered_data) free(_filtered_data); 00047 _filtered_data = (float *)malloc(sizeof(float) * data_size); 00048 _filtered_data_size = data_size; 00049 } 00050 00051 for (unsigned int i = 0; i < data_size; ++i) { 00052 if (data[i] > __radius) { 00053 _filtered_data[i] = __radius; 00054 } else { 00055 _filtered_data[i] = data[i]; 00056 } 00057 } 00058 }