diff --git a/consai2_game/scripts/example/actions/defense.py b/consai2_game/scripts/example/actions/defense.py index d6631cf..b819f57 100644 --- a/consai2_game/scripts/example/actions/defense.py +++ b/consai2_game/scripts/example/actions/defense.py @@ -34,17 +34,17 @@ def get_avoid_ball_pose(ball_pose, target_pose): # ボールレシーブ情報保持用のクラス class Receiving(object): - _recenving = [False] * role.ZONE_DEFENSE_NUM + _recenving = [False] * role.FIELD_PLAYER_NUM @classmethod - def update_receiving(cls, zone_id, param): - Receiving._recenving[zone_id] = param + def update_receiving(cls, role_id, param): + Receiving._recenving[(role_id - 1)] = param @classmethod - def receiving(cls, zone_id): - return Receiving._recenving[zone_id] + def receiving(cls, role_id): + return Receiving._recenving[(role_id - 1)] -def update_receive_ball(ball_info, my_pose, zone_id): +def update_receive_ball(ball_info, my_pose, role_id): # ボール位置 ball_pose = ball_info.pose # ボールスピード @@ -70,16 +70,16 @@ def update_receive_ball(ball_info, my_pose, zone_id): fabs_y = math.fabs(tr_pose.y) # 受け取れる判定 - if Receiving.receiving(zone_id) == False and \ + if Receiving.receiving(role_id) == False and \ fabs_y < _can_receive_dist - _can_receive_hysteresis: - Receiving.update_receiving(zone_id, True) + Receiving.update_receiving(role_id, True) # 受け取れない判定 - elif Receiving.receiving(zone_id) == True and \ + elif Receiving.receiving(role_id) == True and \ fabs_y > _can_receive_dist + _can_receive_hysteresis: - Receiving.update_receiving(zone_id, False) + Receiving.update_receiving(role_id, False) # 受け取れるかつボールが向かう方向にいる - if Receiving.receiving(zone_id) and tr_pose.x > 0.0: + if Receiving.receiving(role_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) diff --git a/consai2_game/scripts/example/actions/sub_attacker.py b/consai2_game/scripts/example/actions/sub_attacker.py new file mode 100644 index 0000000..f78d231 --- /dev/null +++ b/consai2_game/scripts/example/actions/sub_attacker.py @@ -0,0 +1,93 @@ +# coding: UTF-8 +# defense.pyでは、ボールを蹴らないactionを定義する + +import math +import rospy +import sys,os + +from consai2_msgs.msg import BallInfo, RobotInfo, ControlTarget +from geometry_msgs.msg import Pose2D + +sys.path.append(os.pardir) +from field import Field +import role +import tool +import defense + + +# サブアタッカー +def sub_attacker(my_pose, ball_info, control_target, my_role, defense_num, their_robot_info, role_action_enable): + # ゾーンオフェンス用の待機場所 + ZONE_OFFENCE_POSE = Pose2D(Field.field('length')*0.25 ,0,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) + + # 移動目標点の初期化 + target_pose = Pose2D() + + # --------------------------------------------------------- + # キックとドリブルはOFF + control_target.kick_power = 0.0 + control_target.dribble_power = 0.0 + + target_pose = ZONE_OFFENCE_POSE + if role_action_enable: + # 基本的にアタッカーがボールを取りに行くので + # ボールが無い方向に移動してこぼれ球が取れるようにする + if ball_pose.y > 0: + target_pose.y = - quarter_field_width + else: + target_pose.y = quarter_field_width + else: + target_pose.x = -1.5 + target_pose.y = 0 + + # ボールを向く + target_pose.theta = angle_to_ball + + # ボールが来てたらボールを受け取る + receive_ball_result, receive_target_pose = defense.update_receive_ball(ball_info, my_pose, my_role) + 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 diff --git a/consai2_game/scripts/example/actions/zone.py b/consai2_game/scripts/example/actions/zone.py index 7c34b3d..c0ed10a 100644 --- a/consai2_game/scripts/example/actions/zone.py +++ b/consai2_game/scripts/example/actions/zone.py @@ -1,5 +1,4 @@ # coding: UTF-8 -# defense.pyでは、ボールを蹴らないactionを定義する import math import rospy @@ -16,7 +15,7 @@ # ゾーンディフェンス -def defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their_robot_info, zone_enable): +def defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their_robot_info, role_action_enable): # ゴールディフェンスに割り当てる台数 GOAL_DEFENSE_NUM = 2 # 現在のディフェンス数 - ゴールディフェンス数 = ゾーンディフェンスに割り当てられる台数 @@ -70,37 +69,9 @@ def defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their 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: + if ZONE_DEFENSE_NUM > 0: 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))] @@ -126,14 +97,14 @@ def defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their 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 \ + # ボールが自分のゾーンの中に入っている, かつrole_action_enable + if(role_action_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 != []: + elif role_action_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) @@ -151,7 +122,7 @@ def defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their # ボールが来てたらボールを受け取る if zone_id != None: - receive_ball_result, receive_target_pose = defense.update_receive_ball(ball_info, my_pose, zone_id) + receive_ball_result, receive_target_pose = defense.update_receive_ball(ball_info, my_pose, my_role) if receive_ball_result: # ドリブラー回す control_target.dribble_power = DRIBBLE_POWER diff --git a/consai2_game/scripts/example/assign.py b/consai2_game/scripts/example/assign.py index 42ed57c..3a68fbe 100644 --- a/consai2_game/scripts/example/assign.py +++ b/consai2_game/scripts/example/assign.py @@ -1,19 +1,18 @@ # coding: UTF-8 -from actions import center_back, zone, man_mark +from actions import center_back, zone, man_mark, sub_attacker import role -def assign(my_role, ball_info, control_target, my_pose, defense_num, robot_info, zone_enable=False): +def assign(my_role, ball_info, control_target, my_pose, defense_num, robot_info, role_action_enable=False): # ゴール前ディフェンス if role.ROLE_ID['ROLE_CENTER_BACK_1'] <= my_role <= role.ROLE_ID['ROLE_CENTER_BACK_2']: return center_back.center_back(my_pose, ball_info, control_target, my_role, defense_num) + # サブアタッカー elif my_role == role.ROLE_ID['ROLE_SUB_ATTACKER']: - control_target.path = [] - control_target.path.append(my_pose) - return control_target + return sub_attacker.sub_attacker(my_pose, ball_info, control_target, my_role, defense_num, robot_info['their'], role_action_enable) # ゾーンディフェンス elif role.ROLE_ID['ROLE_ZONE_1'] <= my_role <= role.ROLE_ID['ROLE_ZONE_4']: - return zone.defense_zone(my_pose, ball_info, control_target, my_role, defense_num, robot_info['their'], zone_enable) + return zone.defense_zone(my_pose, ball_info, control_target, my_role, defense_num, robot_info['their'], role_action_enable) elif role.ROLE_ID['ROLE_MAN_MARK_1'] <= my_role <= role.ROLE_ID['ROLE_MAN_MARK_2']: control_target.path = [] control_target.path.append(my_pose) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 332a69b..c83a7f6 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -53,7 +53,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de remake_path = False # 経路再生成のフラグ TODO:remake_pathを活用する avoid_obstacle = True # 障害物回避の経路追加フラグ avoid_ball = False # ボール回避の経路追加フラグ - zone_enable = False + role_action_enable = False # パラメータ初期化 self._control_target.dribble_power = 0.0 @@ -67,7 +67,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de elif referee.is_inplay: rospy.logdebug("IN-PLAY") - zone_enable = True + role_action_enable = True if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: if tool.is_in_defense_area(ball_info.pose, 'our'): @@ -94,7 +94,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de else: self._control_target = assign.assign( self._my_role, ball_info, self._control_target, - self._my_pose, defense_num, robot_info, zone_enable) + self._my_pose, defense_num, robot_info, role_action_enable) else: if referee.referee_id == ref.REFEREE_ID["STOP"]: @@ -276,7 +276,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de else: self._control_target = assign.assign( self._my_role, ball_info, self._control_target, - self._my_pose, defense_num, robot_info,zone_enable=True) + self._my_pose, defense_num, robot_info,role_action_enable=True) elif referee.referee_id == ref.REFEREE_ID["THEIR_INDIRECT_FREE"]: rospy.logdebug("THEIR_INDIRECT") @@ -291,7 +291,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de else: self._control_target = assign.assign( self._my_role, ball_info, self._control_target, - self._my_pose, defense_num, robot_info, zone_enable=True) + self._my_pose, defense_num, robot_info, role_action_enable=True) elif referee.referee_id == ref.REFEREE_ID["THEIR_TIMEOUT"]: rospy.logdebug("THEIR_TIMEOUT") diff --git a/consai2_game/scripts/example/role.py b/consai2_game/scripts/example/role.py index 671fb15..f5d9370 100644 --- a/consai2_game/scripts/example/role.py +++ b/consai2_game/scripts/example/role.py @@ -8,17 +8,18 @@ "ROLE_ATTACKER" : 1, "ROLE_CENTER_BACK_1" : 2, "ROLE_CENTER_BACK_2" : 3, - "ROLE_ZONE_1" : 4, - "ROLE_ZONE_2" : 5, - "ROLE_ZONE_3" : 6, - "ROLE_ZONE_4" : 7, - "ROLE_SUB_ATTACKER" : 8, + "ROLE_SUB_ATTACKER" : 4, + "ROLE_ZONE_1" : 5, + "ROLE_ZONE_2" : 6, + "ROLE_ZONE_3" : 7, + "ROLE_ZONE_4" : 8, "ROLE_MAN_MARK_1" : 9, "ROLE_MAN_MARK_2" : 10, "ROLE_NONE" : 99, } -ZONE_DEFENSE_NUM = 4 +# TODO: 名前が正しくない +FIELD_PLAYER_NUM = 10 # Roleの計算をするクラス class RoleDecision(object): @@ -140,8 +141,11 @@ class RoleStocker(object): def __init__(self, max_id, max_role): self._my_role = [ROLE_ID["ROLE_NONE"]] * (max_id + 1) self._role_is_exist = [False] * (max_role + 1) - self._defense_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_CENTER_BACK_1"]: - ROLE_ID["ROLE_SUB_ATTACKER"]] if i is True]) + self._center_back_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_CENTER_BACK_1"]: + (ROLE_ID["ROLE_CENTER_BACK_2"] + 1)] if i is True]) + self._zone_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_ZONE_1"]: + (ROLE_ID["ROLE_ZONE_4"] + 1)] if i is True]) + self._defense_num = self._center_back_num + self._zone_num def set_my_role(self, robot_id, role_num): if self._my_role[robot_id] != ROLE_ID["ROLE_NONE"]: @@ -149,8 +153,11 @@ def set_my_role(self, robot_id, role_num): self._my_role[robot_id] = role_num if role_num != ROLE_ID["ROLE_NONE"]: self._role_is_exist[role_num] = True - self._defense_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_CENTER_BACK_1"]: - ROLE_ID["ROLE_SUB_ATTACKER"]] if i is True]) + self._center_back_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_CENTER_BACK_1"]: + (ROLE_ID["ROLE_CENTER_BACK_2"] + 1)] if i is True]) + self._zone_num = len([i for i in self._role_is_exist[ROLE_ID["ROLE_ZONE_1"]: + (ROLE_ID["ROLE_ZONE_4"] + 1)] if i is True]) + self._defense_num = self._center_back_num + self._zone_num def get_my_role(self, robot_id): return self._my_role[robot_id]