mediator.cpp

robocup源代码2001年清华机器人源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library 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
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "stdafx.h"

#include "global.h"
#include "mediator.h"

/**************   PrioredCommand *************************/
PrioredCommand::PrioredCommand(){
	Command::Command();
	priority = 0.0f;
	actiontype = Action_none;
}

/*************	VisualRequestQueue	*************************/
VisualRequest::VisualRequest(float priority, AngleDeg angle){
	this->priority = priority;
	this->angle = NormalizeAngle(angle);
}

AngleDeg VisualRequest::RequestedAngle_(){
	return MidAngle(angle,angle_inf);
}

float VisualRequest::RequestedPriority(){
	if(view_width == VW_Narrow) return priority * CP_VW_Narrowfactor;
	else return priority;
}

/**********		Mediator	**************************/
Mediator::Mediator(){

	VisualRequest test[10];
	VisualRequest requests[10];
	int total;

	test[0] = VisualRequest(1.0f, 98.5f);
	test[1] = VisualRequest(1.0f, 106.5f);
	test[2] = VisualRequest(1.0f, 40.0f);
	test[3] = VisualRequest(1.0f, 60.0f);
	vr_queue.cleanup();
	vr_queue.enqueue(test[0]);
	vr_queue.enqueue(test[1]);
	vr_queue.enqueue(test[2]);
	vr_queue.enqueue(test[3]);

	total = 0;
	SetVisualRequests(visualrequests, totalvisualrequests, VW_Narrow);
	SetVisualRequests(visualrequests, totalvisualrequests, VW_Normal);
}

void Mediator::ResetBuffer(){
//	visualrequestqueue.cleanup();
//	countervisualrequestqueue.cleanup();
	vr_queue.cleanup();
	ac_queue.cleanup();
	totalvisualrequests = 0;
}


bool Mediator::enroll(PrioredCommand& pcmd){
	DoLog("Enroll cmdtype %d actype %d p %.2f", pcmd.type, pcmd.actiontype, pcmd.priority);
	if(pcmd.type == CMD_none) return false;
	pcmd.priority = priority_mapping(pcmd.actiontype, pcmd.priority);
	return ac_queue.enqueue(pcmd);
}

bool Mediator::enroll(Command& command, ActionType actiontype, float priority){
	return enroll(PrioredCommand(command, actiontype, priority));
}

void Mediator::Getbestaction(PrioredCommand& bestaction){
	bestaction.Reset();
	if(!ac_queue.IsEmpty()){
		bestaction = ac_queue.GetData(ac_queue.ActualHead());
	}
}

void Mediator::SetViewWidth(VIEWWIDTH view_width){
	if(view_width == VW_Narrow)
		DoLog(LOG_MEDIATION, "force narrow mode");
	else if(view_width == VW_Normal)
		DoLog(LOG_MEDIATION, "force normal mode");
	else
		DoLog(LOG_MEDIATION, "force wide mode");
	designated_viewwidth.Setdata(view_width, situation.CurrentTime);
}

void Mediator::DisableViewWidth(){
	designated_viewwidth.time = -2;
}

bool Mediator::enroll(float priority, AngleDeg angle){
	DoLog(LOG_MEDIATION, "enroll %.2f %.2f", priority, angle);
	return vr_queue.enqueue(VisualRequest(priority_mapping(Action_visual, priority), angle));
}

