advancedagent.cpp

RoboCup 2D 仿真组老牌强队Mersad 2005的完整源代码

/*
 *  Copyright 2002-2005, Mersad Team, Allameh Helli High School (NODET).
 *
 *  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.
 *
 *  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 Library General Public License for more details.
 *
 *  This file is created by: Ahmad Boorghany
 *  and is modified by: Mohammad Salehe, Sassan Haradji
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <cmath>
#include <cassert>

#include <Pass.h>
#include <Shoot.h>
#include <Degree.h>
#include <Logger.h>
#include <Offense.h>
#include <Defense.h>
#include <Dribble.h>
#include <Command.h>
#include <Intercept.h>
#include <SRPDribble.h>
#include <AdvancedAgent.h>
#include <Config.h>
#include <RootPlan.h>

using namespace std;


// Library's fuctions

Library::Library(const WorldModel *worldModel): pass(worldModel), positioning(worldModel)
{
	pass.setCurKickCalculate(&worldModel->getCurKickCalculate());
}

// Form's fuctions
Form::Form()
{
	reset();
}

void Form::reset()
{
	sayForm.reset();
	headForm.reset();
	setBreakExperiment = 0;
}

// AdvancedAgent's fuctions
AdvancedAgent::AdvancedAgent(int argc, char **argv): SyncedAgent(argc, argv),
		groupWorks(worldModel), sayDecision(worldModel),
		headDecision(this), pointToDecision(worldModel),
		library(worldModel), radar(worldModel)
{
	statelessPlan = new StatelessPlan(worldModel, form, library);
	if (config["Agent"]["PlanningSystem"]["UseHPS"].asBool())
		rootPlan = new RootPlan(worldModel, form, library);
	else
		rootPlan = NULL;
}

AdvancedAgent::~AdvancedAgent()
{
	delete statelessPlan;
	delete rootPlan;
}

void AdvancedAgent::senseBody(const SExpression &exp)
{
	SyncedAgent::senseBody(exp);

	groupWorks.updateAfterSenseBody();
	if (form.setCycle != worldModel->getCurCycle())
	{
		form.setCycle = worldModel->getCurCycle();
		form.reset();
	}
/*
	if (lastDecide == DT_BLOCK ||
		lastDecide == DT_INTERCEPT ||
		lastDecide == DT_DRIBBLE_INTERCEPT ||
		lastDecide == DT_GOALIE_INTERCEPT)
		worldModel->setBall().setUpdateByHearPermitted(false);
	else*/
	worldModel->setBall().setUpdateByHearPermitted(true);

	worldModel->updateCurInterCalculate();
	worldModel->updateBallStatus();

	if (rootPlan)
		rootPlan->senseBody(exp);
	statelessPlan->senseBody(exp);
}

void AdvancedAgent::see(const SExpression &exp)
{
	SyncedAgent::see(exp);

	worldModel->updateCurInterCalculate();
	worldModel->updateBallStatus();
	worldModel->updateMarkingPlayers();

	statelessPlan->see(exp);
}

