auditorysystem.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 "AuditorySystem.h"
#include "global.h"

/*******************   Class AuditoryProcess     **************/
AuditoryProcess::AuditoryProcess(){
	p_auditorybuffer = 0;
	LastSpeakingTime = -20;
}

void AuditoryProcess::ResetBuffer(){
	p_auditorybuffer = 0;
}

void AuditoryProcess::AddData(MsgData msgdata){
	switch(msgdata.precision){
	case Msg_INT:
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer]," %.0f", msgdata.data);
		break;
	case Msg_ShortFloat:
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer]," %.2f", msgdata.data);
		break;
	case Msg_LongFloat:
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer]," %.4f", msgdata.data);
		break;
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata);
	}
	else{
		AddData(msgdata);
		p_auditorybuffer +=sprintf(&auditorybuffer[p_auditorybuffer],")");
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata1, MsgData msgdata2){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata1, msgdata2);
	}
	else{
		AddData(msgdata1);
		AddMsg(msgNo, msgdata2);
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata1, MsgData msgdata2, MsgData msgdata3){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata1, msgdata2, msgdata3);
	}
	else{
		AddData(msgdata1);
		AddMsg(msgNo, msgdata2, msgdata3);
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata1, MsgData msgdata2, MsgData msgdata3, MsgData msgdata4){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata1, msgdata2, msgdata3, msgdata4);
	}
	else{
		AddData(msgdata1);
		AddMsg(msgNo, msgdata2, msgdata3, msgdata4);
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata1, MsgData msgdata2, MsgData msgdata3, MsgData msgdata4, MsgData msgdata5){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata1, msgdata2, msgdata3, msgdata4, msgdata5);
	}
	else{
		AddData(msgdata1);
		AddMsg(msgNo, msgdata2, msgdata3, msgdata4, msgdata5);
	}
}

void AuditoryProcess::AddMsg(int msgNo, MsgData msgdata1, MsgData msgdata2, MsgData msgdata3, MsgData msgdata4, MsgData msgdata5, MsgData msgdata6){
	if (msgNo >= 0){
		p_auditorybuffer += sprintf(&auditorybuffer[p_auditorybuffer],"(%d", msgNo);
		msgNo = -1;
		AddMsg(msgNo, msgdata1, msgdata2, msgdata3, msgdata4, msgdata5, msgdata6);
	}
	else{
		AddData(msgdata1);
		AddMsg(msgNo, msgdata2, msgdata3, msgdata4, msgdata5, msgdata6);
	}
}

// Add Header for identification
void AuditoryProcess::AddHeader(){
	sprintf(saybuffer, "(%s %d %d) %s", CP_TeamName, encrypt(situation.CurrentTime), encrypt(MyNumber), auditorybuffer);
}
/*************	ProcessMsg	******************/
void AuditoryProcess::ProcessMsg(char * auditorymsg){
	float from_angle = get_float(&auditorymsg);
	get_word(&auditorymsg); 
	// Compare TeamName
	if (strncmp(auditorymsg, CP_TeamName, strlen(CP_TeamName)) != 0)
		return;
	// Get Time
	int time = get_int(&auditorymsg);
	time = decrypt(time);
	if (time > situation.CurrentTime + 1 || time < situation.CurrentTime - 2)
		return;

	int playernum = get_int(&auditorymsg);
	playernum = decrypt(playernum);
	if (playernum <=0 || playernum > SP_team_size)
		return;
	if (playernum == MyNumber)
		return;
	
	char* p = auditorymsg;
	while(*p && *p != '\"') p ++;
	*p = 0;
	auditorymsg += 2; //skip ')' and ' '
	decipher(auditorymsg, strlen(auditorymsg));
	DoRecord(LOG_HEAR, "%s", auditorymsg);
	while(ProcessSetMsg(&auditorymsg)){
		switch(MsgNo){
		case Msg_MyPlayerPos:
			HeardMyPlayerPos(int(msgData[0]), time);
			break;
		case Msg_TheirPlayerPos:
			HeardTheirPlayerPos(int(msgData[0]), time);
			break;
		case Msg_BallPos:
			HeardBallPos(time);
			break;
		case Msg_BallVel:
			HeardBallVel(time);
			break;
		case Msg_Ballcontroller:
			HeardBallcontrollers(time);
			break;
		case Msg_Pass:
			HeardPass(time);
			break;
		}
	}
}

//	Return Ture if it process a set of message, return false
//	if error format or reach the end of message
bool AuditoryProcess::ProcessSetMsg(char** p_buffer){
	char* boundary;
	get_to_left_blanket(p_buffer);
	if (** p_buffer != '(') return false;

	boundary = *p_buffer;
	get_to_right_blanket(&boundary);

	if (*boundary != ')') return false;
	

	MsgNo = int(get_float(p_buffer));
	if (MsgNo < 0 || *p_buffer > boundary) return false;

	MsgDataNum = -1;
	while(*p_buffer <= boundary && MsgDataNum < CP_max_msgdata - 1){
		msgData[++MsgDataNum] = get_float(p_buffer);
	}
	*p_buffer = boundary;
	return true;
}

/**********************	SendMsg	***************************/
void AuditoryProcess::SendMsg(){
	action.say(saybuffer);
	LastSpeakingTime = situation.CurrentTime;
}

/*********************	Encryption&Decryption	*********************/
int AuditoryProcess::ciphertext(char m){
	if (m >= '0' && m <= '9'){
		return m - '0' + 1;
	}
	switch(m){
	case '.':
		return 11;
	case '(':
		return 12;
	case ')':
		return 13;
	case ' ':
		return 14;
	case '-':
		return 15;
	}
	return 0;
}

char AuditoryProcess::deciphertext(int e){
	if (e <= 10) return char(e -1 + '0');
	switch(e){
	case 11:
		return '.';
	case 12:
		return '(';
	case 13:
		return ')';
	case 14:
		return ' ';
	case 15:
		return '-';
	}
	return 0;
}

char AuditoryProcess::map_into_printablechar(int i){
	if (i < 10) return i + '0';
	i -= 10;
	if (i < 26) return i + 'a';
	i -= 26;
	if (i < 26) return i + 'A';
	i -= 26;
	if (i == 0) return '(';
	if (i == 1) return ')';
	if (i == 2) return '.';
	if (i == 3) return '+';
	if (i == 4) return '-';
	if (i == 5) return '*';
	if (i == 6) return '/';
	if (i == 7) return '?';
	if (i == 8) return '<';
	if (i == 9) return '>';
	return '_';
}

int AuditoryProcess::demap(char m){
	if (m >= '0' && m <= '9')
		return m - '0';
	if (m >= 'a' && m <= 'z')
		return m - 'a' + 10;
	if (m >= 'A' && m <= 'Z')
		return m - 'A' + 36;
	if (m == '(')
		return 62;
	if (m == ')')
		return 63;
	if (m == '.')
		return 64;
	if (m == '+')
		return 65;
	if (m == '-')
		return 66;
	if (m == '*')
		return 67;