def avoid_ball_place_line(my_pose, ball_info, target_pose, control_target, force_avoid=False):
# ボール中心に座標変換
angle_ball_to_target = tool.get_angle(ball_info.pose, target_pose)
trans = tool.Trans(ball_info.pose, angle_ball_to_target)
tr_my_pose = trans.transform(my_pose)
tr_target_pose = trans.transform(target_pose)
dist_ball_to_target = tool.distance_2_poses(ball_info.pose, target_pose)
# ライン上にいるロボットは回避位置を生成する
# ラインに対して垂直に移動する
if BALL_PLACE_AREA_NO_DRIBBLE < dist_ball_to_target or force_avoid:
if -BALL_MARGIN_DIST < tr_my_pose.y < BALL_MARGIN_DIST:
if tr_my_pose.y < 0:
tr_my_pose.y -= BALL_MARGIN_DIST
else:
tr_my_pose.y += BALL_MARGIN_DIST
# 避ける位置を生成
target_pose = trans.inverted_transform(tr_my_pose)
control_target.path = []
control_target.path.append(target_pose)
return control_target, True
def basic_recv(control_target, ball_info, atk_pose, recv_pose, target_pose):
# 目標座標までの角度
angle_ball_to_target = tool.get_angle(ball_info.pose, target_pose)
# 目標位置はボールの前方にし、目標角度は、自己位置からみたボール方向にする
trans = tool.Trans(ball_info.pose, angle_ball_to_target)
# ボールとpalcement位置の距離
dist_ball_to_target = tool.distance_2_poses(ball_info.pose, target_pose)
# ボールの速度
ball_velocity = math.hypot(ball_info.velocity.x, ball_info.velocity.y)
# 目標座標までの角度
angle_ball_to_target = tool.get_angle(ball_info.pose, target_pose)
# ---------------------------------------
# placementの行動生成
# デフォルト値
avoid_ball = True
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
new_pose = Pose2D()
if BALL_PLACE_TRESHOLD < dist_ball_to_target:
# 速度が出ている場合レシーブしにいく
if VEL_THRESHOLD < ball_velocity:
# レシーブするための座標生成
new_pose = receive_ball(ball_info, recv_pose)
# ドリブルする
control_target.dribble_power = DRIBBLE_POWER
# placement座標の少し後ろに移動する
else:
new_pose = generating_behind_target_pose(trans, target_pose, SET_POSE_ADD_X_RECV)
new_pose.theta = angle_ball_to_target + math.pi
else:
# placement後に離れる
angle_recv_to_target = tool.get_angle(target_pose, recv_pose)
new_pose.x = target_pose.x + BALL_MARGIN_DIST * math.cos(angle_recv_to_target)
new_pose.y = target_pose.y + BALL_MARGIN_DIST * math.sin(angle_recv_to_target)
new_pose.theta = angle_recv_to_target + math.pi
avoid_ball = False
# パスを追加
control_target.path = []
control_target.path.append(new_pose)
return control_target, avoid_ball
def get_near_robot_id(target_pose, robot_info):
dist = []
for our_info in robot_info['our']:
if our_info.disappeared is False:
# 各ロボットと指定位置の距離
dist.append(tool.distance_2_poses(our_info.pose, target_pose))
else:
# インデックスを揃えるためダミーデータを挿入
dist.append(100)
# 一番距離が近いロボットのID
robot_id = dist.index(min(dist))
return robot_id
def get_avoid_ball_pose(ball_pose, target_pose):
# target_poseがボールの半径0.5 m以内にある場合、
# ボールから離れたtarget_poseを生成する
THRESHOLD_DIST = 0.5 # meters
AVOID_DIST = 0.6 # meters
avoid_target_pose = target_pose
if tool.distance_2_poses(ball_pose, target_pose) < THRESHOLD_DIST:
angle_to_target = tool.get_angle(ball_pose, target_pose)
trans = tool.Trans(ball_pose, angle_to_target)
avoid_target_pose = trans.inverted_transform(Pose2D(AVOID_DIST, 0, 0))
# 目標角度を再設定
avoid_target_pose.theta = target_pose.theta
return avoid_target_pose
def simple_kick(my_pose, ball_info, control_target, kick_power=0.5):
# ボールへまっすぐ向かい、そのまま蹴る
# シュート目標位置を設定しないシンプルなaction
# 目標位置はボールの前方にし、目標角度は、自己位置からみたボール方向にする
angle_robot_to_ball = tool.get_angle(my_pose, ball_info.pose)
trans = tool.Trans(ball_info.pose, angle_robot_to_ball)
tr_position = Pose2D(0.2, 0, 0) # ボールより少し前を目標位置にする
position = trans.inverted_transform(tr_position)
new_goal_pose = Pose2D()
new_goal_pose = position
new_goal_pose.theta = angle_robot_to_ball
# ボールに近づいたらキックフラグをON
if tool.distance_2_poses(my_pose, ball_info.pose) < 0.5:
control_target.kick_power = kick_power
else:
control_target.kick_power = 0.0
# ドリブルは常にオフ
control_target.dribble_power = 0.0
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose,
Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
return control_target
def _inplay_shoot(my_pose,
ball_info,
control_target,
target_pose,
can_shoot_angle=5,
shoot_enable=True,
dribble_dist=0.01):
# インプレイ用のシュートアクション
# デフォルトでゴールを狙う
KICK_POWER = 1.0
DRRIBLE_POWER = 0.6
IS_TOUCH_DIST = 0.2 # meters
IS_TOUCH_ANGLE = 170 # degrees
IS_LOOK_TARGET_ANGLE = 30 # degrees
CAN_DRIBBLE_DIST = 0.5 # meters
CAN_SHOOT_ANGLE = can_shoot_angle # degrees
SHOOT_TARGET = target_pose
DRIBBLE_DIST = dribble_dist
# ボールから見たロボットの座標系を生成
angle_ball_to_robot = tool.get_angle(ball_info.pose, my_pose)
trans_BtoR = tool.Trans(ball_info.pose, angle_ball_to_robot)
tr_robot_pose_BtoR = trans_BtoR.transform(my_pose)
tr_robot_angle_BtoR = trans_BtoR.transform_angle(my_pose.theta)
# ボールから見たターゲットの座標系を生成
angle_ball_to_target = tool.get_angle(ball_info.pose, SHOOT_TARGET)
trans_BtoT = tool.Trans(ball_info.pose, angle_ball_to_target)
tr_robot_angle_BtoT = trans_BtoT.transform_angle(my_pose.theta)
can_look_target = False
can_shoot = False
# ドリブラーがボールにくっついたらcan_look_target is True
# ロボットとボールの直線距離が近いか
if tr_robot_pose_BtoR.x < IS_TOUCH_DIST \
and math.fabs(tr_robot_angle_BtoR) > math.radians(IS_TOUCH_ANGLE): #ロボットがボールを見ているか
can_look_target = True
# ロボットがTargetを向いたらcan_shoot is True
if math.fabs(tr_robot_angle_BtoT) < math.radians(IS_LOOK_TARGET_ANGLE):
can_shoot = True
new_goal_pose = Pose2D()
if can_look_target is False:
# ドリブラーがボールにつくまで移動する
rospy.logdebug("inplay_shoot: approach")
new_goal_pose = trans_BtoR.inverted_transform(Pose2D(-0.1, 0, 0))
new_goal_pose.theta = trans_BtoR.inverted_transform_angle(
math.radians(180))
# キックをオフ
control_target.kick_power = 0.0
elif can_shoot is False:
# 目標角度に値を加えてロボットを回転させる
rospy.logdebug("inplay_shoot: rotate")
# ドリブラーがボールにつくまで移動する
tr_robot_pose_BtoR = trans_BtoR.transform(my_pose)
length = tr_robot_pose_BtoR.x
ADD_ANGLE = math.copysign(80, tr_robot_angle_BtoT) * 1.0
tr_goal_pose_BtoR = Pose2D(length * math.cos(ADD_ANGLE),
length * math.sin(ADD_ANGLE), 0)
# ボールにくっつきながら回転動作を加える
new_goal_pose = trans_BtoR.inverted_transform(tr_goal_pose_BtoR)
new_goal_pose.theta = tool.get_angle(new_goal_pose, ball_info.pose)
# キックをオフ
control_target.kick_power = 0.0
else:
new_goal_pose = trans_BtoT.inverted_transform(
Pose2D(dribble_dist, 0, 0))
new_goal_pose.theta = angle_ball_to_target
# ドリブルをオフ、キックをオン
# 狙いが定まったらシュート
if math.fabs(tr_robot_angle_BtoT) < math.radians(CAN_SHOOT_ANGLE) \
and shoot_enable:
rospy.logdebug("inplay_shoot: shoot")
control_target.kick_power = KICK_POWER
else:
rospy.logdebug("inplay_shoot: pre-shoot")
control_target.kick_power = 0.0
# ボールに近づいたらドリブルをオン
if tool.distance_2_poses(my_pose, ball_info.pose) < CAN_DRIBBLE_DIST:
control_target.dribble_power = DRRIBLE_POWER
else:
control_target.dribble_power = 0.0
# パスを追加
control_target.path = []
control_target.path.append(new_goal_pose)
return control_target
def _setplay_shoot(my_pose,
ball_info,
control_target,
kick_enable,
target_pose,
kick_power=0.8,
receive_enable=False,
receiver_role_exist=False,
robot_info=None):
# セットプレイ用のシュートアクション
# kick_enable is Falseで、ボールの近くまで移動する
# kick_enable is True で、シュートする
KICK_POWER = kick_power
SHOOT_TARGET = target_pose
arrive_threshold = 0.2
angle_ball_to_target = tool.get_angle(ball_info.pose, SHOOT_TARGET)
trans = tool.Trans(ball_info.pose, angle_ball_to_target)
tr_my_pose = trans.transform(my_pose)
# ロボットがボールの裏側に回ったらcan_kick is True
can_kick = False
if tr_my_pose.x < 0.01 and math.fabs(tr_my_pose.y) < 0.05:
can_kick = True
# レシーバにパスする場合、蹴る位置近くにロボットが存在すれば receive_arrive is True
# ただし、指定したroleが存在しなければ関係なし
# can_kick と receive_arrive が両方Trueなら蹴る
if receive_enable and receiver_role_exist:
receive_arrive = True #TODO:試合中の強制的な変更
for robot_id in range(len(robot_info['our'])):
if arrive_threshold > tool.distance_2_poses(
target_pose, robot_info['our'][robot_id].pose):
receive_arrive = True
can_kick = can_kick and receive_arrive
avoid_ball = True # ボールを回避する
new_goal_pose = Pose2D()
if can_kick and kick_enable:
# ボールをける
avoid_ball = False
# ボールの前方に移動する
new_position = trans.inverted_transform(Pose2D(0.02, 0, 0))
new_goal_pose = new_position
new_goal_pose.theta = angle_ball_to_target
# ドリブルとキックをオン
control_target.kick_power = KICK_POWER
else:
# ボールの裏に移動する
new_position = trans.inverted_transform(Pose2D(-0.3, 0, 0))
new_goal_pose = new_position
new_goal_pose.theta = angle_ball_to_target
# ドリブルとキックをオフ
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
# パスを追加
control_target.path = []
control_target.path.append(new_goal_pose)
return control_target, avoid_ball
def basic_atk(control_target, ball_info, atk_pose, recv_pose, target_pose, field_size):
# ボールからみた目標座標までの角度
angle_ball_to_target = tool.get_angle(ball_info.pose, target_pose)
# 目標座標からみたボールまでの角度
angle_target_to_ball = tool.get_angle(target_pose, ball_info.pose)
# ボールからみたATKまでの角度
angle_ball_to_atk = tool.get_angle(ball_info.pose, atk_pose)
# ボールとpalcement位置の距離
dist_ball_to_target = tool.distance_2_poses(ball_info.pose, target_pose)
# ボールとATKの距離
dist_ball_to_atk = tool.distance_2_poses(ball_info.pose, atk_pose)
# ボールとATKの距離
dist_atk_to_target = tool.distance_2_poses(atk_pose, target_pose)
# ローカル座標系に変換する、ボールから目標位置を向く
trans_ball_to_target = tool.Trans(ball_info.pose, angle_ball_to_target)
# ローカル座標系に変換する、ボールからATKを向く
trans_ball_to_atk = tool.Trans(ball_info.pose, angle_ball_to_atk)
# recvがボールのレシーブを行う座標
ball_receiving_pose = generating_behind_target_pose(trans_ball_to_target, target_pose, SET_POSE_ADD_X_RECV)
# ボール速度
ball_velocity = math.hypot(ball_info.velocity.x, ball_info.velocity.y)
# フィールドの情報をまとめる(上、下、左、右の位置)
field_pose = [field_size[0]/2,
-field_size[0]/2,
field_size[1]/2,
-field_size[1]/2]
# 壁際にあるか判定
wall_decision_result = False
if field_pose[0] - WALL_DECISION_DIST < ball_info.pose.x or \
ball_info.pose.x < field_pose[1] + WALL_DECISION_DIST or \
field_pose[2] - WALL_DECISION_DIST < ball_info.pose.y or \
ball_info.pose.y < field_pose[3] + WALL_DECISION_DIST:
wall_decision_result = True
# ---------------------------------------
# placementの行動生成
# デフォルト値
avoid_ball = False
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
new_pose = Pose2D()
# ボールが範囲に入っていない場合はplacementを行う
if BALL_PLACE_TRESHOLD < dist_ball_to_target:
# 指定位置に到着したかどうか
is_atk_arrived = atk_arrived_check(trans_ball_to_target.transform(atk_pose))
is_recv_arrived = recv_arrived_check(tool.distance_2_poses(recv_pose, ball_receiving_pose))
# 蹴ったあとにatkが追いかけない様に対策
if VEL_THRESHOLD < ball_velocity:
new_pose = atk_pose
# ボールが壁際にある場合はドリブルしながら下がる
elif wall_decision_result:
# ボールを保持して下がる
if dist_ball_to_atk < 0.11:
control_target.dribble_power = DRIBBLE_POWER
new_pose = trans_ball_to_target.inverted_transform(Pose2D(0.2, 0, 0))
new_pose.theta = angle_target_to_ball
# ボールの後ろまで移動する
else:
control_target.dribble_power = 0
new_pose = trans_ball_to_target.inverted_transform(Pose2D(0.1, 0, 0))
new_pose.theta = angle_target_to_ball
# atkがボールの後ろに移動する
elif not is_atk_arrived:
# ATKの位置ボールとターゲットの間の場合
if 0 < trans_ball_to_target.transform(atk_pose).x:
# ボールとATKの位置が近い場合は下がる
if dist_ball_to_atk < 0.2:
new_pose = trans_ball_to_atk.inverted_transform(Pose2D(0.3, 0, 0))
# ある程度離れていればボールの後ろへ回り込む
else:
new_pose = trans_ball_to_atk.inverted_transform(Pose2D(0.3, 0.3, 0))
# ボールとATKの位置がある程度離れている
else:
# ボールの後ろに座標を生成
new_pose = trans_ball_to_target.inverted_transform(Pose2D(-SET_POSE_ADD_X_ATK, 0, 0))
# 目標地点のほうを向く
new_pose.theta = angle_ball_to_target
# 障害物回避する
avoid_ball = True
# もしボールとゴールに近い場合はアタッカーが置きにいく
elif dist_ball_to_target < BALL_GET_AREA:
# ドリブルする
control_target = offense.inplay_dribble(atk_pose, ball_info,
control_target,target_pose)
new_pose = control_target.path[-1]
# ドリブルをやめる範囲ならドリブル
control_target.dribble_power = 0.0
if BALL_PLACE_AREA_NO_DRIBBLE < dist_ball_to_target:
control_target.dribble_power = DRIBBLE_POWER
# お互いの位置がセットされたら蹴る
elif is_atk_arrived and is_recv_arrived:
# ボールを確実に保持するためボールの少し前に移動する
new_pose = trans_ball_to_target.inverted_transform(Pose2D(0.2, 0, 0))
new_pose.theta = angle_ball_to_target
# ドリブルとキックをオン
control_target.kick_power = KICK_POWER
control_target.dribble_power = DRIBBLE_POWER
# ボールの後ろに移動
else:
new_pose = trans_ball_to_target.inverted_transform(Pose2D(-SET_POSE_ADD_X_ATK, 0, 0))
new_pose.theta = angle_ball_to_target
avoid_ball = True
# 範囲内にボールがある場合は離れる
else:
# ボールから離れる動作
angle_atk_to_target = tool.get_angle(target_pose, atk_pose)
new_pose.x = target_pose.x + BALL_MARGIN_DIST * math.cos(angle_atk_to_target)
new_pose.y = target_pose.y + BALL_MARGIN_DIST * math.sin(angle_atk_to_target)
new_pose.theta = angle_atk_to_target + math.pi
# パスを追加
control_target.path = []
control_target.path.append(new_pose)
return control_target, avoid_ball
def interpose(ball_info, robot_info, control_target):
# ボールの位置
ball_pose = ball_info.pose
# ボールの速度
ball_vel = ball_info.velocity
# ゴールの位置
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
xr = OUR_GOAL_POSE.x + 0.3
goalie_threshold_y = 1.5
# 敵のロボットの情報
robot_info_their = robot_info['their']
dist = []
for their_info in robot_info_their:
if their_info.detected:
dist.append(tool.distance_2_poses(their_info.pose, ball_pose))
else:
dist.append(100)
# 一番近い敵ロボットのID
min_dist_id = dist.index(min(dist))
# 一番近い敵ロボットの座標
robot_pose = robot_info_their[min_dist_id].pose
# ボールの速度
if ball_vel is None:
v = 0
dx = 0
dy = 0
da = 0
else:
# ボールの速度
vx = ball_vel.x
vy = ball_vel.y
v = math.hypot(ball_vel.x, ball_vel.y)
# ボールの進む角度
angle_ball = math.atan2(vy, vx)
# ボールの変化量を計算（正確ではなく方向を考慮した単位量）
dvx = math.cos(angle_ball)
dvy = math.sin(angle_ball)
# ボールの次の予測位置を取得
ball_pose_next = Pose2D(ball_pose.x + dvx, ball_pose.y + dvy, 0)
if dist[min_dist_id] < 0.15 and robot_pose.x < 0:
rospy.logdebug('their')
angle_their = robot_pose.theta
if angle_their == 0:
a = 1e-10
else:
a = math.tan(angle_their)
b = robot_pose.y - a * robot_pose.x
elif 0.02 < v and dvx < 0:
rospy.logdebug('ball move')
a, b = line_pram(ball_pose, ball_pose_next)
else:
rospy.logdebug('ball stop')
a, b = line_pram(ball_pose, OUR_GOAL_POSE)
# 位置を決める
yr = a*xr + b
# 位置
if yr > goalie_threshold_y:
yr = goalie_threshold_y
elif yr < -goalie_threshold_y:
yr = -goalie_threshold_y
# elif ball_pose.x < xr:
# ボールが後ろに出たらy座標は0にする
# yr = 0
new_goal_pose = Pose2D(xr, yr, 0)
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose, Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
# print(goalie_pose.x, goalie_pose.y, ball_pose.x, ball_pose.y)
return control_target
def interpose(ball_info, robot_info, control_target):
# ボールが動いていると判断するしきい値[m/s]
MOVE_BALL_VELOCITY_THRESHOLD = 0.5
# ロボットがボールを持っていると判断するしきい値[m]
DIST_ROBOT_TO_BALL_THRESHOLD = 0.2
# ゴールライン上ではなく一定距離[m]前を守るための変数
MARGIN_DIST_X = 0.1
# ゴールの位置(自陣のゴールのx座標は絶対負になる)
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
OUR_GOAL_UPPER = Field.goal_pose('our', 'upper')
OUR_GOAL_LOWER = Field.goal_pose('our', 'lower')
# ボールの位置
ball_pose = ball_info.pose
# 敵のロボットの情報
robot_info_their = robot_info['their']
# 敵ロボットとボールの距離
dist = []
for their_info in robot_info_their:
if their_info.disappeared is False:
dist.append(tool.distance_2_poses(their_info.pose, ball_pose))
else:
dist.append(100)
# 一番近い敵ロボットのID
min_dist_id = dist.index(min(dist))
# 一番近い敵ロボットの座標
robot_pose = robot_info_their[min_dist_id].pose
# ボールの速度
ball_velocity_x = ball_info.velocity.x
ball_velocity_y = ball_info.velocity.y
ball_velocity = math.hypot(ball_velocity_x, ball_velocity_y)
# ボールの進む角度
angle_ball = math.atan2(ball_velocity_y, ball_velocity_x)
# ボールの進む変化量を計算（方向を考慮した単位量）
var_ball_velocity_x = math.cos(angle_ball)
var_ball_velocity_y = math.sin(angle_ball)
# ボールの次の予測位置を取得
ball_pose_next = Pose2D(ball_pose.x + var_ball_velocity_x,
ball_pose.y + var_ball_velocity_y, 0)
# 敵ロボットとボールの距離が近い場合は敵の向いている直線を使う
if dist[min_dist_id] < DIST_ROBOT_TO_BALL_THRESHOLD and robot_pose.x < 0:
slope = math.tan(robot_pose.theta)
intercept = robot_pose.y - slope * robot_pose.x
# ボールの速度がある場合かつ近づいてくる場合はボールの向かう直線を使う
elif MOVE_BALL_VELOCITY_THRESHOLD < ball_velocity and ball_velocity_x < 0:
slope, intercept = _get_line_parameters(ball_pose, ball_pose_next)
# その他はゴール中心とボールを結ぶ直線を使う
else:
slope, intercept = _get_line_parameters(ball_pose, OUR_GOAL_POSE)
# ゴーリの新しい座標
goalie_pose_x = OUR_GOAL_POSE.x + MARGIN_DIST_X
goalie_pose_y = slope * goalie_pose_x + intercept
# ゴールから飛び出さないようる上下限を設ける
if OUR_GOAL_UPPER.y < goalie_pose_y:
goalie_pose_y = OUR_GOAL_UPPER.y
elif goalie_pose_y < OUR_GOAL_LOWER.y:
goalie_pose_y = OUR_GOAL_LOWER.y
# ゴーリの新しい座標
new_goalie_pose = Pose2D(goalie_pose_x, goalie_pose_y, 0)
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goalie_pose = control_target.path[-1]
if not tool.is_close(current_goalie_pose, new_goalie_pose,
Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goalie_pose)
return control_target
def basic_atk(control_target, ball_info, atk_pose, recv_pose, target_pose):
# 目標座標までの角度
angle_ball_to_target = tool.get_angle(ball_info.pose, target_pose)
# 目標位置はボールの前方にし、目標角度は、自己位置からみたボール方向にする
trans = tool.Trans(ball_info.pose, angle_ball_to_target)
# ボールとpalcement位置の距離
dist_ball_to_target = tool.distance_2_poses(ball_info.pose, target_pose)
# recvがボールのレシーブを行う座標
ball_receiving_pose = generating_behind_target_pose(trans, target_pose, SET_POSE_ADD_X_RECV)
# ボール速度
ball_velocity = math.hypot(ball_info.velocity.x, ball_info.velocity.y)
# ---------------------------------------
# placementの行動生成
# デフォルト値
avoid_ball = False
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
new_pose = Pose2D()
# ボールが範囲に入っていない場合はplacementを行う
if BALL_PLACE_TRESHOLD < dist_ball_to_target:
# 指定位置に到着したかどうか
is_atk_arrived = atk_arrived_check(trans.transform(atk_pose))
is_recv_arrived = recv_arrived_check(tool.distance_2_poses(recv_pose, ball_receiving_pose))
# 蹴ったあとにatkが追いかけない様に対策
if VEL_THRESHOLD < ball_velocity:
new_pose = atk_pose
# atkがボールの後ろにいなければ移動する
elif not is_atk_arrived:
# ボールの後ろに座標を生成
new_pose = trans.inverted_transform(Pose2D(-SET_POSE_ADD_X_ATK, 0, 0))
new_pose.theta = angle_ball_to_target
avoid_ball = True
# もしボールとゴールに近い場合はアタッカーが置きにいく
elif dist_ball_to_target < BALL_GET_AREA:
# ドリブルする
control_target = offense.inplay_dribble(atk_pose, ball_info,
control_target,target_pose)
new_pose = control_target.path[-1]
# ドリブルをやめる範囲ならドリブル
control_target.dribble_power = 0.0
if BALL_PLACE_AREA_NO_DRIBBLE < dist_ball_to_target:
control_target.dribble_power = DRIBBLE_POWER
# お互いの位置がセットされたら蹴る
elif is_atk_arrived and is_recv_arrived:
# ボールを確実に保持するためボールの少し前に移動する
new_pose = trans.inverted_transform(Pose2D(0.2, 0, 0))
new_pose.theta = angle_ball_to_target
# ドリブルとキックをオン
control_target.kick_power = KICK_POWER
control_target.dribble_power = DRIBBLE_POWER
# ボールの後ろに移動
else:
new_pose = trans.inverted_transform(Pose2D(-SET_POSE_ADD_X_ATK, 0, 0))
new_pose.theta = angle_ball_to_target
avoid_ball = True
# 範囲内にボールがある場合は離れる
else:
# ボールから離れる動作
angle_atk_to_target = tool.get_angle(target_pose, atk_pose)
new_pose.x = target_pose.x + BALL_MARGIN_DIST * math.cos(angle_atk_to_target)
new_pose.y = target_pose.y + BALL_MARGIN_DIST * math.sin(angle_atk_to_target)
new_pose.theta = angle_atk_to_target + math.pi
# パスを追加
control_target.path = []
control_target.path.append(new_pose)
return control_target, avoid_ball
def _draw_referee(self, painter):
# レフェリーの情報を描画する
PLACE_RADIUS = 0.15 # meters
AVOID_LENGTH = 0.5 # meter
if self._decoded_referee is None:
return
ball_pose = self._ball_info.pose
# ボールプレースメントの進入禁止エリアと設置位置を描画
if self._decoded_referee.referee_text == "OUR_BALL_PLACEMENT" \
or self._decoded_referee.referee_text == "THEIR_BALL_PLACEMENT":
replacement_pose = self._decoded_referee.placement_position
# 進入禁止エリアを描画
# Reference: Rule 8.2.3
angle_ball_to_target = tool.get_angle(ball_pose, replacement_pose)
dist_ball_to_target = tool.distance_2_poses(
ball_pose, replacement_pose)
trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)
# 進入禁止エリア長方形の角の座標を取得
avoid_upper_left = trans_BtoT.inverted_transform(
Pose2D(0, AVOID_LENGTH, 0))
avoid_lower_left = trans_BtoT.inverted_transform(
Pose2D(0, -AVOID_LENGTH, 0))
avoid_upper_right = trans_BtoT.inverted_transform(
Pose2D(dist_ball_to_target, AVOID_LENGTH, 0))
avoid_lower_right = trans_BtoT.inverted_transform(
Pose2D(dist_ball_to_target, -AVOID_LENGTH, 0))
# 各座標を描画座標に変換
upper_left_point = self._convert_to_view(avoid_upper_left.x,
avoid_upper_left.y)
lower_left_point = self._convert_to_view(avoid_lower_left.x,
avoid_lower_left.y)
upper_right_point = self._convert_to_view(avoid_upper_right.x,
avoid_upper_right.y)
lower_right_point = self._convert_to_view(avoid_lower_right.x,
avoid_lower_right.y)
# ポリゴンに追加
polygon = QPolygonF()
polygon.append(upper_left_point)
polygon.append(upper_right_point)
polygon.append(lower_right_point)
polygon.append(lower_left_point)
avoid_color = QColor(Qt.red)
avoid_color.setAlphaF(0.3)
painter.setPen(QPen(Qt.black, 1))
painter.setBrush(avoid_color)
painter.drawPolygon(polygon)
replace_point = self._convert_to_view(replacement_pose.x,
replacement_pose.y)
ball_point = self._convert_to_view(ball_pose.x, ball_pose.y)
size = AVOID_LENGTH * self._scale_field_to_view
painter.drawEllipse(replace_point, size, size)
painter.drawEllipse(ball_point, size, size)
# ボール設置位置を描画
size = PLACE_RADIUS * self._scale_field_to_view
place_color = QColor(Qt.white)
place_color.setAlphaF(0.6)
painter.setPen(QPen(Qt.black, 2))
painter.setBrush(place_color)
painter.drawEllipse(replace_point, size, size)
# ボール進入禁止エリアを描画
if self._decoded_referee.keep_out_radius_from_ball != -1:
point = self._convert_to_view(ball_pose.x, ball_pose.y)
size = self._decoded_referee.keep_out_radius_from_ball * self._scale_field_to_view
ball_color = copy.deepcopy(self._COLOR_BALL)
keepout_color = QColor(Qt.red)
keepout_color.setAlphaF(0.3)
painter.setPen(Qt.black)
painter.setBrush(keepout_color)
painter.drawEllipse(point, size, size)
# レフェリーテキストをカーソル周辺に表示する
if self._decoded_referee.referee_text:
# カーソル座標を取得
current_pos = self._convert_to_field(self._current_mouse_pos.x(),
self._current_mouse_pos.y())
# 他のテキストと被らないように位置を微調整
current_point = self._convert_to_view(current_pos.x() + 0.1,
current_pos.y() - 0.15)
text = self._decoded_referee.referee_text
painter.setPen(Qt.red)
painter.drawText(current_point, text)