void AdvancedAgent::hear(const SExpression &exp)
{
	SyncedAgent::hear(exp);
	
	SExpAtomic *at;

	at = dynamic_cast<SExpAtomic *>(exp[2]);
	assert(at);
	if (at->toString() == "referee")
	{
		groupWorks.resetByChangePlayMode();

		if (worldModel->checkPlayModeForPenalty() == 1)
		{
			LOG << "AdvancedAgent::hear : Reversing Field" << endl;
			if (!worldModel->getBody().isGoalie())
			{
				LOG << "Reverse. You are player" << endl;
				worldModel->setFieldData().setPenaltySideRate(-1);
			}
			else
				LOG << "NO! Don't Reverse. You are goalie" << endl;  
		}
		else if (worldModel->checkPlayModeForPenalty() == 2)
		{
			LOG << "AdvancedAgent::hear : -1 Reversing Field" << endl;
			if (worldModel->getBody().isGoalie())
			{
				LOG << "Reverse. You are goalie" << endl;  
				worldModel->setFieldData().setPenaltySideRate(-1);
			}
			else
				LOG << "NO! Don't Reverse. You are player" << endl;   
		}
	}

	if ((worldModel->getHearedHeader() == 'A' ||
		 worldModel->getHearedHeader() == 'B') &&
		worldModel->getHearedSender() == GOALIE_UNIFORM_NUM &&
		worldModel->getHearedInterceptNum() != GOALIE_UNIFORM_NUM &&
	    worldModel->getPlayMode() == PM_GOAL_KICK)
	{
		if (worldModel->getPlayModeRemainCycle() < 199)
		{
			LOG << "GOAL_KICK --> GOAL_KICK_TAKEN" << endl;
			worldModel->setVirtualPlayMode(VPM_GOAL_KICK_TAKEN);
		}
		else
			LOG << "GOAL_KICK --> NO CHANGE TO PLAY MODE" << endl;
	}

	if (worldModel->getHearedHeader() == 'A' ||
		worldModel->getHearedHeader() == 'B')
		groupWorks.setInterceptNumByPassSay(
				worldModel->getHearedInterceptNum());
/*	else if (worldModel->getHearedHeader() == 'Y' ||
			worldModel->getHearedHeader() == 'Z')
	{
		groupWorks.changeInterceptNumByHear(
				worldModel->getHearedSender(),
				worldModel->getHearedInterceptNum());

		groupWorks.setHearedRequestPassNum(
				worldModel->getHearedRequestPassNum());
		groupWorks.setHearedRequestPassSender(
				worldModel->getHearedSender());

		groupWorks.setHearedRequestPassTime(worldModel->getCurTime());
	}*/

/*	if (worldModel->getMustAttention())
	{
		form.sayForm.attentionNum =
				worldModel->getHearedInterceptNum();
	}*/

	worldModel->updateCurInterCalculate();
	worldModel->updateBallStatus();

	statelessPlan->hear(exp);
}

bool AdvancedAgent::bodyDecide(int quickLevel)
{
	LOG << "AdvancedAgent::bodyDecide" << endl;

	if (isBodyCycleCommandLost() && lastBodyCycleCommand->getCreator() == AT_PASS)
		groupWorks.resetByChangePlayMode();

	if (form.setCycle != worldModel->getCurCycle())
	{
		form.setCycle = worldModel->getCurCycle();
		form.reset();
	}

	library.lastBodyCycleCommand = lastBodyCycleCommand;

	library.positioning.setHomePoints();
	groupWorks.updateInterceptNum(library);

	library.gwSelection = groupWorks.getWithoutBallSelection(library);
	library.quickLevel = quickLevel;

	if (quickLevel == 0 && worldModel->getBall().isValid() &&
			worldModel->isBallKickable())
	{
		worldModel->updateCurKickCalculate();
		library.pass.calculate();
	}

	form.headForm.tnMode = TNM_AUTO_PLAY_ON;

	try
	{
		if (rootPlan && !rootPlan->isFinished() &&
				rootPlan->successRate() > 
				config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())
			rootPlan->decide();
		else
		{
			statelessPlan->decide();
		}
	}
	catch (Command *cmd)
	{
		bodyCycleCommand = cmd;
	}

	if (bodyCycleCommand == NULL)
		bodyCycleCommand = new EmptyCommand(AT_NONE);

	if (form.setBreakExperiment)
		library.breakExperiment = form.setBreakExperiment;
	
	if (bodyCycleCommand->getCreator() == AT_POSITIONING ||
		bodyCycleCommand->getCreator() == AT_BASIC_AGENT ||
		bodyCycleCommand->getCreator() == AT_BASIC_COACH ||
		bodyCycleCommand->getCreator() == AT_ADVANCED_COACH ||
		bodyCycleCommand->getCreator() == AT_KICK_CALCULATE ||
		bodyCycleCommand->getCreator() == AT_CROSS_CALCULATE ||
		bodyCycleCommand->getCreator() == AT_SAY_DECISION ||
		bodyCycleCommand->getCreator() == AT_HEAD_DECISION ||
		bodyCycleCommand->getCreator() == AT_POINTTO_DECISION)
		assert(0);


	if (worldModel->getBall().getAbsVec().getMagnitude() < 7.5)
		return true;
	return false;
}

void AdvancedAgent::sayDecide()
{
	radar.sender(form, library);
	radar.receiver(form, library);

/*	try
	{
		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 
			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())
			rootPlan->sayDecide();
		else
			statelessPlan->sayDecide();
	}
	catch (pair<Command *, Command *> cmds)
	{
		sayCycleCommand = cmds.first;
		attentionCycleCommand = cmds.second;
	}*/

	groupWorks.updateInterceptNum(library);

	groupWorks.signInterceptNum(form.sayForm);
	sayDecision.decide(bodyCycleCommand, form.sayForm);

	sayCycleCommand = sayDecision.getSayCommand();
	attentionCycleCommand = sayDecision.getAttentionCommand();
}

