From 295c768d1eeba3f8bcf37c089c4afe3cbd7af287 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Wed, 30 Sep 2020 22:52:00 +0900 Subject: [PATCH 01/16] =?UTF-8?q?=E3=82=B4=E3=83=BC=E3=83=AA=E3=81=AF?= =?UTF-8?q?=E4=B8=8D=E8=A6=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_description/param/game.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/consai2_description/param/game.yaml b/consai2_description/param/game.yaml index d7ac348..8e52a43 100644 --- a/consai2_description/param/game.yaml +++ b/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 : 10 # 自チームのゴールキーパの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 # 実機送信機のボーレート From 8c1462faa94936d7c4162594538936195516c7dc Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Wed, 30 Sep 2020 22:55:51 +0900 Subject: [PATCH 02/16] =?UTF-8?q?STOP=E3=81=A7=E3=82=A2=E3=82=BF=E3=83=83?= =?UTF-8?q?=E3=82=AB=E3=83=BC=E3=81=AF=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=AB?= =?UTF-8?q?=E3=82=82=E3=81=86=E5=B0=91=E3=81=97=E8=BF=91=E3=81=A5=E3=81=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/game.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 422b7af..15ba7b7 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -106,7 +106,7 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de avoid_obstacle = False # 障害物回避しない elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: self._control_target = offense.interpose(ball_info, - self._control_target, dist_from_target=0.7) + self._control_target, dist_from_target=0.3) avoid_ball = True else: self._control_target = assign.assign( From 01c09cee7da6270d1039746175b00640555af5c9 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 19:59:43 +0900 Subject: [PATCH 03/16] =?UTF-8?q?=E3=81=A7=E3=82=82=E3=82=A2=E3=82=BF?= =?UTF-8?q?=E3=83=83=E3=82=AB=E3=83=BC=E3=81=AE=E3=82=B3=E3=83=BC=E3=83=89?= =?UTF-8?q?=E3=82=92inplay=5Fshoot=E3=81=AB=E7=A7=BB=E6=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/example/actions/offense.py | 115 +++++++++++++++++- 1 file changed, 114 insertions(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index 0afd2a3..690a172 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -100,6 +100,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 +156,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 = 15.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(60), 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を狙うシュートアクション From eda11b91027c0026ebf57bbff9efc732b2694f36 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 20:22:28 +0900 Subject: [PATCH 04/16] =?UTF-8?q?=E3=82=B4=E3=83=BC=E3=83=AA=E3=83=BC&?= =?UTF-8?q?=E3=82=A2=E3=82=BF=E3=83=83=E3=82=AB=E3=83=BC=E3=81=A7=E8=A9=A6?= =?UTF-8?q?=E5=90=88=E3=81=99=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E5=A4=89?= =?UTF-8?q?=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_description/param/game.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/consai2_description/param/game.yaml b/consai2_description/param/game.yaml index 8e52a43..08bb53b 100644 --- a/consai2_description/param/game.yaml +++ b/consai2_description/param/game.yaml @@ -2,7 +2,7 @@ max_id : 15 # ゲームに使用できる最大ID player_num : 8 # フィールドに同時に出せるロボット数 our_side : 'left' # フィールドの自チームディフェンス側. left or right our_color : 'blue' # 自チームのマーカ色. blue or yellow -goalie_id : 10 # 自チームのゴールキーパのID +goalie_id : 0 # 自チームのゴールキーパのID vision_addr : '224.5.23.2' # Visionのマルチキャストアドレス vision_port : 10006 # Visionのポート番号 From e6e15eed6ca7d611fa727463d23924853da2e623 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 20:26:30 +0900 Subject: [PATCH 05/16] =?UTF-8?q?=E3=82=B4=E3=83=BC=E3=83=AA=E3=83=BCID?= =?UTF-8?q?=E3=82=921=E3=81=AB=E5=A4=89=E6=9B=B4=E3=80=820=E3=81=AB?= =?UTF-8?q?=E3=81=99=E3=82=8B=E3=81=A8=E5=8B=95=E3=81=8B=E3=81=AA=E3=81=8F?= =?UTF-8?q?=E3=81=AA=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_description/param/game.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/consai2_description/param/game.yaml b/consai2_description/param/game.yaml index 08bb53b..dddd924 100644 --- a/consai2_description/param/game.yaml +++ b/consai2_description/param/game.yaml @@ -2,7 +2,7 @@ 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のポート番号 From 66728790047e2c95010a9154a1ed771e3e4fe0cf Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 20:26:59 +0900 Subject: [PATCH 06/16] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=8C?= =?UTF-8?q?=E3=83=87=E3=82=A3=E3=83=95=E3=82=A7=E3=83=B3=E3=82=B9=E3=82=A8?= =?UTF-8?q?=E3=83=AA=E3=82=A2=E3=81=AB=E5=85=A5=E3=81=A3=E3=81=A6=E3=82=8B?= =?UTF-8?q?=E5=88=A4=E5=AE=9A=E3=82=92=E3=82=86=E3=82=8B=E3=81=8F=E3=81=97?= =?UTF-8?q?=E3=81=9F=E3=80=82=E5=AE=89=E5=85=A8=E3=81=AE=E3=81=9F=E3=82=81?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/tool.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/consai2_game/scripts/example/actions/tool.py b/consai2_game/scripts/example/actions/tool.py index 2b7442a..ae770ce 100644 --- a/consai2_game/scripts/example/actions/tool.py +++ b/consai2_game/scripts/example/actions/tool.py @@ -89,15 +89,15 @@ 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 From 7ed632ef857bda31defadbc2461b5e2db2ea0f40 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 20:36:34 +0900 Subject: [PATCH 07/16] =?UTF-8?q?=E3=83=87=E3=82=A3=E3=83=95=E3=82=A7?= =?UTF-8?q?=E3=83=B3=E3=82=B9=E3=82=A8=E3=83=AA=E3=82=A2=E3=81=AB=E3=83=9C?= =?UTF-8?q?=E3=83=BC=E3=83=AB=E3=81=8C=E3=81=82=E3=82=8B=E3=81=A8=E3=81=8D?= =?UTF-8?q?=E3=80=81=E3=82=B4=E3=83=BC=E3=83=AA=E3=81=AF=E3=83=9C=E3=83=BC?= =?UTF-8?q?=E3=83=AB=E3=82=92=E3=81=91=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/game.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 15ba7b7..8d2f443 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -71,8 +71,11 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: if tool.is_in_defense_area(ball_info.pose, 'our'): - self._control_target = offense.outside_shoot( + # 【デモ用に追加】ゴーリーもとにかくボールをける + self._control_target = offense.inplay_shoot( self._my_pose, ball_info, self._control_target) + # self._control_target = offense.outside_shoot( + # self._my_pose, ball_info, self._control_target) else: self._control_target = goalie.interpose( ball_info, robot_info, self._control_target) From c249700cf990cd881dfd9f21c296e31263f92ea1 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 20:52:32 +0900 Subject: [PATCH 08/16] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=8C?= =?UTF-8?q?=E3=83=95=E3=82=A3=E3=83=BC=E3=83=AB=E3=83=89=E5=A4=96=E3=81=AB?= =?UTF-8?q?=E3=81=82=E3=82=8B=E3=81=A8=E3=81=8D=E3=81=AFHALT=E3=81=AB?= =?UTF-8?q?=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/tool.py | 8 ++++++++ consai2_game/scripts/example/game.py | 4 +++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/actions/tool.py b/consai2_game/scripts/example/actions/tool.py index ae770ce..a1b26a1 100644 --- a/consai2_game/scripts/example/actions/tool.py +++ b/consai2_game/scripts/example/actions/tool.py @@ -102,6 +102,14 @@ def is_in_defense_area(pose, team='our'): 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) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 8d2f443..e9e44d3 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -59,8 +59,10 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de self._control_target.dribble_power = 0.0 self._control_target.kick_power = 0.0 - if referee.can_move_robot is False or ball_info.disappeared: + if referee.can_move_robot is False or ball_info.disappeared \ + or tool.is_in_field(ball_info.pose) is False: # 移動禁止 or ボールの消失で制御を停止する + # ボールがフィールド外に出ても停止する rospy.logdebug("HALT") self._control_target, remake_path= normal.stop(self._control_target) avoid_obstacle = False # 障害物回避しない From 5952f36fedcaa6b0983e1eec8ed2b1ed91e4f6e4 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 21:58:20 +0900 Subject: [PATCH 09/16] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=83=97?= =?UTF-8?q?=E3=83=AC=E3=83=BC=E3=82=B9=E3=83=A1=E3=83=B3=E3=83=88=E3=82=92?= =?UTF-8?q?=E5=AE=89=E5=85=A8=E3=81=AB=E5=AE=9F=E8=A1=8C=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/example/actions/ball_placement.py | 6 ++++-- consai2_game/scripts/example/game.py | 16 ++++++++++++---- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/consai2_game/scripts/example/actions/ball_placement.py b/consai2_game/scripts/example/actions/ball_placement.py index aba5983..4cc64d2 100644 --- a/consai2_game/scripts/example/actions/ball_placement.py +++ b/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: diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index e9e44d3..62b824d 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -227,8 +227,12 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de rospy.logdebug("OUR_BALL_PLACEMENT") replace_pose = referee.placement_position if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: - self._control_target = goalie.interpose( - ball_info, robot_info, self._control_target) + # 【デモで追加】ゴーリーを暴れさせない + self._control_target = normal.move_to( + self._control_target, + Field.goal_pose('our', 'center'), ball_info, look_ball=True) + # self._control_target = goalie.interpose( + # ball_info, robot_info, self._control_target) avoid_obstacle = False # 障害物回避しない elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: self._control_target, avoid_ball = ball_placement.basic_ball_placement(self._control_target, replace_pose, ball_info, robot_info, self._MY_ID, 'atk', [Field.field('length'), Field.field('width')]) @@ -316,8 +320,12 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de rospy.logdebug("THEIR_BALL_PLACEMENT") replace_pose = referee.placement_position if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: - self._control_target = goalie.interpose( - ball_info, robot_info, self._control_target) + # 【デモで追加】ゴーリーを暴れさせない + self._control_target = normal.move_to( + self._control_target, + Field.goal_pose('our', 'center'), ball_info, look_ball=True) + # self._control_target = goalie.interpose( + # ball_info, robot_info, self._control_target) avoid_obstacle = False # 障害物回避しない # elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: # self._control_target = offense.interpose(ball_info, From fcd0f39857637649d717576fb8a8eed535c8c6ee Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Thu, 1 Oct 2020 22:24:43 +0900 Subject: [PATCH 10/16] =?UTF-8?q?=E3=82=BB=E3=83=83=E3=83=88=E3=83=97?= =?UTF-8?q?=E3=83=AC=E3=82=A4=E3=81=A7=E3=82=B4=E3=83=BC=E3=83=AA=E3=83=BC?= =?UTF-8?q?=E3=81=AB=E3=83=91=E3=82=B9=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/offense.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index 690a172..d2e524c 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -2,6 +2,7 @@ # offense.pyでは、ボールを蹴るactionを定義する +import copy import rospy import math import sys,os @@ -319,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(10): can_kick = True # レシーバにパスする場合、蹴る位置近くにロボットが存在すれば receive_arrive is True @@ -434,6 +437,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) From c89ef0763c7b5fb8a1af4c16a8a30b366d1f3019 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 13:42:03 +0900 Subject: [PATCH 11/16] =?UTF-8?q?=E3=82=A2=E3=82=BF=E3=83=83=E3=82=AB?= =?UTF-8?q?=E3=83=BC=E3=81=AE=E3=83=91=E3=83=A9=E3=83=A1=E3=83=BC=E3=82=BF?= =?UTF-8?q?=E3=82=92=E8=89=B2=E3=80=85=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/offense.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index d2e524c..ca68524 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -169,7 +169,7 @@ def _demo_inplay_shoot(my_pose, ball_info, control_target, target_pose, # ボールに近づいた時、近づけたかを判定するしきい値。小さいほどきびしい APPROACH_DIST = 0.15 # meters # ボールに近づいた時、ボールを見ているか判定するしきい値。小さいほどきびしい - APPROACH_ANGLE = 15.0 # degrees + APPROACH_ANGLE = 10.0 # degrees DRIBBLE_POWER = 0.8 # 0.0 ~ 1.0 KICK_POWER = 0.6 # 0.0 ~ 1.0 @@ -260,7 +260,7 @@ def _rotate_around_ball(my_pose, ball_pose, target_pose): tr_robot_angle_BtoT = trans_BtoT.transform_angle(my_pose.theta) length = trans_BtoR.transform(my_pose).x - add_angle = math.copysign(math.radians(60), tr_robot_angle_BtoT) * -1.0 + 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) @@ -435,7 +435,7 @@ def setplay_shoot(my_pose, ball_info, control_target, kick_enable=False, penalty def setplay_pass(my_pose, ball_info, control_target, target_pose, receive_enable=False, receiver_role_exist=None, robot_info=None, direct=False): kick_enable = True - kick_power = 0.3 + kick_power = 0.2 # 【デモで追加】ゴーリーに擬似的にパスする pass_target = copy.deepcopy(Field.goal_pose('our', 'center')) From 3a81a16574e22f40149eff43f1a94225b1789ac1 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 15:21:58 +0900 Subject: [PATCH 12/16] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=81=AE=E3=82=B7?= =?UTF-8?q?=E3=83=A5=E3=83=BC=E3=83=88=E7=8B=99=E3=81=86=E8=A7=92=E5=BA=A6?= =?UTF-8?q?=E3=82=92=E5=8E=B3=E3=81=97=E3=81=8F=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/offense.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index ca68524..bca8950 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -323,7 +323,7 @@ def _setplay_shoot(my_pose, ball_info, control_target, kick_enable, target_pose, # ロボットの方向がゴールを狙っていることも判定 can_kick = False 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(10): + and math.fabs(trans.transform_angle(my_pose.theta)) < math.radians(5): can_kick = True # レシーバにパスする場合、蹴る位置近くにロボットが存在すれば receive_arrive is True From aab75ab1d1829e5fa53ede58b872e0d09411af13 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 16:50:33 +0900 Subject: [PATCH 13/16] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=AD=E3=83=83?= =?UTF-8?q?=E3=82=AF=E3=83=91=E3=83=AF=E3=83=BC=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/offense.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index bca8950..173f084 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -435,7 +435,7 @@ def setplay_shoot(my_pose, ball_info, control_target, kick_enable=False, penalty def setplay_pass(my_pose, ball_info, control_target, target_pose, receive_enable=False, receiver_role_exist=None, robot_info=None, direct=False): kick_enable = True - kick_power = 0.2 + kick_power = 0.15 # 【デモで追加】ゴーリーに擬似的にパスする pass_target = copy.deepcopy(Field.goal_pose('our', 'center')) From 3b22ac8f1baeed5268eaba5e7c47a6ab9d72b35f Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 22:57:47 +0900 Subject: [PATCH 14/16] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=B7=E3=83=A5?= =?UTF-8?q?=E3=83=BC=E3=83=88=E7=94=A8=E3=81=AE=E9=96=A2=E6=95=B0=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/example/actions/goalie.py | 50 +++++++++++++++++++ .../scripts/example/actions/offense.py | 5 +- consai2_game/scripts/example/game.py | 2 + 3 files changed, 55 insertions(+), 2 deletions(-) diff --git a/consai2_game/scripts/example/actions/goalie.py b/consai2_game/scripts/example/actions/goalie.py index e545627..1f62989 100644 --- a/consai2_game/scripts/example/actions/goalie.py +++ b/consai2_game/scripts/example/actions/goalie.py @@ -2,6 +2,7 @@ # defense.pyでは、ボールを蹴らないactionを定義する +import copy import rospy import math import sys,os @@ -131,3 +132,52 @@ def interpose(ball_info, robot_info, control_target): return control_target + +def pass_shoot(ball_info, robot_info, control_target): + + # ゴールライン上ではなく一定距離[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 + else: + 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.5 + control_target.dribble_power = 0.5 + control_target.path = [] + control_target.path.append(new_goal_pose) + + return control_target \ No newline at end of file diff --git a/consai2_game/scripts/example/actions/offense.py b/consai2_game/scripts/example/actions/offense.py index 173f084..b6d0bb2 100644 --- a/consai2_game/scripts/example/actions/offense.py +++ b/consai2_game/scripts/example/actions/offense.py @@ -348,6 +348,7 @@ 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)) @@ -355,7 +356,7 @@ 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 = 0.0 - control_target.dribble_power = 0.0 + control_target.dribble_power = 0.8 # パスを追加 control_target.path = [] @@ -435,7 +436,7 @@ def setplay_shoot(my_pose, ball_info, control_target, kick_enable=False, penalty def setplay_pass(my_pose, ball_info, control_target, target_pose, receive_enable=False, receiver_role_exist=None, robot_info=None, direct=False): kick_enable = True - kick_power = 0.15 + kick_power = 0.3 # 【デモで追加】ゴーリーに擬似的にパスする pass_target = copy.deepcopy(Field.goal_pose('our', 'center')) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 62b824d..2c64318 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -203,6 +203,8 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: self._control_target = goalie.interpose( ball_info, robot_info, self._control_target) + self._control_target = goalie.pass_shoot( + ball_info, robot_info, self._control_target) avoid_obstacle = False # 障害物回避しない elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: self._control_target, avoid_ball = offense.setplay_pass( From de349d3bb9e51e156d42e7335fa0e81c404656f1 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 23:08:10 +0900 Subject: [PATCH 15/16] =?UTF-8?q?=E8=A1=9D=E7=AA=81=E5=9B=9E=E9=81=BF?= =?UTF-8?q?=E3=82=92=E3=82=86=E3=81=A3=E3=81=9F=E3=82=8A=E3=81=AB=E3=81=97?= =?UTF-8?q?=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/avoidance.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/consai2_game/scripts/example/avoidance.py b/consai2_game/scripts/example/avoidance.py index d6d91d8..5fe3aee 100644 --- a/consai2_game/scripts/example/avoidance.py +++ b/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): From a796cd6ebdde77ff88a0337ae44749f4a19a57d0 Mon Sep 17 00:00:00 2001 From: ShotaAk Date: Sat, 3 Oct 2020 23:28:43 +0900 Subject: [PATCH 16/16] =?UTF-8?q?pass=5Fshoot=E9=96=A2=E6=95=B0=E3=82=92de?= =?UTF-8?q?mo=5Fshoot=E9=96=A2=E6=95=B0=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai2_game/scripts/example/actions/goalie.py | 14 +++++++++++--- consai2_game/scripts/example/game.py | 16 ++++++++-------- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/consai2_game/scripts/example/actions/goalie.py b/consai2_game/scripts/example/actions/goalie.py index 1f62989..3f2e8a3 100644 --- a/consai2_game/scripts/example/actions/goalie.py +++ b/consai2_game/scripts/example/actions/goalie.py @@ -11,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 @@ -133,7 +134,7 @@ def interpose(ball_info, robot_info, control_target): return control_target -def pass_shoot(ball_info, robot_info, control_target): +def demo_shoot(ball_info, robot_info, control_target, my_pose = None, inplay_shoot = False): # ゴールライン上ではなく一定距離[m]前を守るための変数 MARGIN_DIST_X = 0.1 @@ -168,15 +169,22 @@ def pass_shoot(ball_info, robot_info, control_target): 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.kick_power = 0.5 - control_target.dribble_power = 0.5 control_target.path = [] control_target.path.append(new_goal_pose) diff --git a/consai2_game/scripts/example/game.py b/consai2_game/scripts/example/game.py index 2c64318..d37b860 100755 --- a/consai2_game/scripts/example/game.py +++ b/consai2_game/scripts/example/game.py @@ -74,12 +74,11 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: if tool.is_in_defense_area(ball_info.pose, 'our'): # 【デモ用に追加】ゴーリーもとにかくボールをける - self._control_target = offense.inplay_shoot( - self._my_pose, ball_info, self._control_target) - # self._control_target = offense.outside_shoot( - # self._my_pose, ball_info, self._control_target) + self._control_target = goalie.demo_shoot( + ball_info, robot_info, self._control_target, + self._my_pose, True) else: - self._control_target = goalie.interpose( + self._control_target = goalie.demo_shoot( ball_info, robot_info, self._control_target) avoid_obstacle = False # 障害物回避しない elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: @@ -201,9 +200,10 @@ def get_action(self, referee, obstacle_avoidance, ball_info, robot_info=None, de rospy.logdebug("OUR_INDIRECT_FREE") if self._my_role == role.ROLE_ID["ROLE_GOALIE"]: - self._control_target = goalie.interpose( - ball_info, robot_info, self._control_target) - self._control_target = goalie.pass_shoot( + # self._control_target = goalie.interpose( + # ball_info, robot_info, self._control_target) + # デモで追加、パスシュート + self._control_target = goalie.demo_shoot( ball_info, robot_info, self._control_target) avoid_obstacle = False # 障害物回避しない elif self._my_role == role.ROLE_ID["ROLE_ATTACKER"]: