小フィールドで試合するための修正

consai2_description/param/game.yaml

@@ -2,13 +2,13 @@ max_id      : 15            # ゲームに使用できる最大ID
 player_num  : 8             # フィールドに同時に出せるロボット数
 our_side    : 'left'        # フィールドの自チームディフェンス側. left or right
 our_color   : 'blue'        # 自チームのマーカ色. blue or yellow
-goalie_id   : 0             # 自チームのゴールキーパのID
+goalie_id   : 1             # 自チームのゴールキーパのID
 
 vision_addr : '224.5.23.2'  # Visionのマルチキャストアドレス
 vision_port : 10006         # Visionのポート番号
 referee_addr : '224.5.23.1' # Refereeのマルチキャストアドレス
 referee_port : 10003        # Refereeのポート番号
-grsim_addr : '127.0.0.1'    # コマンド送信先のgrSimアドレス
+grsim_addr : '192.168.11.232'    # コマンド送信先のgrSimアドレス
 grsim_port : 20011          # コマンド送信先のgrSimポート
 sender_device : '/dev/ttyUSB0'  # 実機送信機のデバイス名
 sender_baudrate : 57600         # 実機送信機のボーレート

consai2_game/scripts/example/actions/ball_placement.py

@@ -44,7 +44,7 @@
 
 # 侵入禁止範囲
 # 0.5m以内に侵入してはだめなので0.5以上にする
-BALL_MARGIN_DIST = 0.65
+BALL_MARGIN_DIST = 0.25
 
 # atkが指定位置に到達したか判定
 def atk_arrived_check(tr_my_pose):