void AdvancedAgent::pointToDecide()
{
/*	try
	{
		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 
			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())
			rootPlan->pointToDecide();
		else
			statelessPlan->pointToDecide();
	}
	catch (Command *cmd)
	{
		pointToCycleCommand = cmd;
	}*/

	pointToDecision.decide(headCycleCommand);

	pointToCycleCommand = pointToDecision.getPointToCommand();
}

void AdvancedAgent::headDecide()
{
/*	try
	{
		if (rootPlan && !rootPlan->isFinished() && rootPlan->successRate() > 
			config["Agent"]["PlanningSystem"]["MinSuccessRate"].asFloat())
			rootPlan->headDecide();
		else
			statelessPlan->headDecide();
	}
	catch (pair<Command *, Command *> cmds)
	{
		LOG << "Catched by Handler" << endl;
		headCycleCommand = cmds.first;
		changeViewCycleCommand = cmds.second;
		LOG << "End Catch" << endl;
	}*/

	LOG << "AdvancedAgent::headDecide" << endl;
	headDecision.decide(bodyCycleCommand, form.headForm);

	headCycleCommand = headDecision.getTurnNeckCommand();
	changeViewCycleCommand = headDecision.getChangeViewCommand();

}


// Radar functions
RadarSystem::RadarSystem(const WorldModel *worldModel): worldModel(worldModel)
{
	sayingsNum = 0;
	for (int i = 0; i < 3; i++)
		sayings[i] = NULL;

	semiRandom = 0;
	lastUpdateTime = 0;

	lastTargetPlayer = NULL;
}

void RadarSystem::sender(Form &form, const Library &library)
{
	LOG << "RadarSystem::sender" << endl;

	if (lastUpdateTime != (int)worldModel->getCurTime())
	{
		for (int i = 0; i < 3; i++)
		{
			saids[i] = sayings[i];
			sayings[i] = NULL;
		}
		saidsNum = sayingsNum;
		sayingsNum = 0;

		lastUpdateTime = worldModel->getCurTime();
	}

	if (worldModel->getBall().getSeeTime() - worldModel->getCurTime() >= 2 ||
		library.gwSelection == WOBS_INTERCEPT ||
		worldModel->isBallKickable())
		return;

	const Player &targetPlayer(worldModel->getNearestTmmToBall());
	if (&targetPlayer != lastTargetPlayer)
	{
		saidsNum = 0;
		for (int i = 0; i < 3; i++)
			saids[i] = NULL;
	}

	lastTargetPlayer = &targetPlayer;

LOG << "targetPlayer: " << targetPlayer.getUniNum() << endl;

	const Player *candidatePlayers[3];
	const Player *starredPlayer = NULL;

	for (int i = 0; i < 3; i++)
		candidatePlayers[i] = NULL;

	switch (worldModel->getBody().getUniNum())
	{
	case 10:
		if (targetPlayer.getUniNum() == 2)
		{
			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 10 - 1);
			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 3 - 1);
		}
		else if (targetPlayer.getUniNum() == 3)
		{
			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 2 - 1);
			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 4 - 1);
		}
		break;

	case 11:
		if (targetPlayer.getUniNum() == 4)
		{
			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 11 - 1);
			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 3 - 1);
		}
		break;

	case 6:
		if (targetPlayer.getUniNum() == 10)
		{
			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 6 - 1);
			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 2 - 1);
		}
		break;

	case 9:
		if (targetPlayer.getUniNum() == 11)
		{
			candidatePlayers[0] = &worldModel->getFullPlayer(TID_TEAMMATE, 9 - 1);
			candidatePlayers[1] = &worldModel->getFullPlayer(TID_TEAMMATE, 4 - 1);
		}
		break;

	}

	if (candidatePlayers[0] == NULL)
		return;

	form.headForm.tnMode = TNM_LOOK_NORMALLY_TO_BALL;
	LOG << "\tTNM_LOOK_NORMALLY_TO_BALL" << endl;

	if (worldModel->getBall().getDistance(targetPlayer) > 5)
		return;

	int counter = 0;
	while (!(candidatePlayers[semiRandom] != NULL &&
		!isSaid(*candidatePlayers[semiRandom]) &&
		candidatePlayers[semiRandom]->isValid()))
	{
		semiRandom++;
		if (semiRandom >= 3)
			semiRandom = 0;

		counter++;
		if (counter == 10) // I have fallen in a loop
			return;
	}

	starredPlayer = candidatePlayers[semiRandom];
