00001 00002 /*************************************************************************** 00003 * laser_mapper.cpp - Mapper LaserProxy to LaserInterface 00004 * 00005 * Created: Tue Oct 21 00:50:26 2008 00006 * Copyright 2006-2008 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 "laser_mapper.h" 00024 00025 #include <core/exceptions/software.h> 00026 #include <utils/math/angle.h> 00027 #include <interfaces/Laser360Interface.h> 00028 #include <libplayerc++/playerc++.h> 00029 00030 /** @class PlayerLaserMapper "laser_mapper.h" 00031 * Laser mapper for player integration. 00032 * This class is used to map a Player lsaer proxy to a Fawkes 00033 * Laser360Interface. 00034 * @author Tim Niemueller 00035 */ 00036 00037 /** Constructor. 00038 * @param varname variable name 00039 * @param interface Fawkes interface instance 00040 * @param proxy Player proxy instance 00041 */ 00042 PlayerLaserMapper::PlayerLaserMapper(std::string varname, 00043 fawkes::Laser360Interface *interface, 00044 PlayerCc::LaserProxy *proxy) 00045 : PlayerProxyFawkesInterfaceMapper(varname) 00046 { 00047 __interface = interface; 00048 __proxy = proxy; 00049 __first_read = true; 00050 } 00051 00052 00053 void 00054 PlayerLaserMapper::sync_player_to_fawkes() 00055 { 00056 //printf("Laser interface, count: %u, min angle= %f, max angle = %f\n", 00057 // __proxy->GetCount(), __proxy->GetMinAngle(), __proxy->GetMaxAngle()); 00058 00059 if ( __proxy->GetCount() != 360 ) return; 00060 00061 if ( __proxy->IsFresh() ) { 00062 00063 if ( __first_read ) { 00064 __index_offset = 360 + fawkes::rad2deg(__proxy->GetMinAngle()); 00065 __first_read = false; 00066 } 00067 00068 //printf("Setting %s to (%f, %f, %f)\n", varname().c_str(), __proxy->GetXPos(), 00069 // __proxy->GetYPos(), __proxy->GetYaw()); 00070 float distances[360]; 00071 for (int i = 0; i < 360; ++i) { 00072 distances[(i + __index_offset) % 360] = (*__proxy)[360 - i]; 00073 } 00074 __interface->set_distances(distances); 00075 __interface->write(); 00076 __proxy->NotFresh(); 00077 } 00078 } 00079 00080 void 00081 PlayerLaserMapper::sync_fawkes_to_player() 00082 { 00083 }