void Mediator::SetVisualRequests(VisualRequest * requests, int& totalrequests, VIEWWIDTH view_width, int max_requests){
	int p, q;
	float ang, priority, sum_priority, angle_inf, angle_sup, view_width_ang;
	VisualRequest treq;

	if (totalrequests < 0) return;
	if (vr_queue.IsEmpty()) return;

	p = q = vr_queue.ActualHead();
	priority = sum_priority = 0.0f;
	
	view_width_ang = sensory.MyViewWidth(view_width);

	//set visual-requests clockwisely
	while(vr_queue.IsValid(p)){
		if (totalrequests >= max_requests)//exceed max requests
			break;

		requests[totalrequests].angle = vr_queue.GetData(p).angle;
		requests[totalrequests].view_width = view_width;

		if (sum_priority > 0){
			requests[totalrequests].priority = sum_priority
				= priority_sub(sum_priority, priority);
		}
		else{
			sum_priority = requests[totalrequests].priority = 0.0f;
		}

		if(p == q){
			requests[totalrequests].angle_inf = requests[totalrequests].angle;
		}
		else{
			requests[totalrequests].angle_inf = angle_inf;
		}
		
		vr_queue.GetData(p).view_width = view_width;
		priority = vr_queue.GetData(p).RequestedPriority();

		do{
			treq = vr_queue.GetData(q);
			treq.view_width = view_width;
			ang = NormalizeAngle(treq.angle - requests[totalrequests].angle, 0.0f);
			if (ang > view_width_ang){
				break;
			}
			angle_inf = requests[totalrequests].angle_inf = treq.angle;
			sum_priority = requests[totalrequests].priority = 
				priority_sum(requests[totalrequests].priority, treq.RequestedPriority());
			q = vr_queue.CircularNext(q);
		}while(q != p && vr_queue.IsValid(q));
		//when searching counterclockwisely, some requests will recur, seting angle_inf
		//will help to detect it
		vr_queue.GetData(p).angle_inf = angle_inf;

		totalrequests ++;
		p = vr_queue.Next(p);
	}
//set visual-requests counter-clockwisely
	p = q = vr_queue.Tail();
	priority = sum_priority = 0.0f;
	int sup_element = -1;
	
	while(!vr_queue.IsHead(p) && vr_queue.IsValid(p)){
		if (totalrequests >= max_requests)//exceed max requests
			break;

		requests[totalrequests].angle_inf = vr_queue.GetData(p).angle;
		requests[totalrequests].view_width = view_width;

		if (sum_priority > 0){
			requests[totalrequests].priority = sum_priority 
				= priority_sub(sum_priority, priority);
		}
		else{
			sum_priority = requests[totalrequests].priority = 0.0f;
		}

		if(p == q){
			requests[totalrequests].angle = requests[totalrequests].angle_inf;
		}
		else{
			requests[totalrequests].angle = angle_sup;
		}

		vr_queue.GetData(p).view_width = view_width;
		priority = vr_queue.GetData(p).RequestedPriority();

		do{
			treq = vr_queue.GetData(q);
			treq.view_width = view_width;
			ang = NormalizeAngle(requests[totalrequests].angle_inf - treq.angle, 0.0f);
			if (ang > view_width_ang){
				break;
			}
			angle_sup = requests[totalrequests].angle = treq.angle;
			sum_priority = requests[totalrequests].priority = 
				priority_sum(requests[totalrequests].priority, treq.RequestedPriority());

			sup_element = q;
			q = vr_queue.CircularPrev(q);
		}while(q != p && vr_queue.IsValid(q));

		if (vr_queue.IsValid(sup_element)){
			if (vr_queue.GetData(sup_element).angle_inf != vr_queue.GetData(p).angle)
				totalrequests ++;
		}

		p = vr_queue.Prev(p);
	}
}


void Mediator::SetVisualRequests(){
	if (designated_viewwidth.time == situation.CurrentTime){
		SetVisualRequests(visualrequests, totalvisualrequests, designated_viewwidth.data);
	}
	else{
		SetVisualRequests(visualrequests, totalvisualrequests, VW_Narrow);
		SetVisualRequests(visualrequests, totalvisualrequests, VW_Normal);
	}
}