LOG << "\tstarredPlayer: " << starredPlayer->getUniNum() << endl;

	sayings[0] = starredPlayer;
	sayingsNum = 1;

	anglesNum = 0;

	int i;
	for (i = 0; i < FULL_PLAYERS_NUM; i++)
	{
		if (worldModel->getFullPlayer(TID_OPPONENT, i).isValid() &&
			!isSaid(worldModel->getFullPlayer(TID_OPPONENT, i)) &&
			worldModel->getFullPlayer(TID_OPPONENT, i).getDistance(worldModel->getBall()) <
				starredPlayer->getDistance(worldModel->getBall()) + 3 &&
			worldModel->getFullPlayer(TID_OPPONENT, i).getDistance(worldModel->getBall()) < 28)
		{
			angPlayers[anglesNum] = &worldModel->getFullPlayer(TID_OPPONENT, i);

			Vector passVec;
			Vector enemyVec;

			enemyVec.setByPoints(targetPlayer.getPos(), worldModel->getFullPlayer(TID_OPPONENT, i).getPos());
			passVec.setByPoints(targetPlayer.getPos(), starredPlayer->getPos());

			anglesList[anglesNum] = fabs(Degree::getDeltaAngle(
					enemyVec.getDirection(), passVec.getDirection()));

LOG << "\tangPlayer[" << anglesNum << "]: " << angPlayers[anglesNum]->getUniNum() << endl;
LOG << "\tanglesList[" << anglesNum << "]: " << anglesList[anglesNum] << endl;

			anglesNum++;
		}
	}

	if (anglesNum != 0)
	{
		int miner = 0;
		for (i = 1; i < anglesNum; i++)
			if (anglesList[i] < anglesList[miner])
				miner = i;
		sayings[1] = angPlayers[miner];
		sayingsNum = 2;

LOG << "\tsayings[1]: " << sayings[1]->getUniNum() << endl;

		if (anglesNum != 1)
		{
			anglesList[miner] = (float)0xFFFF; // putting this out

			miner = 0;
			for (i = 1; i < anglesNum; i++)
				if (anglesList[i] < anglesList[miner])
					miner = i;

			sayings[2] = angPlayers[miner];
			sayingsNum = 3;
LOG << "\tsayings[2]: " << sayings[2]->getUniNum() << endl;
		}
	}

	Ball ball(worldModel->getBall());
	ball.simulateByDynamics(worldModel->getBody());

	Player simPlayers[3];
	for (int i = 0; i < sayingsNum; i++)
	{
		simPlayers[i] = *sayings[i];
		simPlayers[i].simulateByDynamics(worldModel->getBody());
	}

	form.sayForm.radarSay = true;
	form.sayForm.radarPlayersNum = sayingsNum;
	for (int i = 0; i < sayingsNum; i++)
	{
		Vector vec;
		vec.setByPoints(ball.getPos(), simPlayers[i].getPos());
		form.sayForm.radarPlayers[i] = vec;

		int id = simPlayers[i].getTeamId() * 11 +
					simPlayers[i].getUniNum() - 1;
		form.sayForm.ids[i] = id;
	}

}

bool RadarSystem::isSaid(const Player &player)
{
	for (int i = 0; i < saidsNum; i++)
		if (&player == saids[i])
			return true;

	return false;
}

void RadarSystem::receiver(Form &form, const Library &library)
{

	if (worldModel->isBallKickable() ||
		(library.gwSelection == WOBS_INTERCEPT &&
		 worldModel->getBall().getAbsVec().getMagnitude() < 5))
	{
	}
	else
		return;

	switch (worldModel->getBody().getUniNum())
	{
		case 2: case 3:
			form.sayForm.attentionNum = 10;
			break;

		case 4:
			form.sayForm.attentionNum = 11;
			break;

		case 10:
			form.sayForm.attentionNum = 6;
			break;

		case 11:
			form.sayForm.attentionNum = 9;
			break;
	}

	LOG << "RadarSystem::receiver setted" << endl;
}