Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

trainercommandparser.cpp

Go to the documentation of this file.
00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
00002 
00003    this file is part of the trainer for rcssserver3D
00004    Fri May 9 2003
00005    Copyright (C) 2002,2003 Koblenz University
00006    Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
00007 
00008    This program is free software; you can redistribute it and/or modify
00009    it under the terms of the GNU General Public License as published by
00010    the Free Software Foundation; version 2 of the License.
00011 
00012    This program is distributed in the hope that it will be useful,
00013    but WITHOUT ANY WARRANTY; without even the implied warranty of
00014    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015    GNU General Public License for more details.
00016 
00017    You should have received a copy of the GNU General Public License
00018    along with this program; if not, write to the Free Software
00019    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00020 
00021    Parser that gets a list of predicates and interprets the trainer
00022    commands contained in them
00023 
00024 */
00025 
00026 #include <oxygen/physicsserver/body.h>
00027 #include <soccerbase/soccerbase.h>
00028 #include <agentstate/agentstate.h>
00029 #include <soccertypes.h>
00030 #include <gamestateaspect/gamestateaspect.h>
00031 #include "trainercommandparser.h"
00032 
00033 using namespace std;
00034 using namespace boost;
00035 using namespace zeitgeist;
00036 using namespace oxygen;
00037 
00038 TrainerCommandParser::TrainerCommandParser() : MonitorCmdParser()
00039 {
00040     // setup command map
00041     mCommandMap["agent"] = CT_PLAYER;
00042     mCommandMap["ball"] = CT_BALL;
00043     mCommandMap["playMode"] = CT_PLAYMODE;
00044     mCommandMap["dropBall"] = CT_DROP_BALL;
00045     mCommandMap["kickOff"] = CT_KICK_OFF;
00046     mCommandMap["getAck"] = CT_ACK;
00047 
00048     // setup team index map
00049     // Originally  team sides were "L","R" and "N"
00050     // But this seems to be unused
00051     // ParseKickOffCommand depends on the long names
00052     mTeamIndexMap["Left"]  = TI_LEFT;
00053     mTeamIndexMap["Right"] = TI_RIGHT;
00054     mTeamIndexMap["None"]  = TI_NONE;
00055 
00056     // setup play mode map
00057     mPlayModeMap[STR_PM_BeforeKickOff] = PM_BeforeKickOff;
00058     mPlayModeMap[STR_PM_KickOff_Left]  = PM_KickOff_Left;
00059     mPlayModeMap[STR_PM_KickOff_Right] = PM_KickOff_Right;
00060     mPlayModeMap[STR_PM_PlayOn]        = PM_PlayOn;
00061     mPlayModeMap[STR_PM_KickIn_Left]   = PM_KickIn_Left;
00062     mPlayModeMap[STR_PM_KickIn_Right]  = PM_KickIn_Right;
00063     mPlayModeMap[STR_PM_CORNER_KICK_LEFT] = PM_CORNER_KICK_LEFT;
00064     mPlayModeMap[STR_PM_CORNER_KICK_RIGHT] = PM_CORNER_KICK_RIGHT;
00065     mPlayModeMap[STR_PM_GOAL_KICK_LEFT] = PM_GOAL_KICK_LEFT;
00066     mPlayModeMap[STR_PM_GOAL_KICK_RIGHT] = PM_GOAL_KICK_RIGHT;
00067     mPlayModeMap[STR_PM_OFFSIDE_LEFT] = PM_OFFSIDE_LEFT;
00068     mPlayModeMap[STR_PM_OFFSIDE_RIGHT] = PM_OFFSIDE_RIGHT;
00069     mPlayModeMap[STR_PM_FREE_KICK_LEFT] = PM_FREE_KICK_LEFT;
00070     mPlayModeMap[STR_PM_FREE_KICK_RIGHT] = PM_FREE_KICK_RIGHT;
00071     mPlayModeMap[STR_PM_Goal_Left]     = PM_Goal_Left;
00072     mPlayModeMap[STR_PM_Goal_Right]    = PM_Goal_Right;
00073     mPlayModeMap[STR_PM_GameOver]      = PM_GameOver;
00074 
00075     mGetAck = false;
00076 }
00077 
00078 TrainerCommandParser::~TrainerCommandParser()
00079 {
00080 
00081 }
00082 
00083 bool
00084 TrainerCommandParser::SendAck(std::string &reply)
00085 {
00086     if (!mGetAck)
00087     {
00088         return false;
00089     }
00090 
00091     reply = "best";
00092     mGetAck= false;
00093     return true;
00094 }
00095 
00096 void
00097 TrainerCommandParser::OnLink()
00098 {
00099     // we need the SexpParser to generate the predicates
00100     // from S-Expressions
00101     mSexpParser = shared_dynamic_cast<oxygen::BaseParser>(GetCore()->New("SexpParser"));
00102 
00103     if (mSexpParser.get() == 0)
00104         {
00105             GetLog()->Error() << "ERROR: (TrainerCommnadParser) failed to create SexpParser\n";
00106             return;
00107         }
00108 }
00109 
00110 void TrainerCommandParser::OnUnlink()
00111 {
00112     mSexpParser.reset();
00113 }
00114 
00115 void TrainerCommandParser::ParseMonitorMessage(const std::string& data)
00116 {
00117     if (mSexpParser.get() == 0)
00118     {
00119         GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get SexpParser\n";
00120         return;
00121     }
00122 
00123     shared_ptr<PredicateList> predList = mSexpParser->Parse(data);
00124     ParsePredicates(*predList);
00125 }
00126 
00127 void TrainerCommandParser::ParsePredicates(oxygen::PredicateList & predList)
00128 {
00129         for (
00130          PredicateList::TList::const_iterator iter = predList.begin();
00131          iter != predList.end();
00132          ++iter
00133          )
00134     {
00135         const Predicate & predicate = (*iter);
00136 
00137         if (! ParsePredicate(predicate))
00138         {
00139             continue;
00140         }
00141     }
00142 }
00143 
00144 bool
00145 TrainerCommandParser::ParsePredicate(const oxygen::Predicate & predicate)
00146 {
00147     SoccerBase::GetGameState(*this,mGameState);
00148     SoccerBase::GetSoccerRuleAspect(*this,mSoccerRule);
00149 
00150     // lookup the command type corresponding to the predicate name
00151     TCommandMap::iterator iter = mCommandMap.find(predicate.name);
00152 
00153     if (iter == mCommandMap.end())
00154     {
00155         return false;
00156     }
00157 
00158     switch ((*iter).second)
00159     {
00160     case CT_PLAYER:
00161         ParsePlayerCommand(predicate);
00162         break;
00163     case CT_BALL:
00164         ParseBallCommand(predicate);
00165         break;
00166     case CT_PLAYMODE:
00167         ParsePlayModeCommand(predicate);
00168         break;
00169     case CT_DROP_BALL:
00170         mSoccerRule->DropBall();
00171         break;
00172     case CT_KICK_OFF:
00173         ParseKickOffCommand(predicate);
00174         break;
00175     case CT_ACK:
00176     {
00177         mGetAck=true;
00178         // Predicate::Iterator ackParam(predicate);
00179         // ++ackParam;
00180 
00181         /* if (! predicate.GetValue(ackParam, mAckString))
00182          {
00183             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get mAckString\n";
00184             return false;
00185             }*/
00186 
00187         break;
00188     }
00189     default:
00190         return false;
00191     }
00192 
00193     return true;
00194 }
00195 
00196 void TrainerCommandParser::ParsePlayerCommand(const oxygen::Predicate & predicate)
00197 {
00198 
00199     Predicate::Iterator unumParam(predicate);
00200     int                 unum;
00201 
00202     // extract unum
00203     if (predicate.FindParameter(unumParam, "unum"))
00204     {
00205         if (! predicate.GetValue(unumParam, unum))
00206         {
00207             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get unum\n";
00208             return;
00209         }
00210     }
00211 
00212     string              team;
00213     TTeamIndex          idx;
00214     Predicate::Iterator teamParam(predicate);
00215 
00216     // extract side
00217     if (predicate.FindParameter(teamParam, "team"))
00218     {
00219         if (! predicate.GetValue(teamParam, team))
00220         {
00221             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get team name\n";
00222             return;
00223         }
00224 
00225         idx = mTeamIndexMap[team];
00226     }
00227 
00228     Predicate::Iterator posParam(predicate);
00229 
00230     if (predicate.FindParameter(posParam, "pos"))
00231     {
00232         salt::Vector3f pos;
00233 
00234         // extract position vector
00235         if (! predicate.GetValue(posParam, pos))
00236         {
00237             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent pos\n";
00238             return;
00239         }
00240 
00241         shared_ptr<Body> body;
00242 
00243         if (SoccerBase::GetAgentBody(*this, idx, unum, body))
00244         {
00245             // set new velocity in the body
00246             body->SetPosition(pos);
00247         }
00248         else
00249         {
00250             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent body\n";
00251             return;
00252         }
00253     }
00254 
00255     Predicate::Iterator velParam(predicate);
00256 
00257     if (predicate.FindParameter(velParam, "vel"))
00258     {
00259         salt::Vector3f vel;
00260 
00261         // extract velocity vector
00262         if (! predicate.GetValue(velParam, vel))
00263         {
00264             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent vel\n";
00265             return;
00266         }
00267 
00268         shared_ptr<Body> body;
00269 
00270         if (SoccerBase::GetAgentBody(*this, idx, unum, body))
00271         {
00272             // set new velocity in the body
00273             body->SetVelocity(vel);
00274             body->SetAngularVelocity(salt::Vector3f(0.0f,0.0f,0.0f));
00275         }
00276         else
00277         {
00278             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent body\n";
00279             return;
00280         }
00281     }
00282 
00283     Predicate::Iterator batParam(predicate);
00284 
00285     if (predicate.FindParameter(batParam, "battery"))
00286     {
00287         double battery;
00288 
00289         // extract battery value
00290         if (! predicate.GetValue(batParam, battery))
00291         {
00292             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get battery value\n";
00293             return;
00294         }
00295 
00296         shared_ptr<AgentState> agentState;
00297 
00298         // get agent state
00299         if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
00300         {
00301             // set new battery
00302             agentState->SetBattery(battery);
00303         }
00304         else
00305         {
00306             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent state\n";
00307             return;
00308         }
00309     }
00310 
00311     Predicate::Iterator tempParam(predicate);
00312 
00313     if (predicate.FindParameter(tempParam, "temperature"))
00314     {
00315         float temperature;
00316 
00317         // extract temperature value
00318         if (! predicate.GetValue(tempParam, temperature))
00319         {
00320             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get temperatur value\n";
00321             return;
00322         }
00323 
00324         shared_ptr<AgentState> agentState;
00325 
00326         // get agent state
00327         if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
00328         {
00329             // set new temperature
00330             agentState->SetBattery(temperature);
00331         }
00332         else
00333         {
00334             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent state\n";
00335             return;
00336         }
00337     }
00338 }
00339 
00340 void
00341 TrainerCommandParser::ParseBallCommand(const oxygen::Predicate& predicate)
00342 {
00343     Predicate::Iterator posParam(predicate);
00344 
00345     if (predicate.FindParameter(posParam, "pos"))
00346     {
00347         salt::Vector3f pos;
00348 
00349         // extract position vector
00350         if (! predicate.GetValue(posParam, pos))
00351         {
00352             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball pos\n";
00353             return;
00354         }
00355 
00356         shared_ptr<Body> body;
00357 
00358         if (SoccerBase::GetBallBody(*this, body))
00359         {
00360             // set new velocity in the body
00361             body->SetPosition(pos);
00362         }
00363         else
00364         {
00365             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball body\n";
00366 
00367             return;
00368         }
00369     }
00370 
00371     Predicate::Iterator velParam(predicate);
00372 
00373     if (predicate.FindParameter(velParam, "vel"))
00374     {
00375         salt::Vector3f vel;
00376 
00377         // extract velocity vector
00378         if (! predicate.GetValue(velParam, vel))
00379         {
00380             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball vel\n";
00381             return;
00382         }
00383 
00384         shared_ptr<Body> body;
00385 
00386         if (SoccerBase::GetBallBody(*this, body))
00387         {
00388             // set new velocity in the body
00389             body->SetVelocity(vel);
00390             body->SetAngularVelocity(salt::Vector3f(0.0f,0.0f,0.0f));
00391         }
00392         else
00393         {
00394             GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball body\n";
00395             return;
00396         }
00397     }
00398 }
00399 
00400 void
00401 TrainerCommandParser::ParsePlayModeCommand(const oxygen::Predicate& predicate)
00402 {
00403     Predicate::Iterator pmParam(predicate);
00404     string mode;
00405 
00406     if (predicate.GetValue(pmParam,mode))
00407     {
00408         TPlayModeMap::const_iterator playmode = mPlayModeMap.find(mode);
00409         if (playmode != mPlayModeMap.end())
00410         {
00411             mGameState->SetPlayMode(playmode->second);
00412         }
00413         else
00414         {
00415             GetLog()->Debug()
00416                 << "(TrainerCommandParser) ERROR: an unknown playmode"
00417                 << mode << " was passed\n";
00418         }
00419     }
00420     else
00421     {
00422         GetLog()->Debug()
00423             << "(TrainerCommandParser) ERROR: could not parse playmode "
00424             << mode << "\n";
00425     }
00426 }
00427 
00428 
00429 void
00430 TrainerCommandParser::ParseKickOffCommand(const oxygen::Predicate& predicate)
00431 {
00432     Predicate::Iterator koParam(predicate);
00433     string team;
00434 
00435 
00436     if (predicate.GetValue(koParam,team))
00437     {
00438         TTeamIndexMap::const_iterator kickoffteam = mTeamIndexMap.find(team);
00439         if (kickoffteam != mTeamIndexMap.end())
00440         {
00441             if (mGameState.get() == 0)
00442             {
00443                 GetLog()->Error() << "(TrainerCommandParser) ERROR "
00444                                   << "no GameStateAspect found, cannot kick off\n";
00445             }
00446             else
00447             {
00448                 mGameState->KickOff(kickoffteam->second);
00449             }
00450         }
00451         else
00452         {
00453             GetLog()->Error() << "(TrainerCommandParser) ERROR: unknown team"
00454                               << team << "\n";
00455             
00456         }
00457     }
00458     else
00459     {
00460         GetLog()->Debug()
00461             << "(TrainerCommandParser) ERROR: could not parse team "
00462             << team << "\n";
00463     }
00464 }
00465 

Generated on Thu Apr 6 15:25:40 2006 for rcssserver3d by  doxygen 1.4.4