📄 main.cpp
字号:
/*************************************************************************** * Copyright (C) 2004 - 2006 by ZJUBase *
* National Lab of Industrial Control Tech. * * Zhejiang University, China * * * * Achievements of ZJUBase in RoboCup Soccer 3D Simulation League: *
* In RoboCup championship, *
* - June 2006 - 3rd place in RoboCup 2006, Bremen, Germany (lost * * the semi-final with a coin toss) *
* - July 2005 - 3rd place in RoboCup 2005, Osaka, Japan (share the * * 3rd place with team Caspian) *
* In RoboCup local events, *
* - April 2006 - 2nd place in RoboCup Iran Open 2006, Tehran, Iran * * (lost the final with a coin toss) *
* - December 2005 - 2nd place in AI Games 2005, Isfahan, Iran *
* - July 2005 - champion in China Robot Competition 2005, Changzhou, * * China *
* - October 2004 - champion in China Soccer Robot Competition 2004, * * Guangzhou, China *
* * * Team members: *
* Currently the team leader is, * * Hao JIANG (jianghao@iipc.zju.edu.cn; riveria@gmail.com) *
* In the next season, the leader will be * * Yifeng ZHANG (yfzhang@iipc.zju.edu.cn) *
* ZJUBase 3D agent is created by * * Dijun LUO (djluo@iipc.zju.edu.cn) *
* All the members who has ever contributed: * * Jun JIANG *
* Xinfeng DU (xfdu@iipc.zju.edu.cn) *
* Yang ZHOU (yzhou@iipc.zju.edu.cn) *
* Zhipeng YANG *
* Xiang FAN *
* *
* Team Manager: *
* Ms. Rong XIONG (rxiong@iipc.zju.edu.cn) *
* *
* If you met any problems or you have something to discuss about * * ZJUBase. Please feel free to contact us through EMails given below. * * * * 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; either version 2 of the License, or * * (at your option) any later version. * * * * 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., * * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * ***************************************************************************/#ifndef WIN32
#include <sstream>
#include <iostream>
#include "soccertypes.h"
#include "worldmodel.h"
#include "commserver.h"
#include "kicknrun.h"
#include "behavior.h"
#include "soccer.h"
#include "Global_main.h"
#include "Agent.h"
#include "Global.h"
#include "Filter.h"
#include "Predictor.h"
using namespace std;
bool flag = false;
string teamName = "ZJUBase";
string sayMsgSign = "esaBUJZ";
int teamUnum = 0;
FILE *f = NULL;
//==================================================================
// class Sensor
//==================================================================
// set to 1 to write debug information to stdout
#define ENABLE_LOGGING 1
#ifdef ENABLE_LOGGING
void
Log(const char* out)
{
printf(out);
fflush(0);
}
#else
#define Log(x)
#endif
void
printHelp()
{
cout << "\nusage: agenttest [options]" << endl;
cout << "\noptions:" << endl;
cout << " --help prints this message." << endl;
cout << " --teamname sets the team name. " << endl;
cout << " --unum sets the uniform number." << endl;
cout << "\n";
}
void
ReadOptions(int argc, char* argv[])
{
for( int i = 0; i < argc; i++)
{
if( strcmp( argv[i], "--teamname" ) == 0 )
{
if( i+1 < argc)
{
teamName = argv[i+1];
++i;
}
}
else if( strcmp( argv[i], "--help" ) == 0 )
{
printHelp();
exit(0);
} else if (strcmp( argv[i], "--unum" ) == 0 ) {
teamUnum = atoi(argv[i+1]);
++i;
ostringstream ss;
ss << "setting uniform number to " << teamUnum << "\n";
Log(ss.str().c_str());
}
}
Log("setting teamname to ");
Log(teamName.c_str());
Log("\n");
}
CommServer* g_comm = new CommServer;
WorldModel* g_wm = new WorldModel;
Behavior* g_behave = new KickNRun;
int
Init(int argc, char* argv[])
{
// read command line options
ReadOptions(argc,argv);
// register and create the CommServer
// g_comm = new CommServer;
// register and create the WorldModel
// g_wm = new WorldModel;
// get the installed behavior
// g_behave = new KickNRun;
dynamic_cast<Soccer *>(g_behave)->SetTeamName(teamName);
g_wm->SetTeamName(teamName);
dynamic_cast<Soccer *>(g_behave)->SetTeamUnum(teamUnum);
g_wm->SetupVisionObjectMap();
return 0;
}
void
Run()
{
Log( "*****************************************************************\n");
Log( "* ZJUBase : ZheJiang University, China *\n");
Log( "* Team Coordinator: JIANG Hao (jianghao@iipc.zju.edu.cn) *\n");
Log( "* Team Member: ZHANG Yifeng, DU Xinfeng *\n");
Log( "* YANG Zhipeng, FAN Xiang *\n");
Log( "* Copyright 2004-2006. *\n");
Log( "* All rights reserved. *\n");
Log( "*****************************************************************\n");
Agent agent;
agent.Run();
}
int
main(int argc, char* argv[])
{
//init zeitgeist and oxygen
int ret = Init(argc, argv);
if (ret > 0)
{
return ret;
}
Run();
}
void CMDExcutor::ExcuteCommands(){
int i;
char *strCMD;
char buff[1024];
// cout<<"ExcuteCommands(){"<<endl;
if(global.wm.subCycle==0){
for(i=1;i<global.nCommand;i++){
sprintf(buff,"R%d",global.wm.simTime + i);
// cout<<buff<<endl;
g_comm->PutOutput(buff);
}
}
int index = global.wm.notifyTime-global.wm.simTime;
string s;
if(index>=0&&index<=COMMAND_MAX){
// cout<<index<<endl;
s = "";
// cout<<"commands"<<global.commands[index].size()<<endl;
for(i=0;i<global.commands[index].size();i++){
strCMD = global.commands[index][i].getCommandString();
// cout<<strCMD<<endl;
s += strCMD;
if(global.commands[index][i].ct == CT_DRIVE)
global.driveF = global.commands[index][i].vvalue[0];
}
if(global.commands[index].size()>0)
g_comm->PutOutput(s.c_str());
cout<<s<<endl;
}
g_comm->PutOutput("D");
// cout<<"ExcuteCommands(){ end"<<endl;
}
bool Sensor::ShouldIThink()
{
if (! g_comm->GetInput())
{
bShouldILeave = true;
return false;
}
int simtime,dTemp;
// cerr << "==Msg von Server an Spieler: " <<g_comm->GetMsg() <<endl;
switch(g_comm->GetMsg()[0])
{
case 'D' :
g_behave->ProcessInitMessage();
break;
case 'K' :
g_behave->ProcessThinkTimeMessage();
break;
case 'S' :
if (g_wm->Parse(g_comm->GetMsg())) {
updateGlobal();
}
sscanf(g_comm->GetMsg(),"S%d %d",&dTemp,&simtime);
// cout<<"S:"<<simtime<<endl;
global.wm.simTime = simtime;
global.wm.subCycle = 0;
global.wm.notifyTime = simtime;
return true;
break;
case 'T':
global.wm.subCycle ++;
sscanf(g_comm->GetMsg(),"T%d",&global.wm.notifyTime);
// cout<<"T"<<global.wm.notifyTime<<endl;
return true;
break;
case 'O':
global.wm.subCycle ++;
sscanf(g_comm->GetMsg(),"O%d",&global.wm.notifyTime);
// cout<<"OO"<<global.wm.notifyTime<<endl;
// behave->ProcessThinkTimeMessage();
break;
default:
break;
}
return false;
}
bool Sensor::ShouldILeave()
{
return bShouldILeave;
}
void Sensor::updateGlobal()
{
//cout << "Sensor::updateGlobal() begins" << endl;
//=============
//cerr<<"T: "<<global.wm.currentTime<<endl;
//global.wm.myPanAngle=g_wm->mAgentPanAngle;
//cerr << "panAngle: " << global.wm.myPanAngle << endl;
//cerr << "tiltAngle: " << global.wm.myTiltAngle << endl;
Vector3 vMyPosFromFlag[8],vRelPos[8];
int i;
Vector3 vFlagReal[8];
double FlagHeight = 0.0f;
//get absolute coordinate of 8 flags
strcpy(global.strTeamName,teamName.c_str());
vFlag[0] = Vector3(-g_wm->mFieldLength/2.0f, -g_wm->mFieldWidth/2.0f, FlagHeight);// 1l
vFlag[1] = Vector3(g_wm->mFieldLength/2.0f, -g_wm->mFieldWidth/2.0f, FlagHeight);// 1r
vFlag[2] = Vector3(-g_wm->mFieldLength/2.0f, g_wm->mFieldWidth/2.0f, FlagHeight);// 2l
vFlag[3] = Vector3(g_wm->mFieldLength/2.0f, g_wm->mFieldWidth/2.0f, FlagHeight);// 2r
vFlag[4] = Vector3(-g_wm->mFieldLength/2.0f, -g_wm->mGoalWidth/2.0f, 0.0);// 1l
vFlag[5] = Vector3(g_wm->mFieldLength/2.0f, -g_wm->mGoalWidth/2.0f, 0.0);// 1r
vFlag[6] = Vector3(-g_wm->mFieldLength/2.0f, g_wm->mGoalWidth/2.0f, 0.0);// 2l
vFlag[7] = Vector3(g_wm->mFieldLength/2.0f, g_wm->mGoalWidth/2.0f, 0.0);// 2r
if(g_wm->mTeamIndex == TI_RIGHT){
for(i = 0; i < 8; i++){
vFlagReal[i] = -vFlag[i];
}
}
else{
for(i = 0; i < 8; i++){
vFlagReal[i] = vFlag[i];
}
}
//get relative coordinate of 8 flags
for(i=0;i<8;i++){
vRelPos[i]= g_wm->GetDriveVec(g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_FLAG1L+i)));
vRelPos[i].rotate2d(global.wm.myPanAngle);
vMyPosFromFlag[i] =- vRelPos[i] + vFlagReal[i];
data[i] = g_wm->GetVisionSense((WorldModel::VisionObject)(WorldModel::VO_FLAG1L+i));
}
Circle cFlag[8];
Vector3 agentPos[32],agent1Pos[32],agent2Pos[32];
double tmpDis[8];
int pt1,pt2;
int count = 0;
Angle agent1Theta,agent2Theta,panAddTheta;
Angle panAngleFromDist[64];
Angle diffAngle1,diffAngle2;
double dx,dy,dist_pt1_pt2;
double a,arg,h;
double px_r0,px_r1,py_r0,py_r1;
double pan_x[64], pan_y[64], tilt_dist;
double sigma_dist, sigma_theta, sigma_phi;
double sigmaPan[64], sigmaTilt[8];
double sigmaX[32],sigmaY[32];
double sumX_deno,sumY_deno,sumX_nume,sumY_nume;
double sumPan_deno, sumTilt_deno, sumPan_nume, sumTilt_nume;
sigma_dist = 0.0965;
sigma_theta = 0.1225*pow(PI/180, 2);
sigma_phi = 0.1480*pow(PI/180, 2);
sumX_deno = sumY_deno = sumX_nume = sumY_nume = 0;
sumPan_deno = sumTilt_deno = sumPan_nume = sumTilt_nume = 0;
//set radius and center of cirlce for 8 flags
for(i = 0; i < 8; i++){
cFlag[i].setCenter(vFlagReal[i]);
tmpDis[i]= sqrt(data[i].distance*data[i].distance-0.22*0.22);
if(isnan(tmpDis[i]))
tmpDis[i] = data[i].distance;
cFlag[i].setRadius(tmpDis[i]);
}
//get agent position from intersection points between two circles
for(pt1 = 0; pt1 < 8; pt1++){
if(data[pt1].distance <EPS){
global.wm.noflag[pt1]++;
// cerr<<"flag["<<pt1<<"]="<<global.wm.noflag[pt1]<<endl;
continue;
}
else
global.wm.noflag[pt1]=0;
for(i = 1; i < 8-pt1; i++){
pt2 = pt1 + i;
if(data[pt2].distance <EPS){
continue;
}
int num_sol;
num_sol = cFlag[pt1].getIntersectionPoints(cFlag[pt2], & agent1Pos[count], & agent2Pos[count]);
//cerr << "num_sol: " << num_sol << endl;
if(num_sol == 0)
continue;
if(!agent1Pos[count].isInField(-15) && !agent2Pos[count].isInField(-15))
continue;
agent1Theta = (agent1Pos[count]-vFlagReal[pt1]).ang();
agent2Theta = (agent2Pos[count]-vFlagReal[pt1]).ang();
panAddTheta = (vMyPosFromFlag[pt1]-vFlagReal[pt1]).ang();
//diffAngle1 = panAddTheta-agent1Theta;
//diffAngle2 = panAddTheta-agent2Theta;
//diffAngle1 = Normalize(diffAngle1);
//diffAngle2 = Normalize(diffAngle2);
double agent1DiffDist,agent2DiffDist;
agent1DiffDist = (agent1Pos[count] - vMyPosFromFlag[pt1]).mod();
agent2DiffDist = (agent2Pos[count] - vMyPosFromFlag[pt1]).mod();
dx = vFlagReal[pt2].x - vFlagReal[pt1].x;
dy = vFlagReal[pt2].y - vFlagReal[pt1].y;
dist_pt1_pt2 = sqrt(dx*dx + dy*dy);
dx = dx / dist_pt1_pt2;
dy = dy / dist_pt1_pt2;
a = (data[pt1].distance*data[pt1].distance + dist_pt1_pt2*dist_pt1_pt2 - data[pt2].distance*data[pt2].distance) / (2.0 * dist_pt1_pt2);
arg = data[pt1].distance*data[pt1].distance - a*a;
h = sqrt(arg);
//if(abs(diffAngle1)<abs(diffAngle2))
bool chooseAgent1 = false;
if(agent1DiffDist < agent2DiffDist)
chooseAgent1 = true;
if(!agent1Pos[count].isInField(-15))
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -