Fawkes API Fawkes Development Version
|
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 }