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 
28 #include <cstdio>
29 
30 using namespace PlayerCc;
31 using namespace fawkes;
32 
33 /** @class PlayerMotorPositionMapper "motor_mapper.h"
34  * Motor to position mapper for player integration.
35  * This class is used to map a Player position2d proxy to a Fawkes
36  * MotorInterface.
37  *
38  * It maps
39  * - MotorInterface::GotoMessage to Position2dProxy::GoTo()
40  * - MotorInterface::SetMotorStateMessage to Position2dProxy::SetMotorEnable()
41  * - MotorInterface::ResetOdometryMessage to Position2dProxy::ResetOdometry()
42  *
43  * @author Tim Niemueller
44  */
45 
46 /** Constructor.
47  * @param varname variable name
48  * @param interface Fawkes interface instance
49  * @param proxy Player proxy instance
50  */
52  fawkes::MotorInterface * interface,
53  PlayerCc::Position2dProxy *proxy)
55 {
56  interface_ = interface;
57  proxy_ = proxy;
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()) {
81  proxy_->SetMotorEnable(m->motor_state() == MotorInterface::MOTOR_ENABLED);
82  } else if (interface_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) {
83  proxy_->ResetOdometry();
84  } else if (interface_->msgq_first_is<MotorInterface::GotoMessage>()) {
86  proxy_->GoTo(m->x(), m->y(), m->phi());
87  }
88  // all others are silently ignored
89 
90  interface_->msgq_pop();
91  }
92 }
virtual void sync_fawkes_to_player()
Sync Fawkes interface to Player proxy.
PlayerMotorPositionMapper(const std::string &varname, fawkes::MotorInterface *interface, PlayerCc::Position2dProxy *proxy)
Constructor.
virtual void sync_player_to_fawkes()
Sync Player proxy to Fawkes interface.
Player proxy to Fawkes interface mapper interface.
Definition: mapper.h:29
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
GotoMessage Fawkes BlackBoard Interface Message.
float phi() const
Get phi value.
ResetOdometryMessage Fawkes BlackBoard Interface Message.
SetMotorStateMessage Fawkes BlackBoard Interface Message.
uint32_t motor_state() const
Get motor_state value.
MotorInterface Fawkes BlackBoard Interface.
void set_odometry_orientation(const float new_odometry_orientation)
Set odometry_orientation value.
void set_odometry_position_x(const float new_odometry_position_x)
Set odometry_position_x value.
void set_odometry_position_y(const float new_odometry_position_y)
Set odometry_position_y value.
Fawkes library namespace.