Fawkes API  Fawkes Development Version
laser_mapper.cpp
1 
2 /***************************************************************************
3  * laser_mapper.cpp - Mapper LaserProxy to LaserInterface
4  *
5  * Created: Tue Oct 21 00:50:26 2008
6  * Copyright 2006-2008 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "laser_mapper.h"
24 
25 #include <core/exceptions/software.h>
26 #include <utils/math/angle.h>
27 #include <interfaces/Laser360Interface.h>
28 #include <libplayerc++/playerc++.h>
29 
30 /** @class PlayerLaserMapper "laser_mapper.h"
31  * Laser mapper for player integration.
32  * This class is used to map a Player lsaer proxy to a Fawkes
33  * Laser360Interface.
34  * @author Tim Niemueller
35  */
36 
37 /** Constructor.
38  * @param varname variable name
39  * @param interface Fawkes interface instance
40  * @param proxy Player proxy instance
41  */
43  fawkes::Laser360Interface *interface,
44  PlayerCc::LaserProxy *proxy)
46 {
47  __interface = interface;
48  __proxy = proxy;
49  __first_read = true;
50 }
51 
52 
53 void
55 {
56  //printf("Laser interface, count: %u, min angle= %f, max angle = %f\n",
57  // __proxy->GetCount(), __proxy->GetMinAngle(), __proxy->GetMaxAngle());
58 
59  if ( __proxy->GetCount() != 360 ) return;
60 
61  if ( __proxy->IsFresh() ) {
62 
63  if ( __first_read ) {
64  __index_offset = 360 + fawkes::rad2deg(__proxy->GetMinAngle());
65  __first_read = false;
66  }
67 
68  //printf("Setting %s to (%f, %f, %f)\n", varname().c_str(), __proxy->GetXPos(),
69  // __proxy->GetYPos(), __proxy->GetYaw());
70  float distances[360];
71  for (int i = 0; i < 360; ++i) {
72  distances[(i + __index_offset) % 360] = (*__proxy)[360 - i];
73  }
74  __interface->set_distances(distances);
75  __interface->write();
76  __proxy->NotFresh();
77  }
78 }
79 
80 void
82 {
83 }
Laser360Interface Fawkes BlackBoard Interface.
virtual void sync_fawkes_to_player()
Sync Fawkes interface to Player proxy.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void sync_player_to_fawkes()
Sync Player proxy to Fawkes interface.
Player proxy to Fawkes interface mapper interface.
Definition: mapper.h:28
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
PlayerLaserMapper(std::string varname, fawkes::Laser360Interface *interface, PlayerCc::LaserProxy *proxy)
Constructor.