simspark-cvs

[Oliver O. <fr...@us...>]

Update of /cvsroot/simspark/simspark/simulations/soccer/plugin/trainercommandparser
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv24718/trainercommandparser

Added Files:
      Tag: projectx
	trainercommandparser.cpp trainercommandparser.h 
	trainercommandparser_c.cpp 
Log Message:
soccer plugins from rcssserver3D

--- NEW FILE: trainercommandparser_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of the trainer for rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

   Parser that gets a list of predicates and interprets the trainer
   commands contained in them

*/

#include "trainercommandparser.h"

void
CLASS(TrainerCommandParser)::DefineClass()
{
    DEFINE_BASECLASS(zeitgeist/Leaf);
}

--- NEW FILE: trainercommandparser.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of the trainer for rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

   Parser that gets a list of predicates and interprets the trainer
   commands contained in them

*/

#include <oxygen/physicsserver/body.h>
#include <soccerbase/soccerbase.h>
#include <agentstate/agentstate.h>
#include <soccertypes.h>
#include <gamestateaspect/gamestateaspect.h>
#include "trainercommandparser.h"

using namespace std;
using namespace boost;
using namespace zeitgeist;
using namespace oxygen;

TrainerCommandParser::TrainerCommandParser() : MonitorCmdParser()
{
    // setup command map
    mCommandMap["agent"] = CT_PLAYER;
    mCommandMap["ball"] = CT_BALL;
    mCommandMap["playMode"] = CT_PLAYMODE;
    mCommandMap["dropBall"] = CT_DROP_BALL;
    mCommandMap["kickOff"] = CT_KICK_OFF;
    mCommandMap["getAck"] = CT_ACK;

    // setup team index map
    // Originally  team sides were "L","R" and "N"
    // But this seems to be unused
    // ParseKickOffCommand depends on the long names
    mTeamIndexMap["Left"]  = TI_LEFT;
    mTeamIndexMap["Right"] = TI_RIGHT;
    mTeamIndexMap["None"]  = TI_NONE;

    // setup play mode map
    mPlayModeMap[STR_PM_BeforeKickOff] = PM_BeforeKickOff;
    mPlayModeMap[STR_PM_KickOff_Left]  = PM_KickOff_Left;
    mPlayModeMap[STR_PM_KickOff_Right] = PM_KickOff_Right;
    mPlayModeMap[STR_PM_PlayOn]        = PM_PlayOn;
    mPlayModeMap[STR_PM_KickIn_Left]   = PM_KickIn_Left;
    mPlayModeMap[STR_PM_KickIn_Right]  = PM_KickIn_Right;
    mPlayModeMap[STR_PM_CORNER_KICK_LEFT] = PM_CORNER_KICK_LEFT;
    mPlayModeMap[STR_PM_CORNER_KICK_RIGHT] = PM_CORNER_KICK_RIGHT;
    mPlayModeMap[STR_PM_GOAL_KICK_LEFT] = PM_GOAL_KICK_LEFT;
    mPlayModeMap[STR_PM_GOAL_KICK_RIGHT] = PM_GOAL_KICK_RIGHT;
    mPlayModeMap[STR_PM_OFFSIDE_LEFT] = PM_OFFSIDE_LEFT;
    mPlayModeMap[STR_PM_OFFSIDE_RIGHT] = PM_OFFSIDE_RIGHT;
    mPlayModeMap[STR_PM_FREE_KICK_LEFT] = PM_FREE_KICK_LEFT;
    mPlayModeMap[STR_PM_FREE_KICK_RIGHT] = PM_FREE_KICK_RIGHT;
    mPlayModeMap[STR_PM_Goal_Left]     = PM_Goal_Left;
    mPlayModeMap[STR_PM_Goal_Right]    = PM_Goal_Right;
    mPlayModeMap[STR_PM_GameOver]      = PM_GameOver;

    mGetAck = false;
}

TrainerCommandParser::~TrainerCommandParser()
{

}

bool
TrainerCommandParser::SendAck(std::string &reply)
{
    if (!mGetAck)
    {
        return false;
    }

    reply = "best";
    mGetAck= false;
    return true;
}

void
TrainerCommandParser::OnLink()
{
    // we need the SexpParser to generate the predicates
    // from S-Expressions
    mSexpParser = shared_dynamic_cast<oxygen::BaseParser>(GetCore()->New("SexpParser"));

    if (mSexpParser.get() == 0)
        {
            GetLog()->Error() << "ERROR: (TrainerCommnadParser) failed to create SexpParser\n";
            return;
        }
}

void TrainerCommandParser::OnUnlink()
{
    mSexpParser.reset();
}

void TrainerCommandParser::ParseMonitorMessage(const std::string& data)
{
    if (mSexpParser.get() == 0)
    {
        GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get SexpParser\n";
        return;
    }

    shared_ptr<PredicateList> predList = mSexpParser->Parse(data);
    ParsePredicates(*predList);
}

