Feature/sub attacker

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)

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

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

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)

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")
 

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,17 +141,23 @@ 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"]:
             self._role_is_exist[self._my_role[robot_id]] = False 
         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]