void Mediator::mediation(){
	
	PrioredCommand bestaction, retraction;
	Command cmd1, cmd2;
	VisualRequest tmprequest;
	float ang, bdfy, sum_priority, maxpriority, maxvisualpriority, bestaction_bdfy, ang_inf, ang_sup;
	int vreq_idx, p, max_vreq_idx;

	SetVisualRequests();
	//sort the visualrequests by priority
	for(int i =0; i < totalvisualrequests - 1; i ++){
		for(int j =i+1; j < totalvisualrequests; j ++){
			if (visualrequests[j].priority > visualrequests[i].priority){
				Swap(visualrequests[i], visualrequests[j]);
			}
		}
	}

	//see if the visualrequests is reachable
	ang = Min(SP_max_neck_angle + sensory.MyViewAngle(visualrequests[i].view_width) + action.Max_TurnAng(),
		180.0f);
	ang_inf = NormalizeAngle(Self.bodyfacing - ang);
	ang_sup = 2 * ang;
	max_vreq_idx = -1;
	maxvisualpriority = 0.0f;
	for(i = 0; i < totalvisualrequests; i ++){
		ang = Max(NormalizeAngle(visualrequests[i].angle - ang_inf, 0.0f),
				NormalizeAngle(visualrequests[i].angle_inf - ang_inf, 0.0f));
		if (ang > ang_sup) visualrequests[i].valid = false;
		else{
			visualrequests[i].valid = true;
			if (max_vreq_idx == -1){
				max_vreq_idx = i;
				maxvisualpriority = visualrequests[i].priority;
			}
		}
	}

	for(i =0; i < totalvisualrequests; i ++){
		if (!visualrequests[i].valid) continue;
//		DoLog(LOG_MEDIATION, "v r %d p %f (%.2f %.2f) %d", i,
//			visualrequests[i].priority, visualrequests[i].angle, visualrequests[i].angle_inf, visualrequests[i].view_width);
	}

	bestaction.Reset();
	vreq_idx = -1;
	maxpriority = 0.0f;

	
	p = ac_queue.ActualHead();
	//while(ac_queue.IsValid(p)){
	if(ac_queue.IsValid(p)){
		retraction = ac_queue.GetData(p);
		DoLog(LOG_ACTION,"action priority %.2f",retraction.GetPriority());

	//	if (priority_sum(retraction.priority, maxvisualpriority) <= maxpriority)
	//		break;

		bdfy = Self.bodyfacing;//bdfy when exec this command
		if (retraction.type == CMD_turn){
			bdfy = NormalizeAngle(retraction.angle * action.Turnrate() + bdfy);
		}

		//check if visual reuqests are compatible with the action
		if (retraction.type == CMD_stay){
			i = max_vreq_idx;
		}
		else{
			for(i =0; i < totalvisualrequests; i ++){
				if (!visualrequests[i].valid) continue;
				ang_inf = NormalizeAngle(bdfy - SP_max_neck_angle - sensory.MyViewAngle(visualrequests[i].view_width));
				ang_sup = 2 * (SP_max_neck_angle + sensory.MyViewAngle(visualrequests[i].view_width));
				
				ang = Max(NormalizeAngle(visualrequests[i].angle - ang_inf, 0.0f),
					NormalizeAngle(visualrequests[i].angle_inf - ang_inf, 0.0f));

				if (ang > ang_sup) continue;

				break;
			}
		}

		if (i >= totalvisualrequests){
		//no visual request that is compatible with this action
			sum_priority = retraction.priority;
			i = -1;
		}
		else{
			sum_priority = priority_sum(retraction.priority, visualrequests[i].priority);
		}

		if (sum_priority > maxpriority){
			maxpriority = sum_priority;
			vreq_idx = i;
			bestaction = retraction;
			bestaction_bdfy = bdfy;
		}

		p= ac_queue.Next(p);
	}

	if (max_vreq_idx != -1){
		if (visualrequests[max_vreq_idx].priority > maxpriority){
			bestaction.Reset();
			vreq_idx = max_vreq_idx;
		}
	}

//execute it
	if (bestaction.type != CMD_none && bestaction.type != CMD_stay){
		action.execute(bestaction);
		if (vreq_idx != -1){
			DoLog(LOG_MEDIATION, "view width %d %d", visualrequests[vreq_idx].view_width, sensory.LastViewWidth);
			if(Self.viewwidth.Data(situation.CurrentTime) != visualrequests[vreq_idx].view_width){
				action.change_view(visualrequests[vreq_idx].view_width, VQ_High);
			}

			motion.turn_neck_to(visualrequests[vreq_idx].RequestedAngle_(),
				bestaction_bdfy, cmd2);
		}
		else if (designated_viewwidth.time == situation.CurrentTime){			
			DoLog(LOG_MEDIATION, "view width %d", designated_viewwidth.data);
			if(Self.viewwidth.Data(situation.CurrentTime) != designated_viewwidth.data){
				action.change_view(designated_viewwidth.data, VQ_High);
			}
		}
		action.execute(cmd2);
	}
	else{
		if (vreq_idx != -1){
			DoLog(LOG_MEDIATION, "view width %d %d", visualrequests[vreq_idx].view_width, sensory.LastViewWidth);
			if(Self.viewwidth.Data(situation.CurrentTime) != visualrequests[vreq_idx].view_width){
				action.change_view(visualrequests[vreq_idx].view_width, VQ_High);
			}
			bdfy = NormalizeAngle(Self.bodyfacing +
				motion.turn_to(visualrequests[vreq_idx].RequestedAngle_(), cmd1));
			motion.turn_neck_to(visualrequests[vreq_idx].RequestedAngle_(),
				bdfy, cmd2);
			action.execute(cmd1);
			action.execute(cmd2);
		}
		if (designated_viewwidth.time == situation.CurrentTime){
			DoLog(LOG_MEDIATION, "view width %d %d", designated_viewwidth.data, sensory.LastViewWidth);
			if(Self.viewwidth.Data(situation.CurrentTime) != designated_viewwidth.data){
				action.change_view(designated_viewwidth.data, VQ_High);
			}
		}
	}
//feedback
	feedback(bestaction, vreq_idx);
}

