behavior.cpp

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

/*
   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.
*/
#include "behavior.h"
#include <sstream>
#include "types.h"
#include "tactic.h"
#include "./WorldModel/fieldinfo.h"
#include "./WorldModel/worldmodel.h"
#include "action.h"
#include "./WorldModel/self.h"
#include "NaoGetUp.h"


#define CALL_MEMBER_FN(object, ptrToMember)  ((object).*(ptrToMember))

using namespace std;
using namespace salt;
using namespace boost;

Behavior::Behavior() :
    mCycle(0)
{
}

Behavior::~Behavior()
{
}

void Behavior::OnUnlink()
{
    mSelf.reset();
}

int Behavior::GetCycle() const
{
    return mCycle;
}

const string& Behavior::GetCommand() const
{
    return mCommand;
}

void Behavior::SetCommand(const string& command)
{
    mCommand = command;
}

string Behavior::InitRobot() const
{	
	if (! SELF.Init())
    	{
        cerr << "[Behavior] Self init failed!\n";
   	}
    	stringstream ss;
    	ss << "(scene " << SELF.GetModelFileName() << ")";
    	return ss.str();
}

string Behavior::InitAgent() const
{
    stringstream ss;
    ss << "(init (unum " << SELF.GetUnum() << ")";
    ss << "(teamname " << FIELDINFO.our_teamname << "))";
    return ss.str();
}

const string& Behavior::Think(const string& message)
{	
    	++mCycle;
    	mCommand.clear();
   		WM.Update(message);

    	/*if (mCycle == 1)
    	{
        	mCommand = InitRobot();
        	return mCommand;
    	}
    	else if (mCycle == 2)
    	{
        	mCommand = InitAgent();
        	return mCommand;
   		}//zm
		*/
    	BehaviorDebug();
    	return mCommand;
}

void Behavior::BehaviorDebug()
{	//cout << "SELFPOS: " << SELF.pos << endl;
    if (mCycle < 20)
    {
            stringstream ss;
            ss << "(beam -2 0 0)";
            mCommand = ss.str();
    }
    else
    {
       TACTIC->DoTactic();
        if (mAction.get() == 0)
        {	//cout << "mActionType" << endl;
            mActionType = TACTIC->GetActionType();
            mAction = SELF.GetAction(mActionType);
            if (mAction.get() != 0) NAOGETUP->SetParam(/*TACTIC->GetActionParams()*/);
        }

        if (mAction.get() != 0 && NAOGETUP->DoAction())
        {	//cout << "setAction" << endl;
            mCommand = NAOGETUP->GetActionCommand();
        }
    }
}