@@ -326,7 +326,9 @@ def avoid_ball_place_line(my_pose, ball_info, target_pose, control_target, force
     # ライン上にいるロボットは回避位置を生成する
     # ラインに対して垂直に移動する
     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 -BALL_MARGIN_DIST < tr_my_pose.y < BALL_MARGIN_DIST \
+            and tr_my_pose.x > -BALL_MARGIN_DIST \
+            and tr_my_pose.x < tr_target_pose.x + BALL_MARGIN_DIST:
             if tr_my_pose.y < 0:
                 tr_my_pose.y -= BALL_MARGIN_DIST
             else:

consai2_game/scripts/example/actions/goalie.py

@@ -2,6 +2,7 @@
 
 # defense.pyでは、ボールを蹴らないactionを定義する
 
+import copy
 import rospy
 import math
 import sys,os
@@ -10,6 +11,7 @@
 from consai2_msgs.msg import ControlTarget
 from geometry_msgs.msg import Pose2D
 import tool
+import offense
 
 sys.path.append(os.pardir)
 from field import Field
@@ -131,3 +133,59 @@ def interpose(ball_info, robot_info, control_target):
 
     return control_target
 
+
+def demo_shoot(ball_info, robot_info, control_target, my_pose = None, inplay_shoot = False):
+
+    # ゴールライン上ではなく一定距離[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
+    ball_velocity = ball_info.velocity
+    ball_velocity_angle = tool.get_angle_from_center(ball_velocity)
+    ball_vel_pose = Pose2D(
+        ball_pose.x + 10 * math.cos(ball_velocity_angle),
+        ball_pose.y + 10 * math.sin(ball_velocity_angle),
+        0.0
+    )
+
+    new_goal_pose = Pose2D()
+    # ボールが動いてたら、ディフェンスラインと、ボール速度の交点に移動
+    defense_pose_upper = copy.deepcopy(OUR_GOAL_UPPER)
+    defense_pose_upper.x += MARGIN_DIST_X
+    defense_pose_lower = copy.deepcopy(OUR_GOAL_LOWER)
+    defense_pose_lower.x += MARGIN_DIST_X
+
+    intersection_pose = tool.get_intersection(
+        ball_pose, ball_vel_pose, defense_pose_upper, defense_pose_lower)
+    if intersection_pose \
+        and tool.get_size_from_center(ball_info.velocity) > 0.5 \
+        and math.fabs(intersection_pose.y) <= defense_pose_upper.y \
+        and intersection_pose.x < defense_pose_lower.x + 0.1:
+        new_goal_pose = intersection_pose
+        new_goal_pose.x -= 0.09  # ロボットの中心からドリブラの位置までの距離を下げる:w
+        new_goal_pose.theta = 0.0
+        control_target.kick_power = 0.5
+        control_target.dribble_power = 0.5
+    else:
+        # ボールが止まっていたら
+
+        if inplay_shoot:
+            return offense.inplay_shoot(my_pose, ball_info, control_target)
+
+        new_goal_pose.x = OUR_GOAL_POSE.x + MARGIN_DIST_X
+        new_goal_pose.y = 0.0
+        new_goal_pose.theta = 0.0
+        # to_ball_angle = tool.get_angle(new_goal_pose, ball_pose)
+        # new_goal_pose.theta = to_ball_angle
+        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

consai2_game/scripts/example/actions/offense.py

@@ -2,6 +2,7 @@
 
 # offense.pyでは、ボールを蹴るactionを定義する
 
+import copy
 import rospy
 import math
 import sys,os
@@ -100,6 +101,7 @@ def _inplay_shoot(my_pose, ball_info, control_target, target_pose,
 
     new_goal_pose = Pose2D()
     if can_look_target is False:
+
         # ドリブラーがボールにつくまで移動する
         rospy.logdebug("inplay_shoot: approach")
 
@@ -155,13 +157,125 @@ def _inplay_shoot(my_pose, ball_info, control_target, target_pose,
     return control_target
 
 
+def _demo_inplay_shoot(my_pose, ball_info, control_target, target_pose,
+        can_shoot_angle = 5.0, shoot_enable=True, dribble_dist=0.01):
+
+    # パラメータ
+    APPROACH = 1
+    ROTATE = 2
+    AIM = 3
+    SHOOT = 4
+
+    # ボールに近づいた時、近づけたかを判定するしきい値。小さいほどきびしい
+    APPROACH_DIST = 0.15  # meters
+    # ボールに近づいた時、ボールを見ているか判定するしきい値。小さいほどきびしい
+    APPROACH_ANGLE = 10.0  # degrees
+    DRIBBLE_POWER = 0.8  # 0.0 ~ 1.0
+    KICK_POWER = 0.6  # 0.0 ~ 1.0
+
+    # 必要な変数の生成
+    ball_pose = ball_info.pose
+    dist_to_ball = tool.distance_2_poses(my_pose, ball_pose) 
+
+    angle_ball_to_robot = tool.get_angle(ball_pose, my_pose)
+    trans_BtoR = tool.Trans(ball_pose, angle_ball_to_robot)
+    look_angle = math.fabs(trans_BtoR.transform_angle(my_pose.theta))
+
+    angle_ball_to_target = tool.get_angle(ball_pose, target_pose)
+    trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)
+    tr_robot_angle_BtoT = trans_BtoT.transform_angle(my_pose.theta)
+
+    state = APPROACH
+
+    # 行動判定
+    if dist_to_ball > APPROACH_DIST \
+        or look_angle < math.radians(180 - APPROACH_ANGLE):
+        # ロボットがボールから離れた or ボールを見つめていなかったら
+        state = APPROACH
+    elif math.fabs(tr_robot_angle_BtoT) > math.radians(can_shoot_angle):
+        # ロボットがボールから少し離れた or ボールを見つめていなかったら
+        # or ロボットがシュートターゲットを見つめていなかったら
+        state = ROTATE
+    else:
+        state = SHOOT
+
+    new_goal_pose = Pose2D()
+    control_target.dribble_power = 0.0
+    control_target.kick_power =  0.0
+    if state == APPROACH:
+        # 今いる位置からボールの裏に回り込む
+        new_goal_pose = _get_behind_ball(
+            my_pose, ball_pose, target_pose)
+
+    elif state == ROTATE or SHOOT:
+        # ボールを中心に旋回し、targetを見る
+        new_goal_pose = _rotate_around_ball(
+            my_pose, ball_pose, target_pose)
+        control_target.dribble_power = DRIBBLE_POWER
+
+        if state == SHOOT and shoot_enable:
+            control_target.kick_power =  KICK_POWER
+
+    # パスを追加
+    control_target.path = []
+    control_target.path.append(new_goal_pose)
+
+    return control_target
+
+def _get_behind_ball(my_pose, ball_pose, target_pose):
+    # ボールの後側へ回り込む
+    control_target_pose = Pose2D()
+
+    angle_ball_to_target = tool.get_angle(ball_pose, target_pose)
+    trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)
+
+    tr_robot_pose_BtoT = trans_BtoT.transform(my_pose)
+
+    # ボールの前方にいる場合は、ボールの斜め後ろに移動する
+    DISTANCE = 0.2
+    if tr_robot_pose_BtoT.x > 0:
+        control_target_pose = trans_BtoT.inverted_transform(
+            Pose2D(-DISTANCE, math.copysign(DISTANCE, tr_robot_pose_BtoT.y), 0))
+        # control_target_pose.theta = angle_ball_to_target
+        control_target_pose.theta = tool.get_angle(my_pose, ball_pose) 
+    else:
+        # ボールを見ながらボールに近づく
+        tr_pose_angle = tool.get_angle_from_center(tr_robot_pose_BtoT)
+        inv_pos_x = -DISTANCE * (1.0 - math.fabs(tr_pose_angle) / math.pi)
+        control_target_pose = trans_BtoT.inverted_transform(
+            Pose2D(inv_pos_x, 0, 0))
+        control_target_pose.theta = tool.get_angle(my_pose, ball_pose) 
+
+    return control_target_pose
+
+def _rotate_around_ball(my_pose, ball_pose, target_pose):
+    # ドリブルしながらボールを中心に旋回し、targetを見る
+    control_target_pose = Pose2D()
+
+    angle_ball_to_robot = tool.get_angle(ball_pose, my_pose)
+    trans_BtoR = tool.Trans(ball_pose, angle_ball_to_robot)
+    angle_ball_to_target = tool.get_angle(ball_pose, target_pose)
+    trans_BtoT = tool.Trans(ball_pose, angle_ball_to_target)
+
+    tr_robot_angle_BtoT = trans_BtoT.transform_angle(my_pose.theta)
+
+    length = trans_BtoR.transform(my_pose).x
+    add_angle = math.copysign(math.radians(45), tr_robot_angle_BtoT) * -1.0
+    tr_goal_pose_BtoR = Pose2D(length*math.cos(add_angle), length*math.sin(add_angle), 0)
+
+    control_target_pose = trans_BtoR.inverted_transform(tr_goal_pose_BtoR)
+    control_target_pose.theta = tool.get_angle(control_target_pose, ball_pose)
+
+    return control_target_pose
+
 def inplay_shoot(my_pose, ball_info, control_target):
     # インプレイ用のシュートアクション
     # デフォルトでゴールを狙う
     SHOOT_TARGET = Field.goal_pose('their', 'center')
     CAN_SHOOT_ANGLE = 5 # degrees
 
-    return _inplay_shoot(my_pose, ball_info, control_target, SHOOT_TARGET, CAN_SHOOT_ANGLE)
+    # return _inplay_shoot(my_pose, ball_info, control_target, SHOOT_TARGET, CAN_SHOOT_ANGLE)
+    return _demo_inplay_shoot(my_pose, ball_info, control_target, SHOOT_TARGET, CAN_SHOOT_ANGLE)
 
 def inplay_shoot_to_target(my_pose, ball_info, control_target, shoot_target, can_shoot_angle=5):
     # インプレイ用のtargeを狙うシュートアクション
@@ -206,8 +320,10 @@ def _setplay_shoot(my_pose, ball_info, control_target, kick_enable, target_pose,
     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:
+    if tr_my_pose.x < 0.01 and math.fabs(tr_my_pose.y) < 0.05 \
+        and math.fabs(trans.transform_angle(my_pose.theta)) < math.radians(5):
         can_kick = True
 
     # レシーバにパスする場合、蹴る位置近くにロボットが存在すれば receive_arrive is True
@@ -232,14 +348,15 @@ def _setplay_shoot(my_pose, ball_info, control_target, kick_enable, target_pose,
         new_goal_pose.theta = angle_ball_to_target
         # ドリブルとキックをオン
         control_target.kick_power = KICK_POWER
+        control_target.dribble_power = 0.8
     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.dribble_power = 0.8
 
     # パスを追加
     control_target.path = []
@@ -321,6 +438,11 @@ def setplay_pass(my_pose, ball_info, control_target, target_pose, receive_enable
     kick_enable = True
     kick_power = 0.3
 
+    # 【デモで追加】ゴーリーに擬似的にパスする
+    pass_target = copy.deepcopy(Field.goal_pose('our', 'center'))
+    pass_target.x += 0.1
+    return _setplay_shoot(my_pose, ball_info, control_target, kick_enable, pass_target, kick_power)
+
     # ダイレクトかつ、直接シュートが無理の無い位置だった場合は直シュート
     if direct and ball_info.pose.x < Field.penalty_pose('their','upper_front').x:
         return _setplay_shoot(my_pose, ball_info, control_target, kick_enable, Field.goal_pose('their','center'), kick_power)

consai2_game/scripts/example/actions/tool.py

@@ -89,19 +89,27 @@ def is_in_defense_area(pose, team='our'):
 
     if team == 'our':
         # 自チームディフェンスエリアに入っているか
-        if pose.x < PENALTY_UPPER_FRONT.x \
-                and pose.y < PENALTY_UPPER_FRONT.y \
-                and pose.y > PENALTY_LOWER_FRONT.y:
+        if pose.x < PENALTY_UPPER_FRONT.x + 0.1:
+                # and pose.y < PENALTY_UPPER_FRONT.y \
+                # and pose.y > PENALTY_LOWER_FRONT.y:
             pose_is_in_area = True
     else:
         # 相手チームディフェンスエリアに入っているか
-        if pose.x > PENALTY_UPPER_FRONT.x \
-                and pose.y < PENALTY_UPPER_FRONT.y \
-                and pose.y > PENALTY_LOWER_FRONT.y:
+        if pose.x > PENALTY_UPPER_FRONT.x :
+                # and pose.y < PENALTY_UPPER_FRONT.y \
+                # and pose.y > PENALTY_LOWER_FRONT.y:
             pose_is_in_area = True
 
     return pose_is_in_area
 
+def is_in_field(pose):
+    # フィールドに入ってるか判定する
+    if math.fabs(pose.x) < Field.field('half_length') \
+        and math.fabs(pose.y) < Field.field('half_width'):
+        return True
+    else:
+        return False
+
 def get_size_from_center(pose):
     return math.hypot(pose.x, pose.y)
 

consai2_game/scripts/example/avoidance.py

@@ -26,8 +26,8 @@ def __init__(self):
         self._range_tr_x = 0.2
 
         # 経路生成時の相対的に移動する位置(ロボットから見た座標系)
-        self._tr_move_x = 0.2
-        self._tr_move_y = 0.5
+        self._tr_move_x = -0.1
+        self._tr_move_y = 0.4
 
     # フィールドのボールとロボットの情報を更新
     def update_obstacles(self, ball_info, robot_info):