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
30using namespace PlayerCc;
31using 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
60void
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
74void
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:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
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.