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)
45
:
PlayerProxyFawkesInterfaceMapper
(varname),
46
interface_(interface),
47
proxy_(proxy),
48
first_read_(true),
49
index_offset_(0)
50
{
51
}
52
53
void
54
PlayerLaserMapper::sync_player_to_fawkes
()
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
81
PlayerLaserMapper::sync_fawkes_to_player
()
82
{
83
}
PlayerLaserMapper::sync_fawkes_to_player
virtual void sync_fawkes_to_player()
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:172
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:52
PlayerLaserMapper::sync_player_to_fawkes
virtual void sync_player_to_fawkes()
Definition:
laser_mapper.cpp:54
fawkes::Laser360Interface
Definition:
Laser360Interface.h:39
PlayerProxyFawkesInterfaceMapper
Definition:
mapper.h:28
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition:
interface.cpp:499
src
plugins
player
laser_mapper.cpp
Generated by
1.8.17