plan.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: Arash Rahimi, Sassan Haradji
 *
 *  Released on Monday 1 August 2005, 10 Mordad 1384 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <Plan.h>
#include <Logger.h>
#include <cstdarg>
#include <cassert>


#define PLAN_LOG_LEVEL 0

using namespace std;

Plan::Plan(std::string name, const WorldModel *wm, Form &form,
		const Library &library): library(library), form(form), subPlans(),
		threshold(THRESHOLD), wm(wm), name(name), planLogLevel(PLAN_LOG_LEVEL)
{
}

Plan::~Plan()
{
}

void Plan::decide() 
{
	while(!subPlans.empty())
	{
		Plan *plan = subPlans.top();
		if (!tryPlan(plan))
		{
	    	subPlans.pop();
			delete plan;
		}
	}
}

bool Plan::tryPlan(Plan *plan, double threshold)
{
	if (!plan->isFinished() && plan->successRate() >= threshold)
	{
		if (planLogLevel >= 1)
			LOG << plan->name << " Plan" << " With " << plan->successRate()
					<< " Now Run." << endl;
		plan->decide();
		return true;
	}
	if (planLogLevel >= 1)
	{
		if (plan->isFinished())
			LOG << plan->name << " Plan" << " is Finished" << endl;
		if (plan->successRate() < threshold)
			LOG << plan->name << " Plan" << " With " << plan->successRate()
					<< " Failed." << endl;
	}
	return false;
}

bool Plan::startPlan(Plan *plan, double threshold) 
{
	subPlans.push(plan);
	bool returnValue = tryPlan(plan, threshold);
	subPlans.pop();
	delete plan;
	return returnValue;
}

Plan* Plan::bestPlan(vector<Plan *> &plans, bool deletePlans)
{
	LOG << "bestPlan Run" << endl;
	LOG << "WARNING : this function (bestPlan by vector) is incomplete" << endl;

	assert(plans.capacity() >= 1);

	Plan * bestPlan = NULL;
	double bestSuccessRate = 0;
	
	for (unsigned i = 0; i < plans.capacity(); i++)
	{
		LOG << "Plan " << plans[i]->name << " has " << plans[i]->successRate()
				<< " successRate" << endl;
		if (plans[i])
			if (plans[i]->successRate() > bestSuccessRate || bestPlan == NULL)
			{
				if (deletePlans) delete bestPlan;
				bestPlan = plans[i];
				bestSuccessRate = plans[i]->successRate();
			}
			else
				if (deletePlans) delete plans[i];
	}	
	
	LOG << "bestPlan finished : Plan " << bestPlan->name << " with "
			<< bestPlan->successRate() << " successRate" << endl;
	
	return bestPlan;
}

Plan* Plan::bestPlan(int num, bool deletePlans, ...)
{
	LOG << "bestPlan Run" << endl;
	assert(num >= 1);

  va_list planList;
  va_start(planList, deletePlans);
	
	Plan * bestPlan = NULL, * tempPlan;
	double bestSuccessRate = 0;
	
	for (int i = 0; i < num; i++)
	{
		tempPlan = va_arg(planList, Plan*);
		if (!tempPlan) continue;
		LOG << "Plan " << tempPlan->name << " Has " << tempPlan->successRate()
				<< " successRate" << endl;
		if (tempPlan->successRate() > bestSuccessRate || bestPlan == NULL)
		{
			if (deletePlans) if (bestPlan != NULL) delete bestPlan;
			bestPlan = tempPlan;
			bestSuccessRate = bestPlan->successRate();
		}
		else
			if (deletePlans) delete tempPlan;
	}	

	va_end(planList);
	LOG << "bestPlan finished : Plan " << bestPlan->name << " with "
			<< bestPlan->successRate() << " successRate" << endl;
	
	return bestPlan;
}

void Plan::resetPlans()
{
	while(!subPlans.empty())
	{
		delete subPlans.top();
		subPlans.pop();
	}
}

double Plan::getThreshold() 
{
    return threshold;
}

void Plan::setThreshold(double t)
{
    threshold = t;
}

bool Plan::isFinished()
{
	return true;
}

double Plan::successRate()
{
	return 0;
}

std::string Plan::getName()
{
	return name;
}

void Plan::setName(std::string n)
{
	name = n;
}


Plan* Plan::bestPlan(vector<Plan *> &plans, vector<float> addedSuccessRates,
		bool deletePlans)
{
	Plan* bestPlan = NULL;
	double bestSuccessRate = 0;

	for (unsigned i = 0; i < plans.size(); i++)
	{
		float tmpPlanSuccessRate = plans[i]->successRate();
		LOG << "Plan " << plans[i]->getName() << " has "
				<< tmpPlanSuccessRate + addedSuccessRates[i] << " successRate"
				<< "(" << tmpPlanSuccessRate << " real successRate, "
				<< addedSuccessRates[i] << " added successRate)"
				<< endl;
		if (tmpPlanSuccessRate + addedSuccessRates[i] >
				bestSuccessRate)
		{
			bestPlan = plans[i];
			bestSuccessRate = tmpPlanSuccessRate + addedSuccessRates[i];
		}
	}	

	for (vector<Plan*>::iterator i = plans.begin(); i != plans.end();)
	{
		Plan* j = *i;
		i++;
		if (bestPlan != j && deletePlans)
			delete j;
	}

	return bestPlan;
}