void TrainerCommandParser::ParsePredicates(oxygen::PredicateList & predList)
{
        for (
         PredicateList::TList::const_iterator iter = predList.begin();
         iter != predList.end();
         ++iter
         )
    {
        const Predicate & predicate = (*iter);

        if (! ParsePredicate(predicate))
        {
            continue;
        }
    }
}

bool
TrainerCommandParser::ParsePredicate(const oxygen::Predicate & predicate)
{
    SoccerBase::GetGameState(*this,mGameState);
    SoccerBase::GetSoccerRuleAspect(*this,mSoccerRule);

    // lookup the command type corresponding to the predicate name
    TCommandMap::iterator iter = mCommandMap.find(predicate.name);

    if (iter == mCommandMap.end())
    {
        return false;
    }

    switch ((*iter).second)
    {
    case CT_PLAYER:
        ParsePlayerCommand(predicate);
        break;
    case CT_BALL:
        ParseBallCommand(predicate);
        break;
    case CT_PLAYMODE:
        ParsePlayModeCommand(predicate);
        break;
    case CT_DROP_BALL:
        mSoccerRule->DropBall();
        break;
    case CT_KICK_OFF:
        ParseKickOffCommand(predicate);
        break;
    case CT_ACK:
    {
        mGetAck=true;
        // Predicate::Iterator ackParam(predicate);
        // ++ackParam;

        /* if (! predicate.GetValue(ackParam, mAckString))
         {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get mAckString\n";
            return false;
            }*/

        break;
    }
    default:
        return false;
    }

    return true;
}

void TrainerCommandParser::ParsePlayerCommand(const oxygen::Predicate & predicate)
{

    Predicate::Iterator unumParam(predicate);
    int                 unum;

    // extract unum
    if (predicate.FindParameter(unumParam, "unum"))
    {
        if (! predicate.GetValue(unumParam, unum))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get unum\n";
            return;
        }
    }

    string              team;
    TTeamIndex          idx;
    Predicate::Iterator teamParam(predicate);

    // extract side
    if (predicate.FindParameter(teamParam, "team"))
    {
        if (! predicate.GetValue(teamParam, team))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get team name\n";
            return;
        }

        idx = mTeamIndexMap[team];
    }

    Predicate::Iterator posParam(predicate);

    if (predicate.FindParameter(posParam, "pos"))
    {
        salt::Vector3f pos;

        // extract position vector
        if (! predicate.GetValue(posParam, pos))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent pos\n";
            return;
        }

        shared_ptr<Body> body;

        if (SoccerBase::GetAgentBody(*this, idx, unum, body))
        {
            // set new velocity in the body
            body->SetPosition(pos);
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent body\n";
            return;
        }
    }

    Predicate::Iterator velParam(predicate);

    if (predicate.FindParameter(velParam, "vel"))
    {
        salt::Vector3f vel;

        // extract velocity vector
        if (! predicate.GetValue(velParam, vel))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent vel\n";
            return;
        }

        shared_ptr<Body> body;

        if (SoccerBase::GetAgentBody(*this, idx, unum, body))
        {
            // set new velocity in the body
            body->SetVelocity(vel);
            body->SetAngularVelocity(salt::Vector3f(0.0f,0.0f,0.0f));
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent body\n";
            return;
        }
    }

    Predicate::Iterator batParam(predicate);

    if (predicate.FindParameter(batParam, "battery"))
    {
        double battery;

        // extract battery value
        if (! predicate.GetValue(batParam, battery))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get battery value\n";
            return;
        }

        shared_ptr<AgentState> agentState;

        // get agent state
        if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
        {
            // set new battery
            agentState->SetBattery(battery);
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent state\n";
            return;
        }
    }

    Predicate::Iterator tempParam(predicate);

    if (predicate.FindParameter(tempParam, "temperature"))
    {
        float temperature;

        // extract temperature value
        if (! predicate.GetValue(tempParam, temperature))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get temperatur value\n";
            return;
        }

        shared_ptr<AgentState> agentState;

        // get agent state
        if (SoccerBase::GetAgentState(*this, idx, unum, agentState))
        {
            // set new temperature
            agentState->SetBattery(temperature);
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get agent state\n";
            return;
        }
    }
}

void
TrainerCommandParser::ParseBallCommand(const oxygen::Predicate& predicate)
{
    Predicate::Iterator posParam(predicate);

    if (predicate.FindParameter(posParam, "pos"))
    {
        salt::Vector3f pos;

        // extract position vector
        if (! predicate.GetValue(posParam, pos))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball pos\n";
            return;
        }

        shared_ptr<Body> body;

        if (SoccerBase::GetBallBody(*this, body))
        {
            // set new position in the body
            body->SetPosition(pos);
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball body\n";

            return;
        }
    }

    Predicate::Iterator velParam(predicate);

    if (predicate.FindParameter(velParam, "vel"))
    {
        salt::Vector3f vel;

        // extract velocity vector
        if (! predicate.GetValue(velParam, vel))
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball vel\n";
            return;
        }

        shared_ptr<Body> body;

        if (SoccerBase::GetBallBody(*this, body))
        {
            // set new velocity in the body
            body->SetVelocity(vel);
            body->SetAngularVelocity(salt::Vector3f(0.0f,0.0f,0.0f));
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: can't get ball body\n";
            return;
        }
    }
}

