00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
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
00049
00050
00051
00052 mTeamIndexMap["Left"] = TI_LEFT;
00053 mTeamIndexMap["Right"] = TI_RIGHT;
00054 mTeamIndexMap["None"] = TI_NONE;
00055
00056
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
00100
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
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
00179
00180
00181
00182
00183
00184
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
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
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
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
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
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
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
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
00299 if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
00300 {
00301
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
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
00327 if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
00328 {
00329
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
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
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
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
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