void Mediator::feedback(const PrioredCommand& bestaction, int vr_idx){
	lastactiontype = bestaction.actiontype;
	lastcmdtype = bestaction.type;
	switch (bestaction.actiontype){
	case Action_interception:
		DoLog(LOG_ACTION, "Interception");
		break;
	case Action_turn_ball:
		DoLog(LOG_ACTION,"Turn Ball");
		break;
	case Action_shoot:
		DoLog(LOG_ACTION,"Shoot (spd %.2f angle %.2f)", motion.GetMinShootSpeed(), motion.GetShootAngle());
		break;
	case Action_dribble:
		DoLog(LOG_ACTION,"Dribble ang %.1f",motion.GetDribbleAngle());
		break;
	case Action_fastdribble:
		DoLog(LOG_ACTION,"FasetDribble");
		break;
	case Action_positioning_coordinate:
		DoLog(LOG_ACTION,"Coordinated attack");
		break;
	case Action_avoidenemy:
		DoLog(LOG_ACTION,"Avoid enemy");
		break;
	case Action_pass:
		PassOut.time = situation.CurrentTime;
		DoLog(LOG_ACTION,"Pass to %d(spd %.2f,angle %.1f)",motion.GetReceiver(), motion.GetPassSpeed(), motion.GetPassAngle());
		break;
	case Action_passwait:
		DoLog(LOG_ACTION,"(Waiting)Pass to %d(spd %.2f,angle %.1f)",motion.GetReceiver(), motion.GetPassSpeed(), motion.GetPassAngle());
		break;
	case Action_holdball:
		DoLog(LOG_ACTION, "Hold ball");
		break;
	case Action_positioning_offense:
		DoLog(LOG_ACTION, "Position Offense");
		break;
	case Action_positioning_defense:
		DoLog(LOG_ACTION, "Position Defense");
		break;
	case Action_positioning_block:
		DoLog(LOG_ACTION, "Position Block");
		break;
	case Action_positioning_mark:
		DoLog(LOG_ACTION, "Position Mark");
		break;
	case Action_press:
		DoLog(LOG_ACTION, "Press");
		break;
	case Action_other:
		DoLog(LOG_ACTION, "Other");
		break;
	case Action_clear:
		DoLog(LOG_ACTION, "ClearBall");
		break;
	case Action_none:
		if (!situation.ClockStopped && situation.playmode == PM_Play_On)
			DoLog(LOG_ACTION, "None");
		break;
	default:
		DoLog(LOG_ACTION, "Not defined");
		break;
	}
	if (vr_idx != -1){
		DoLog(LOG_ACTION, "Visual request %d (%.2f %.2f)", vr_idx, visualrequests[vr_idx].angle, visualrequests[vr_idx].angle_inf);
	}
}

ActionType Mediator::LastActionType(){
	return lastactiontype;
}

CMDType Mediator::LastCmdType(){
	return lastcmdtype;
}

float Mediator::priority_mapping(ActionType actiontype, float priority){
	switch(actiontype){
	case Action_pass:
	case Action_passwait:
	case Action_shoot:
	case Action_dribble:
	case Action_fastdribble:
	case Action_catch:
	case Action_turn_ball:
	case Action_holdball:
	case Action_avoidenemy:
	case Action_clear:
		return priority * 0.2f + 0.8f;
	case Action_visual:
		return priority * 0.4f;
	case Action_interception:
		return priority * 0.4f + 0.4f;
	case Action_positioning_offense:
	case Action_positioning_defense:
	case Action_positioning_block:
	case Action_positioning_mark:
	case Action_positioning_coordinate:
	case Action_positioning_setplay:
	case Action_press:
		return priority * 0.4f + 0.4f;
	default:
		break;
	}
	return priority;
}