diff --git a/consai_game/consai_game/tactic/composite/composite_defense.py b/consai_game/consai_game/tactic/composite/composite_defense.py index 39bcea90..e6fdca94 100644 --- a/consai_game/consai_game/tactic/composite/composite_defense.py +++ b/consai_game/consai_game/tactic/composite/composite_defense.py @@ -12,22 +12,29 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -条件に応じてディフェンス動作やキックやパスを切り替えるTactic -""" +"""条件に応じてディフェンス動作やキックやパスを切り替えるTactic.""" from consai_game.core.tactic.tactic_base import TacticBase from consai_game.core.tactic.composite_tactic_base import CompositeTacticBase from consai_game.tactic.kick import Kick from consai_game.tactic.receive import Receive from consai_game.world_model.world_model import WorldModel + from consai_tools.geometry import geometry_tools as tool from consai_msgs.msg import MotionCommand class CompositeDefense(CompositeTacticBase): + """条件に応じてディフェンス動作を切り替えるTactic.""" + def __init__(self, tactic_default: TacticBase, do_receive: bool = True): + """ + コンストラクタ. + + 使用するTacticを指定と各変数の初期化を実行. + """ + super().__init__( tactic_shoot=Kick(is_pass=False), tactic_pass=Kick(is_pass=True), @@ -69,16 +76,16 @@ def run(self, world_model: WorldModel) -> MotionCommand: def control_the_ball(self, world_model: WorldModel) -> MotionCommand: """ボールを制御するためのTacticを実行する関数.""" - if world_model.kick_target.best_shoot_target.success_rate > 50: + if world_model.evaluation.kick_target.best_shoot_target.success_rate > 50: # シュートできる場合 - self.tactic_shoot.target_pos = world_model.kick_target.best_shoot_target.pos + self.tactic_shoot.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos return self.run_sub_tactic(self.tactic_shoot, world_model) - elif world_model.kick_target.best_pass_target.success_rate > 30: + elif world_model.evaluation.kick_target.best_pass_target.success_rate > 30: # パスできる場合 - self.tactic_pass.target_pos = world_model.kick_target.best_pass_target.robot_pos + self.tactic_pass.target_pos = world_model.evaluation.kick_target.best_pass_target.robot_pos return self.run_sub_tactic(self.tactic_pass, world_model) # シュート成功率が一番高いところに向かってパスする(クリア) - self.tactic_pass.target_pos = world_model.kick_target.best_shoot_target.pos + self.tactic_pass.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos return self.run_sub_tactic(self.tactic_pass, world_model) diff --git a/consai_game/consai_game/tactic/composite/composite_offense.py b/consai_game/consai_game/tactic/composite/composite_offense.py index 8aa4ddce..11e63019 100644 --- a/consai_game/consai_game/tactic/composite/composite_offense.py +++ b/consai_game/consai_game/tactic/composite/composite_offense.py @@ -12,29 +12,34 @@ # See the License for the specific language governing permissions and # limitations under the License. -""" -条件に応じてキックやパスを切り替えるTactic -""" +"""条件に応じてキックやパスを切り替えるTactic.""" + import copy +import math +from typing import Optional, Set + from consai_game.core.tactic.tactic_base import TacticBase from consai_game.core.tactic.composite_tactic_base import CompositeTacticBase from consai_game.tactic.kick import Kick from consai_game.tactic.receive import Receive -from consai_game.world_model.world_model import WorldModel from consai_game.tactic.steal_ball import StealBall - +from consai_game.world_model.world_model import WorldModel from consai_msgs.msg import MotionCommand from consai_msgs.msg import State2D + from consai_tools.geometry import geometry_tools as tools -import math -from typing import Optional, Set class SharedInfo: """CompositeOffenseの情報を共有するクラス""" def __init__(self): + """ + コンストラクタ. + + メンバ変数の初期化を実行. + """ self.assigned_robot_ids: Set[int] = set() # CompositeOffenseを担当しているロボットのID self.update_conter: int = 0 # 内部用更新カウンター self.can_control_ball_id: Optional[bool] = None # ボールを操作できるロボットのID @@ -113,9 +118,17 @@ def ball_receiving_candidates(self, world_model: WorldModel) -> tuple[Optional[i class CompositeOffense(CompositeTacticBase): + """条件に応じてキックやパスを切り替えるTactic.""" + shared_info = SharedInfo() def __init__(self, tactic_default: TacticBase, is_setplay=False, force_pass=False, kick_score_threshold=30): + """ + コンストラクタ. + + 使用するtacticやメンバ変数の初期化を実行. + """ + super().__init__( tactic_shoot=Kick(is_pass=False, is_setplay=is_setplay), tactic_pass=Kick(is_pass=True, is_setplay=is_setplay), @@ -139,6 +152,7 @@ def reset(self, robot_id: int) -> None: self.shared_info.register_robot(robot_id) def exit(self): + """Exit the tactic and unregister the robot ID.""" super().exit() # ロボットのIDを登録解除する @@ -169,18 +183,19 @@ def control_the_ball(self, world_model: WorldModel) -> MotionCommand: return self.run_sub_tactic(self.tactic_steal, world_model) if ( - world_model.kick_target.best_shoot_target.success_rate > self.kick_score_threshold - self.SHOOTING_MARGIN + world_model.evaluation.kick_target.best_shoot_target.success_rate + > self.kick_score_threshold - self.SHOOTING_MARGIN and self.force_pass is False ): # シュートできる場合かつforce_passがFalseの場合 - self.tactic_shoot.target_pos = world_model.kick_target.best_shoot_target.pos + self.tactic_shoot.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos # シュート相手がコロコロ切り替わらないようにマージンを設定 self.SHOOTING_MARGIN = 20 return self.run_sub_tactic(self.tactic_shoot, world_model) - elif world_model.kick_target.best_pass_target.success_rate > 30 or self.force_pass: + elif world_model.evaluation.kick_target.best_pass_target.success_rate > 30 or self.force_pass: # パスできる場合 か force_passがTrueの場合 - self.tactic_pass.target_pos = copy.deepcopy(world_model.kick_target.best_pass_target.robot_pos) + self.tactic_pass.target_pos = copy.deepcopy(world_model.evaluation.kick_target.best_pass_target.robot_pos) # パスターゲットの候補を探そうとしているのでシュートターゲットのマージンを0にする self.SHOOTING_MARGIN = 0 @@ -188,7 +203,7 @@ def control_the_ball(self, world_model: WorldModel) -> MotionCommand: # TODO: 前進しつつ、敵がいない方向にドリブルしたい # シュート成功率が一番高いところに向かってドリブルする - self.tactic_tapping.target_pos = world_model.kick_target.best_shoot_target.pos + self.tactic_tapping.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos return self.run_sub_tactic(self.tactic_tapping, world_model) def receive_the_ball(self, world_model: WorldModel) -> MotionCommand: diff --git a/consai_game/consai_game/tactic/dribble.py b/consai_game/consai_game/tactic/dribble.py index 6df0fd55..9722ac8c 100644 --- a/consai_game/consai_game/tactic/dribble.py +++ b/consai_game/consai_game/tactic/dribble.py @@ -156,7 +156,9 @@ def run(self, world_model: WorldModel) -> MotionCommand: dist_robot_to_ball=dist_robot_to_ball, dist_ball_to_target=dist_ball_to_target, dribble_diff_angle=np.rad2deg(dribble_diff_angle), - ball_is_front=self.ball_is_front(ball_pos=ball_pos, robot_pos=robot_pos), + ball_is_front=world_model.evaluation.relative_position.is_ball_front( + robot_pos=robot_pos, ball_pos=ball_pos, target_pos=self.target_pos + ), ) command = MotionCommand() @@ -178,28 +180,6 @@ def run(self, world_model: WorldModel) -> MotionCommand: self.append_machine_state_to_name() # デバッグのため、状態を名前に追加 return command - def ball_is_front(self, ball_pos: State2D, robot_pos: State2D) -> bool: - """ボールがロボットの前にあるかどうかを判定する.""" - FRONT_DIST_THRESHOLD = 0.15 # 正面方向にどれだけ離れることを許容するか - SIDE_DIST_THRESHOLD = 0.05 # 横方向にどれだけ離れることを許容するか - - # ロボットを中心に、ターゲットを+x軸とした座標系を作る - trans = tool.Trans(robot_pos, tool.get_angle(robot_pos, self.target_pos)) - tr_ball_pos = trans.transform(ball_pos) - - # ボールがロボットの後ろにある - if tr_ball_pos.x < 0: - return False - - # ボールが正面から離れすぎている - if tr_ball_pos.x > FRONT_DIST_THRESHOLD: - return False - - # ボールが横方向に離れすぎている - if abs(tr_ball_pos.y) > SIDE_DIST_THRESHOLD: - return False - return True - def approach_to_ball(self, command: MotionCommand, ball_pos: State2D) -> MotionCommand: """ボールに近づくコマンドを返す.""" APPROACH_DIST = 0.09 # ボールに近づく距離。ロボットの半径と同じくらいにする diff --git a/consai_game/consai_game/tactic/kick.py b/consai_game/consai_game/tactic/kick.py index fa9a448d..eb9fe5b2 100644 --- a/consai_game/consai_game/tactic/kick.py +++ b/consai_game/consai_game/tactic/kick.py @@ -14,18 +14,20 @@ """キック動作に関するTacticを定義するモジュール.""" -import numpy as np import copy -from consai_msgs.msg import MotionCommand, State2D - -from consai_tools.geometry import geometry_tools as tool from consai_game.world_model.world_model import WorldModel from consai_game.core.tactic.tactic_base import TacticBase from consai_game.utils.generate_dummy_ball_position import generate_dummy_ball_position +from consai_msgs.msg import MotionCommand, State2D + +from consai_tools.geometry import geometry_tools as tool + from transitions.extensions import GraphMachine +import numpy as np + class KickStateMachine(GraphMachine): """状態遷移マシン.""" @@ -136,9 +138,11 @@ def run(self, world_model: WorldModel) -> MotionCommand: width_threshold = 0.03 self.machine.update( - robot_is_backside=self.robot_is_backside(robot_pos, ball_pos, self.final_target_pos), - robot_is_on_kick_line=self.robot_is_on_kick_line( - robot_pos, ball_pos, self.final_target_pos, width_threshold=width_threshold + robot_is_backside=world_model.evaluation.relative_position.is_robot_backside( + robot_pos, ball_pos, self.final_target_pos, self.ANGLE_BALL_TO_ROBOT_THRESHOLD + ), + robot_is_on_kick_line=world_model.evaluation.relative_position.is_robot_on_kick_line( + robot_pos, ball_pos, self.final_target_pos, width_threshold ), ) @@ -201,47 +205,6 @@ def run(self, world_model: WorldModel) -> MotionCommand: self.append_machine_state_to_name() # デバッグのため、状態を名前に追加 return command - def robot_is_backside(self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D) -> bool: - """ボールからターゲットを見て、ロボットが後側に居るかを判定する.""" - # ボールからターゲットへの座標系を作成 - trans = tool.Trans(ball_pos, tool.get_angle(ball_pos, target_pos)) - tr_robot_pos = trans.transform(robot_pos) - - # ボールから見たロボットの位置の角度 - # ボールの後方にいれば角度は90度以上 - tr_ball_to_robot_angle = tool.get_angle(State2D(x=0.0, y=0.0), tr_robot_pos) - - if abs(tr_ball_to_robot_angle) > np.deg2rad(self.ANGLE_BALL_TO_ROBOT_THRESHOLD): - return True - return False - - def robot_is_on_kick_line( - self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float - ) -> bool: - """ボールからターゲットまでの直線上にロボットが居るかを判定する. - - ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. - """ - MINIMAL_THETA_THRESHOLD = 45 # 最低限満たすべきロボットの角度 - - # ボールからターゲットへの座標系を作成 - trans = tool.Trans(ball_pos, tool.get_angle(ball_pos, target_pos)) - tr_robot_pos = trans.transform(robot_pos) - tr_robot_theta = trans.transform_angle(robot_pos.theta) - - # ボールより前にロボットが居る場合 - if tr_robot_pos.x > 0.0: - return False - - # ターゲットを向いていない - if abs(tr_robot_theta) > np.deg2rad(MINIMAL_THETA_THRESHOLD): - return False - - if abs(tr_robot_pos.y) > width_threshold: - return False - - return True - def move_to_backside_pose( self, ball_pos: State2D, robot_pos: State2D, target_pos: State2D, distance: float ) -> State2D: @@ -290,6 +253,6 @@ def pass_power(self, ball_pos: State2D, target_pos: State2D, world_model: WorldM return MAX_KICK_POWER else: # 線形補間 - return MIN_PASS_POWER + (MAX_KICK_POWER - MIN_PASS_POWER) * ( - distance_to_target - MIN_PASS_DISTANCE - ) / (MAX_PASS_DISTANCE - MIN_PASS_DISTANCE) + return MIN_PASS_POWER + (MAX_KICK_POWER - MIN_PASS_POWER) * (distance_to_target - MIN_PASS_DISTANCE) / ( + MAX_PASS_DISTANCE - MIN_PASS_DISTANCE + ) diff --git a/consai_game/consai_game/visualization/visualize_msg_publisher_node.py b/consai_game/consai_game/visualization/visualize_msg_publisher_node.py index 7650453a..15df20f1 100644 --- a/consai_game/consai_game/visualization/visualize_msg_publisher_node.py +++ b/consai_game/consai_game/visualization/visualize_msg_publisher_node.py @@ -44,7 +44,7 @@ def __init__(self): def publish(self, world_model: WorldModel): """WorldModelをGUIに描画するためのトピックをpublishする.""" self.pub_visualizer_objects.publish( - self.kick_target_to_vis_msg(kick_target=world_model.kick_target, ball=world_model.ball) + self.kick_target_to_vis_msg(kick_target=world_model.evaluation.kick_target, ball=world_model.ball) ) self.pub_visualizer_objects.publish( @@ -52,7 +52,7 @@ def publish(self, world_model: WorldModel): ) self.pub_visualizer_objects.publish( - self.threats_to_vis_msg(threats=world_model.threats, robots=world_model.robots) + self.threats_to_vis_msg(threats=world_model.evaluation.threats_evaluation, robots=world_model.robots) ) self.pub_visualizer_objects.publish( self.robot_activity_to_vis_msg(robot_activity=world_model.robot_activity, robots=world_model.robots) diff --git a/consai_game/consai_game/world_model/ball_activity_model.py b/consai_game/consai_game/world_model/ball_activity_model.py index 1b97ae5d..c45f319a 100644 --- a/consai_game/consai_game/world_model/ball_activity_model.py +++ b/consai_game/consai_game/world_model/ball_activity_model.py @@ -31,6 +31,7 @@ from consai_game.world_model.referee_model import RefereeModel from consai_game.world_model.game_config_model import GameConfigModel from consai_game.world_model.field_model import FieldPoints +from consai_game.world_model.perception.ball_prediction import BallPrediction from consai_msgs.msg import State2D @@ -64,8 +65,6 @@ class BallActivityModel: HAS_BALL_MARGIN = 0.1 # ヒステリシス処理に使用する MOVING_VELOCITY_THRESHOLD = 0.1 # ボールが動いているとみなす速度の閾値 MOVING_VELOCITY_MARGIN = 0.05 # ヒステリシス処理に使用する - # 速度に対するボール移動量を算出する比率[s]: 実質的に移動時間 - MOVEMENT_GAIN = 0.1 def __init__(self): """BallActivityModelの初期化処理.""" @@ -89,6 +88,9 @@ def __init__(self): # ボール移動判定用の変数 self.last_ball_pos_to_detect_moving: Optional[State2D] = None + # ボールの予測クラスのインスタンスを生成 + self.ball_prediction = BallPrediction() + def update( self, ball: BallModel, @@ -111,7 +113,7 @@ def update( self.ball_is_moving = self.is_ball_moving(ball) # ボールの予測位置を更新する - self.prediction_next_ball_pos(ball) + self.next_ball_pos = self.ball_prediction.next_ball_pos(ball) # 最終的なボール状態を更新する self.update_ball_state() @@ -119,13 +121,20 @@ def update( # ボールがプレースメントエリアにあるかを更新する self.update_ball_on_placement_area(ball, referee) + self.ball_prediction.update( + ball_is_moving=self.ball_is_moving, + field_points=field_points, + game_config=game_config, + ) + # ボールの最終的な停止位置を予測する - self.ball_stop_position = self.predict_ball_stop_position(ball=ball, game_config=game_config) + self.ball_stop_position = self.ball_prediction.ball_stop_position( + ball=ball, + ) # ボールが相手のゴールに入るかを判定する - self.ball_will_enter_their_goal = self.is_ball_will_enter_their_goal( + self.ball_will_enter_their_goal = self.ball_prediction.is_ball_will_enter_their_goal( ball=ball, - field_points=field_points, ) def update_ball_state(self): @@ -247,27 +256,6 @@ def nearest_robot_of_team(self, ball: BallModel, visible_robots: dict[int, Robot return nearest_robot, nearest_distance - def prediction_next_ball_pos(self, ball: BallModel): - """ - 次のボールの位置を予測するメソッド - - 暫定的に0.1[m]移動すると仮定 - """ - # 将来の位置 - _future_ball_pos = State2D() - _future_ball_pos.x = ball.pos.x + ball.vel.x - _future_ball_pos.y = ball.pos.y + ball.vel.y - # 軌道角度を計算 - self.angle_trajectory = tools.get_angle(ball.pos, _future_ball_pos) - - # ボール移動量 - self.ball_movement.x = ball.vel.x * self.MOVEMENT_GAIN # * np.cos(self.angle_trajectory) - self.ball_movement.y = ball.vel.y * self.MOVEMENT_GAIN # * np.sin(self.angle_trajectory) - - # 予測位置を算出 - self.next_ball_pos.x = ball.pos.x + self.ball_movement.x - self.next_ball_pos.y = ball.pos.y + self.ball_movement.y - def is_ball_moving(self, ball: BallModel) -> bool: """ボールが動いているかを判定するメソッド.""" if not ball.is_visible: @@ -317,34 +305,6 @@ def update_ball_on_placement_area(self, ball: BallModel, referee: RefereeModel): else: self.ball_is_on_placement_area = False - def predict_ball_stop_position(self, ball: BallModel, game_config: GameConfigModel) -> State2D: - """ボールが止まる位置を予測するメソッド.""" - # ボールの速度が小さい場合は、現在の位置を返す - if not self.ball_is_moving: - return ball.pos - - # ボールを中心に、ボール速度方向への座標系を作成 - trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) - - vel_norm = tools.get_norm(ball.vel) - - # 減速距離 - a = game_config.ball_friction_coeff * game_config.gravity - distance = (vel_norm ** 2) / (2 * a) - - return trans.inverted_transform(State2D(x=distance, y=0.0)) - - def is_ball_will_enter_their_goal(self, ball: BallModel, field_points: FieldPoints) -> bool: - """ボールが相手のゴールに入るかを判定するメソッド.""" - # ボールが動いていない場合は、Falseを返す - if not self.ball_is_moving: - return False - - # 2つの線が交差するかで判定する - return tools.is_intersect( - p1=ball.pos, p2=self.ball_stop_position, q1=field_points.their_goal_top, q2=field_points.their_goal_bottom - ) - @property def is_our_team_ball_holder(self) -> bool: """ボール保持者が自分チームか判定を返す関数""" diff --git a/consai_game/consai_game/world_model/evaluation/__init__.py b/consai_game/consai_game/world_model/evaluation/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/consai_game/consai_game/world_model/evaluation/evaluation.py b/consai_game/consai_game/world_model/evaluation/evaluation.py new file mode 100644 index 00000000..19652d39 --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/evaluation.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +# coding: UTF-8 + +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""評価を統合したEvaluationの定義モジュール.""" + +from dataclasses import dataclass + +from consai_game.world_model.evaluation.kick_target_evaluation import KickTargetEvaluation +from consai_game.world_model.evaluation.relative_position_evaluation import RelativePositionEvaluation +from consai_game.world_model.evaluation.threats_evaluation import ThreatsEvaluation +from consai_game.world_model.field_model import Field, FieldPoints + + +@dataclass +class Evaluation: + """評価に関する関数やクラスを統合的に保持するデータクラス.""" + + field: Field = Field() + field_points: FieldPoints = FieldPoints.create_field_points(field) + kick_target: KickTargetEvaluation = KickTargetEvaluation() + relative_position: RelativePositionEvaluation = RelativePositionEvaluation() + threats_evaluation: ThreatsEvaluation = ThreatsEvaluation(field, field_points) diff --git a/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py new file mode 100644 index 00000000..00cdce93 --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py @@ -0,0 +1,260 @@ +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +キックターゲットを管理し予測するモジュール. + +シュートを試みるための最適なターゲット位置を計算し, キックターゲットの成功率を更新する. +""" + +import numpy as np + +from copy import deepcopy +from dataclasses import dataclass, field +from operator import attrgetter + +from consai_game.utils.geometry import Point +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.field_model import Field +from consai_game.world_model.robots_model import RobotsModel + +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tool + +from consai_game.world_model.perception.robot_decision import RobotDecision + + +@dataclass +class ShootTarget: + """キックターゲットの位置と成功率を保持するデータクラス.""" + + pos: State2D = field(default_factory=State2D) + success_rate: int = 0 + + +@dataclass +class PassTarget: + """パスをするロボットの位置と成功率を保持するデータクラス.""" + + robot_id: int = 0 + robot_pos: State2D = field(default_factory=State2D) + success_rate: int = 0 + + +class KickTargetEvaluation: + """キックターゲットを評価するクラス.""" + + ROBOT_RADIUS = 0.1 # ロボット半径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 60 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 20 # スコア計算時のシュートターゲットの最大角度スコア + MAX_GOALIE_LEAVE_SCORE = 20 # スコア計算時のシュートターゲットがgoalieからどれくらい離れているかの最大スコア + RATE_MARGIN = 50 # ヒステリシスのためのマージン + ROBOT_DIAMETER = 0.1 * 2 # ロボット直径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 55 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 45 # スコア計算時のシュートターゲットの最大角度スコア + HYSTERESIS_DISTANCE = 0.3 + HALF_WIDTH = 4.5 + + def __init__(self): + """KickTargetEvaluationの初期化関数.""" + + # shoot_targetの位置と成功率を保持するリスト + self.shoot_target_list: list[ShootTarget] = [] + self._goal_pos_list = [ShootTarget()] + + # pass_targetの位置と成功率を保持するリスト + self.pass_target_list: list[PassTarget] = [] + + def update_field_pos_list(self, field: Field) -> None: + """フィールドの位置候補を更新する関数.""" + + quarter_width = field.half_goal_width / 2 + one_eighth_width = field.half_goal_width / 4 + + self._goal_pos_list = [ + ShootTarget(pos=Point(field.half_length, 0.0)), + ShootTarget(pos=Point(field.half_length, one_eighth_width)), + ShootTarget(pos=Point(field.half_length, -one_eighth_width)), + ShootTarget(pos=Point(field.half_length, quarter_width)), + ShootTarget(pos=Point(field.half_length, -quarter_width)), + ShootTarget(pos=Point(field.half_length, quarter_width + one_eighth_width)), + ShootTarget(pos=Point(field.half_length, -(quarter_width + one_eighth_width))), + ] + + self.HALF_WIDTH = field.half_width + self._defense_area = Point(field.half_length - field.penalty_depth, field.half_width - field.half_penalty_width) + + def update( + self, + ball_model: BallModel, + robots_model: RobotsModel, + ) -> None: + """キックターゲットを更新する関数.""" + # 最も成功するshoot_targetの座標を取得 + self.best_shoot_target = self._search_shoot_pos(ball=ball_model, robots=robots_model, search_ours=True) + + self.best_pass_target = self._search_pass_robot(ball=ball_model, robots=robots_model, search_ours=False) + + def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: + """各シュートターゲットの成功率を計算し, リストを更新する関数.""" + + # 相手のgoalieの位置でシュートターゲットのスコア計算 + goalie_pos = None + for their in robots.their_visible_robots.values(): + if their.pos.x > self._defense_area.x and abs(their.pos.y) < self._defense_area.y: + # 相手のgoalieの位置を取得 + goalie_pos = their.pos + + for target in self._goal_pos_list: + score = 0 + if ( + RobotDecision.obstacle_exists( + target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=self.ROBOT_RADIUS + ) + and search_ours + ): + target.success_rate = score + elif RobotDecision.obstacle_exists( + target=target.pos, ball=ball, robots=robots.their_visible_robots, tolerance=self.ROBOT_RADIUS + ): + target.success_rate = score + else: + # ボールからの角度（目標方向がゴール方向と合っているか） + angle = abs(tool.get_angle(ball.pos, target.pos)) + score += max( + 0, self.MAX_ANGLE_SCORE - np.rad2deg(angle) * self.MAX_ANGLE_SCORE / 60 + ) # 小さい角度（正面）ほど高得点とし、60度以上角度がついていれば0点 + + # 距離（近いほうが成功率が高そう） + distance = tool.get_distance(ball.pos, target.pos) + score += max( + 0, self.MAX_DISTANCE_SCORE - (distance - self.MARGIN) * self.MAX_DISTANCE_SCORE / 6 + ) # ディフェンスエリア外から6m以内ならOK + + if goalie_pos is None: + score += self.MAX_GOALIE_LEAVE_SCORE + else: + # 相手のgoalieから離れていればスコアを加算（ロボット直径3台分以上離れて入れば満点） + trans = tool.Trans(ball.pos, tool.get_angle(ball.pos, target.pos)) + tr_goalie_pos = trans.transform(goalie_pos) + score += ( + min(abs(tr_goalie_pos.y), robots.robot_radius * 6) + * self.MAX_GOALIE_LEAVE_SCORE + / (robots.robot_radius * 6) + ) + target.success_rate = int(score) + + def _sort_kick_targets_by_success_rate(self, targets: list[ShootTarget]) -> list[ShootTarget]: + """スコアの高いターゲット順にソートする関数.""" + return sorted(targets, key=attrgetter("success_rate"), reverse=True) + + def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=False) -> ShootTarget: + """ボールからの直線上にロボットがいないシュート位置を返す関数.""" + + last_shoot_target_list = self.shoot_target_list.copy() + self._update_shoot_scores(ball=ball, robots=robots, search_ours=search_ours) + shoot_target_list = self._goal_pos_list.copy() + self.shoot_target_list = self._sort_kick_targets_by_success_rate(shoot_target_list) + + if not last_shoot_target_list: + return self.shoot_target_list[0] + + if self.shoot_target_list[0].pos == last_shoot_target_list[0].pos: + return self.shoot_target_list[0] + + # ヒステリシス処理 + if self.shoot_target_list[0].success_rate > last_shoot_target_list[0].success_rate + self.RATE_MARGIN: + return self.shoot_target_list[0] + return last_shoot_target_list[0] + + def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: + """各パスターゲットの成功率を計算し, リストを返す関数.""" + + pass_target_list: list[PassTarget] = [] + + for robot in robots.our_visible_robots.values(): + score = 0 + if ( + RobotDecision.obstacle_exists( + target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=self.ROBOT_DIAMETER + ) + and search_ours + ): + score = 0 + elif RobotDecision.obstacle_exists( + target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=self.ROBOT_DIAMETER + ): + score = 0 + elif tool.get_distance(ball.pos, robot.pos) < 0.5: + score = 0 + elif RobotDecision.is_robot_inside_pass_area(ball, robot) is False: + score = 0 + else: + # ボールとパスを受けるロボットの距離 + distance = tool.get_distance(ball.pos, robot.pos) + score += max( + 0, self.MAX_DISTANCE_SCORE - (distance - self.MARGIN) * self.MAX_DISTANCE_SCORE / 4 + ) # ディフェンスエリア外から4m以内ならOK + # ボールからの角度（目標方向がロボット方向と合っているか） + angle = abs(tool.get_angle(ball.pos, robot.pos)) + score += max(0, self.MAX_ANGLE_SCORE - np.rad2deg(angle) * 0.5) # 小さい角度ほど高得点 + # ロボットと相手ゴールの距離 + distance = tool.get_distance(robot.pos, self._goal_pos_list[0].pos) + score -= max(0, 20 - (distance - self.MARGIN) * 10) # ボールからディフェンスエリアまで2m以内だったら減点 + pass_target_list.append( + PassTarget( + robot_id=robot.robot_id, + robot_pos=robot.pos, + success_rate=int(score), + ) + ) + + # スコアの高いターゲット順にソート + return sorted(pass_target_list, key=attrgetter("success_rate"), reverse=True) + + def _search_pass_robot(self, ball: BallModel, robots: RobotsModel, search_ours=False) -> PassTarget: + """ + パスをするロボットのIDと位置を返す関数. + + 内部でpass_target_listを更新する. + """ + # RATE_MARGIN = 50 + + # 前回のターゲットを保存する + last_pass_target_list = deepcopy(self.pass_target_list) + + self.pass_target_list = self.make_pass_target_list(ball=ball, robots=robots, search_ours=search_ours) + + # 今回ターゲットが見つからなければ、無効なターゲットを返す + if not self.pass_target_list: + return PassTarget(robot_id=-1) + + # 前回のターゲットが空白であれば、今回のターゲットをそのまま返す + if not last_pass_target_list: + return self.pass_target_list[0] + + # 前回と今回のベストターゲットが同じであれば、今回のターゲットをそのまま返す + if last_pass_target_list[0].robot_pos == self.pass_target_list[0].robot_pos: + return self.pass_target_list[0] + + # TODO: 本来いれるべきだが、これによりターゲットが切り替わり続けるため一旦無効化 + # ベストターゲットが変わった場合、 + # 前回のターゲットより十分にスコアが大きければ、新しいターゲットを返す + # if self.pass_target_list[0].success_rate > last_pass_target_list[0].success_rate + RATE_MARGIN: + # return self.pass_target_list[0] + + return last_pass_target_list[0] diff --git a/consai_game/consai_game/world_model/evaluation/relative_position_evaluation.py b/consai_game/consai_game/world_model/evaluation/relative_position_evaluation.py new file mode 100644 index 00000000..4869a954 --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/relative_position_evaluation.py @@ -0,0 +1,94 @@ +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +"""相対的な位置関係を評価するモジュール.""" + +import numpy as np + +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tools + + +class RelativePositionEvaluation: + """相対的な位置関係を評価するクラス.""" + + def is_robot_backside( + self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D, angle_ball_to_robot_threshold: int + ) -> bool: + """ボールからターゲットを見て、ロボットが後側に居るかを判定するメソッド.""" + + # ボールからターゲットへの座標系を作成 + trans = tools.Trans(ball_pos, tools.get_angle(ball_pos, target_pos)) + tr_robot_pos = trans.transform(robot_pos) + + # ボールから見たロボットの位置の角度 + # ボールの後方にいれば角度は90度以上 + tr_ball_to_robot_angle = tools.get_angle(State2D(x=0.0, y=0.0), tr_robot_pos) + + if abs(tr_ball_to_robot_angle) > np.deg2rad(angle_ball_to_robot_threshold): + return True + return False + + def is_robot_on_kick_line( + self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float + ) -> bool: + """ボールからターゲットまでの直線上にロボットが居るかを判定するメソッド. + + ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. + """ + + minimal_theta_threshold = 45 # 最低限満たすべきロボットの角度 + + # ボールからターゲットへの座標系を作成 + trans = tools.Trans(ball_pos, tools.get_angle(ball_pos, target_pos)) + tr_robot_pos = trans.transform(robot_pos) + tr_robot_theta = trans.transform_angle(robot_pos.theta) + + # ボールより前にロボットが居る場合 + if tr_robot_pos.x > 0.0: + return False + + # ターゲットを向いていない + if abs(tr_robot_theta) > np.deg2rad(minimal_theta_threshold): + return False + + if abs(tr_robot_pos.y) > width_threshold: + return False + + return True + + def is_ball_front(self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D) -> bool: + """ボールがロボットの前にあるかどうかを判定するメソッド.""" + + front_dist_threshold = 0.15 # 正面方向にどれだけ離れることを許容するか + side_dist_threshold = 0.05 # 横方向にどれだけ離れることを許容するか + + # ロボットを中心に、ターゲットを+x軸とした座標系を作る + trans = tools.Trans(robot_pos, tools.get_angle(robot_pos, target_pos)) + tr_ball_pos = trans.transform(ball_pos) + + # ボールがロボットの後ろにある + if tr_ball_pos.x < 0: + return False + + # ボールが正面から離れすぎている + if tr_ball_pos.x > front_dist_threshold: + return False + + # ボールが横方向に離れすぎている + if abs(tr_ball_pos.y) > side_dist_threshold: + return False + return True diff --git a/consai_game/consai_game/world_model/evaluation/threats_evaluation.py b/consai_game/consai_game/world_model/evaluation/threats_evaluation.py new file mode 100644 index 00000000..da6b7bcd --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/threats_evaluation.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +# coding: UTF-8 + +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +敵ロボットの驚異度を計算し、驚異度の高い順にソートするモジュール. + +驚異度は敵ロボットのゴールへの距離, シュートできるか, ボールとの距離の3つの要素から計算される. +""" + +from dataclasses import dataclass +from typing import Dict, List + +from consai_msgs.msg import State2D + +from consai_game.world_model.field_model import Field, FieldPoints +from consai_game.world_model.robots_model import RobotsModel +from consai_game.world_model.ball_model import BallModel + +from consai_tools.geometry import geometry_tools as tools + + +@dataclass +class Threat: + """敵ロボットの脅威度情報を保持するデータクラス""" + + score: int # 0以上 + robot_id: int # 相手のロボットID + + +class ThreatsEvaluation: + """敵ロボットの脅威度を評価し順位付けするクラス""" + + ALPHA = 0.1 # ローパスフィルターの係数（0-1、小さいほど変化が遅い） + + def __init__(self, field: Field, field_points: FieldPoints): + """TreatsEvalutionの初期化""" + self.threats: List[Threat] = [] + self._field = field + self._field_points = field_points + self._prev_scores: Dict[int, float] = {} # ロボットIDごとの前回のスコア + + def _apply_low_pass_filter(self, robot_id: int, new_score: float) -> float: + """ローパスフィルターを適用してスコアを平滑化する + + Args: + robot_id: ロボットID + new_score: 新しいスコア + + Returns: + 平滑化されたスコア + """ + if robot_id not in self._prev_scores: + self._prev_scores[robot_id] = new_score + return new_score + + # 前回のスコアと新しいスコアを重み付けして結合 + filtered_score = self._prev_scores[robot_id] * (1 - self.ALPHA) + new_score * self.ALPHA + self._prev_scores[robot_id] = filtered_score + return filtered_score + + def update(self, ball: BallModel, robots: RobotsModel): + """敵ロボットの驚異度を更新する + + Args: + ball: ボールの情報 + robots: ロボットのリスト + """ + self.threats = [] + + # 敵ロボットのみを対象とする（is_visibleがtrueのものだけ） + for robot_id, robot in robots.their_robots.items(): + if not robot.is_visible: + continue + + # A: ゴールへの距離を計算 + goal = State2D(x=self._field_points.our_goal_top.x, y=0.0) + goal_distance = tools.get_distance(goal, robot.pos) + + # B: シュートできるか（ゴールとの間に障害物があるか） + can_shoot = True + for other_robot in robots.our_robots.values(): + if not other_robot.is_visible: + continue + # ロボットとゴールを結ぶ直線上に他のロボットがいるかチェック + if tools.is_on_line( + pose=other_robot.pos, line_pose1=robot.pos, line_pose2=goal, tolerance=0.1 # ロボットの半径を考慮 + ): + can_shoot = False + break + + # C: ボールとの距離を計算 + ball_distance = tools.get_distance(robot.pos, ball.pos) + + # 各要素のスコアを計算 + # A: ゴールへの距離（近いほど高スコア） + max_distance = self._field.length + score_a = int((max_distance - goal_distance) * 100 / max_distance) + + # B: シュートできるか（できる場合高スコア） + score_b = 100 if can_shoot else 0 + + # C: ボールとの距離（近いほど高スコア） + max_ball_distance = self._field.length + score_c = int((max_ball_distance - ball_distance) * 100 / max_ball_distance) + + # 総合スコアを計算 + # Bは一旦無視 + total_score = int(score_a * 0.8 + score_b * 0.0 + score_c * 0.2) + + # ローパスフィルターを適用 + filtered_score = self._apply_low_pass_filter(robot_id, total_score) + + threat = Threat(score=int(filtered_score), robot_id=robot_id) + self.threats.append(threat) + + # スコアの高い順にソート + self.threats.sort(key=lambda x: x.score, reverse=True) diff --git a/consai_game/consai_game/world_model/kick_target_model.py b/consai_game/consai_game/world_model/kick_target_model.py index 0bd8db7a..5ec42a95 100644 --- a/consai_game/consai_game/world_model/kick_target_model.py +++ b/consai_game/consai_game/world_model/kick_target_model.py @@ -19,21 +19,19 @@ """ import numpy as np -from operator import attrgetter +from copy import deepcopy from dataclasses import dataclass, field - - -from consai_msgs.msg import State2D - -from consai_tools.geometry import geometry_tools as tool +from operator import attrgetter from consai_game.utils.geometry import Point from consai_game.world_model.ball_model import BallModel from consai_game.world_model.field_model import Field from consai_game.world_model.robots_model import Robot, RobotsModel -from copy import deepcopy +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tool @dataclass diff --git a/consai_game/consai_game/world_model/perception/__init__.py b/consai_game/consai_game/world_model/perception/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/consai_game/consai_game/world_model/perception/ball_decision.py b/consai_game/consai_game/world_model/perception/ball_decision.py new file mode 100644 index 00000000..df817494 --- /dev/null +++ b/consai_game/consai_game/world_model/perception/ball_decision.py @@ -0,0 +1,131 @@ +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +""" +ボールの状態とボール保持者を管理するモジュール. + +ボールの状態や移動状況を更新し, ボールを保持しているロボットを追跡する機能を提供する. +""" + +from copy import deepcopy + +from consai_tools.geometry import geometry_tools as tools + +from consai_game.world_model.ball_model import BallModel + +import numpy as np + +from consai_msgs.msg import State2D + +from consai_game.world_model.field_model import Field +from consai_game.world_model.field_model import FieldPoints +from consai_game.world_model.ball_activity_model import BallActivityModel + + +class BallDecision: + """ボールの位置や動きを判定するクラス.""" + + BALL_MOVING_DIST_THRESHOLD = 0.3 # ボールが動いたと判断する距離. ここが小さすぎるとノイズにより動いた判定になる. + MOVING_VELOCITY_THRESHOLD = 0.1 # ボールが動いているとみなす速度の閾値 + SIDE_DIST_THRESHOLD = 0.05 # 横方向にどれだけ離れることを許容するか + THETA_THRESHOLD = 5 # 最低限守るべきロボットの姿勢 deg + GOAL_WITH_MARGIN = 0.5 + + def __init__(self, x=0.0, y=0.0): + """Initialize the DefendGoal tactic.""" + super().__init__() + self.target_pos = State2D(x=x, y=y) + + def is_ball_moving(self, ball: BallModel) -> bool: + """ボールが動いているかを判定するメソッド.""" + + if not ball.is_visible: + return False + + vel_norm = tools.get_norm(ball.vel) + + if vel_norm < self.MOVING_VELOCITY_THRESHOLD: + # ボールの速度が小さいときは、ボールが停止したと判断して、位置をキャプチャする + if self.last_ball_pos_to_detect_moving is None: + self.last_ball_pos_to_detect_moving = deepcopy(ball.pos) + else: + # ボールの速度が大きくて、前回の位置情報を持っていないときは + # 移動が継続していると判断してTrueを返す + if self.last_ball_pos_to_detect_moving is None: + return True + + # 停止時のボール位置からの移動距離 + move_distance = tools.get_distance(ball.pos, self.last_ball_pos_to_detect_moving) + + if move_distance > self.BALL_MOVING_DIST_THRESHOLD: + # 一定距離以上離れたら、動いたと判定してキャプチャした位置をリセット + self.last_ball_pos_to_detect_moving = None + return True + + # 一定距離移動してなければ、ボールは止まっていると判断 + return False + + # back_dribble.py + def ball_is_front(self, ball_pos: State2D, robot_pos: State2D, dist_threshold: float, target_pos: State2D) -> bool: + """ロボットがボールの前方にいるかどうかを判定する関数.""" + + # ボールを中心に、ターゲットを+x軸とした座標系を作る + trans = tools.Trans(ball_pos, tools.get_angle(ball_pos, target_pos)) + tr_robot_pos = trans.transform(robot_pos) + tr_robot_theta = trans.transform_angle(robot_pos.theta) + + # ロボットがボールの後ろにいる + if tr_robot_pos.x < 0: + return False + + # ボールが正面から離れすぎている + if tr_robot_pos.x > dist_threshold: + return False + + # ロボットがボールを見ていない + if abs(tr_robot_theta) < np.deg2rad(180 - self.THETA_THRESHOLD): + return False + + # ボールが横方向に離れすぎている + if abs(tr_robot_pos.y) > self.SIDE_DIST_THRESHOLD: + return False + return True + + # composite_goalie.py + def is_likely_to_score( + self, field: Field, field_points: FieldPoints, ball: BallModel, ball_activity: BallActivityModel + ) -> bool: + """ボールがゴールに入りそうかどうかを判定する関数.""" + goal_y_top = field.half_goal_width + goal_y_bottom = -field.half_goal_width + ball_pos = ball.pos + ball_stop_position = ball_activity.ball_stop_position + + # ゴールの上端・下端の座標 + # マージンを足して少し広く取る + goal_top_with_margin = State2D(x=-field.half_length, y=goal_y_top + self.GOAL_WITH_MARGIN) + goal_bottom_with_margin = State2D(x=-field.half_length, y=goal_y_bottom - self.GOAL_WITH_MARGIN) + + # ボール進行方向がゴールに交差するかどうかを判定する + intersection = tools.get_line_intersection( + ball_pos, ball_stop_position, goal_top_with_margin, goal_bottom_with_margin + ) + + # ボールがゴールに到達するか + # 到着点がディフェンスエリア側にありそうかどうか + if intersection is not None and ball_stop_position.x < field_points.our_defense_area.top_right.x: + return True + else: + return False diff --git a/consai_game/consai_game/world_model/perception/ball_prediction.py b/consai_game/consai_game/world_model/perception/ball_prediction.py new file mode 100644 index 00000000..569b9784 --- /dev/null +++ b/consai_game/consai_game/world_model/perception/ball_prediction.py @@ -0,0 +1,92 @@ +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""予測を管理するモジュール.""" + +from consai_tools.geometry import geometry_tools as tools + +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.game_config_model import GameConfigModel +from consai_game.world_model.field_model import FieldPoints + +from consai_msgs.msg import State2D + + +class BallPrediction: + """ボールの位置や動きを予測するクラス.""" + + # 速度に対するボール移動量を算出する比率[s]: 実質的に移動時間 + MOVEMENT_GAIN = 0.1 + + def update(self, ball_is_moving: bool, field_points: FieldPoints, game_config: GameConfigModel): + """ボールやフィールドなどの状態を更新するメソッド.""" + + self.ball_is_moving = ball_is_moving + self.field_points = field_points + self.game_config = game_config + + def next_ball_pos(self, ball: BallModel): + """ + 次のボールの位置を予測するメソッド + + 暫定的に0.1[m]移動すると仮定 + """ + + # TODO: 本来であれば角度から移動量を求めるべきだが暫定的に定量動くと予測している + # 将来の位置 + # _future_ball_pos = State2D() + # _future_ball_pos.x = ball.pos.x + ball.vel.x + # _future_ball_pos.y = ball.pos.y + ball.vel.y + # # 軌道角度を計算 + # angle_trajectory = tools.get_angle(ball.pos, _future_ball_pos) + + # 予測位置を算出 + next_ball_pos = State2D() + next_ball_pos.x = ball.pos.x + ball.vel.x * self.MOVEMENT_GAIN # * np.cos(self.angle_trajectory) + next_ball_pos.y = ball.pos.y + ball.vel.y * self.MOVEMENT_GAIN # * np.sin(self.angle_trajectory) + + return next_ball_pos + + def ball_stop_position(self, ball: BallModel) -> State2D: + """ボールが止まる位置を予測するメソッド.""" + + # ボールの速度が小さい場合は、現在の位置を返す + if not self.ball_is_moving: + return ball.pos + + # ボールを中心に、ボール速度方向への座標系を作成 + trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) + + vel_norm = tools.get_norm(ball.vel) + + # 減速距離 + a = self.game_config.ball_friction_coeff * self.game_config.gravity + distance = (vel_norm ** 2) / (2 * a) + + return trans.inverted_transform(State2D(x=distance, y=0.0)) + + def is_ball_will_enter_their_goal(self, ball: BallModel) -> bool: + """ボールが相手のゴールに入るかを判定(予測)するメソッド.""" + + # ボールが動いていない場合は、Falseを返す + if not self.ball_is_moving: + return False + + # 2つの線が交差するかで判定する + return tools.is_intersect( + p1=ball.pos, + p2=self.ball_stop_position(ball), + q1=self.field_points.their_goal_top, + q2=self.field_points.their_goal_bottom, + ) diff --git a/consai_game/consai_game/world_model/perception/perception.py b/consai_game/consai_game/world_model/perception/perception.py new file mode 100644 index 00000000..5292c7be --- /dev/null +++ b/consai_game/consai_game/world_model/perception/perception.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +# coding: UTF-8 + +# Copyright 2025 Roots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""予測を統合したPerceptionの定義モジュール.""" + +from dataclasses import dataclass + +from consai_game.world_model.perception.ball_decision import BallDecision +from consai_game.world_model.perception.ball_prediction import BallPrediction +from consai_game.world_model.perception.robot_decision import RobotDecision + + +@dataclass +class Perception: + """予測に関する関数やクラスを統合的に保持するデータクラス.""" + + ball_decision: BallDecision = BallDecision() + ball_prediction: BallPrediction = BallPrediction() + robot_decision: RobotDecision = RobotDecision() diff --git a/consai_game/consai_game/world_model/perception/robot_decision.py b/consai_game/consai_game/world_model/perception/robot_decision.py new file mode 100644 index 00000000..2a72f179 --- /dev/null +++ b/consai_game/consai_game/world_model/perception/robot_decision.py @@ -0,0 +1,218 @@ +import numpy as np + +from consai_game.utils.geometry import Point +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.robots_model import Robot + +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tools + +from consai_msgs.msg import MotionCommand + +from dataclasses import dataclass, field + + +@dataclass +class ReceiveScore: + """ボールをどれだけ受け取りやすいかを保持するデータクラス.""" + + robot_id: int = 0 + intercept_time: float = float("inf") # あと何秒後にボールを受け取れるか + + +@dataclass +class OurRobotsArrived: + """自ロボットが目標位置に到達したか保持するデータクラス.""" + + robot_id: int = 0 + arrived: bool = False + + +@dataclass +class RobotInfo: + """単一のロボット情報を保持するデータクラス.""" + + # ロボットID + robot_id: int = 0 + + # 目標位置までの距離 + desired_distance: float = float("inf") + # ボールまでの距離 + ball_distance: float = float("inf") + # プレースメント位置までの距離 + placement_distance: float = float("inf") + + # 目標位置に到着しているかのフラグ + arrived: bool = False + + +@dataclass +class RobotsInfo: + """自ロボットの情報を保持するデータクラス.""" + + robots: dict[int, RobotInfo] = field(default_factory=dict) + + def clear(self): + """全ロボット情報を初期化して空にするメソッド.""" + self.robots.clear() + + def visible_ids(self) -> list[int]: + """可視ロボットのIDリストを返すメソッド.""" + return list(self.robots.keys()) + + def arrived_ids(self) -> list[int]: + """目標位置に到達したロボットのIDリストを返すメソッド.""" + return [r.robot_id for r in self.robots.values() if r.arrived] + + def all_arrived(self) -> bool: + """全ロボットが目標位置に到達しているかを返すメソッド.""" + return all(r.arrived for r in self.robots.values()) + + def get(self, robot_id: int) -> RobotInfo: + """指定したロボットIDのRobotInfoを返す。存在しない場合はKeyErrorメソッド.""" + return self.robots[robot_id] + + def __getitem__(self, robot_id: int) -> RobotInfo: + """辞書のようにロボットIDでRobotInfoへアクセスできるようにするメソッド.""" + return self.robots[robot_id] + + def __setitem__(self, robot_id: int, value: RobotInfo): + """辞書のようにロボットIDでRobotInfoを設定できるようにするメソッド.""" + self.robots[robot_id] = value + + def __contains__(self, robot_id: int) -> bool: + """ロボットIDが含まれているか判定するメソッド.""" + return robot_id in self.robots + + def __len__(self): + """可視ロボット数を返すメソッド.""" + return len(self.robots) + + def keys(self): + """可視ロボットのID一覧を返すメソッド.""" + return self.robots.keys() + + def values(self): + """可視ロボットのRobotInfo一覧を返すメソッド.""" + return self.robots.values() + + def items(self): + """可視ロボットの(ID, RobotInfo)タプル一覧を返すメソッド.""" + return self.robots.items() + + +class RobotDecision: + """ロボットやボールの位置関係を判定するクラス.""" + + ANGLE_BALL_TO_ROBOT_THRESHOLD = 120 # ボールが後方に居るとみなす角度[degree] + MINIMAL_THETA_THRESHOLD = 45 # 最低限満たすべきロボットの角度 + WIDTH_THRESHOLD = 0.03 # 直線に乗っているかの距離 + DIST_ROBOT_TO_DESIRED_THRESHOLD = 0.1 # ロボットが目標位置に到着したと判定する距離[m] + + def obstacle_exists(ball: BallModel, robots: dict[int, Robot], target: State2D, tolerance) -> bool: + """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" + + for robot in robots.values(): + if tools.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): + return True + return False + + def is_robot_inside_pass_area(ball: BallModel, robot: Robot) -> bool: + """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + + _half_width = 4.5 + + if robot.pos.x < 0.0: + return False + + upper_side_slope, upper_side_intercept, flag = tools.get_line_parameter(ball.pos, Point(0.0, _half_width)) + lower_side_slope, lower_side_intercept, flag = tools.get_line_parameter(ball.pos, Point(0.0, _half_width)) + + if upper_side_slope is None or lower_side_slope is None: + if ball.pos.x > robot.pos.x: + return False + else: + upper_y_on_line = upper_side_intercept + upper_side_slope * robot.pos.x + lower_y_on_line = lower_side_intercept + lower_side_slope * robot.pos.x + if robot.pos.y < upper_y_on_line and robot.pos.y < lower_y_on_line: + return False + return True + + def is_robot_on_kick_line( + robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float + ) -> bool: + """ボールからターゲットまでの直線上にロボットが居るかを判定するメソッド. + + ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. + """ + + minimal_theta_threshold = 45 # 最低限満たすべきロボットの角度 + + # ボールからターゲットへの座標系を作成 + trans = tools.Trans(ball_pos, tools.get_angle(ball_pos, target_pos)) + tr_robot_pos = trans.transform(robot_pos) + tr_robot_theta = trans.transform_angle(robot_pos.theta) + + # ボールより前にロボットが居る場合 + if tr_robot_pos.x > 0.0: + return False + + # ターゲットを向いていない + if abs(tr_robot_theta) > np.deg2rad(minimal_theta_threshold): + return False + + if abs(tr_robot_pos.y) > width_threshold: + return False + + return True + + def update_our_robots_arrived( + self, robots: dict[int, Robot], commands: list[MotionCommand], our_visible_robots: RobotInfo + ) -> bool: + """各ロボットが目標位置に到達したかをRobotInfoにセット""" + for command in commands: + if command.robot_id not in robots: + continue + robot = robots[command.robot_id] + dist = tools.get_distance(robot.pos, command.desired_pose) + if command.robot_id in our_visible_robots: + our_visible_robots[command.robot_id].arrived = dist < self.DIST_ROBOT_TO_DESIRED_THRESHOLD + + # ball_approach.py + def robot_is_backside(self, robot_pos: State2D, ball_pos: State2D, ball_stop_pos: State2D) -> bool: + """ボールからターゲットを見て、ロボットが後側に居るかを判定する.""" + # ボール最終目標地点からボールへの座標系を作成 + trans = tools.Trans(ball_stop_pos, tools.get_angle(ball_stop_pos, ball_pos)) + tr_robot_pos = trans.transform(robot_pos) + + # ボールから見たロボットの位置の角度 + # ボールの後方にいれば角度は90度以上 + tr_ball_to_robot_angle = tools.get_angle(State2D(x=0.0, y=0.0), tr_robot_pos) + + if abs(tr_ball_to_robot_angle) > np.deg2rad(self.ANGLE_BALL_TO_ROBOT_THRESHOLD): + return True + return False + + def robot_is_on_receiving_line(self, robot_pos: State2D, ball_pos: State2D, ball_stop_pos: State2D) -> bool: + """ボールからターゲットまでの直線上にロボットが居るかを判定する. + + ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. + """ + + # ボールからターゲットへの座標系を作成 + trans = tools.Trans(ball_pos, tools.get_angle(ball_stop_pos, ball_pos)) + tr_robot_pos = trans.transform(robot_pos) + + # ボールより前にロボットが居る場合 + if tr_robot_pos.x > 0.0: + return False + + # ターゲットを向いていない + if abs(tr_robot_pos.theta) > np.deg2rad(self.MINIMAL_THETA_THRESHOLD): + return False + + if abs(tr_robot_pos.y) > self.WIDTH_THRESHOLD: + return False + + return True diff --git a/consai_game/consai_game/world_model/world_model.py b/consai_game/consai_game/world_model/world_model.py index d98519f7..4ac92c97 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -22,6 +22,7 @@ from consai_game.world_model.ball_activity_model import BallActivityModel from consai_game.world_model.ball_model import BallModel from consai_game.world_model.ball_position_model import BallPositionModel +from consai_game.world_model.evaluation.evaluation import Evaluation from consai_game.world_model.field_model import Field, FieldPoints from consai_game.world_model.referee_model import RefereeModel from consai_game.world_model.robot_activity_model import RobotActivityModel @@ -48,3 +49,5 @@ class WorldModel: game_config: GameConfigModel = GameConfigModel() threats: ThreatsModel = ThreatsModel(field, field_points) meta: WorldMetaModel = WorldMetaModel() + + evaluation: Evaluation = Evaluation(field, field_points) diff --git a/consai_game/consai_game/world_model/world_model_provider_node.py b/consai_game/consai_game/world_model/world_model_provider_node.py index 4ac11ad8..85a04442 100644 --- a/consai_game/consai_game/world_model/world_model_provider_node.py +++ b/consai_game/consai_game/world_model/world_model_provider_node.py @@ -148,12 +148,12 @@ def update(self) -> None: field_points=self.world_model.field_points, ) # 最適なシュートターゲットを更新 - self.world_model.kick_target.update( + self.world_model.evaluation.kick_target.update( self.world_model.ball, self.world_model.robots, ) # 敵ロボットの驚異度を更新 - self.world_model.threats.update( + self.world_model.evaluation.threats_evaluation.update( ball=self.world_model.ball, robots=self.world_model.robots, ) @@ -233,7 +233,7 @@ def update_field_model(self) -> None: self.world_model.field.half_penalty_width = self.world_model.field.penalty_width / 2 self.world_model.field_points = self.world_model.field_points.create_field_points(self.world_model.field) - self.world_model.kick_target.update_field_pos_list(self.world_model.field) + self.world_model.evaluation.kick_target.update_field_pos_list(self.world_model.field) def update_game_config(self) -> None: """self.msg_param_control、self.msg_param_strategyを元にゲーム設定を更新する.""" diff --git a/setup.cfg b/setup.cfg index cf3096d5..e3c1f229 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,3 +1,3 @@ [flake8] max-line-length = 120 -ignore = D100, D400, D104, D202, D403, I100, Q000, W503 \ No newline at end of file +ignore = A003, D100, D400, D104, D202, D403, I100, Q000, W503 \ No newline at end of file