def _update_interpose(self):
base_pose = WorldModel.get_pose(self._base)
target_pose = WorldModel.get_pose(self._target)
if base_pose is None or target_pose is None:
return False
angle_to_target = tool.getAngle(base_pose, target_pose)
interposed_pose = Pose(0, 0, 0)
if not self._to_dist is None:
trans = tool.Trans(base_pose, angle_to_target)
tr_interposed_pose = Pose(self._to_dist, 0.0, 0)
interposed_pose = trans.invertedTransform(tr_interposed_pose)
elif not self._from_dist is None:
angle_to_base = tool.getAngle(target_pose, base_pose)
trans = tool.Trans(target_pose, angle_to_base)
tr_interposed_pose = Pose(self._from_dist, 0.0, 0)
interposed_pose = trans.invertedTransform(tr_interposed_pose)
interposed_pose.theta = angle_to_target
self.pose = interposed_pose
return True
def interpose(target_info,
control_target,
dist_from_goal=None,
dist_from_target=None):
# 自チームのゴール中心とtarget_info.poseを直線で結び、その直線上に移動する
# dist_from_goal is not Noneなら、ゴール中心からdist分離れた位置に移動する
# dist_from_target is not Noneなら、target_info.poseからdist分離れた位置に移動する
# 到達姿勢の計算とcontrol_targetの更新(path以外)
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
# 両方設定されてなかったらdist_from_targetを優先する
if dist_from_goal is None and dist_from_target is None:
dist_from_target = 0.6 # 適当な値
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
angle_to_target = tool.get_angle(OUR_GOAL_POSE, target_info.pose)
new_goal_pose = Pose2D()
if dist_from_goal is not None:
trans = tool.Trans(GOAL_POSE, angle_to_target)
tr_goal_pose = Pose2D(dist_from_goal, 0, 0)
new_goal_pose = trans.inverted_transform(tr_goal_pose)
else:
angle_to_goal = tool.get_angle(target_info.pose, OUR_GOAL_POSE)
trans = tool.Trans(target_info.pose, angle_to_goal)
tr_goal_pose = Pose2D(dist_from_target, 0, 0)
new_goal_pose = trans.inverted_transform(tr_goal_pose)
new_goal_pose.theta = angle_to_target
# ---------------------------------------------------------
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 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 _update_receive_ball(self):
ball_pose = WorldModel.get_pose('Ball')
ball_vel = WorldModel.get_velocity('Ball')
result = False
if WorldModel.ball_is_moving():
angle_velocity = tool.getAngleFromCenter(ball_vel)
trans = tool.Trans(ball_pose, angle_velocity)
role_pose = WorldModel.get_pose(self._my_role)
if role_pose is None:
return False
tr_pose = trans.transform(role_pose)
fabs_y = math.fabs(tr_pose.y)
if self._receiving == False and \
fabs_y < self._can_receive_dist - self._can_receive_hysteresis:
self._receiving = True
elif self._receiving == True and \
fabs_y > self._can_receive_dist + self._can_receive_hysteresis:
self._receiving = False
if self._receiving and tr_pose.x > 0.0:
tr_pose.y = 0.0
inv_pose = trans.invertedTransform(tr_pose)
angle_to_ball = tool.getAngle(inv_pose, ball_pose)
self.pose = Pose(inv_pose.x, inv_pose.y, angle_to_ball)
result = True
return result
def _penalty_shoot(my_pose,
ball_info,
control_target,
kick_enable,
target_pose,
kick_power=1.0):
# PK用のシュートアクション
# kick_enable is Falseで、ボールの近くまで移動する
# kick_enable is True で、シュートする
KICK_POWER = kick_power
SHOOT_TARGET = target_pose
DRIBBLE_POWER = 0.5
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)
random_num = Observer.random_zero_one()
if random_num == 0:
target_goal_side = Field.goal_pose('their', 'upper')
else:
target_goal_side = Field.goal_pose('their', 'lower')
angle_to_target_side = tool.get_angle(my_pose, target_goal_side)
# ロボットがボールの裏側に回ったらcan_kick is True
can_kick = False
if tr_my_pose.x > -0.2 and math.fabs(tr_my_pose.y) < 0.05:
can_kick = True
avoid_ball = True # ボールを回避する
new_goal_pose = Pose2D()
if can_kick and kick_enable:
# ボールをける
avoid_ball = False
# ボールの前方に移動する
new_position = trans.inverted_transform(Pose2D(0, 0, 0))
new_goal_pose = new_position
new_goal_pose.theta = angle_to_target_side
# ドリブルとキックをオン
control_target.dribble_power = DRIBBLE_POWER
control_target.kick_power = KICK_POWER
else:
Observer.update_random_zero_one()
# ボールの裏に移動する
new_position = trans.inverted_transform(Pose2D(-0.1, 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_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 outside_shoot(my_pose, ball_info, control_target):
# ゴールに入らないように外側にボールをけるアクション
OUR_GOAL = Field.goal_pose('our', 'center')
TARGET_LENGTH = 4.0
CAN_SHOOT_ANGLE = 10 # degrees
shoot_target = Pose2D()
angle_goal_to_ball = tool.get_angle(OUR_GOAL, ball_info.pose)
trans = tool.Trans(ball_info.pose, angle_goal_to_ball)
shoot_target = trans.inverted_transform(Pose2D(TARGET_LENGTH, 0, 0))
return _inplay_shoot(my_pose, ball_info, control_target, shoot_target,
CAN_SHOOT_ANGLE)
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 update_receive_ball(ball_info, my_pose, zone_id):
# ボール位置
ball_pose = ball_info.pose
# ボールスピード
ball_vel = ball_info.velocity
# 受け取れると判断する距離
_can_receive_dist = 1.0
# ヒステリシス
_can_receive_hysteresis = 0.3
result = False
target_pose = Pose2D()
# ボールが動いている
if Observer.ball_is_moving():
# ボール速度ベクトルの角度
angle_velocity = tool.get_angle_from_center(ball_vel)
trans = tool.Trans(ball_pose, angle_velocity)
tr_pose = trans.transform(my_pose)
# ボール速度の線と垂直な距離
fabs_y = math.fabs(tr_pose.y)
# 受け取れる判定
if Receiving.receiving(zone_id) == False and \
fabs_y < _can_receive_dist - _can_receive_hysteresis:
Receiving.update_receiving(zone_id, True)
# 受け取れない判定
elif Receiving.receiving(zone_id) == True and \
fabs_y > _can_receive_dist + _can_receive_hysteresis:
Receiving.update_receiving(zone_id, False)
# 受け取れるかつボールが向かう方向にいる
if Receiving.receiving(zone_id) and tr_pose.x > 0.0:
tr_pose.y = 0.0
inv_pose = trans.inverted_transform(tr_pose)
angle_to_ball = tool.get_angle(inv_pose, ball_pose)
target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball)
result = True
return result, 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 receive_ball(ball_info, my_pose):
ball_pose = ball_info.pose
_can_receive_dist = 1.0
_can_receive_hysteresis = 0.3
target_pose = Pose2D()
if Observer.ball_is_moving():
angle_velocity = tool.get_angle_from_center(ball_info.velocity)
trans = tool.Trans(ball_pose, angle_velocity)
tr_pose = trans.transform(my_pose)
fabs_y = math.fabs(tr_pose.y)
tr_pose.y = 0.0
inv_pose = trans.inverted_transform(tr_pose)
angle_to_ball = tool.get_angle(inv_pose, ball_pose)
target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball)
return target_pose
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 _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)
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 _update_approach_to_shoot(self):
# Reference to this idea
# http://wiki.robocup.org/images/f/f9/Small_Size_League_-_RoboCup_2014_-_ETDP_RoboDragons.pdf
ball_pose = WorldModel.get_pose('Ball')
target_pose = WorldModel.get_pose(self._target)
role_pose = WorldModel.get_pose(self._my_role)
if target_pose is None or role_pose is None:
return False
# ボールからターゲットを見た座標系で計算する
angle_ball_to_target = tool.getAngle(ball_pose, target_pose)
trans = tool.Trans(ball_pose, angle_ball_to_target)
tr_role_pose = trans.transform(role_pose)
# tr_role_poseのloser_side判定にヒステリシスをもたせる
if self._role_is_lower_side == True and \
tr_role_pose.y > self._role_pose_hystersis:
self._role_is_lower_side = False
elif self._role_is_lower_side == False and \
tr_role_pose.y < - self._role_pose_hystersis:
self._role_is_lower_side = True
if self._role_is_lower_side:
tr_role_pose.y *= -1.0
tr_approach_pose = Pose(0, 0, 0)
if tr_role_pose.x > 0:
# 1.ボールの斜め後ろへ近づく
# copysign(x,y)でyの符号に合わせたxを取得できる
tr_approach_pose = Pose(
-self._pose_max.x,
math.copysign(self._pose_max.y, tr_role_pose.y), 0)
else:
# ボール裏へ回るためのピボットを生成
pivot_pose = Pose(0, self._tuning_param_pivot_y, 0)
angle_pivot_to_role = tool.getAngle(pivot_pose, tr_role_pose)
limit_angle = self._tuning_angle + math.pi * 0.5
if tr_role_pose.y > self._tuning_param_pivot_y and \
angle_pivot_to_role < limit_angle:
# 2.ボール後ろへ回りこむ
diff_angle = tool.normalize(limit_angle - angle_pivot_to_role)
decrease_coef = diff_angle / self._tuning_angle
tr_approach_pose = Pose(-self._pose_max.x,
self._pose_max.y * decrease_coef, 0)
else:
# 3.ボールに向かう
diff_angle = tool.normalize(angle_pivot_to_role - limit_angle)
approach_coef = diff_angle / (math.pi * 0.5 -
self._tuning_angle)
if approach_coef > 1.0:
approach_coef = 1.0
pos_x = approach_coef * (
2.0 * constants.BallRadius -
self._tuning_param_x) + self._tuning_param_x
tr_approach_pose = Pose(-pos_x, 0, 0)
# 上下反転していたapproach_poseを元に戻す
if self._role_is_lower_side:
tr_approach_pose.y *= -1.0
self.pose = trans.invertedTransform(tr_approach_pose)
self.pose.theta = angle_ball_to_target
return True
def defense_zone(my_pose, ball_info, control_target, my_role, defense_num,
their_robot_info, zone_enable):
# ゴールディフェンスに割り当てる台数
GOAL_DEFENSE_NUM = 2
# 現在のディフェンス数 - ゴールディフェンス数 = ゾーンディフェンスに割り当てられる台数
ZONE_DEFENSE_NUM = defense_num - GOAL_DEFENSE_NUM
# ゾーンディフェンスが始まるROLE_ID
ZONE_START_ROLE_ID = role.ROLE_ID["ROLE_ZONE_1"]
# ゾーンオフェンス用の待機場所
ZONE_OFFENCE_POSE = Pose2D(Field.field('length') * 0.25, 0, 0)
# センターライン用のマージン
MARGIN_CENTER = 0.6
# ちょっと前進用のマージン
MARGIN_LITTLE_FORWARD = 1.0
# ドリブルパワー
DRIBBLE_POWER = 0.6
# ボール位置
ball_pose = ball_info.pose
# Field情報からペナルティエリアの情報を取得
# フィールド幅
field_width = Field.field('width')
# フィールド幅の半分
half_field_width = float(field_width) / 2
# フィールド幅の1/4
quarter_field_width = float(field_width) / 4
# フィールド長さ
field_length = Field.field('length')
# フィールド長さの1/4 → 自陣側の長さの半分
half_our_field_length = -float(field_length) / 4
# ゴール中心
goal_center = Field.goal_pose('our', 'center')
# ペナルティエリアの角
left_penalty_corner = Field.penalty_pose('our', 'upper_front')
right_penalty_corner = Field.penalty_pose('our', 'lower_front')
# 自分からボールへの角度（ボールの方向を向くため）
angle_to_ball = tool.get_angle(my_pose, ball_pose)
# ゴール中心からボールへの角度
angle_to_ball_from_goal = tool.get_angle(goal_center, ball_pose)
# ゾーンディフェンス用のID
zone_id = None
# 移動目標点の初期化
target_pose = Pose2D()
# ---------------------------------------------------------
# キックとドリブルはOFF
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
# ゾーンオフェンス判定用フラグ
my_role_is_offence = False
# ボールが相手フィールドにあるとき
# ゾーンから1台ゾーンオフェンスに出す
# 相手キックオフ時などに前に出ないように
# マージンを持って相手フィールド側かを判断している
if ZONE_DEFENSE_NUM > 1 and ball_pose.x > MARGIN_CENTER:
# 1台ディフェンスが減る
ZONE_DEFENSE_NUM -= 1
# ゾーンディフェンスが始まるROLE_IDをずらす
ZONE_START_ROLE_ID = role.ROLE_ID["ROLE_ZONE_2"]
# ROLE_ZONE_1をゾーンオフェンスとして出す
if my_role is role.ROLE_ID["ROLE_ZONE_1"]:
my_role_is_offence = True
# 私はゾーンオフェンスです
if my_role_is_offence:
zone_id = 0
target_pose = ZONE_OFFENCE_POSE
# 基本的にアタッカーがボールを取りに行くので
# ボールが無い方向に移動してこぼれ球が取れるようにする
if ball_pose.y > 0:
target_pose.y = -quarter_field_width
else:
target_pose.y = quarter_field_width
# ボールを向く
target_pose.theta = angle_to_ball
# ゾーンオフェンス以外
if ZONE_DEFENSE_NUM > 0 and not my_role_is_offence:
step = float(field_width) / (ZONE_DEFENSE_NUM * 2)
# ゾーンディフェンスの数でフィールド幅を等分した配列を作る
split_field = [
i * step - half_field_width
for i in range(0, (ZONE_DEFENSE_NUM * 2 + 1))
]
# ゾーンディフェンスの数でフィールド幅を等分した時の
# それぞれのゾーンの中心の配列を作る
split_field_center = [i * step - half_field_width for i in range(0,(ZONE_DEFENSE_NUM * 2)) \
if i % 2 != 0]
# 参照エラー対策のtry
try:
# ゾーンIDの計算
# ロボットが8台生きていてゾーンオフェンスがいなければ
# ゾーンIDは0~3
zone_id = my_role - ZONE_START_ROLE_ID
# ゾーンの中心を目標位置とする
target_pose.y = split_field_center[zone_id]
# 自分のゾーンに入っている敵チェック
# their_robot_infoのposeを抜き出してそれぞれチェック
# 敵が自陣側にいる、かつ、自分のゾーンの幅の中にいる、を確認
# 当てはまらない場合配列は空っぽ
invader_pose = [i.pose for i in their_robot_info \
if split_field[zone_id * 2] < i.pose.y < split_field[(zone_id + 1) * 2] and \
i.pose.x < 0]
# ボールが自分のゾーンの中に入っている, かつzone_enable
if(zone_enable and \
ball_pose.x < 0 and \
split_field[zone_id * 2] < ball_pose.y < split_field[(zone_id + 1) * 2]):
trans = tool.Trans(ball_pose, angle_to_ball_from_goal)
target_pose = trans.inverted_transform(Pose2D(-0.9, 0, 0))
# 自分のゾーンにボールはないけど敵がいる場合は割り込む
elif zone_enable and invader_pose != []:
# 敵とボールの間に割り込む
angle_to_ball_from_invader = tool.get_angle(
invader_pose[0], ball_pose)
trans = tool.Trans(invader_pose[0], angle_to_ball_from_invader)
target_pose = trans.inverted_transform(Pose2D(0.5, 0, 0))
else:
# ボールが敵陣の時はディフェンスちょっと前進
if ball_pose.x > MARGIN_CENTER:
target_pose.x = half_our_field_length + MARGIN_LITTLE_FORWARD
else:
target_pose.x = half_our_field_length
except IndexError:
target_pose = my_pose
target_pose.theta = angle_to_ball
# ボールが来てたらボールを受け取る
if zone_id != None:
receive_ball_result, receive_target_pose = defense.update_receive_ball(
ball_info, my_pose, zone_id)
if receive_ball_result:
# ドリブラー回す
control_target.dribble_power = DRIBBLE_POWER
target_pose = receive_target_pose
else:
# ボールに近づいてたら離れる
target_pose = defense.get_avoid_ball_pose(ball_pose, target_pose)
# ペナルティエリアには入らない
if((left_penalty_corner.y + 0.2 > target_pose.y > right_penalty_corner.y - 0.2) and \
target_pose.x < left_penalty_corner.x + 0.3):
target_pose.x = half_our_field_length
control_target.path = []
control_target.path.append(target_pose)
return control_target
def center_back(my_pose, ball_info, control_target, my_role, defense_num):
# ゴール前ディフェンスは、ペナルティエリアに沿って守備を行う
# ボールとゴールを結ぶ直線と、ペナルティエリアのラインの交点を基準に移動
# ボールの位置によって、ライン左、ライン正面、ライン右のどのラインの前に移動するか変わる
# ペナルティエリアからどれだけ離れるか
MARGIN_LINE = 0.2
# 2台でディフェンスする時のお互いの距離
MARGIN_ROBOT = 0
# ライン左からライン正面に移動する時等に、
# 一時的にスピードアップするための数値
MARGIN_FOR_SPEED = 0.5
# ディフェンスが2台以上いる時はMARGIN_ROBOTを変更
if defense_num > 1:
if my_role == role.ROLE_ID["ROLE_CENTER_BACK_1"]:
MARGIN_ROBOT = 0.15
else:
MARGIN_ROBOT = -0.15
# ボール位置
ball_pose = ball_info.pose
# ボールの位置判定用フラグ
ball_is_center = False
ball_is_left = False
ball_is_right = False
# 自分の位置判定用フラグ
my_pose_is_left = False
my_pose_is_right = False
# 移動すべき場所判定用フラグ
target_is_center = False
target_is_left = False
target_is_right = False
# Field情報からペナルティエリアの情報を取得
# ペナルティエリアの左角
left_penalty_corner = Field.penalty_pose('our', 'upper_front')
# ペナルティエリアの右角
right_penalty_corner = Field.penalty_pose('our', 'lower_front')
# ペナルティエリアの左側のゴールラインとの交点
left_penalty_goalside = Field.penalty_pose('our', 'upper_back')
# ペナルティエリアの右側のゴールラインとの交点
right_penalty_goalside = Field.penalty_pose('our', 'lower_back')
# ゴールの中心
goal_center = Field.goal_pose('our', 'center')
# ゴール中心からペナルティエリア左角への角度
angle_to_left_penalty_corner = tool.get_angle(goal_center,
left_penalty_corner)
# ゴール中心からペナルティエリア右角への角度
angle_to_right_penalty_corner = tool.get_angle(goal_center,
right_penalty_corner)
# 自分からボールへの角度（ボールの方向を向くため）
angle_to_ball = tool.get_angle(my_pose, ball_pose)
# ゴールを背にした左角を中心とした座標軸へ変換
trans_left = tool.Trans(left_penalty_corner, angle_to_left_penalty_corner)
tr_left_ball_pose = trans_left.transform(ball_pose)
# ゴールを背にした右角を中心とした座標軸へ変換
trans_right = tool.Trans(right_penalty_corner,
angle_to_right_penalty_corner)
tr_right_ball_pose = trans_right.transform(ball_pose)
# ボールの位置を判定
if tr_left_ball_pose.y > 0:
ball_is_left = True
elif tr_right_ball_pose.y < 0:
ball_is_right = True
else:
ball_is_center = True
# ---------------------------------------------------------
# キックとドリブルはOFF
control_target.kick_power = 0.0
control_target.dribble_power = 0.0
# ボールは真ん中にある
if ball_is_center:
# ペナルティエリアの左角と右角の線分(正面の線)と、ゴール中心とボールの線分の交点
target_pose = tool.get_intersection(left_penalty_corner,
right_penalty_corner, goal_center,
ball_pose)
if target_pose is not None:
# ペナルティエリアに侵入しないように+MARGIN_LINE
target_pose.x += MARGIN_LINE
# ロボットが正面の線より後ろにいる
if my_pose.x < left_penalty_corner.x:
# ダッシュで正面に移動
target_pose.x += MARGIN_FOR_SPEED
# ペナルティエリアを沿って移動
if my_pose.y > 0:
target_pose.y = left_penalty_corner.y + MARGIN_LINE
else:
target_pose.y = right_penalty_corner.y - MARGIN_LINE
else:
target_pose.y += MARGIN_ROBOT
else:
target_pose = Pose2D()
# ボールは左側にある
elif ball_is_left:
# ペナルティエリアの左側の線分と、ゴール中心とボールの線分の交点
target_pose = tool.get_intersection(left_penalty_corner,
left_penalty_goalside, goal_center,
ball_pose)
if target_pose is not None:
# ペナルティエリアに侵入しないように+MARGIN_LINE
target_pose.y += MARGIN_LINE
# ロボットが左側にいない
if my_pose.y < left_penalty_corner.y:
# 左側にいないかつ後ろにいる場合は右側を沿う
if my_pose.x < left_penalty_corner.x and my_pose.y < 0:
target_pose.x = left_penalty_corner.x + MARGIN_FOR_SPEED
target_pose.y = right_penalty_corner.y - MARGIN_LINE
# 左側にダッシュで移動
else:
target_pose.x = left_penalty_corner.x + MARGIN_LINE
target_pose.y += MARGIN_FOR_SPEED
else:
target_pose.x -= MARGIN_ROBOT
else:
target_pose = Pose2D()
# ボールは右側にある
elif ball_is_right:
target_pose = tool.get_intersection(right_penalty_corner,
right_penalty_goalside,
goal_center, ball_pose)
if target_pose is not None:
# ペナルティエリアに侵入しないように-MARGIN_LINE
target_pose.y -= MARGIN_LINE
# ロボットが右側にいない
if my_pose.y > right_penalty_corner.y:
# 右側にいないかつ後ろにいる場合は左側を沿う
if my_pose.x < left_penalty_corner.x and my_pose.y > 0:
target_pose.x = left_penalty_corner.x + MARGIN_FOR_SPEED
target_pose.y = left_penalty_corner.y + MARGIN_LINE
# 右側にダッシュで移動
else:
target_pose.x = right_penalty_corner.x + MARGIN_LINE
target_pose.y -= MARGIN_FOR_SPEED
else:
target_pose.x += MARGIN_ROBOT
else:
target_pose = Pose2D()
# フィールドから出ないように
if target_pose.x < goal_center.x:
target_pose.x = goal_center.x
# 向きはボールの方向
target_pose.theta = angle_to_ball
control_target.path = []
control_target.path.append(target_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 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
