motor_mapper.cpp

00001 
00002 /***************************************************************************
00003  *  motor_mapper.cpp - Mapper Position2dProxy to MotorInterface
00004  *
00005  *  Created: Tue Sep 30 14:45:30 2008
00006  *  Copyright  2006-2008  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "motor_mapper.h"
00024 
00025 #include <interfaces/MotorInterface.h>
00026 #include <libplayerc++/playerc++.h>
00027 #include <cstdio>
00028 
00029 using namespace PlayerCc;
00030 using namespace fawkes;
00031 
00032 /** @class PlayerMotorPositionMapper "motor_mapper.h"
00033  * Motor to position mapper for player integration.
00034  * This class is used to map a Player position2d proxy to a Fawkes
00035  * MotorInterface.
00036  *
00037  * It maps
00038  * - MotorInterface::GotoMessage to Position2dProxy::GoTo()
00039  * - MotorInterface::SetMotorStateMessage to Position2dProxy::SetMotorEnable()
00040  * - MotorInterface::ResetOdometryMessage to Position2dProxy::ResetOdometry()
00041  *
00042  * @author Tim Niemueller
00043  */
00044 
00045 /** Constructor.
00046  * @param varname variable name
00047  * @param interface Fawkes interface instance
00048  * @param proxy Player proxy instance
00049  */
00050 PlayerMotorPositionMapper::PlayerMotorPositionMapper(std::string varname,
00051                                                      fawkes::MotorInterface *interface,
00052                                                      PlayerCc::Position2dProxy *proxy)
00053   : PlayerProxyFawkesInterfaceMapper(varname)
00054 {
00055   __interface = interface;
00056   __proxy     = proxy;
00057 }
00058 
00059 
00060 void
00061 PlayerMotorPositionMapper::sync_player_to_fawkes()
00062 {
00063   if ( __proxy->IsFresh() ) {
00064     //printf("Setting %s to (%f, %f, %f)\n", varname().c_str(), __proxy->GetXPos(),
00065     //       __proxy->GetYPos(), __proxy->GetYaw());
00066     __interface->set_odometry_position_x(__proxy->GetXPos());
00067     __interface->set_odometry_position_y(__proxy->GetYPos());
00068     __interface->set_odometry_orientation(__proxy->GetYaw());
00069     __interface->write();
00070     __proxy->NotFresh();
00071   }
00072 }
00073 
00074 void
00075 PlayerMotorPositionMapper::sync_fawkes_to_player()
00076 {
00077   while ( ! __interface->msgq_empty() ) {
00078     if ( __interface->msgq_first_is<MotorInterface::SetMotorStateMessage>() ) {
00079       MotorInterface::SetMotorStateMessage *m = __interface->msgq_first<MotorInterface::SetMotorStateMessage>();
00080       __proxy->SetMotorEnable(m->motor_state() == MotorInterface::MOTOR_ENABLED);
00081     } else if ( __interface->msgq_first_is<MotorInterface::ResetOdometryMessage>() ) {
00082       __proxy->ResetOdometry();
00083     } else if ( __interface->msgq_first_is<MotorInterface::GotoMessage>() ) {
00084       MotorInterface::GotoMessage *m = __interface->msgq_first<MotorInterface::GotoMessage>();
00085       __proxy->GoTo(m->x(), m->y(), m->phi());
00086     }
00087     // all others are silently ignored
00088 
00089     __interface->msgq_pop();
00090   }
00091 }

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1