def _penalty_shoot(my_pose, ball_info, control_target, kick_enable, target_pose, kick_power=1.0): # PK用のシュートアクション # kick_enable is Falseで、ボールの近くまで移動する # kick_enable is True で、シュートする KICK_POWER = kick_power SHOOT_TARGET = target_pose DRIBBLE_POWER = 0.5 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) random_num = Observer.random_zero_one() if random_num == 0: target_goal_side = Field.goal_pose('their', 'upper') else: target_goal_side = Field.goal_pose('their', 'lower') angle_to_target_side = tool.get_angle(my_pose, target_goal_side) # ロボットがボールの裏側に回ったらcan_kick is True can_kick = False if tr_my_pose.x > -0.2 and math.fabs(tr_my_pose.y) < 0.05: can_kick = True avoid_ball = True # ボールを回避する new_goal_pose = Pose2D() if can_kick and kick_enable: # ボールをける avoid_ball = False # ボールの前方に移動する new_position = trans.inverted_transform(Pose2D(0, 0, 0)) new_goal_pose = new_position new_goal_pose.theta = angle_to_target_side # ドリブルとキックをオン control_target.dribble_power = DRIBBLE_POWER control_target.kick_power = KICK_POWER else: Observer.update_random_zero_one() # ボールの裏に移動する new_position = trans.inverted_transform(Pose2D(-0.1, 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_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 interpose(target_info, control_target, dist_from_goal=None, dist_from_target=None): # 自チームのゴール中心とtarget_info.poseを直線で結び、その直線上に移動する # dist_from_goal is not Noneなら、ゴール中心からdist分離れた位置に移動する # dist_from_target is not Noneなら、target_info.poseからdist分離れた位置に移動する # 到達姿勢の計算とcontrol_targetの更新(path以外) control_target.kick_power = 0.0 control_target.dribble_power = 0.0 # 両方設定されてなかったらdist_from_targetを優先する if dist_from_goal is None and dist_from_target is None: dist_from_target = 0.6 # 適当な値 OUR_GOAL_POSE = Field.goal_pose('our', 'center') angle_to_target = tool.get_angle(OUR_GOAL_POSE, target_info.pose) new_goal_pose = Pose2D() if dist_from_goal is not None: trans = tool.Trans(GOAL_POSE, angle_to_target) tr_goal_pose = Pose2D(dist_from_goal, 0, 0) new_goal_pose = trans.inverted_transform(tr_goal_pose) else: angle_to_goal = tool.get_angle(target_info.pose, OUR_GOAL_POSE) trans = tool.Trans(target_info.pose, angle_to_goal) tr_goal_pose = Pose2D(dist_from_target, 0, 0) new_goal_pose = trans.inverted_transform(tr_goal_pose) new_goal_pose.theta = angle_to_target # --------------------------------------------------------- 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 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 outside_shoot(my_pose, ball_info, control_target): # ゴールに入らないように外側にボールをけるアクション OUR_GOAL = Field.goal_pose('our', 'center') TARGET_LENGTH = 4.0 CAN_SHOOT_ANGLE = 10 # degrees shoot_target = Pose2D() angle_goal_to_ball = tool.get_angle(OUR_GOAL, ball_info.pose) trans = tool.Trans(ball_info.pose, angle_goal_to_ball) shoot_target = trans.inverted_transform(Pose2D(TARGET_LENGTH, 0, 0)) return _inplay_shoot(my_pose, ball_info, control_target, shoot_target, CAN_SHOOT_ANGLE)
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 update_receive_ball(ball_info, my_pose, zone_id): # ボール位置 ball_pose = ball_info.pose # ボールスピード ball_vel = ball_info.velocity # 受け取れると判断する距離 _can_receive_dist = 1.0 # ヒステリシス _can_receive_hysteresis = 0.3 result = False target_pose = Pose2D() # ボールが動いている if Observer.ball_is_moving(): # ボール速度ベクトルの角度 angle_velocity = tool.get_angle_from_center(ball_vel) trans = tool.Trans(ball_pose, angle_velocity) tr_pose = trans.transform(my_pose) # ボール速度の線と垂直な距離 fabs_y = math.fabs(tr_pose.y) # 受け取れる判定 if Receiving.receiving(zone_id) == False and \ fabs_y < _can_receive_dist - _can_receive_hysteresis: Receiving.update_receiving(zone_id, True) # 受け取れない判定 elif Receiving.receiving(zone_id) == True and \ fabs_y > _can_receive_dist + _can_receive_hysteresis: Receiving.update_receiving(zone_id, False) # 受け取れるかつボールが向かう方向にいる if Receiving.receiving(zone_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) target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball) result = True return result, 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 receive_ball(ball_info, my_pose): ball_pose = ball_info.pose _can_receive_dist = 1.0 _can_receive_hysteresis = 0.3 target_pose = Pose2D() if Observer.ball_is_moving(): angle_velocity = tool.get_angle_from_center(ball_info.velocity) trans = tool.Trans(ball_pose, angle_velocity) tr_pose = trans.transform(my_pose) fabs_y = math.fabs(tr_pose.y) tr_pose.y = 0.0 inv_pose = trans.inverted_transform(tr_pose) angle_to_ball = tool.get_angle(inv_pose, ball_pose) target_pose = Pose2D(inv_pose.x, inv_pose.y, angle_to_ball) return target_pose
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 _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)
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 _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 defense_zone(my_pose, ball_info, control_target, my_role, defense_num, their_robot_info, zone_enable): # ゴールディフェンスに割り当てる台数 GOAL_DEFENSE_NUM = 2 # 現在のディフェンス数 - ゴールディフェンス数 = ゾーンディフェンスに割り当てられる台数 ZONE_DEFENSE_NUM = defense_num - GOAL_DEFENSE_NUM # ゾーンディフェンスが始まるROLE_ID ZONE_START_ROLE_ID = role.ROLE_ID["ROLE_ZONE_1"] # ゾーンオフェンス用の待機場所 ZONE_OFFENCE_POSE = Pose2D(Field.field('length') * 0.25, 0, 0) # センターライン用のマージン MARGIN_CENTER = 0.6 # ちょっと前進用のマージン MARGIN_LITTLE_FORWARD = 1.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) # ゾーンディフェンス用のID zone_id = None # 移動目標点の初期化 target_pose = Pose2D() # --------------------------------------------------------- # キックとドリブルはOFF 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: 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)) ] # ゾーンディフェンスの数でフィールド幅を等分した時の # それぞれのゾーンの中心の配列を作る split_field_center = [i * step - half_field_width for i in range(0,(ZONE_DEFENSE_NUM * 2)) \ if i % 2 != 0] # 参照エラー対策のtry try: # ゾーンIDの計算 # ロボットが8台生きていてゾーンオフェンスがいなければ # ゾーンIDは0~3 zone_id = my_role - ZONE_START_ROLE_ID # ゾーンの中心を目標位置とする target_pose.y = split_field_center[zone_id] # 自分のゾーンに入っている敵チェック # their_robot_infoのposeを抜き出してそれぞれチェック # 敵が自陣側にいる、かつ、自分のゾーンの幅の中にいる、を確認 # 当てはまらない場合配列は空っぽ invader_pose = [i.pose for i in their_robot_info \ 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 \ 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 != []: # 敵とボールの間に割り込む angle_to_ball_from_invader = tool.get_angle( invader_pose[0], ball_pose) trans = tool.Trans(invader_pose[0], angle_to_ball_from_invader) target_pose = trans.inverted_transform(Pose2D(0.5, 0, 0)) else: # ボールが敵陣の時はディフェンスちょっと前進 if ball_pose.x > MARGIN_CENTER: target_pose.x = half_our_field_length + MARGIN_LITTLE_FORWARD else: target_pose.x = half_our_field_length except IndexError: target_pose = my_pose target_pose.theta = angle_to_ball # ボールが来てたらボールを受け取る if zone_id != None: receive_ball_result, receive_target_pose = defense.update_receive_ball( ball_info, my_pose, zone_id) 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
# 车辆启动 car.lib.send_cmd(base_speed, 1500) # 主程序 while True: # 模型预测 frame = car.read_frame() img = preprocess_det(frame, (128, 128)) result_det = ssd_lite.predict(img) img = preprocess_car_line(frame, (128, 128)) result_car_line = car_line.predict(img) # 预测结果后处理 angle = get_angle(result_car_line) results_list = draw_bbox(result_det, frame=None, label_list=label_list, draw_threshold=0.7, frame_shape=[120, 160]) # 若仅测试检测模型时,请将angle设置为1500,保持直行 # angle = 1500 # 前方检测到限速标志的操作 if 'limit_10' in results_list: if results_list['limit_10'][1] > 40: if limit_Flag == 0: limit_Flag = 1 base_speed = 1525
def center_back(my_pose, ball_info, control_target, my_role, defense_num): # ゴール前ディフェンスは、ペナルティエリアに沿って守備を行う # ボールとゴールを結ぶ直線と、ペナルティエリアのラインの交点を基準に移動 # ボールの位置によって、ライン左、ライン正面、ライン右のどのラインの前に移動するか変わる # ペナルティエリアからどれだけ離れるか MARGIN_LINE = 0.2 # 2台でディフェンスする時のお互いの距離 MARGIN_ROBOT = 0 # ライン左からライン正面に移動する時等に、 # 一時的にスピードアップするための数値 MARGIN_FOR_SPEED = 0.5 # ディフェンスが2台以上いる時はMARGIN_ROBOTを変更 if defense_num > 1: if my_role == role.ROLE_ID["ROLE_CENTER_BACK_1"]: MARGIN_ROBOT = 0.15 else: MARGIN_ROBOT = -0.15 # ボール位置 ball_pose = ball_info.pose # ボールの位置判定用フラグ ball_is_center = False ball_is_left = False ball_is_right = False # 自分の位置判定用フラグ my_pose_is_left = False my_pose_is_right = False # 移動すべき場所判定用フラグ target_is_center = False target_is_left = False target_is_right = False # Field情報からペナルティエリアの情報を取得 # ペナルティエリアの左角 left_penalty_corner = Field.penalty_pose('our', 'upper_front') # ペナルティエリアの右角 right_penalty_corner = Field.penalty_pose('our', 'lower_front') # ペナルティエリアの左側のゴールラインとの交点 left_penalty_goalside = Field.penalty_pose('our', 'upper_back') # ペナルティエリアの右側のゴールラインとの交点 right_penalty_goalside = Field.penalty_pose('our', 'lower_back') # ゴールの中心 goal_center = Field.goal_pose('our', 'center') # ゴール中心からペナルティエリア左角への角度 angle_to_left_penalty_corner = tool.get_angle(goal_center, left_penalty_corner) # ゴール中心からペナルティエリア右角への角度 angle_to_right_penalty_corner = tool.get_angle(goal_center, right_penalty_corner) # 自分からボールへの角度(ボールの方向を向くため) angle_to_ball = tool.get_angle(my_pose, ball_pose) # ゴールを背にした左角を中心とした座標軸へ変換 trans_left = tool.Trans(left_penalty_corner, angle_to_left_penalty_corner) tr_left_ball_pose = trans_left.transform(ball_pose) # ゴールを背にした右角を中心とした座標軸へ変換 trans_right = tool.Trans(right_penalty_corner, angle_to_right_penalty_corner) tr_right_ball_pose = trans_right.transform(ball_pose) # ボールの位置を判定 if tr_left_ball_pose.y > 0: ball_is_left = True elif tr_right_ball_pose.y < 0: ball_is_right = True else: ball_is_center = True # --------------------------------------------------------- # キックとドリブルはOFF control_target.kick_power = 0.0 control_target.dribble_power = 0.0 # ボールは真ん中にある if ball_is_center: # ペナルティエリアの左角と右角の線分(正面の線)と、ゴール中心とボールの線分の交点 target_pose = tool.get_intersection(left_penalty_corner, right_penalty_corner, goal_center, ball_pose) if target_pose is not None: # ペナルティエリアに侵入しないように+MARGIN_LINE target_pose.x += MARGIN_LINE # ロボットが正面の線より後ろにいる if my_pose.x < left_penalty_corner.x: # ダッシュで正面に移動 target_pose.x += MARGIN_FOR_SPEED # ペナルティエリアを沿って移動 if my_pose.y > 0: target_pose.y = left_penalty_corner.y + MARGIN_LINE else: target_pose.y = right_penalty_corner.y - MARGIN_LINE else: target_pose.y += MARGIN_ROBOT else: target_pose = Pose2D() # ボールは左側にある elif ball_is_left: # ペナルティエリアの左側の線分と、ゴール中心とボールの線分の交点 target_pose = tool.get_intersection(left_penalty_corner, left_penalty_goalside, goal_center, ball_pose) if target_pose is not None: # ペナルティエリアに侵入しないように+MARGIN_LINE target_pose.y += MARGIN_LINE # ロボットが左側にいない if my_pose.y < left_penalty_corner.y: # 左側にいないかつ後ろにいる場合は右側を沿う if my_pose.x < left_penalty_corner.x and my_pose.y < 0: target_pose.x = left_penalty_corner.x + MARGIN_FOR_SPEED target_pose.y = right_penalty_corner.y - MARGIN_LINE # 左側にダッシュで移動 else: target_pose.x = left_penalty_corner.x + MARGIN_LINE target_pose.y += MARGIN_FOR_SPEED else: target_pose.x -= MARGIN_ROBOT else: target_pose = Pose2D() # ボールは右側にある elif ball_is_right: target_pose = tool.get_intersection(right_penalty_corner, right_penalty_goalside, goal_center, ball_pose) if target_pose is not None: # ペナルティエリアに侵入しないように-MARGIN_LINE target_pose.y -= MARGIN_LINE # ロボットが右側にいない if my_pose.y > right_penalty_corner.y: # 右側にいないかつ後ろにいる場合は左側を沿う if my_pose.x < left_penalty_corner.x and my_pose.y > 0: target_pose.x = left_penalty_corner.x + MARGIN_FOR_SPEED target_pose.y = left_penalty_corner.y + MARGIN_LINE # 右側にダッシュで移動 else: target_pose.x = right_penalty_corner.x + MARGIN_LINE target_pose.y -= MARGIN_FOR_SPEED else: target_pose.x += MARGIN_ROBOT else: target_pose = Pose2D() # フィールドから出ないように if target_pose.x < goal_center.x: target_pose.x = goal_center.x # 向きはボールの方向 target_pose.theta = angle_to_ball control_target.path = [] control_target.path.append(target_pose) return control_target
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