Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
motor_mapper.cpp
1
2
/***************************************************************************
3
* motor_mapper.cpp - Mapper Position2dProxy to MotorInterface
4
*
5
* Created: Tue Sep 30 14:45:30 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 "motor_mapper.h"
24
25
#include <interfaces/MotorInterface.h>
26
#include <libplayerc++/playerc++.h>
27
#include <cstdio>
28
29
using namespace
PlayerCc;
30
using namespace
fawkes;
31
32
/** @class PlayerMotorPositionMapper "motor_mapper.h"
33
* Motor to position mapper for player integration.
34
* This class is used to map a Player position2d proxy to a Fawkes
35
* MotorInterface.
36
*
37
* It maps
38
* - MotorInterface::GotoMessage to Position2dProxy::GoTo()
39
* - MotorInterface::SetMotorStateMessage to Position2dProxy::SetMotorEnable()
40
* - MotorInterface::ResetOdometryMessage to Position2dProxy::ResetOdometry()
41
*
42
* @author Tim Niemueller
43
*/
44
45
/** Constructor.
46
* @param varname variable name
47
* @param interface Fawkes interface instance
48
* @param proxy Player proxy instance
49
*/
50
PlayerMotorPositionMapper::PlayerMotorPositionMapper
(std::string varname,
51
fawkes::MotorInterface
*interface,
52
PlayerCc::Position2dProxy *proxy)
53
:
PlayerProxyFawkesInterfaceMapper
(varname)
54
{
55
__interface = interface;
56
__proxy = proxy;
57
}
58
59
60
void
61
PlayerMotorPositionMapper::sync_player_to_fawkes
()
62
{
63
if
( __proxy->IsFresh() ) {
64
//printf("Setting %s to (%f, %f, %f)\n", varname().c_str(), __proxy->GetXPos(),
65
// __proxy->GetYPos(), __proxy->GetYaw());
66
__interface->
set_odometry_position_x
(__proxy->GetXPos());
67
__interface->
set_odometry_position_y
(__proxy->GetYPos());
68
__interface->
set_odometry_orientation
(__proxy->GetYaw());
69
__interface->
write
();
70
__proxy->NotFresh();
71
}
72
}
73
74
void
75
PlayerMotorPositionMapper::sync_fawkes_to_player
()
76
{
77
while
( ! __interface->
msgq_empty
() ) {
78
if
( __interface->
msgq_first_is
<
MotorInterface::SetMotorStateMessage
>() ) {
79
MotorInterface::SetMotorStateMessage
*m = __interface->
msgq_first
<
MotorInterface::SetMotorStateMessage
>();
80
__proxy->SetMotorEnable(m->
motor_state
() == MotorInterface::MOTOR_ENABLED);
81
}
else
if
( __interface->
msgq_first_is
<
MotorInterface::ResetOdometryMessage
>() ) {
82
__proxy->ResetOdometry();
83
}
else
if
( __interface->
msgq_first_is
<
MotorInterface::GotoMessage
>() ) {
84
MotorInterface::GotoMessage
*m = __interface->
msgq_first
<
MotorInterface::GotoMessage
>();
85
__proxy->GoTo(m->
x
(), m->
y
(), m->
phi
());
86
}
87
// all others are silently ignored
88
89
__interface->
msgq_pop
();
90
}
91
}
src
plugins
player
motor_mapper.cpp
Generated by
1.8.3.1