void
TrainerCommandParser::ParsePlayModeCommand(const oxygen::Predicate& predicate)
{
    Predicate::Iterator pmParam(predicate);
    string mode;

    if (predicate.GetValue(pmParam,mode))
    {
        TPlayModeMap::const_iterator playmode = mPlayModeMap.find(mode);
        if (playmode != mPlayModeMap.end())
        {
            mGameState->SetPlayMode(playmode->second);
        }
        else
        {
            GetLog()->Debug()
                << "(TrainerCommandParser) ERROR: an unknown playmode"
                << mode << " was passed\n";
        }
    }
    else
    {
        GetLog()->Debug()
            << "(TrainerCommandParser) ERROR: could not parse playmode "
            << mode << "\n";
    }
}


void
TrainerCommandParser::ParseKickOffCommand(const oxygen::Predicate& predicate)
{
    Predicate::Iterator koParam(predicate);
    string team;


    if (predicate.GetValue(koParam,team))
    {
        TTeamIndexMap::const_iterator kickoffteam = mTeamIndexMap.find(team);
        if (kickoffteam != mTeamIndexMap.end())
        {
            if (mGameState.get() == 0)
            {
                GetLog()->Error() << "(TrainerCommandParser) ERROR "
                                  << "no GameStateAspect found, cannot kick off\n";
            }
            else
            {
                mGameState->KickOff(kickoffteam->second);
            }
        }
        else
        {
            GetLog()->Error() << "(TrainerCommandParser) ERROR: unknown team"
                              << team << "\n";
            
        }
    }
    else
    {
        GetLog()->Debug()
            << "(TrainerCommandParser) ERROR: could not parse team "
            << team << "\n";
    }
}


--- NEW FILE: trainercommandparser.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of the trainer for rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

   Parser that gets a list of predicates and interprets the trainer
   commands contained in them

*/
#ifndef TRAINERCOMMANDPARSER_H
#define TRAINERCOMMANDPARSER_H

#include <string>
#include <oxygen/monitorserver/monitorcmdparser.h>
#include <oxygen/gamecontrolserver/predicate.h>
#include <oxygen/gamecontrolserver/baseparser.h>
#include <salt/vector.h>
#include <soccer/soccertypes.h>
#include <soccer/soccerruleaspect/soccerruleaspect.h>

class TrainerCommandParser : public oxygen::MonitorCmdParser
{
public:

    enum ECommandType
    {
        CT_PLAYER,
        CT_BALL,
        CT_PLAYMODE,
        CT_DROP_BALL,
        CT_KICK_OFF,
        CT_ACK
    };

    typedef std::map<std::string, ECommandType>  TCommandMap;
    typedef std::map<std::string, TTeamIndex> TTeamIndexMap;

    // mapping from string to TPlayMode
    typedef std::map<std::string, TPlayMode> TPlayModeMap;

public:
    TrainerCommandParser();

    virtual ~TrainerCommandParser();

    bool SendAck(std::string &reply);

    /** This function will be called be called from the monitor server
        implementation to parse any command strings received from the
        monitor client process
     */
    virtual void ParseMonitorMessage(const std::string& data);

    virtual void OnLink();

    virtual void OnUnlink();

protected:
    /** parses the list of predicates; returns true on success
     */
    void ParsePredicates(oxygen::PredicateList& predList);

    /** parses the given predicate and calls one of the specialized
        parse methods given below depending the predicate name;
        returns true if successful
     */
    bool ParsePredicate(const oxygen::Predicate & predicate);

    /** parses and executes the player command contained in the given
        predicate
    */
    void ParsePlayerCommand(const oxygen::Predicate & predicate);

    /** parses and executes the ball command contained in the given
        predicate
    */
    void ParseBallCommand(const oxygen::Predicate & predicate);

    /** parses and executes the play mode command contained in the given
        predicate
    */
    void ParsePlayModeCommand(const oxygen::Predicate & predicate);

    /** parses and executes the kick off command contained in the given
        predicate
    */
    void ParseKickOffCommand(const oxygen::Predicate & predicate);

protected:
    TCommandMap    mCommandMap;

    TTeamIndexMap  mTeamIndexMap;

    TPlayModeMap   mPlayModeMap;

    //! cached reference for the gamestate aspect
    boost::shared_ptr<GameStateAspect> mGameState;
    //! cached reference for the soccer rule aspect
    boost::shared_ptr<SoccerRuleAspect> mSoccerRule;
    //! the parser used to create the PredicateList
    boost::shared_ptr<oxygen::BaseParser> mSexpParser;

    bool mGetAck;
    std::string mAckString;
};

DECLARE_CLASS(TrainerCommandParser);

#endif // TRAINERCOMMANDPARSER_H