00001 00002 /*************************************************************************** 00003 * reverse_angle.h - Reverse the angle in which laser data is taken 00004 * 00005 * Created: Wed Jan 06 17:15:38 2010 00006 * Copyright 2006-2010 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 "reverse_angle.h" 00024 00025 #include <core/exception.h> 00026 #include <utils/math/angle.h> 00027 #include <cstdlib> 00028 00029 /** @class LaserReverseAngleDataFilter "reverse_angle.h" 00030 * Reverse the angle of beams. 00031 * This filter will reverse the direction in which the beams are stored. 00032 * If the original interface stores the data in clockwise direction, the 00033 * outcome will be in counter-clockwise direction and vice versa. This is 00034 * required for example to convert between the (clockwise) RCSoftX angles, 00035 * and the (counter-clockwise) angles in the Fawkes coordinate system. 00036 * @author Tim Niemueller 00037 */ 00038 00039 /** Constructor. 00040 * @param data_size Data size of the input. Output size will be the same. The 00041 * input data must have the exact same data size. 00042 */ 00043 LaserReverseAngleDataFilter::LaserReverseAngleDataFilter(unsigned int data_size) 00044 { 00045 _filtered_data = (float *)malloc(sizeof(float) * data_size); 00046 _filtered_data_size = data_size; 00047 } 00048 00049 void 00050 LaserReverseAngleDataFilter::filter(const float *data, unsigned int data_size) 00051 { 00052 if ( data_size != _filtered_data_size ) { 00053 throw fawkes::Exception("Expected %u values, but got %u", 00054 _filtered_data_size, data_size); 00055 } 00056 00057 for (unsigned int i = 0; i < _filtered_data_size; ++i) { 00058 _filtered_data[i] = data[data_size - i]; 00059 } 00060 }