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 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 get_near_robot_id(target_pose, robot_info): dist = [] for our_info in robot_info['our']: if our_info.disappeared is False: # 各ロボットと指定位置の距離 dist.append(tool.distance_2_poses(our_info.pose, target_pose)) else: # インデックスを揃えるためダミーデータを挿入 dist.append(100) # 一番距離が近いロボットのID robot_id = dist.index(min(dist)) return robot_id
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 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 _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 _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 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
def interpose(ball_info, robot_info, control_target): # ボールの位置 ball_pose = ball_info.pose # ボールの速度 ball_vel = ball_info.velocity # ゴールの位置 OUR_GOAL_POSE = Field.goal_pose('our', 'center') xr = OUR_GOAL_POSE.x + 0.3 goalie_threshold_y = 1.5 # 敵のロボットの情報 robot_info_their = robot_info['their'] dist = [] for their_info in robot_info_their: if their_info.detected: dist.append(tool.distance_2_poses(their_info.pose, ball_pose)) else: dist.append(100) # 一番近い敵ロボットのID min_dist_id = dist.index(min(dist)) # 一番近い敵ロボットの座標 robot_pose = robot_info_their[min_dist_id].pose # ボールの速度 if ball_vel is None: v = 0 dx = 0 dy = 0 da = 0 else: # ボールの速度 vx = ball_vel.x vy = ball_vel.y v = math.hypot(ball_vel.x, ball_vel.y) # ボールの進む角度 angle_ball = math.atan2(vy, vx) # ボールの変化量を計算(正確ではなく方向を考慮した単位量) dvx = math.cos(angle_ball) dvy = math.sin(angle_ball) # ボールの次の予測位置を取得 ball_pose_next = Pose2D(ball_pose.x + dvx, ball_pose.y + dvy, 0) if dist[min_dist_id] < 0.15 and robot_pose.x < 0: rospy.logdebug('their') angle_their = robot_pose.theta if angle_their == 0: a = 1e-10 else: a = math.tan(angle_their) b = robot_pose.y - a * robot_pose.x elif 0.02 < v and dvx < 0: rospy.logdebug('ball move') a, b = line_pram(ball_pose, ball_pose_next) else: rospy.logdebug('ball stop') a, b = line_pram(ball_pose, OUR_GOAL_POSE) # 位置を決める yr = a*xr + b # 位置 if yr > goalie_threshold_y: yr = goalie_threshold_y elif yr < -goalie_threshold_y: yr = -goalie_threshold_y # elif ball_pose.x < xr: # ボールが後ろに出たらy座標は0にする # yr = 0 new_goal_pose = Pose2D(xr, yr, 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) # print(goalie_pose.x, goalie_pose.y, ball_pose.x, ball_pose.y) return control_target
def interpose(ball_info, robot_info, control_target): # ボールが動いていると判断するしきい値[m/s] MOVE_BALL_VELOCITY_THRESHOLD = 0.5 # ロボットがボールを持っていると判断するしきい値[m] DIST_ROBOT_TO_BALL_THRESHOLD = 0.2 # ゴールライン上ではなく一定距離[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 # 敵のロボットの情報 robot_info_their = robot_info['their'] # 敵ロボットとボールの距離 dist = [] for their_info in robot_info_their: if their_info.disappeared is False: dist.append(tool.distance_2_poses(their_info.pose, ball_pose)) else: dist.append(100) # 一番近い敵ロボットのID min_dist_id = dist.index(min(dist)) # 一番近い敵ロボットの座標 robot_pose = robot_info_their[min_dist_id].pose # ボールの速度 ball_velocity_x = ball_info.velocity.x ball_velocity_y = ball_info.velocity.y ball_velocity = math.hypot(ball_velocity_x, ball_velocity_y) # ボールの進む角度 angle_ball = math.atan2(ball_velocity_y, ball_velocity_x) # ボールの進む変化量を計算(方向を考慮した単位量) var_ball_velocity_x = math.cos(angle_ball) var_ball_velocity_y = math.sin(angle_ball) # ボールの次の予測位置を取得 ball_pose_next = Pose2D(ball_pose.x + var_ball_velocity_x, ball_pose.y + var_ball_velocity_y, 0) # 敵ロボットとボールの距離が近い場合は敵の向いている直線を使う if dist[min_dist_id] < DIST_ROBOT_TO_BALL_THRESHOLD and robot_pose.x < 0: slope = math.tan(robot_pose.theta) intercept = robot_pose.y - slope * robot_pose.x # ボールの速度がある場合かつ近づいてくる場合はボールの向かう直線を使う elif MOVE_BALL_VELOCITY_THRESHOLD < ball_velocity and ball_velocity_x < 0: slope, intercept = _get_line_parameters(ball_pose, ball_pose_next) # その他はゴール中心とボールを結ぶ直線を使う else: slope, intercept = _get_line_parameters(ball_pose, OUR_GOAL_POSE) # ゴーリの新しい座標 goalie_pose_x = OUR_GOAL_POSE.x + MARGIN_DIST_X goalie_pose_y = slope * goalie_pose_x + intercept # ゴールから飛び出さないようる上下限を設ける if OUR_GOAL_UPPER.y < goalie_pose_y: goalie_pose_y = OUR_GOAL_UPPER.y elif goalie_pose_y < OUR_GOAL_LOWER.y: goalie_pose_y = OUR_GOAL_LOWER.y # ゴーリの新しい座標 new_goalie_pose = Pose2D(goalie_pose_x, goalie_pose_y, 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_goalie_pose = control_target.path[-1] if not tool.is_close(current_goalie_pose, new_goalie_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_goalie_pose) return control_target
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 _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)