laser_mapper.cpp

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 }

Generated on Tue Feb 22 13:31:29 2011 for Fawkes API by  doxygen 1.4.7