Fawkes API  Fawkes Development Version
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  */
51  fawkes::MotorInterface *interface,
52  PlayerCc::Position2dProxy *proxy)
54 {
55  __interface = interface;
56  __proxy = proxy;
57 }
58 
59 
60 void
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
76 {
77  while ( ! __interface->msgq_empty() ) {
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>() ) {
85  __proxy->GoTo(m->x(), m->y(), m->phi());
86  }
87  // all others are silently ignored
88 
89  __interface->msgq_pop();
90  }
91 }
SetMotorStateMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
Fawkes library namespace.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
float phi() const
Get phi value.
ResetOdometryMessage Fawkes BlackBoard Interface Message.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
Player proxy to Fawkes interface mapper interface.
Definition: mapper.h:28
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
uint32_t motor_state() const
Get motor_state value.
virtual void sync_fawkes_to_player()
Sync Fawkes interface to Player proxy.
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
MotorInterface Fawkes BlackBoard Interface.
GotoMessage Fawkes BlackBoard Interface Message.
PlayerMotorPositionMapper(std::string varname, fawkes::MotorInterface *interface, PlayerCc::Position2dProxy *proxy)
Constructor.
virtual void sync_player_to_fawkes()
Sync Player proxy to Fawkes interface.