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 <interfaces/Laser360Interface.h>
27 #include <libplayerc++/playerc++.h>
28 #include <utils/math/angle.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  */
42 PlayerLaserMapper::PlayerLaserMapper(const std::string & varname,
43  fawkes::Laser360Interface *interface,
44  PlayerCc::LaserProxy * proxy)
46  interface_(interface),
47  proxy_(proxy),
48  first_read_(true),
49  index_offset_(0)
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)
60  return;
61 
62  if (proxy_->IsFresh()) {
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 }
PlayerLaserMapper::sync_fawkes_to_player
virtual void sync_fawkes_to_player()
Sync Fawkes interface to Player proxy.
Definition: laser_mapper.cpp:81
fawkes::Laser360Interface::set_distances
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Definition: Laser360Interface.cpp:163
PlayerLaserMapper::PlayerLaserMapper
PlayerLaserMapper(const std::string &varname, fawkes::Laser360Interface *interface, PlayerCc::LaserProxy *proxy)
Constructor.
Definition: laser_mapper.cpp:42
fawkes::rad2deg
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46
PlayerLaserMapper::sync_player_to_fawkes
virtual void sync_player_to_fawkes()
Sync Player proxy to Fawkes interface.
Definition: laser_mapper.cpp:54
fawkes::Laser360Interface
Laser360Interface Fawkes BlackBoard Interface.
Definition: Laser360Interface.h:34
PlayerProxyFawkesInterfaceMapper
Player proxy to Fawkes interface mapper interface.
Definition: mapper.h:29
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494