河原林研究室 >> EcoBe!世界一への道 >> 作成プログラム
- ここでは本研究で開発したMRリーグ用のEco-Be!エージェントのプログラムソースを載せる
ゴールキーパ †
- 以下で本研究で開発したGKのプログラムソースを載せる
main関数 †
- このmain関数は mr-soccer-server2008/soccer_client の中のSampleAgent?.cppの中に書かれている
int main(int argc, char* argv[])
{
Properties props;
std::string host;
int port;
Properties client_properties;
int act = 0;
double z, x , y , my_x , my_y , ball_x , ball_y ; //X軸 Y軸
double p1 , p2 ,p_flg1[2] , p_flg2[2] , ball[2]; //座標計算
double goal_xl , goal_xr , goal_xc;
double angle;
double my_c , posi[2] , my_angle , ps_angle , my_c1angle;
double a , b , c , fai[3] , tilt , cons;
double pt1[2] , pt2[2] , xyr[3] , xy[2] , pnear[2];
double ball_length;
try {
/**
* Read the properties from the configuration files
*/
props = read_file_properties("config.ini");
/**
* If necessary override those properties with any given c ommand-line
*/
if (!override_properties_with_cmd(argc, argv, props)) {
return -1;
}
host = props.propertyValue("SERVER_IP");
port = convert_to<int> (props.propertyValue("PORT_TEAM"));
} catch (ClientException& ex) {
std::cout << ex.what() << std::endl;
return -1;
}
ConnectionParameters params;
params.nickname(props.Get("NICK_NAME"));
params.teamname(props.Get("TEAMNAME"));
params.vtId(convert_to<int>(props.Get("VT_ID")));
params.rcId(convert_to<int>(props.Get("RC_ID")));
params.movementMode("wheel_velocities");
int tries = convert_to<int>(props.Get("TRIES"));
int sleep = convert_to<int>(props.Get("SLEEP"));
ServerProperties server_props;
ClientConnector client_connector(host, port);
client_connector.tries(tries);
client_connector.retryTime(sleep);
try
{
server_props = client_connector.ConnectToServer(params);
}
catch (ClientException& ex)
{
std::cout << ex.what() << std::endl;
return -1;
}
mrTeam my_team = client_team(server_props, port);
WorldData gWorldData(sleep, tries, my_team);
gWorldData.setConnection(client_connector.getConnection());
std::cout << "World connection successfully initialized." << std::endl;
BasicPlayer gPlayer(my_team);
gPlayer.setConnection(client_connector.getConnection());
std::cout << "Basic player connection successfully initialized."
<< std::endl;
gPlayer.setWorldData(&gWorldData);
bool quit = false;
while (!quit) {
if (gWorldData.updateAll()) {
if (gWorldData.pv_is_game_on())
{
// std::cout << "Game is on." << std::endl;
}
else if (gWorldData.mrPlayMode() == "warn ending")
{
std::cout << "game near to the end." << std::endl;
}
else if (gWorldData.mrPlayMode() == "time over") {
quit = true;
continue;
}
vector_t my_goal_center;
vector_t op_goal_center;
vector_t top_center = gWorldData.mrTopCenter();
vector_t bottom_center = gWorldData.mrBottomCenter();
vector_t ball_deg = gWorldData.pv_ball();
vector_t ball_rad = gWorldData.pv_ball();
vector_t mycorner1 = gWorldData.pv_mycorner_flag1();//左
vector_t mycorner2 = gWorldData.pv_mycorner_flag2();//右
vector_t opcorner1 = gWorldData.pv_opcorner_flag1();
vector_t opcorner2 = gWorldData.pv_opcorner_flag2();
vector_t mygoal1 = gWorldData.pv_mygoal_pole1();
vector_t mygoal2 = gWorldData.pv_mygoal_pole2();
vector_t opgoal1 = gWorldData.pv_opgoal_pole1();
vector_t opgoal2 = gWorldData.pv_opgoal_pole2();
vector_t position;
if (!gWorldData.pv_i_am_lost()) {
//度数表現 M_PI = 3.14159265358979323846264338327950288419
ball_deg.angle = (ball_deg.angle * 180.0) / M_PI;
mycorner1.angle = (mycorner1.angle * 180.0) / M_PI;
mycorner2.angle = (mycorner2.angle * 180.0) / M_PI;
opcorner1.angle = (opcorner1.angle * 180.0) / M_PI;
opcorner2.angle = (opcorner2.angle * 180.0) / M_PI;
mygoal1.angle = (mygoal1.angle * 180.0) / M_PI;
mygoal2.angle = (mygoal2.angle * 180.0) / M_PI;
opgoal1.angle = (opgoal1.angle * 180.0) / M_PI;
opgoal2.angle = (opgoal2.angle * 180.0) / M_PI;
//フィールド横幅の計算 X軸 Y軸
p_flg1[0] = mycorner2.length * cos(mycorner2.angle * M_PI / 180.0);
p_flg1[1] = mycorner2.length * sin(mycorner2.angle * M_PI / 180.0);
p_flg2[0] = mycorner1.length * cos(mycorner1.angle * M_PI / 180.0);
p_flg2[1] = mycorner1.length * sin(mycorner1.angle * M_PI / 180.0);
p1 = (p_flg2[0] - p_flg1[0]) * (p_flg2[0] - p_flg1[0]);
p2 = (p_flg2[1] - p_flg1[1]) * (p_flg2[1] - p_flg1[1]);
x = sqrt(p1 + p2);
//printf("X = %lf \n",x);
p_flg1[0] = opcorner1.length * cos(opcorner1.angle * M_PI / 180.0);
p_flg1[1] = opcorner1.length * sin(opcorner1.angle * M_PI / 180.0);
p_flg2[0] = mycorner2.length * cos(mycorner2.angle * M_PI / 180.0);
p_flg2[1] = mycorner2.length * sin(mycorner2.angle * M_PI / 180.0);
p1 = (p_flg2[0] - p_flg1[0]) * (p_flg2[0] - p_flg1[0]);
p2 = (p_flg2[1] - p_flg1[1]) * (p_flg2[1] - p_flg1[1]);
y = sqrt(p1 + p2);
//printf("Y = %lf \n",y);
//ボールの x軸 y軸計算
p_flg1[0] = mycorner2.length * cos(mycorner2.angle * M_PI / 180.0);
p_flg1[1] = mycorner2.length * sin(mycorner2.angle * M_PI / 180.0);
p_flg2[0] = mycorner1.length * cos(mycorner1.angle * M_PI / 180.0);
p_flg2[1] = mycorner1.length * sin(mycorner1.angle * M_PI / 180.0);
ball[0] = ball_deg.length * cos(ball_deg.angle * M_PI / 180.0);
ball[1] = ball_deg.length * sin(ball_deg.angle * M_PI / 180.0);
p1 = sqrt((p_flg1[0] - ball[0] ) * (p_flg1[0] - ball[0]) + (p_flg1[1] - ball[1]) * (p_flg1[1] - ball[1]));
p2 = sqrt((ball[0] - p_flg2[0]) * (ball[0] - p_flg2[0]) + (ball[1] - p_flg2[1]) * (ball[1] - p_flg2[1]));
z = ((x * x) + (p1 * p1) - (p2 * p2)) / (2.0 * x * p1);
ball_x = p1 * z;
ball_y = p1 * sqrt(1 - (z * z));
//printf("Ball X = %lf \n",ball_x);
//printf("Ball Y = %lf \n",ball_y);
// ゴール座標
p_flg1[0] = mycorner2.length * cos(mycorner2.angle * M_PI / 180.0);
p_flg1[1] = mycorner2.length * sin(mycorner2.angle * M_PI / 180.0);
p_flg2[0] = mygoal2.length * cos(mygoal2.angle * M_PI / 180.0);
p_flg2[1] = mygoal2.length * sin(mygoal2.angle * M_PI / 180.0);
p1 = (p_flg2[0] - p_flg1[0]) * (p_flg2[0] - p_flg1[0]);
p2 = (p_flg2[1] - p_flg1[1]) * (p_flg2[1] - p_flg1[1]);
goal_xr = sqrt(p1 + p2);
printf("GOAL XR = %lf \n",goal_xr);
p_flg1[0] = mycorner2.length * cos(mycorner2.angle * M_PI / 180.0);
p_flg1[1] = mycorner2.length * sin(mycorner2.angle * M_PI / 180.0);
p_flg2[0] = mygoal1.length * cos(mygoal1.angle * M_PI / 180.0);
p_flg2[1] = mygoal1.length * sin(mygoal1.angle * M_PI / 180.0);
p1 = (p_flg2[0] - p_flg1[0]) * (p_flg2[0] - p_flg1[0]);
p2 = (p_flg2[1] - p_flg1[1]) * (p_flg2[1] - p_flg1[1]);
goal_xl = sqrt(p1 + p2);
printf("GOAL XL = %lf \n",goal_xl);
//MYゴール中心までのベクトル計算
goal_xc = (goal_xl - goal_xr) / 2.0;
my_goal_center.length = sqrt((my_x - goal_xc) * (my_x - goal_xc) + (my_y) * (my_y));
if(mygoal1.angle >= 0.0 && mygoal2.angle <= 0.0){
if(fabs(mygoal1.angle) <= fabs(mygoal2.angle)){
my_goal_center.angle = mygoal1.angle - (fabs(mygoal1.angle) + fabs(mygoal2.angle)) / 2.0;
}else if(fabs(mygoal1.angle) > fabs(mygoal2.angle)){
my_goal_center.angle = mygoal2.angle + (fabs(mygoal1.angle) + fabs(mygoal2.angle)) / 2.0;
}
}else if(mygoal1.angle > 0.0 && mygoal2.angle > 0){
my_goal_center.angle = mygoal2.angle + (fabs(mygoal1.angle) - fabs(mygoal2.angle)) / 2.0;
}else if(mygoal1.angle < 0.0 && mygoal2.angle > 0.0){
if(fabs(mygoal1.angle) <= fabs(mygoal2.angle)){
my_goal_center.angle = mygoal1.angle - (360.0 - (fabs(mygoal1.angle)+fabs(mygoal2.angle)) / 2.0);
}else if(fabs(mygoal1.angle) > fabs(mygoal2.angle)){
my_goal_center.angle = mygoal2.angle + (360.0 - (fabs(mygoal1.angle)+fabs(mygoal2.angle)) / 2.0); }
}else if(mygoal1.angle < 0.0 && mygoal2.angle < 0.0){
my_goal_center.angle = mygoal1.angle - (fabs(mygoal2.angle) - fabs(mygoal1.angle));
}
if(fabs(my_goal_center.angle) >= 180.0){
my_goal_center.angle = 0;
}
//OPゴール中心までのベクトル計算
op_goal_center.length = sqrt((my_x - goal_xc) * (my_x - goal_xc) + (y - my_y) * (y - my_y));
if(opgoal1.angle >= 0.0 && opgoal2.angle <= 0.0){
if(fabs(opgoal1.angle) <= fabs(opgoal2.angle)){
op_goal_center.angle = opgoal1.angle - (fabs(opgoal1.angle) + fabs(opgoal2.angle)) / 2.0;
}else if(fabs(opgoal1.angle) > fabs(opgoal2.angle)){
op_goal_center.angle = opgoal2.angle + (fabs(opgoal1.angle) + fabs(opgoal2.angle)) / 2.0;
}
}else if(opgoal1.angle > 0.0 && opgoal2.angle > 0){
op_goal_center.angle = opgoal2.angle + (fabs(opgoal1.angle) - fabs(opgoal2.angle)) / 2.0;
}else if(opgoal1.angle < 0.0 && opgoal2.angle > 0.0){
if(fabs(opgoal1.angle) <= fabs(opgoal2.angle)){
op_goal_center.angle = opgoal1.angle - (360.0 - (fabs(opgoal1.angle)+fabs(opgoal2.angle)) / 2.0);
}else if(fabs(mygoal1.angle) > fabs(mygoal2.angle)){
op_goal_center.angle = opgoal2.angle + (360.0 - (fabs(opgoal1.angle)+fabs(opgoal2.angle)) / 2.0); }
}else if(opgoal1.angle < 0.0 && opgoal2.angle < 0.0){
op_goal_center.angle = opgoal1.angle - (fabs(opgoal2.angle) - fabs(opgoal1.angle));
}
if(fabs(op_goal_center.angle) >= 180.0){
op_goal_center.angle = 180.0;
}
my_goal_center.angle = (my_goal_center.angle * M_PI / 180.0);
op_goal_center.angle = (op_goal_center.angle * M_PI / 180.0);
//自分の x軸 y軸計算
z = ((x * x) + (mycorner2.length * mycorner2.length) -
(mycorner1.length * mycorner1.length)) / (2.0 * x * mycorner2.length);
my_x = mycorner2.length * z;
my_y = mycorner2.length * sqrt(1 - (z * z));
printf("My X = %lf \n",my_x);
printf("My Y = %lf \n",my_y);
ball_length = sqrt((x/2 - ball_x) * (x/2 - ball_x) + ball_y * ball_y);
//2等分線の計算
a = sqrt((goal_xl - ball_x) * (goal_xl - ball_x) + ball_y * ball_y);
b = goal_xl - goal_xr;
c = sqrt((goal_xr - ball_x) * (goal_xr - ball_x) + ball_y * ball_y);
fai[1] = acos((a*a + c*c - b*b) / (2*a*c));
fai[2] = acos((a*a + b*b - c*c) / (2*a*b));
fai[0] = M_PI-(fai[1] / 2 + fai[2]);
tilt = tan(fai[0]);
cons = ball_y - tilt * ball_x;
std::cout << "fai 1 = " << (fai[1] *180) / M_PI<< std::endl;
std::cout << "fai 2 = " << (fai[2] *180) / M_PI<< std::endl;
std::cout << "katamuki= " << tilt << "\n fai= " << (fai[0]*180)/M_PI << std::endl;
std::cout << "cons =" << cons << std::endl;
//position座標計算
pt1[0] = ball_x; //pt1:ball座標
pt1[1] = ball_y;
pt2[0] = (0 - cons) / tilt; //pt2:x軸との交点
pt2[1] = 0;
xyr[0] = x / 2.0; //xyr:円の中心と半径
xyr[1] = 0;
xyr[2] = 50;
pnear[0] = x / 2.0;
pnear[1] = 50;
if(gPlayer.lncl( pt1 , pt2 , xyr , pnear , xy ) < 0){
printf("Error,交点が存在しない。\n");
if(goal_xl <= ball_x && goal_xr >= ball_x){
xy[0]=x/2;
xy[1]=50;
}else if(goal_xl > ball_x && goal_xr > ball_x){
xy[0]=x/2-20;
xy[1]=40;
std::cout << "Right" << std::endl;
}else if(goal_xl < ball_x && goal_xr < ball_x){
xy[0]=x/2+20;
xy[1]=40;
std::cout << "Left " << std::endl;
}
}
posi[0] = xy[0];
posi[1] = xy[1];
printf("px＝%lf\npy＝%lf",posi[0],posi[1]);
//positionまでのベクトル計算
my_c = sqrt(my_x * my_x + my_y * my_y);
my_angle = acos( my_y / my_c );
my_angle = my_angle * 180.0 / M_PI;
my_c1angle = mycorner2.angle;
my_angle = my_angle + my_c1angle;
my_angle = (int(my_angle) +360) % 360; //-180〜180→0〜360
if(my_y < posi[1] && my_x > posi[0]){
ps_angle = atan( (my_x - posi[0]) / (posi[1] - my_y) );
ps_angle = 180 - ps_angle * 180.0 /M_PI;
}else if(my_y > posi[1] && my_x > posi[0]){
ps_angle = atan( (my_x - posi[0]) / (my_y - posi[1]) );
ps_angle = ps_angle * 180.0 /M_PI;
}else if(my_y > posi[1] && my_x < posi[0]){
ps_angle = atan( (posi[0] - my_x) / (my_y - posi[1]) );
ps_angle = ps_angle * 180.0 /M_PI;
ps_angle = 360 - ps_angle;
}else if(my_y < posi[1] && my_x < posi[0]){
ps_angle = atan( (posi[0] - my_x) / (posi[1] - my_y) );
ps_angle = 180 - ps_angle * 180.0 /M_PI;
ps_angle = 360 - ps_angle;
}
position.angle = my_angle - ps_angle; //positionと自分の向きの角度
if(position.angle < -180.0)
position.angle = position.angle +360.0;
if(position.angle > 180.0)
position.angle = 180 - position.angle;
position.length = sqrt((my_x - posi[0]) * (my_x - posi[0]) + (posi[1] - my_y) * (posi[1] - my_y));
printf("\n角度＝%lf\n距離＝%lf\n",position.angle,position.length);
std::cout << "My_Angle =" << my_angle << std::endl;
std::cout << "Ps_Angle =" << ps_angle << std::endl;
//deg -> rad
mycorner1.angle = (mycorner1.angle * M_PI) / 180.0;
mycorner2.angle = (mycorner2.angle * M_PI) / 180.0;
opcorner1.angle = (opcorner1.angle * M_PI) / 180.0;
opcorner2.angle = (opcorner2.angle * M_PI) / 180.0;
mygoal1.angle = (mygoal1.angle * M_PI) / 180.0;
mygoal2.angle = (mygoal2.angle * M_PI) / 180.0;
opgoal1.angle = (opgoal1.angle * M_PI) / 180.0;
opgoal2.angle = (opgoal2.angle * M_PI) / 180.0;
angle = (op_goal_center.angle * 180.0) / M_PI + 90.0;
position.angle = position.angle * M_PI / 180.0;
//action決定 1:ポジショニング 2:ボールを蹴る
if(ball_length < 50)
act = 2;
else if(ball_deg.length < 50)
act = 2;
else
act = 1;
std::cout << "act = " << act << std::endl;
switch(act){
case 1://keeper action
gPlayer.pv_go_to(position);
break;
case 2://クリア
if(fabs(ball_deg.angle) > 15.0)
ball_deg.angle < 0.0 ? gPlayer.pv_set_velocity(0.0 , 40.0):gPlayer.pv_set_velocity(40.0 , 0.0);
if(ball_deg.length < 48)
ball_deg.angle < 0.0 ? gPlayer.pv_set_velocity( -30 , 30):gPlayer.pv_set_velocity(30 , -30);
if(fabs(ball_deg.angle) < 30.0){
if(ball_deg.length > 40)
gPlayer.pv_go_straight(30.0);
else{
//クリア
/*team_mate = gWorldData.pv_me();
tid = gWorldData.pv_closest_teammate_to(team_mate);
team_mate = gWorldData.pv_teammate(tid);
gPlayer.pv_kick(team_mate.angle,1.0);*/
if(top_center.length < bottom_center.length)
gPlayer.pv_kick(top_center.length,1.0);
else
gPlayer.pv_kick(bottom_center.length,1.0);
}
}
break;
}
gPlayer.pv_flush();
} else {
// Just for the sake of clarity
// std::cout << "Where I'am?!" << std::endl;
}
} else {
std::cout << "Didn't receive any update message from server."
<< std::endl;
}
}
std::cout << "Ending program." << std::endl;
exit(EXIT_SUCCESS);
}
直線の方程式を計算する関数 †
int
BasicPlayer :: lnprm( double *pt1, double *pt2, double *a, double *b, double *c )
{
double xlk, ylk, rsq, rinv;
double accy = 1.0E-15;
xlk = pt2[0] - pt1[0];
ylk = pt2[1] - pt1[1];
rsq = pow(xlk, 2.0) + pow(ylk, 2.0);
if(rsq < accy){
return (-1);
}else{
rinv = 1.0 / sqrt(rsq);
*a = -ylk * rinv;
*b = xlk * rinv;
*c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv;
} /* end if */
return (0);
}
円と直線の交点を計算する関数 †
int
BasicPlayer :: lncl( double *pt1, double *pt2, double *xyr, double *pnear, double *xy)
{
double root, factor, xo, yo, f, g, fsq, gsq, fgsq, xjo, yjo, a, b, c;
double fygx, fxgy, t, fginv, t1, t2, x1, y1, x2, y2, sqdst1, sqdst2;
double accy = 1.0E-15;
if(lnprm(pt1, pt2, &a, &b, &c)) return (-1);
root = 1.0 / (a*a + b*b);
factor = -c * root;
xo = a * factor;
yo = b * factor;
root = sqrt(root);
f = b * root;
g = -a * root;
fsq = f*f;
gsq = g*g;
fgsq = fsq+gsq;
if(fgsq < accy){
return (3);
}else{
xjo = xyr[0] - xo;
yjo = xyr[1] - yo;
fygx = f*yjo - g*xjo;
root = xyr[2]*xyr[2]*fgsq - fygx*fygx;
if (root < -accy){ /* 交点なし */
return (-1);
}else{
fxgy = f*xjo + g*yjo;
if (root < accy){ /* 直線と円は接する。*/
t = fxgy / fgsq;
xy[0] = xo + f*t;
xy[1] = yo + g*t;
return (1);
}else{
root = sqrt(root);
fginv = 1.0 / fgsq;
t1 = (fxgy - root)*fginv;
t2 = (fxgy + root)*fginv;
x1 = xo + f*t1;
y1 = yo + g*t1;
x2 = xo + f*t2;
y2 = yo + g*t2;
} /* end if */
} /* end if */
} /* end if */
sqdst1 = pow((pnear[0] - x1), 2.0) + pow((pnear[1] - y1), 2.0);
sqdst2 = pow((pnear[0] - x2), 2.0) + pow((pnear[1] - y2), 2.0);
if (sqdst1 < sqdst2){ /* pnearに近い方の交点を返す。*/
xy[0] = x1;
xy[1] = y1;
}else{
xy[0] = x2;
xy[1] = y2;
} /* end if */
return (2);
}
最新の10件
2011-03-03
2010-11-08
2010-10-08
2010-10-29
2010-11-25
2010-10-22
2010-11-22
2013-02-12
2011-05-13
2011-04-20
今日の10件
- counter: 860
- today: 1
- yesterday: 0
- online: 2
