From 6dd533f788bddc609a710d21e140113ebe494ec3 Mon Sep 17 00:00:00 2001 From: agich073 Date: Tue, 12 Aug 2025 18:01:13 +0900 Subject: [PATCH 01/13] =?UTF-8?q?world=5Fmodel=E3=81=A8tactic=E3=81=8B?= =?UTF-8?q?=E3=82=89=E3=81=AE=E8=A9=95=E4=BE=A1=E9=96=A2=E6=95=B0=E3=81=AE?= =?UTF-8?q?=E6=8A=9C=E3=81=8D=E5=87=BA=E3=81=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../consai_game/evaluation/__init__.py | 0 .../consai_game/evaluation/ball_evaluation.py | 268 ++++++++++++++++ .../consai_game/evaluation/evaluation.py | 31 ++ .../evaluation/evaluation_meta_data.py | 21 ++ .../evaluation/evaluation_provider_node.py | 102 ++++++ .../evaluation/kick_target_evaluation.py | 286 +++++++++++++++++ .../evaluation/robot_evaluation.py | 295 ++++++++++++++++++ consai_game/consai_game/main.py | 5 + .../consai_game/perception/__init__.py | 0 .../consai_game/perception/ball_perception.py | 77 +++++ .../perception/robot_perception.py | 0 .../tactic/composite/composite_defense.py | 16 +- .../tactic/composite/composite_offense.py | 22 +- consai_game/consai_game/tactic/dribble.py | 38 +-- consai_game/consai_game/tactic/kick.py | 71 +++-- .../visualize_msg_publisher_node.py | 11 +- .../world_model/ball_activity_model.py | 84 ++--- .../world_model/kick_target_model.py | 49 +-- .../consai_game/world_model/world_model.py | 4 +- .../world_model/world_model_provider_node.py | 10 +- 20 files changed, 1250 insertions(+), 140 deletions(-) create mode 100644 consai_game/consai_game/evaluation/__init__.py create mode 100644 consai_game/consai_game/evaluation/ball_evaluation.py create mode 100644 consai_game/consai_game/evaluation/evaluation.py create mode 100644 consai_game/consai_game/evaluation/evaluation_meta_data.py create mode 100644 consai_game/consai_game/evaluation/evaluation_provider_node.py create mode 100644 consai_game/consai_game/evaluation/kick_target_evaluation.py create mode 100644 consai_game/consai_game/evaluation/robot_evaluation.py create mode 100644 consai_game/consai_game/perception/__init__.py create mode 100644 consai_game/consai_game/perception/ball_perception.py create mode 100644 consai_game/consai_game/perception/robot_perception.py diff --git a/consai_game/consai_game/evaluation/__init__.py b/consai_game/consai_game/evaluation/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/consai_game/consai_game/evaluation/ball_evaluation.py b/consai_game/consai_game/evaluation/ball_evaluation.py new file mode 100644 index 00000000..d8842f00 --- /dev/null +++ b/consai_game/consai_game/evaluation/ball_evaluation.py @@ -0,0 +1,268 @@ +#!/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. + +""" +ボールの位置情報を管理するためのクラスを提供する. + +ボールの位置更新, フィールド境界の判定（ヒステリシス付き）, および各エリア内かどうかの判定をする. +""" + +import math + +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.field_model import Field, FieldPoints + +from consai_msgs.msg import State2D + +# ball_position.py +class BallPositionModel: + """ボールの位置情報を管理するクラス.""" + + def __init__(self, field: Field, field_points: FieldPoints): + """インスタンス生成時の初期化.""" + self._pos = State2D() + self._field = field + self._field_points = field_points + self._outside_margin = 0.05 # フィールド外判定のマージン(m) + self._hysteresis = 0.02 # ヒステリシスの閾値(m) + self._last_left_state = False # 前回の左側判定状態 + self._last_right_state = False # 前回の右側判定状態 + self._last_top_state = False # 前回の上側判定状態 + self._last_bottom_state = False # 前回の下側判定状態 + self._last_our_defense_state = False # 前回の自チームディフェンスエリア判定状態 + self._last_their_defense_state = False # 前回の相手チームディフェンスエリア判定状態 + self._last_our_side_state = False # 前回の自チームサイド判定状態 + self._last_their_side_state = False # 前回の相手チームサイド判定状態 + + def update_position(self, ball_model: BallModel, field_model: Field, field_points: FieldPoints) -> None: + """ボールの位置を更新する.""" + self._pos = ball_model.pos + self._field = field_model + self._field_points = field_points + + def is_outside_of_left(self) -> bool: + """ボールが左側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x < -self._field.half_length + if current_pos_state != self._last_left_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.x < -self._field.half_length - self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.x < -self._field.half_length + self._hysteresis + self._last_left_state = current_pos_state + return current_pos_state + + def is_outside_of_right(self) -> bool: + """ボールが右側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x > self._field.half_length + if current_pos_state != self._last_right_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.x > self._field.half_length + self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.x > self._field.half_length - self._hysteresis + self._last_right_state = current_pos_state + return current_pos_state + + def is_outside_of_top(self) -> bool: + """ボールが上側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.y > self._field.half_width + if current_pos_state != self._last_top_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.y > self._field.half_width + self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.y > self._field.half_width - self._hysteresis + self._last_top_state = current_pos_state + return current_pos_state + + def is_outside_of_bottom(self) -> bool: + """ボールが下側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.y < -self._field.half_width + if current_pos_state != self._last_bottom_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.y < -self._field.half_width - self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.y < -self._field.half_width + self._hysteresis + self._last_bottom_state = current_pos_state + return current_pos_state + + def is_outside(self) -> bool: + """ボールがフィールド外にあるか判定する.""" + return ( + self.is_outside_of_left() + or self.is_outside_of_right() + or self.is_outside_of_top() + or self.is_outside_of_bottom() + ) + + def is_outside_of_left_with_margin(self) -> bool: + """マージンを考慮して、ボールが左側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin + if current_pos_state != self._last_left_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin - self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin + self._hysteresis + self._last_left_state = current_pos_state + return current_pos_state + + def is_outside_of_right_with_margin(self) -> bool: + """マージンを考慮して、ボールが右側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x > self._field.half_length + self._outside_margin + if current_pos_state != self._last_right_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.x > self._field.half_length + self._outside_margin + self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.x > self._field.half_length + self._outside_margin - self._hysteresis + self._last_right_state = current_pos_state + return current_pos_state + + def is_outside_of_top_with_margin(self) -> bool: + """マージンを考慮して、ボールが上側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.y > self._field.half_width + self._outside_margin + if current_pos_state != self._last_top_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.y > self._field.half_width + self._outside_margin + self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.y > self._field.half_width + self._outside_margin - self._hysteresis + self._last_top_state = current_pos_state + return current_pos_state + + def is_outside_of_bottom_with_margin(self) -> bool: + """マージンを考慮して、ボールが下側のフィールド外にあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin + if current_pos_state != self._last_bottom_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 外に出た + current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin - self._hysteresis + else: + # 内に入った + current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin + self._hysteresis + self._last_bottom_state = current_pos_state + return current_pos_state + + def is_outside_with_margin(self) -> bool: + """マージンを考慮して、ボールがフィールド外にあるか判定する.""" + return ( + self.is_outside_of_left_with_margin() + or self.is_outside_of_right_with_margin() + or self.is_outside_of_top_with_margin() + or self.is_outside_of_bottom_with_margin() + ) + + def is_in_our_defense_area(self) -> bool: + """ボールが自チームのディフェンスエリア内にあるか判定する.ヒステリシス付き.""" + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width + and self._pos.x < -self._field.half_length + self._field.penalty_depth + ) + if current_pos_state != self._last_our_defense_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 内に入った + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width - self._hysteresis + and self._pos.x < -self._field.half_length + self._field.penalty_depth - self._hysteresis + ) + else: + # 外に出た + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width + self._hysteresis + and self._pos.x < -self._field.half_length + self._field.penalty_depth + self._hysteresis + ) + self._last_our_defense_state = current_pos_state + return current_pos_state + + def is_in_their_defense_area(self) -> bool: + """ボールが相手チームのディフェンスエリア内にあるか判定する.ヒステリシス付き.""" + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width + and self._pos.x > self._field.half_length - self._field.penalty_depth + ) + if current_pos_state != self._last_their_defense_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 内に入った + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width - self._hysteresis + and self._pos.x > self._field.half_length - self._field.penalty_depth + self._hysteresis + ) + else: + # 外に出た + current_pos_state = ( + math.fabs(self._pos.y) < self._field.half_penalty_width + self._hysteresis + and self._pos.x > self._field.half_length - self._field.penalty_depth - self._hysteresis + ) + self._last_their_defense_state = current_pos_state + return current_pos_state + + def is_in_our_side(self) -> bool: + """ボールが自チームのサイドにあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x < 0 + if current_pos_state != self._last_our_side_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 自チームサイドに入った + current_pos_state = self._pos.x < -self._hysteresis + else: + # 自チームサイドから出た + current_pos_state = self._pos.x < self._hysteresis + self._last_our_side_state = current_pos_state + + # 相手チームサイドの判定と競合する場合は、相手チームサイドを優先 + if self._last_their_side_state: + return False + return current_pos_state + + def is_in_their_side(self) -> bool: + """ボールが相手チームのサイドにあるか判定する.ヒステリシス付き.""" + current_pos_state = self._pos.x > 0 + if current_pos_state != self._last_their_side_state: + # 状態が変化する場合、ヒステリシスを考慮 + if current_pos_state: + # 相手チームサイドに入った + current_pos_state = self._pos.x > self._hysteresis + else: + # 相手チームサイドから出た + current_pos_state = self._pos.x > -self._hysteresis + self._last_their_side_state = current_pos_state + + # 自チームサイドの判定と競合する場合は、相手チームサイドを優先 + if self._last_our_side_state: + return False + return current_pos_state + diff --git a/consai_game/consai_game/evaluation/evaluation.py b/consai_game/consai_game/evaluation/evaluation.py new file mode 100644 index 00000000..4e2d0f36 --- /dev/null +++ b/consai_game/consai_game/evaluation/evaluation.py @@ -0,0 +1,31 @@ +#!/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.evaluation.kick_target_evaluation import KickTargetEvaluation +from consai_game.evaluation.evaluation_meta_data import EvaluationMetaData + + +@dataclass +class Evaluation: + """試合全体の状態を統合的に保持するデータクラス.""" + + kick_target: KickTargetEvaluation = KickTargetEvaluation() + meta: EvaluationMetaData = EvaluationMetaData() diff --git a/consai_game/consai_game/evaluation/evaluation_meta_data.py b/consai_game/consai_game/evaluation/evaluation_meta_data.py new file mode 100644 index 00000000..161325a3 --- /dev/null +++ b/consai_game/consai_game/evaluation/evaluation_meta_data.py @@ -0,0 +1,21 @@ +# 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 dataclasses import dataclass + + +@dataclass +class EvaluationMetaData: + update_rate: float = 0.0 # 更新周期 Hz + update_counter: int = 0 # 更新カウンタ diff --git a/consai_game/consai_game/evaluation/evaluation_provider_node.py b/consai_game/consai_game/evaluation/evaluation_provider_node.py new file mode 100644 index 00000000..01472fb9 --- /dev/null +++ b/consai_game/consai_game/evaluation/evaluation_provider_node.py @@ -0,0 +1,102 @@ +#!/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. + +""" +WorldModelProviderNode モジュール. + +このモジュールは ROS2 ノードとして動作する. +Referee メッセージや TrackedFrame を受け取り, ワールドモデルをリアルタイムに更新する. +""" +import copy +import json +import threading +from typing import Optional + +from consai_game.utils.process_info import process_info +from consai_game.evaluation.evaluation import Evaluation +from consai_game.world_model.world_model import WorldModel + +from rclpy.node import Node + + +class EvaluationProviderNode(Node): + """ + """ + + def __init__(self, update_hz: float = 10): + """ + 初期化. + + EvaluationProviderNode を初期化する. + """ + super().__init__("evaluation_provider_node") + self.lock = threading.Lock() + + self.timer = self.create_timer(1.0 / update_hz, self.update) + + self.world_model = WorldModel() + self.evaluation = Evaluation() + self.evaluation.meta.update_rate = update_hz + + # subscribeするトピック + # self.msg_param_rule: Optional[String] = None + # self.msg_param_control: Optional[String] = None + # self.msg_param_strategy: Optional[String] = None + # self.msg_motion_commands = MotionCommandArray() + + # consai_referee_parserのための補助情報 + # self.pub_referee_info = self.create_publisher(RefereeSupportInfo, "parsed_referee/referee_support_info", 10) + + # self.sub_referee = self.create_subscription(Referee, "referee", self.callback_referee, 10) + # self.sub_detection_traced = self.create_subscription( + # TrackedFrame, "detection_tracked", self.callback_detection_traced, 10 + # ) + + # qos_profile = QoSProfile( + # depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE + # ) + + # self.sub_param_rule = self.create_subscription( + # String, "consai_param/rule", self.callback_param_rule, qos_profile + # ) + # self.sub_param_control = self.create_subscription( + # String, "consai_param/control", self.callback_param_control, qos_profile + # ) + # self.sub_param_strategy = self.create_subscription( + # String, "consai_param/strategy", self.callback_param_strategy, qos_profile + # ) + # # motion_commandのsubscriber + # self._sub_motion_command = self.create_subscription( + # MotionCommandArray, "motion_commands", self.callback_motion_commands, 10 + # ) + + def update(self) -> None: + """ + タイマーにより定期的に呼び出され、WorldModelの状態を更新する. + + ロボットのアクティビティやボール位置を再計算する. + """ + with self.lock: + self.get_logger().debug(f"EvaluationProvider update, {process_info()}") + # メタ情報を更新 + self.evaluation.meta.update_counter += 1 + + # 最適なシュートターゲットを更新 + self.evaluation.kick_target.update( + self.world_model.ball, + self.world_model.robots, + ) diff --git a/consai_game/consai_game/evaluation/kick_target_evaluation.py b/consai_game/consai_game/evaluation/kick_target_evaluation.py new file mode 100644 index 00000000..bcd4f259 --- /dev/null +++ b/consai_game/consai_game/evaluation/kick_target_evaluation.py @@ -0,0 +1,286 @@ +# 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 operator import attrgetter + +from dataclasses import dataclass, field + + +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tool + +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 consai_game.evaluation.robot_evaluation import _is_robot_inside_pass_area, _obstacle_exists + +from copy import deepcopy + + +@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: + """キックターゲットを保持するクラス.""" + + def __init__(self): + """KickTargetModelの初期化関数.""" + self.hysteresis_distance = 0.3 + + # shoot_targetの位置と成功率を保持するリスト + self.shoot_target_list: list[ShootTarget] = [] + self._goal_pos_list = [ShootTarget()] + + # pass_targetの位置と成功率を保持するリスト + self.pass_target_list: list[PassTarget] = [] + + self._half_width = 4.5 + + self.update_field_pos_list(Field()) + + 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 _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: + """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" + for robot in robots.values(): + if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): + return True + return False + + def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: + """各シュートターゲットの成功率を計算し, リストを更新する関数.""" + TOLERANCE = robots.robot_radius # ロボット半径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 60 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 20 # スコア計算時のシュートターゲットの最大角度スコア + MAX_GOALIE_LEAVE_SCORE = 20 # スコア計算時のシュートターゲットがgoalieからどれくらい離れているかの最大スコア + + # 相手の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 ( + self._obstacle_exists( + target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + ) + and search_ours + ): + target.success_rate = score + elif self._obstacle_exists( + target=target.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE + ): + target.success_rate = score + else: + # ボールからの角度（目標方向がゴール方向と合っているか） + angle = abs(tool.get_angle(ball.pos, target.pos)) + score += max( + 0, MAX_ANGLE_SCORE - np.rad2deg(angle) * MAX_ANGLE_SCORE / 60 + ) # 小さい角度（正面）ほど高得点とし、60度以上角度がついていれば0点 + + # 距離（近いほうが成功率が高そう） + distance = tool.get_distance(ball.pos, target.pos) + score += max( + 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 6 + ) # ディフェンスエリア外から6m以内ならOK + + if goalie_pos is None: + score += 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) + * 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: + """ボールからの直線上にロボットがいないシュート位置を返す関数.""" + RATE_MARGIN = 50 # ヒステリシスのためのマージン + 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 + RATE_MARGIN: + return self.shoot_target_list[0] + return last_shoot_target_list[0] + + def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: + """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + if robot.pos.x < 0.0: + return False + + upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) + lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: + """各パスターゲットの成功率を計算し, リストを返す関数.""" + TOLERANCE = robots.robot_radius * 2 # ロボット直径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 55 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 45 # スコア計算時のシュートターゲットの最大角度スコア + + pass_target_list: list[PassTarget] = [] + + for robot in robots.our_visible_robots.values(): + score = 0 + if ( + _obstacle_exists( + target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + ) + and search_ours + ): + score = 0 + elif _obstacle_exists( + target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE + ): + score = 0 + elif tool.get_distance(ball.pos, robot.pos) < 0.5: + score = 0 + elif _is_robot_inside_pass_area(ball, robot, Field.half_width) is False: + score = 0 + else: + # ボールとパスを受けるロボットの距離 + distance = tool.get_distance(ball.pos, robot.pos) + score += max( + 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 4 + ) # ディフェンスエリア外から4m以内ならOK + # ボールからの角度（目標方向がロボット方向と合っているか） + angle = abs(tool.get_angle(ball.pos, robot.pos)) + score += max(0, 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 - 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/evaluation/robot_evaluation.py b/consai_game/consai_game/evaluation/robot_evaluation.py new file mode 100644 index 00000000..89cd9558 --- /dev/null +++ b/consai_game/consai_game/evaluation/robot_evaluation.py @@ -0,0 +1,295 @@ +# 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 + +from consai_game.world_model.world_model import WorldModel + +from consai_game.utils.generate_dummy_ball_position import generate_dummy_ball_position + +from consai_game.world_model.robots_model import Robot, RobotsModel +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.field_model import Field, FieldPoints +from dataclasses import dataclass +from typing import List, Dict + +from consai_game.utils.geometry import Point + +from consai_game.world_model.ball_activity_model import BallActivityModel +from consai_game.world_model.game_config_model import GameConfigModel + + +# kick.py +def robot_is_backside(robot_id: int, world_model: WorldModel, target_pos: State2D, angle_ball_to_robot_threshold: int) -> bool: + """ボールからターゲットを見て、ロボットが後側に居るかを判定する.""" + + robot_pos = world_model.robots.our_robots.get(robot_id).pos + + # ボールが消えることを想定して、仮想的なボール位置を生成する + ball_pos = generate_dummy_ball_position(ball=world_model.ball, robot_pos=robot_pos) + + # ボールからターゲットへの座標系を作成 + 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 + +# kick.py +def robot_is_on_kick_line( + robot_id: int, world_model: WorldModel, target_pos: State2D, width_threshold: float +) -> bool: + """ボールからターゲットまでの直線上にロボットが居るかを判定する. + + ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. + """ + MINIMAL_THETA_THRESHOLD = 45 # 最低限満たすべきロボットの角度 + + robot_pos = world_model.robots.our_robots.get(robot_id).pos + + # ボールが消えることを想定して、仮想的なボール位置を生成する + ball_pos = generate_dummy_ball_position(ball=world_model.ball, robot_pos=robot_pos) + + # ボールからターゲットへの座標系を作成 + 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 + + +# dribble.py +def ball_is_front(ball_pos: State2D, robot_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 + + +# threats_model.py +@dataclass +class Threat: + score: int # 0以上 + robot_id: int # 相手のロボットID + +class ThreatsModel: + def __init__(self, field: Field, field_points: FieldPoints): + self.threats: List[Threat] = [] + self._field = field + self._field_points = field_points + self._prev_scores: Dict[int, float] = {} # ロボットIDごとの前回のスコア + self._alpha = 0.1 # ローパスフィルターの係数（0-1、小さいほど変化が遅い） + + 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) + + +# kick_target_model.py +def _obstacle_exists(target: State2D, ball: BallModel, robots: dict[int, Robot], 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, _half_width: Field) -> bool: + """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + + 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 + + +# robot_activity_model.py +"""未完了.""" +@dataclass +class ReceiveScore: + """ボールをどれだけ受け取りやすいかを保持するデータクラス.""" + + robot_id: int = 0 + intercept_time: float = float("inf") # あと何秒後にボールを受け取れるか + +def calc_ball_receive_score_list( + robots: dict[int, Robot], ball: BallModel, ball_activity: BallActivityModel, game_config: GameConfigModel +) -> list[ReceiveScore]: + """ロボットごとにボールを受け取れるスコアを計算する.""" + + # ボールが動いていない場合は、スコアをデフォルト値にする + if not ball_activity.ball_is_moving: + return [ReceiveScore(robot_id=robot.robot_id) for robot in robots.values()] + + score_list = [] + for robot in robots.values(): + score_list.append( + ReceiveScore( + robot_id=robot.robot_id, + intercept_time=calc_intercept_time(robot, ball, game_config), + ) + ) + + # intercept_timeが小さい順にソート + score_list.sort(key=lambda x: x.intercept_time) + return score_list + +def calc_intercept_time(robot: Robot, ball: BallModel, game_config: GameConfigModel) -> float: + """ロボットがボールを受け取るまでの時間を計算する関数.""" + + # ボールを中心に、ボールの速度方向を+x軸にした座標系を作る + trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) + + # ロボットの位置を変換 + tr_robot_pos = trans.transform(robot.pos) + + # TODO: ボールを後ろから追いかけて受け取れるようになったら、計算を変更する + if tr_robot_pos.x < 0: + return float("inf") + + # ロボットからボール軌道まで垂線を引き、 + # その交点にボールが到達するまでの時間を計算する + ball_arrival_distance = tr_robot_pos.x + intercept_time = ball_arrival_distance / tools.get_norm(ball.vel) + + # ボールが到達するまでの時間で、ロボットがどれだけ移動できるかを計算する + # TODO: ロボットの現在速度、加速度を考慮すべき + available_distance = intercept_time * game_config.robot_max_linear_vel + + # ボール軌道からロボットまでの距離 + robot_arrival_distance = abs(tr_robot_pos.y) + + # ボールが到着するまでにロボットが移動できれば、intercept_timeを返す + if available_distance >= robot_arrival_distance: + return intercept_time + return float("inf") diff --git a/consai_game/consai_game/main.py b/consai_game/consai_game/main.py index cfda420c..83455d06 100755 --- a/consai_game/consai_game/main.py +++ b/consai_game/consai_game/main.py @@ -31,6 +31,7 @@ from consai_game.core.play.play_node import PlayNode from consai_game.core.tactic.agent_scheduler_node import AgentSchedulerNode from consai_game.visualization.visualize_msg_publisher_node import VisualizeMsgPublisherNode +from consai_game.evaluation.evaluation_provider_node import EvaluationProviderNode from consai_game.world_model.world_model_provider_node import WorldModelProviderNode @@ -69,6 +70,9 @@ def main(): goalie_id=args.goalie, invert=args.invert, ) + evaluation_provider_node = EvaluationProviderNode( + update_hz=UPDATE_HZ, + ) # TODO: agent_numをplay_nodeから取得したい agent_scheduler_node = AgentSchedulerNode(update_hz=UPDATE_HZ, team_is_yellow=team_is_yellow, agent_num=11) play_node.set_update_role_callback(agent_scheduler_node.set_roles) @@ -79,6 +83,7 @@ def main(): executor = MultiThreadedExecutor() executor.add_node(play_node) executor.add_node(world_model_provider_node) + executor.add_node(evaluation_provider_node) executor.add_node(agent_scheduler_node) executor.add_node(vis_msg_publisher_node) diff --git a/consai_game/consai_game/perception/__init__.py b/consai_game/consai_game/perception/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/consai_game/consai_game/perception/ball_perception.py b/consai_game/consai_game/perception/ball_perception.py new file mode 100644 index 00000000..fa940b86 --- /dev/null +++ b/consai_game/consai_game/perception/ball_perception.py @@ -0,0 +1,77 @@ +from copy import deepcopy +from dataclasses import dataclass +from enum import Enum, auto +from typing import Optional + +from consai_tools.geometry import geometry_tools as tools + +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.robots_model import Robot, RobotsModel +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_msgs.msg import State2D + + +# ball_activity_model.py +def prediction_next_ball_pos(ball: BallModel): + """ + 次のボールの位置を予測するメソッド + + 暫定的に0.1[m]移動すると仮定 + """ + + # 速度に対するボール移動量を算出する比率[s]: 実質的に移動時間 + MOVEMENT_GAIN = 0.1 + # ボールの移動量 + ball_movement = State2D() + # ボールの将来の予測位置 + next_ball_pos = State2D() + # 将来の位置 + _future_ball_pos = State2D() + _future_ball_pos.x = ball.pos.x + ball.vel.x + _future_ball_pos.y = ball.pos.y + ball.vel.y + # ボール移動量 + ball_movement.x = ball.vel.x * MOVEMENT_GAIN # * np.cos(self.angle_trajectory) + ball_movement.y = ball.vel.y * MOVEMENT_GAIN # * np.sin(self.angle_trajectory) + + # 予測位置を算出 + next_ball_pos.x = ball.pos.x + ball_movement.x + next_ball_pos.y = ball.pos.y + ball_movement.y + +def predict_ball_stop_position(ball: BallModel, game_config: GameConfigModel) -> State2D: + """ボールが止まる位置を予測するメソッド.""" + + ball_is_moving = False + + # ボールの速度が小さい場合は、現在の位置を返す + if not 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(ball: BallModel, field_points: FieldPoints) -> bool: + """ボールが相手のゴールに入るかを判定するメソッド.""" + + ball_is_moving = False + + # ボールが最終的に止まる予測位置 + ball_stop_position = State2D() + # ボールが動いていない場合は、Falseを返す + if not ball_is_moving: + return False + + # 2つの線が交差するかで判定する + return tools.is_intersect( + p1=ball.pos, p2=ball_stop_position, q1=field_points.their_goal_top, q2=field_points.their_goal_bottom + ) \ No newline at end of file diff --git a/consai_game/consai_game/perception/robot_perception.py b/consai_game/consai_game/perception/robot_perception.py new file mode 100644 index 00000000..e69de29b diff --git a/consai_game/consai_game/tactic/composite/composite_defense.py b/consai_game/consai_game/tactic/composite/composite_defense.py index 39bcea90..e9b07c5e 100644 --- a/consai_game/consai_game/tactic/composite/composite_defense.py +++ b/consai_game/consai_game/tactic/composite/composite_defense.py @@ -25,6 +25,8 @@ from consai_msgs.msg import MotionCommand +from consai_game.evaluation.evaluation import Evaluation + class CompositeDefense(CompositeTacticBase): def __init__(self, tactic_default: TacticBase, do_receive: bool = True): @@ -39,6 +41,8 @@ def __init__(self, tactic_default: TacticBase, do_receive: bool = True): self.very_close_to_ball_threshold = 0.3 self.do_receive = do_receive + self.evaluation: Evaluation = Evaluation() + def run(self, world_model: WorldModel) -> MotionCommand: """状況に応じて実行するtacticを切り替えてrunする.""" @@ -69,16 +73,18 @@ 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: + evaluation = self.evaluation + + if 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 = 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 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 = 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 = 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..e8121409 100644 --- a/consai_game/consai_game/tactic/composite/composite_offense.py +++ b/consai_game/consai_game/tactic/composite/composite_offense.py @@ -20,9 +20,13 @@ 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.field_model import Field + +from consai_game.evaluation.evaluation import Evaluation +from consai_game.evaluation.evaluation_provider_node import EvaluationProviderNode +from consai_game.world_model.world_model import WorldModel from consai_msgs.msg import MotionCommand from consai_msgs.msg import State2D @@ -128,6 +132,10 @@ def __init__(self, tactic_default: TacticBase, is_setplay=False, force_pass=Fals self.kick_score_threshold = kick_score_threshold self.SHOOTING_MARGIN = 0 + self.evaluation: Evaluation = Evaluation() + # self.field: Field = Field() + # self.evaluation_provider_node: EvaluationProviderNode = EvaluationProviderNode() + # 最初の動作を強制的にpassにするかの設定 self.force_pass = force_pass @@ -163,24 +171,26 @@ def run(self, world_model: WorldModel) -> MotionCommand: def control_the_ball(self, world_model: WorldModel) -> MotionCommand: """ボールを制御するためのTacticを実行する関数.""" + evaluation = self.evaluation + # 相手がボールを持ってる場合は奪いに行く if world_model.ball_activity.is_their_team_ball_holder: # ボールを奪う 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 + 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 = 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 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(evaluation.kick_target.best_pass_target.robot_pos) # パスターゲットの候補を探そうとしているのでシュートターゲットのマージンを0にする self.SHOOTING_MARGIN = 0 @@ -188,7 +198,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 = 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..251c1617 100644 --- a/consai_game/consai_game/tactic/dribble.py +++ b/consai_game/consai_game/tactic/dribble.py @@ -31,6 +31,8 @@ from transitions.extensions import GraphMachine +from consai_game.evaluation.robot_evaluation import ball_is_front + class DribbleStateMachine(GraphMachine): """ドリブルの状態遷移マシン.""" @@ -156,7 +158,7 @@ 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=ball_is_front(ball_pos=ball_pos, robot_pos=robot_pos, target_pos=self.target_pos), ) command = MotionCommand() @@ -178,27 +180,27 @@ 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 # 横方向にどれだけ離れることを許容するか + # 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) + # # ロボットを中心に、ターゲットを+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 < 0: + # return False - # ボールが正面から離れすぎている - if tr_ball_pos.x > FRONT_DIST_THRESHOLD: - 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 + # # ボールが横方向に離れすぎている + # if abs(tr_ball_pos.y) > SIDE_DIST_THRESHOLD: + # return False + # return True def approach_to_ball(self, command: MotionCommand, ball_pos: State2D) -> MotionCommand: """ボールに近づくコマンドを返す.""" diff --git a/consai_game/consai_game/tactic/kick.py b/consai_game/consai_game/tactic/kick.py index fa9a448d..8cb4e83d 100644 --- a/consai_game/consai_game/tactic/kick.py +++ b/consai_game/consai_game/tactic/kick.py @@ -26,6 +26,9 @@ from transitions.extensions import GraphMachine +from consai_game.evaluation.robot_evaluation import robot_is_backside, robot_is_on_kick_line +# from consai_game.evaluation.evaluation import Evaluation + class KickStateMachine(GraphMachine): """状態遷移マシン.""" @@ -136,9 +139,9 @@ 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=robot_is_backside(self.robot_id, world_model, self.final_target_pos, self.ANGLE_BALL_TO_ROBOT_THRESHOLD), + robot_is_on_kick_line=robot_is_on_kick_line( + self.robot_id, world_model, self.final_target_pos, width_threshold=width_threshold ), ) @@ -201,46 +204,46 @@ 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) + # 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) + # # ボールから見たロボットの位置の角度 + # # ボールの後方にいれば角度は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 + # 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: - """ボールからターゲットまでの直線上にロボットが居るかを判定する. + # def robot_is_on_kick_line( + # self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float + # ) -> bool: + # """ボールからターゲットまでの直線上にロボットが居るかを判定する. - ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. - """ - MINIMAL_THETA_THRESHOLD = 45 # 最低限満たすべきロボットの角度 + # ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. + # """ + # 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) + # # ボールからターゲットへの座標系を作成 + # 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 tr_robot_pos.x > 0.0: + # return False - # ターゲットを向いていない - if abs(tr_robot_theta) > np.deg2rad(MINIMAL_THETA_THRESHOLD): - return False + # # ターゲットを向いていない + # if abs(tr_robot_theta) > np.deg2rad(MINIMAL_THETA_THRESHOLD): + # return False - if abs(tr_robot_pos.y) > width_threshold: - return False + # if abs(tr_robot_pos.y) > width_threshold: + # return False - return True + # return True def move_to_backside_pose( self, ball_pos: State2D, robot_pos: State2D, target_pos: State2D, distance: float 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..b325fc7c 100644 --- a/consai_game/consai_game/visualization/visualize_msg_publisher_node.py +++ b/consai_game/consai_game/visualization/visualize_msg_publisher_node.py @@ -27,7 +27,8 @@ from consai_game.world_model.ball_model import BallModel from consai_game.world_model.ball_activity_model import BallActivityModel, BallState from consai_game.world_model.robot_activity_model import RobotActivityModel -from consai_game.world_model.kick_target_model import KickTargetModel +# from consai_game.world_model.kick_target_model import KickTargetModel +from consai_game.evaluation.kick_target_evaluation import KickTargetEvaluation from consai_game.world_model.robots_model import RobotsModel from consai_game.world_model.world_model import WorldModel @@ -43,9 +44,9 @@ 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.pub_visualizer_objects.publish( + # self.kick_target_to_vis_msg(kick_target=kick_target_evaluation, ball=world_model.ball) + # ) self.pub_visualizer_objects.publish( self.ball_activity_to_vis_msg(activity=world_model.ball_activity, ball=world_model.ball) @@ -63,7 +64,7 @@ def set_robot_tactic_status_list(self, robot_tactic_status_list: list[RobotTacti """ロボットの戦術状態をセットすする関数""" self.robot_tactic_status_list = robot_tactic_status_list - def kick_target_to_vis_msg(self, kick_target: KickTargetModel, ball: BallModel) -> Objects: + def kick_target_to_vis_msg(self, kick_target: KickTargetEvaluation, ball: BallModel) -> Objects: """kick_targetをObjectsメッセージに変換する.""" vis_obj = Objects() vis_obj.layer = "game" 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..91746f01 100644 --- a/consai_game/consai_game/world_model/ball_activity_model.py +++ b/consai_game/consai_game/world_model/ball_activity_model.py @@ -34,6 +34,8 @@ from consai_msgs.msg import State2D +from consai_game.perception.ball_perception import is_ball_will_enter_their_goal, predict_ball_stop_position, prediction_next_ball_pos + class BallState(Enum): """ボールの状態を表す列挙型.""" @@ -111,7 +113,7 @@ def update( self.ball_is_moving = self.is_ball_moving(ball) # ボールの予測位置を更新する - self.prediction_next_ball_pos(ball) + prediction_next_ball_pos(ball) # 最終的なボール状態を更新する self.update_ball_state() @@ -120,10 +122,10 @@ def update( self.update_ball_on_placement_area(ball, referee) # ボールの最終的な停止位置を予測する - self.ball_stop_position = self.predict_ball_stop_position(ball=ball, game_config=game_config) + self.ball_stop_position = predict_ball_stop_position(ball=ball, game_config=game_config) # ボールが相手のゴールに入るかを判定する - self.ball_will_enter_their_goal = self.is_ball_will_enter_their_goal( + self.ball_will_enter_their_goal = is_ball_will_enter_their_goal( ball=ball, field_points=field_points, ) @@ -247,26 +249,26 @@ 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): - """ - 次のボールの位置を予測するメソッド + # 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) + # 暫定的に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.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 + # # 予測位置を算出 + # 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: """ボールが動いているかを判定するメソッド.""" @@ -317,33 +319,33 @@ 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 + # 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)) + # # ボールを中心に、ボール速度方向への座標系を作成 + # trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) - vel_norm = tools.get_norm(ball.vel) + # vel_norm = tools.get_norm(ball.vel) - # 減速距離 - a = game_config.ball_friction_coeff * game_config.gravity - distance = (vel_norm ** 2) / (2 * a) + # # 減速距離 + # 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)) + # 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 + # 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 - ) + # # 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/kick_target_model.py b/consai_game/consai_game/world_model/kick_target_model.py index 0bd8db7a..e7ddc5d3 100644 --- a/consai_game/consai_game/world_model/kick_target_model.py +++ b/consai_game/consai_game/world_model/kick_target_model.py @@ -32,6 +32,7 @@ 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 consai_game.evaluation.robot_evaluation import _is_robot_inside_pass_area, _obstacle_exists from copy import deepcopy @@ -98,12 +99,12 @@ def update( self.best_pass_target = self._search_pass_robot(ball=ball_model, robots=robots_model, search_ours=False) - def _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: - """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" - for robot in robots.values(): - if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): - return True - return False + # def _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: + # """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" + # for robot in robots.values(): + # if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): + # return True + # return False def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: """各シュートターゲットの成功率を計算し, リストを更新する関数.""" @@ -182,23 +183,23 @@ def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=Fa return self.shoot_target_list[0] return last_shoot_target_list[0] - def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: - """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" - if robot.pos.x < 0.0: - return False + # def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: + # """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + # if robot.pos.x < 0.0: + # return False - upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) - lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._half_width)) + # upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) + # lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 + # 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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: """各パスターゲットの成功率を計算し, リストを返す関数.""" @@ -212,19 +213,19 @@ def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_our for robot in robots.our_visible_robots.values(): score = 0 if ( - self._obstacle_exists( + _obstacle_exists( target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE ) and search_ours ): score = 0 - elif self._obstacle_exists( + elif _obstacle_exists( target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE ): score = 0 elif tool.get_distance(ball.pos, robot.pos) < 0.5: score = 0 - elif self._is_robot_inside_pass_area(ball, robot) is False: + elif _is_robot_inside_pass_area(ball, robot, Field.half_width) is False: score = 0 else: # ボールとパスを受けるロボットの距離 diff --git a/consai_game/consai_game/world_model/world_model.py b/consai_game/consai_game/world_model/world_model.py index d98519f7..4629d76c 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -26,7 +26,7 @@ from consai_game.world_model.referee_model import RefereeModel from consai_game.world_model.robot_activity_model import RobotActivityModel from consai_game.world_model.robots_model import RobotsModel -from consai_game.world_model.kick_target_model import KickTargetModel +# from consai_game.world_model.kick_target_model import KickTargetModel from consai_game.world_model.game_config_model import GameConfigModel from consai_game.world_model.threats_model import ThreatsModel from consai_game.world_model.world_meta_model import WorldMetaModel @@ -44,7 +44,7 @@ class WorldModel: ball_position: BallPositionModel = BallPositionModel(field, field_points) robot_activity: RobotActivityModel = RobotActivityModel() ball_activity: BallActivityModel = BallActivityModel() - kick_target: KickTargetModel = KickTargetModel() + # kick_target: KickTargetModel = KickTargetModel() game_config: GameConfigModel = GameConfigModel() threats: ThreatsModel = ThreatsModel(field, field_points) meta: WorldMetaModel = WorldMetaModel() 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..7c6a3a99 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,10 +148,10 @@ def update(self) -> None: field_points=self.world_model.field_points, ) # 最適なシュートターゲットを更新 - self.world_model.kick_target.update( - self.world_model.ball, - self.world_model.robots, - ) + # self.world_model.kick_target.update( + # self.world_model.ball, + # self.world_model.robots, + # ) # 敵ロボットの驚異度を更新 self.world_model.threats.update( ball=self.world_model.ball, @@ -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.kick_target.update_field_pos_list(self.world_model.field) def update_game_config(self) -> None: """self.msg_param_control、self.msg_param_strategyを元にゲーム設定を更新する.""" From 95374899139150ed52d121ce0dad778ab66977ce Mon Sep 17 00:00:00 2001 From: uchikun2493 Date: Wed, 13 Aug 2025 14:41:12 +0900 Subject: [PATCH 02/13] =?UTF-8?q?evaluation=E3=82=92world=5Fmodel=E3=81=AE?= =?UTF-8?q?=E7=9B=B4=E4=B8=8B=E3=81=AB=E7=A7=BB=E5=8B=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../consai_game/evaluation/ball_evaluation.py | 268 ---------------- .../evaluation/evaluation_meta_data.py | 21 -- .../evaluation/evaluation_provider_node.py | 102 ------ .../evaluation/kick_target_evaluation.py | 286 ----------------- .../evaluation/robot_evaluation.py | 295 ------------------ .../consai_game/perception/__init__.py | 0 .../consai_game/perception/ball_perception.py | 77 ----- .../perception/robot_perception.py | 0 .../tactic/composite/composite_defense.py | 29 +- .../tactic/composite/composite_offense.py | 46 +-- consai_game/consai_game/tactic/dribble.py | 28 +- consai_game/consai_game/tactic/kick.py | 66 +--- .../visualize_msg_publisher_node.py | 11 +- .../world_model/ball_activity_model.py | 84 +++-- .../{ => world_model}/evaluation/__init__.py | 0 .../evaluation/evaluation.py | 8 +- .../relative_position_evaluation.py | 94 ++++++ .../world_model/kick_target_model.py | 61 ++-- .../consai_game/world_model/world_model.py | 7 +- .../world_model/world_model_provider_node.py | 10 +- setup.cfg | 2 +- 21 files changed, 239 insertions(+), 1256 deletions(-) delete mode 100644 consai_game/consai_game/evaluation/ball_evaluation.py delete mode 100644 consai_game/consai_game/evaluation/evaluation_meta_data.py delete mode 100644 consai_game/consai_game/evaluation/evaluation_provider_node.py delete mode 100644 consai_game/consai_game/evaluation/kick_target_evaluation.py delete mode 100644 consai_game/consai_game/evaluation/robot_evaluation.py delete mode 100644 consai_game/consai_game/perception/__init__.py delete mode 100644 consai_game/consai_game/perception/ball_perception.py delete mode 100644 consai_game/consai_game/perception/robot_perception.py rename consai_game/consai_game/{ => world_model}/evaluation/__init__.py (100%) rename consai_game/consai_game/{ => world_model}/evaluation/evaluation.py (68%) create mode 100644 consai_game/consai_game/world_model/evaluation/relative_position_evaluation.py diff --git a/consai_game/consai_game/evaluation/ball_evaluation.py b/consai_game/consai_game/evaluation/ball_evaluation.py deleted file mode 100644 index d8842f00..00000000 --- a/consai_game/consai_game/evaluation/ball_evaluation.py +++ /dev/null @@ -1,268 +0,0 @@ -#!/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. - -""" -ボールの位置情報を管理するためのクラスを提供する. - -ボールの位置更新, フィールド境界の判定（ヒステリシス付き）, および各エリア内かどうかの判定をする. -""" - -import math - -from consai_game.world_model.ball_model import BallModel -from consai_game.world_model.field_model import Field, FieldPoints - -from consai_msgs.msg import State2D - -# ball_position.py -class BallPositionModel: - """ボールの位置情報を管理するクラス.""" - - def __init__(self, field: Field, field_points: FieldPoints): - """インスタンス生成時の初期化.""" - self._pos = State2D() - self._field = field - self._field_points = field_points - self._outside_margin = 0.05 # フィールド外判定のマージン(m) - self._hysteresis = 0.02 # ヒステリシスの閾値(m) - self._last_left_state = False # 前回の左側判定状態 - self._last_right_state = False # 前回の右側判定状態 - self._last_top_state = False # 前回の上側判定状態 - self._last_bottom_state = False # 前回の下側判定状態 - self._last_our_defense_state = False # 前回の自チームディフェンスエリア判定状態 - self._last_their_defense_state = False # 前回の相手チームディフェンスエリア判定状態 - self._last_our_side_state = False # 前回の自チームサイド判定状態 - self._last_their_side_state = False # 前回の相手チームサイド判定状態 - - def update_position(self, ball_model: BallModel, field_model: Field, field_points: FieldPoints) -> None: - """ボールの位置を更新する.""" - self._pos = ball_model.pos - self._field = field_model - self._field_points = field_points - - def is_outside_of_left(self) -> bool: - """ボールが左側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x < -self._field.half_length - if current_pos_state != self._last_left_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.x < -self._field.half_length - self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.x < -self._field.half_length + self._hysteresis - self._last_left_state = current_pos_state - return current_pos_state - - def is_outside_of_right(self) -> bool: - """ボールが右側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x > self._field.half_length - if current_pos_state != self._last_right_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.x > self._field.half_length + self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.x > self._field.half_length - self._hysteresis - self._last_right_state = current_pos_state - return current_pos_state - - def is_outside_of_top(self) -> bool: - """ボールが上側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.y > self._field.half_width - if current_pos_state != self._last_top_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.y > self._field.half_width + self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.y > self._field.half_width - self._hysteresis - self._last_top_state = current_pos_state - return current_pos_state - - def is_outside_of_bottom(self) -> bool: - """ボールが下側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.y < -self._field.half_width - if current_pos_state != self._last_bottom_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.y < -self._field.half_width - self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.y < -self._field.half_width + self._hysteresis - self._last_bottom_state = current_pos_state - return current_pos_state - - def is_outside(self) -> bool: - """ボールがフィールド外にあるか判定する.""" - return ( - self.is_outside_of_left() - or self.is_outside_of_right() - or self.is_outside_of_top() - or self.is_outside_of_bottom() - ) - - def is_outside_of_left_with_margin(self) -> bool: - """マージンを考慮して、ボールが左側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin - if current_pos_state != self._last_left_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin - self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.x < -self._field.half_length - self._outside_margin + self._hysteresis - self._last_left_state = current_pos_state - return current_pos_state - - def is_outside_of_right_with_margin(self) -> bool: - """マージンを考慮して、ボールが右側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x > self._field.half_length + self._outside_margin - if current_pos_state != self._last_right_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.x > self._field.half_length + self._outside_margin + self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.x > self._field.half_length + self._outside_margin - self._hysteresis - self._last_right_state = current_pos_state - return current_pos_state - - def is_outside_of_top_with_margin(self) -> bool: - """マージンを考慮して、ボールが上側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.y > self._field.half_width + self._outside_margin - if current_pos_state != self._last_top_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.y > self._field.half_width + self._outside_margin + self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.y > self._field.half_width + self._outside_margin - self._hysteresis - self._last_top_state = current_pos_state - return current_pos_state - - def is_outside_of_bottom_with_margin(self) -> bool: - """マージンを考慮して、ボールが下側のフィールド外にあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin - if current_pos_state != self._last_bottom_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 外に出た - current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin - self._hysteresis - else: - # 内に入った - current_pos_state = self._pos.y < -self._field.half_width - self._outside_margin + self._hysteresis - self._last_bottom_state = current_pos_state - return current_pos_state - - def is_outside_with_margin(self) -> bool: - """マージンを考慮して、ボールがフィールド外にあるか判定する.""" - return ( - self.is_outside_of_left_with_margin() - or self.is_outside_of_right_with_margin() - or self.is_outside_of_top_with_margin() - or self.is_outside_of_bottom_with_margin() - ) - - def is_in_our_defense_area(self) -> bool: - """ボールが自チームのディフェンスエリア内にあるか判定する.ヒステリシス付き.""" - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width - and self._pos.x < -self._field.half_length + self._field.penalty_depth - ) - if current_pos_state != self._last_our_defense_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 内に入った - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width - self._hysteresis - and self._pos.x < -self._field.half_length + self._field.penalty_depth - self._hysteresis - ) - else: - # 外に出た - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width + self._hysteresis - and self._pos.x < -self._field.half_length + self._field.penalty_depth + self._hysteresis - ) - self._last_our_defense_state = current_pos_state - return current_pos_state - - def is_in_their_defense_area(self) -> bool: - """ボールが相手チームのディフェンスエリア内にあるか判定する.ヒステリシス付き.""" - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width - and self._pos.x > self._field.half_length - self._field.penalty_depth - ) - if current_pos_state != self._last_their_defense_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 内に入った - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width - self._hysteresis - and self._pos.x > self._field.half_length - self._field.penalty_depth + self._hysteresis - ) - else: - # 外に出た - current_pos_state = ( - math.fabs(self._pos.y) < self._field.half_penalty_width + self._hysteresis - and self._pos.x > self._field.half_length - self._field.penalty_depth - self._hysteresis - ) - self._last_their_defense_state = current_pos_state - return current_pos_state - - def is_in_our_side(self) -> bool: - """ボールが自チームのサイドにあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x < 0 - if current_pos_state != self._last_our_side_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 自チームサイドに入った - current_pos_state = self._pos.x < -self._hysteresis - else: - # 自チームサイドから出た - current_pos_state = self._pos.x < self._hysteresis - self._last_our_side_state = current_pos_state - - # 相手チームサイドの判定と競合する場合は、相手チームサイドを優先 - if self._last_their_side_state: - return False - return current_pos_state - - def is_in_their_side(self) -> bool: - """ボールが相手チームのサイドにあるか判定する.ヒステリシス付き.""" - current_pos_state = self._pos.x > 0 - if current_pos_state != self._last_their_side_state: - # 状態が変化する場合、ヒステリシスを考慮 - if current_pos_state: - # 相手チームサイドに入った - current_pos_state = self._pos.x > self._hysteresis - else: - # 相手チームサイドから出た - current_pos_state = self._pos.x > -self._hysteresis - self._last_their_side_state = current_pos_state - - # 自チームサイドの判定と競合する場合は、相手チームサイドを優先 - if self._last_our_side_state: - return False - return current_pos_state - diff --git a/consai_game/consai_game/evaluation/evaluation_meta_data.py b/consai_game/consai_game/evaluation/evaluation_meta_data.py deleted file mode 100644 index 161325a3..00000000 --- a/consai_game/consai_game/evaluation/evaluation_meta_data.py +++ /dev/null @@ -1,21 +0,0 @@ -# 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 dataclasses import dataclass - - -@dataclass -class EvaluationMetaData: - update_rate: float = 0.0 # 更新周期 Hz - update_counter: int = 0 # 更新カウンタ diff --git a/consai_game/consai_game/evaluation/evaluation_provider_node.py b/consai_game/consai_game/evaluation/evaluation_provider_node.py deleted file mode 100644 index 01472fb9..00000000 --- a/consai_game/consai_game/evaluation/evaluation_provider_node.py +++ /dev/null @@ -1,102 +0,0 @@ -#!/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. - -""" -WorldModelProviderNode モジュール. - -このモジュールは ROS2 ノードとして動作する. -Referee メッセージや TrackedFrame を受け取り, ワールドモデルをリアルタイムに更新する. -""" -import copy -import json -import threading -from typing import Optional - -from consai_game.utils.process_info import process_info -from consai_game.evaluation.evaluation import Evaluation -from consai_game.world_model.world_model import WorldModel - -from rclpy.node import Node - - -class EvaluationProviderNode(Node): - """ - """ - - def __init__(self, update_hz: float = 10): - """ - 初期化. - - EvaluationProviderNode を初期化する. - """ - super().__init__("evaluation_provider_node") - self.lock = threading.Lock() - - self.timer = self.create_timer(1.0 / update_hz, self.update) - - self.world_model = WorldModel() - self.evaluation = Evaluation() - self.evaluation.meta.update_rate = update_hz - - # subscribeするトピック - # self.msg_param_rule: Optional[String] = None - # self.msg_param_control: Optional[String] = None - # self.msg_param_strategy: Optional[String] = None - # self.msg_motion_commands = MotionCommandArray() - - # consai_referee_parserのための補助情報 - # self.pub_referee_info = self.create_publisher(RefereeSupportInfo, "parsed_referee/referee_support_info", 10) - - # self.sub_referee = self.create_subscription(Referee, "referee", self.callback_referee, 10) - # self.sub_detection_traced = self.create_subscription( - # TrackedFrame, "detection_tracked", self.callback_detection_traced, 10 - # ) - - # qos_profile = QoSProfile( - # depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE - # ) - - # self.sub_param_rule = self.create_subscription( - # String, "consai_param/rule", self.callback_param_rule, qos_profile - # ) - # self.sub_param_control = self.create_subscription( - # String, "consai_param/control", self.callback_param_control, qos_profile - # ) - # self.sub_param_strategy = self.create_subscription( - # String, "consai_param/strategy", self.callback_param_strategy, qos_profile - # ) - # # motion_commandのsubscriber - # self._sub_motion_command = self.create_subscription( - # MotionCommandArray, "motion_commands", self.callback_motion_commands, 10 - # ) - - def update(self) -> None: - """ - タイマーにより定期的に呼び出され、WorldModelの状態を更新する. - - ロボットのアクティビティやボール位置を再計算する. - """ - with self.lock: - self.get_logger().debug(f"EvaluationProvider update, {process_info()}") - # メタ情報を更新 - self.evaluation.meta.update_counter += 1 - - # 最適なシュートターゲットを更新 - self.evaluation.kick_target.update( - self.world_model.ball, - self.world_model.robots, - ) diff --git a/consai_game/consai_game/evaluation/kick_target_evaluation.py b/consai_game/consai_game/evaluation/kick_target_evaluation.py deleted file mode 100644 index bcd4f259..00000000 --- a/consai_game/consai_game/evaluation/kick_target_evaluation.py +++ /dev/null @@ -1,286 +0,0 @@ -# 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 operator import attrgetter - -from dataclasses import dataclass, field - - -from consai_msgs.msg import State2D - -from consai_tools.geometry import geometry_tools as tool - -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 consai_game.evaluation.robot_evaluation import _is_robot_inside_pass_area, _obstacle_exists - -from copy import deepcopy - - -@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: - """キックターゲットを保持するクラス.""" - - def __init__(self): - """KickTargetModelの初期化関数.""" - self.hysteresis_distance = 0.3 - - # shoot_targetの位置と成功率を保持するリスト - self.shoot_target_list: list[ShootTarget] = [] - self._goal_pos_list = [ShootTarget()] - - # pass_targetの位置と成功率を保持するリスト - self.pass_target_list: list[PassTarget] = [] - - self._half_width = 4.5 - - self.update_field_pos_list(Field()) - - 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 _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: - """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" - for robot in robots.values(): - if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): - return True - return False - - def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: - """各シュートターゲットの成功率を計算し, リストを更新する関数.""" - TOLERANCE = robots.robot_radius # ロボット半径 - MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る - MAX_DISTANCE_SCORE = 60 # スコア計算時のシュートターゲットの最大スコア - MAX_ANGLE_SCORE = 20 # スコア計算時のシュートターゲットの最大角度スコア - MAX_GOALIE_LEAVE_SCORE = 20 # スコア計算時のシュートターゲットがgoalieからどれくらい離れているかの最大スコア - - # 相手の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 ( - self._obstacle_exists( - target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE - ) - and search_ours - ): - target.success_rate = score - elif self._obstacle_exists( - target=target.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE - ): - target.success_rate = score - else: - # ボールからの角度（目標方向がゴール方向と合っているか） - angle = abs(tool.get_angle(ball.pos, target.pos)) - score += max( - 0, MAX_ANGLE_SCORE - np.rad2deg(angle) * MAX_ANGLE_SCORE / 60 - ) # 小さい角度（正面）ほど高得点とし、60度以上角度がついていれば0点 - - # 距離（近いほうが成功率が高そう） - distance = tool.get_distance(ball.pos, target.pos) - score += max( - 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 6 - ) # ディフェンスエリア外から6m以内ならOK - - if goalie_pos is None: - score += 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) - * 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: - """ボールからの直線上にロボットがいないシュート位置を返す関数.""" - RATE_MARGIN = 50 # ヒステリシスのためのマージン - 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 + RATE_MARGIN: - return self.shoot_target_list[0] - return last_shoot_target_list[0] - - def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: - """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" - if robot.pos.x < 0.0: - return False - - upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) - lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: - """各パスターゲットの成功率を計算し, リストを返す関数.""" - TOLERANCE = robots.robot_radius * 2 # ロボット直径 - MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る - MAX_DISTANCE_SCORE = 55 # スコア計算時のシュートターゲットの最大スコア - MAX_ANGLE_SCORE = 45 # スコア計算時のシュートターゲットの最大角度スコア - - pass_target_list: list[PassTarget] = [] - - for robot in robots.our_visible_robots.values(): - score = 0 - if ( - _obstacle_exists( - target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE - ) - and search_ours - ): - score = 0 - elif _obstacle_exists( - target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE - ): - score = 0 - elif tool.get_distance(ball.pos, robot.pos) < 0.5: - score = 0 - elif _is_robot_inside_pass_area(ball, robot, Field.half_width) is False: - score = 0 - else: - # ボールとパスを受けるロボットの距離 - distance = tool.get_distance(ball.pos, robot.pos) - score += max( - 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 4 - ) # ディフェンスエリア外から4m以内ならOK - # ボールからの角度（目標方向がロボット方向と合っているか） - angle = abs(tool.get_angle(ball.pos, robot.pos)) - score += max(0, 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 - 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/evaluation/robot_evaluation.py b/consai_game/consai_game/evaluation/robot_evaluation.py deleted file mode 100644 index 89cd9558..00000000 --- a/consai_game/consai_game/evaluation/robot_evaluation.py +++ /dev/null @@ -1,295 +0,0 @@ -# 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 - -from consai_game.world_model.world_model import WorldModel - -from consai_game.utils.generate_dummy_ball_position import generate_dummy_ball_position - -from consai_game.world_model.robots_model import Robot, RobotsModel -from consai_game.world_model.ball_model import BallModel -from consai_game.world_model.field_model import Field, FieldPoints -from dataclasses import dataclass -from typing import List, Dict - -from consai_game.utils.geometry import Point - -from consai_game.world_model.ball_activity_model import BallActivityModel -from consai_game.world_model.game_config_model import GameConfigModel - - -# kick.py -def robot_is_backside(robot_id: int, world_model: WorldModel, target_pos: State2D, angle_ball_to_robot_threshold: int) -> bool: - """ボールからターゲットを見て、ロボットが後側に居るかを判定する.""" - - robot_pos = world_model.robots.our_robots.get(robot_id).pos - - # ボールが消えることを想定して、仮想的なボール位置を生成する - ball_pos = generate_dummy_ball_position(ball=world_model.ball, robot_pos=robot_pos) - - # ボールからターゲットへの座標系を作成 - 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 - -# kick.py -def robot_is_on_kick_line( - robot_id: int, world_model: WorldModel, target_pos: State2D, width_threshold: float -) -> bool: - """ボールからターゲットまでの直線上にロボットが居るかを判定する. - - ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する. - """ - MINIMAL_THETA_THRESHOLD = 45 # 最低限満たすべきロボットの角度 - - robot_pos = world_model.robots.our_robots.get(robot_id).pos - - # ボールが消えることを想定して、仮想的なボール位置を生成する - ball_pos = generate_dummy_ball_position(ball=world_model.ball, robot_pos=robot_pos) - - # ボールからターゲットへの座標系を作成 - 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 - - -# dribble.py -def ball_is_front(ball_pos: State2D, robot_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 - - -# threats_model.py -@dataclass -class Threat: - score: int # 0以上 - robot_id: int # 相手のロボットID - -class ThreatsModel: - def __init__(self, field: Field, field_points: FieldPoints): - self.threats: List[Threat] = [] - self._field = field - self._field_points = field_points - self._prev_scores: Dict[int, float] = {} # ロボットIDごとの前回のスコア - self._alpha = 0.1 # ローパスフィルターの係数（0-1、小さいほど変化が遅い） - - 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) - - -# kick_target_model.py -def _obstacle_exists(target: State2D, ball: BallModel, robots: dict[int, Robot], 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, _half_width: Field) -> bool: - """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" - - 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 - - -# robot_activity_model.py -"""未完了.""" -@dataclass -class ReceiveScore: - """ボールをどれだけ受け取りやすいかを保持するデータクラス.""" - - robot_id: int = 0 - intercept_time: float = float("inf") # あと何秒後にボールを受け取れるか - -def calc_ball_receive_score_list( - robots: dict[int, Robot], ball: BallModel, ball_activity: BallActivityModel, game_config: GameConfigModel -) -> list[ReceiveScore]: - """ロボットごとにボールを受け取れるスコアを計算する.""" - - # ボールが動いていない場合は、スコアをデフォルト値にする - if not ball_activity.ball_is_moving: - return [ReceiveScore(robot_id=robot.robot_id) for robot in robots.values()] - - score_list = [] - for robot in robots.values(): - score_list.append( - ReceiveScore( - robot_id=robot.robot_id, - intercept_time=calc_intercept_time(robot, ball, game_config), - ) - ) - - # intercept_timeが小さい順にソート - score_list.sort(key=lambda x: x.intercept_time) - return score_list - -def calc_intercept_time(robot: Robot, ball: BallModel, game_config: GameConfigModel) -> float: - """ロボットがボールを受け取るまでの時間を計算する関数.""" - - # ボールを中心に、ボールの速度方向を+x軸にした座標系を作る - trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) - - # ロボットの位置を変換 - tr_robot_pos = trans.transform(robot.pos) - - # TODO: ボールを後ろから追いかけて受け取れるようになったら、計算を変更する - if tr_robot_pos.x < 0: - return float("inf") - - # ロボットからボール軌道まで垂線を引き、 - # その交点にボールが到達するまでの時間を計算する - ball_arrival_distance = tr_robot_pos.x - intercept_time = ball_arrival_distance / tools.get_norm(ball.vel) - - # ボールが到達するまでの時間で、ロボットがどれだけ移動できるかを計算する - # TODO: ロボットの現在速度、加速度を考慮すべき - available_distance = intercept_time * game_config.robot_max_linear_vel - - # ボール軌道からロボットまでの距離 - robot_arrival_distance = abs(tr_robot_pos.y) - - # ボールが到着するまでにロボットが移動できれば、intercept_timeを返す - if available_distance >= robot_arrival_distance: - return intercept_time - return float("inf") diff --git a/consai_game/consai_game/perception/__init__.py b/consai_game/consai_game/perception/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/consai_game/consai_game/perception/ball_perception.py b/consai_game/consai_game/perception/ball_perception.py deleted file mode 100644 index fa940b86..00000000 --- a/consai_game/consai_game/perception/ball_perception.py +++ /dev/null @@ -1,77 +0,0 @@ -from copy import deepcopy -from dataclasses import dataclass -from enum import Enum, auto -from typing import Optional - -from consai_tools.geometry import geometry_tools as tools - -from consai_game.world_model.ball_model import BallModel -from consai_game.world_model.robots_model import Robot, RobotsModel -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_msgs.msg import State2D - - -# ball_activity_model.py -def prediction_next_ball_pos(ball: BallModel): - """ - 次のボールの位置を予測するメソッド - - 暫定的に0.1[m]移動すると仮定 - """ - - # 速度に対するボール移動量を算出する比率[s]: 実質的に移動時間 - MOVEMENT_GAIN = 0.1 - # ボールの移動量 - ball_movement = State2D() - # ボールの将来の予測位置 - next_ball_pos = State2D() - # 将来の位置 - _future_ball_pos = State2D() - _future_ball_pos.x = ball.pos.x + ball.vel.x - _future_ball_pos.y = ball.pos.y + ball.vel.y - # ボール移動量 - ball_movement.x = ball.vel.x * MOVEMENT_GAIN # * np.cos(self.angle_trajectory) - ball_movement.y = ball.vel.y * MOVEMENT_GAIN # * np.sin(self.angle_trajectory) - - # 予測位置を算出 - next_ball_pos.x = ball.pos.x + ball_movement.x - next_ball_pos.y = ball.pos.y + ball_movement.y - -def predict_ball_stop_position(ball: BallModel, game_config: GameConfigModel) -> State2D: - """ボールが止まる位置を予測するメソッド.""" - - ball_is_moving = False - - # ボールの速度が小さい場合は、現在の位置を返す - if not 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(ball: BallModel, field_points: FieldPoints) -> bool: - """ボールが相手のゴールに入るかを判定するメソッド.""" - - ball_is_moving = False - - # ボールが最終的に止まる予測位置 - ball_stop_position = State2D() - # ボールが動いていない場合は、Falseを返す - if not ball_is_moving: - return False - - # 2つの線が交差するかで判定する - return tools.is_intersect( - p1=ball.pos, p2=ball_stop_position, q1=field_points.their_goal_top, q2=field_points.their_goal_bottom - ) \ No newline at end of file diff --git a/consai_game/consai_game/perception/robot_perception.py b/consai_game/consai_game/perception/robot_perception.py deleted file mode 100644 index e69de29b..00000000 diff --git a/consai_game/consai_game/tactic/composite/composite_defense.py b/consai_game/consai_game/tactic/composite/composite_defense.py index e9b07c5e..2b072327 100644 --- a/consai_game/consai_game/tactic/composite/composite_defense.py +++ b/consai_game/consai_game/tactic/composite/composite_defense.py @@ -12,24 +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 -from consai_game.evaluation.evaluation import Evaluation - 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), @@ -41,8 +46,6 @@ def __init__(self, tactic_default: TacticBase, do_receive: bool = True): self.very_close_to_ball_threshold = 0.3 self.do_receive = do_receive - self.evaluation: Evaluation = Evaluation() - def run(self, world_model: WorldModel) -> MotionCommand: """状況に応じて実行するtacticを切り替えてrunする.""" @@ -73,18 +76,16 @@ def run(self, world_model: WorldModel) -> MotionCommand: def control_the_ball(self, world_model: WorldModel) -> MotionCommand: """ボールを制御するためのTacticを実行する関数.""" - evaluation = self.evaluation - - if evaluation.kick_target.best_shoot_target.success_rate > 50: + if world_model.kick_target.best_shoot_target.success_rate > 50: # シュートできる場合 - self.tactic_shoot.target_pos = evaluation.kick_target.best_shoot_target.pos + self.tactic_shoot.target_pos = world_model.kick_target.best_shoot_target.pos return self.run_sub_tactic(self.tactic_shoot, world_model) - elif evaluation.kick_target.best_pass_target.success_rate > 30: + elif world_model.kick_target.best_pass_target.success_rate > 30: # パスできる場合 - self.tactic_pass.target_pos = evaluation.kick_target.best_pass_target.robot_pos + self.tactic_pass.target_pos = world_model.kick_target.best_pass_target.robot_pos return self.run_sub_tactic(self.tactic_pass, world_model) # シュート成功率が一番高いところに向かってパスする(クリア) - self.tactic_pass.target_pos = evaluation.kick_target.best_shoot_target.pos + self.tactic_pass.target_pos = world_model.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 e8121409..d8613c64 100644 --- a/consai_game/consai_game/tactic/composite/composite_offense.py +++ b/consai_game/consai_game/tactic/composite/composite_offense.py @@ -12,33 +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.tactic.steal_ball import StealBall - -from consai_game.world_model.field_model import Field - -from consai_game.evaluation.evaluation import Evaluation -from consai_game.evaluation.evaluation_provider_node import EvaluationProviderNode 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 @@ -117,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), @@ -132,10 +141,6 @@ def __init__(self, tactic_default: TacticBase, is_setplay=False, force_pass=Fals self.kick_score_threshold = kick_score_threshold self.SHOOTING_MARGIN = 0 - self.evaluation: Evaluation = Evaluation() - # self.field: Field = Field() - # self.evaluation_provider_node: EvaluationProviderNode = EvaluationProviderNode() - # 最初の動作を強制的にpassにするかの設定 self.force_pass = force_pass @@ -147,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を登録解除する @@ -171,26 +177,24 @@ def run(self, world_model: WorldModel) -> MotionCommand: def control_the_ball(self, world_model: WorldModel) -> MotionCommand: """ボールを制御するためのTacticを実行する関数.""" - evaluation = self.evaluation - # 相手がボールを持ってる場合は奪いに行く if world_model.ball_activity.is_their_team_ball_holder: # ボールを奪う return self.run_sub_tactic(self.tactic_steal, world_model) if ( - evaluation.kick_target.best_shoot_target.success_rate > self.kick_score_threshold - self.SHOOTING_MARGIN + world_model.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 = evaluation.kick_target.best_shoot_target.pos + self.tactic_shoot.target_pos = world_model.kick_target.best_shoot_target.pos # シュート相手がコロコロ切り替わらないようにマージンを設定 self.SHOOTING_MARGIN = 20 return self.run_sub_tactic(self.tactic_shoot, world_model) - elif evaluation.kick_target.best_pass_target.success_rate > 30 or self.force_pass: + elif world_model.kick_target.best_pass_target.success_rate > 30 or self.force_pass: # パスできる場合 か force_passがTrueの場合 - self.tactic_pass.target_pos = copy.deepcopy(evaluation.kick_target.best_pass_target.robot_pos) + self.tactic_pass.target_pos = copy.deepcopy(world_model.kick_target.best_pass_target.robot_pos) # パスターゲットの候補を探そうとしているのでシュートターゲットのマージンを0にする self.SHOOTING_MARGIN = 0 @@ -198,7 +202,7 @@ def control_the_ball(self, world_model: WorldModel) -> MotionCommand: # TODO: 前進しつつ、敵がいない方向にドリブルしたい # シュート成功率が一番高いところに向かってドリブルする - self.tactic_tapping.target_pos = evaluation.kick_target.best_shoot_target.pos + self.tactic_tapping.target_pos = world_model.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 251c1617..9722ac8c 100644 --- a/consai_game/consai_game/tactic/dribble.py +++ b/consai_game/consai_game/tactic/dribble.py @@ -31,8 +31,6 @@ from transitions.extensions import GraphMachine -from consai_game.evaluation.robot_evaluation import ball_is_front - class DribbleStateMachine(GraphMachine): """ドリブルの状態遷移マシン.""" @@ -158,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=ball_is_front(ball_pos=ball_pos, robot_pos=robot_pos, target_pos=self.target_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() @@ -180,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 8cb4e83d..eb9fe5b2 100644 --- a/consai_game/consai_game/tactic/kick.py +++ b/consai_game/consai_game/tactic/kick.py @@ -14,20 +14,19 @@ """キック動作に関する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 -from consai_game.evaluation.robot_evaluation import robot_is_backside, robot_is_on_kick_line -# from consai_game.evaluation.evaluation import Evaluation +import numpy as np class KickStateMachine(GraphMachine): @@ -139,9 +138,11 @@ def run(self, world_model: WorldModel) -> MotionCommand: width_threshold = 0.03 self.machine.update( - robot_is_backside=robot_is_backside(self.robot_id, world_model, self.final_target_pos, self.ANGLE_BALL_TO_ROBOT_THRESHOLD), - robot_is_on_kick_line=robot_is_on_kick_line( - self.robot_id, world_model, 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 ), ) @@ -204,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: @@ -293,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 b325fc7c..7650453a 100644 --- a/consai_game/consai_game/visualization/visualize_msg_publisher_node.py +++ b/consai_game/consai_game/visualization/visualize_msg_publisher_node.py @@ -27,8 +27,7 @@ from consai_game.world_model.ball_model import BallModel from consai_game.world_model.ball_activity_model import BallActivityModel, BallState from consai_game.world_model.robot_activity_model import RobotActivityModel -# from consai_game.world_model.kick_target_model import KickTargetModel -from consai_game.evaluation.kick_target_evaluation import KickTargetEvaluation +from consai_game.world_model.kick_target_model import KickTargetModel from consai_game.world_model.robots_model import RobotsModel from consai_game.world_model.world_model import WorldModel @@ -44,9 +43,9 @@ 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=kick_target_evaluation, ball=world_model.ball) - # ) + self.pub_visualizer_objects.publish( + self.kick_target_to_vis_msg(kick_target=world_model.kick_target, ball=world_model.ball) + ) self.pub_visualizer_objects.publish( self.ball_activity_to_vis_msg(activity=world_model.ball_activity, ball=world_model.ball) @@ -64,7 +63,7 @@ def set_robot_tactic_status_list(self, robot_tactic_status_list: list[RobotTacti """ロボットの戦術状態をセットすする関数""" self.robot_tactic_status_list = robot_tactic_status_list - def kick_target_to_vis_msg(self, kick_target: KickTargetEvaluation, ball: BallModel) -> Objects: + def kick_target_to_vis_msg(self, kick_target: KickTargetModel, ball: BallModel) -> Objects: """kick_targetをObjectsメッセージに変換する.""" vis_obj = Objects() vis_obj.layer = "game" 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 91746f01..a0c046cd 100644 --- a/consai_game/consai_game/world_model/ball_activity_model.py +++ b/consai_game/consai_game/world_model/ball_activity_model.py @@ -34,8 +34,6 @@ from consai_msgs.msg import State2D -from consai_game.perception.ball_perception import is_ball_will_enter_their_goal, predict_ball_stop_position, prediction_next_ball_pos - class BallState(Enum): """ボールの状態を表す列挙型.""" @@ -113,7 +111,7 @@ def update( self.ball_is_moving = self.is_ball_moving(ball) # ボールの予測位置を更新する - prediction_next_ball_pos(ball) + self.prediction_next_ball_pos(ball) # 最終的なボール状態を更新する self.update_ball_state() @@ -122,10 +120,10 @@ def update( self.update_ball_on_placement_area(ball, referee) # ボールの最終的な停止位置を予測する - self.ball_stop_position = predict_ball_stop_position(ball=ball, game_config=game_config) + self.ball_stop_position = self.prediction_ball_stop_position(ball=ball, game_config=game_config) # ボールが相手のゴールに入るかを判定する - self.ball_will_enter_their_goal = is_ball_will_enter_their_goal( + self.ball_will_enter_their_goal = self.is_ball_will_enter_their_goal( ball=ball, field_points=field_points, ) @@ -249,26 +247,26 @@ 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): - # """ - # 次のボールの位置を予測するメソッド + 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) + 暫定的に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.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 + # 予測位置を算出 + 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: """ボールが動いているかを判定するメソッド.""" @@ -319,33 +317,33 @@ 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 + def prediction_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)) + # ボールを中心に、ボール速度方向への座標系を作成 + trans = tools.Trans(ball.pos, tools.get_vel_angle(ball.vel)) - # vel_norm = tools.get_norm(ball.vel) + vel_norm = tools.get_norm(ball.vel) - # # 減速距離 - # a = game_config.ball_friction_coeff * game_config.gravity - # distance = (vel_norm ** 2) / (2 * a) + # 減速距離 + 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)) + 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 + 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 - # ) + # 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/evaluation/__init__.py b/consai_game/consai_game/world_model/evaluation/__init__.py similarity index 100% rename from consai_game/consai_game/evaluation/__init__.py rename to consai_game/consai_game/world_model/evaluation/__init__.py diff --git a/consai_game/consai_game/evaluation/evaluation.py b/consai_game/consai_game/world_model/evaluation/evaluation.py similarity index 68% rename from consai_game/consai_game/evaluation/evaluation.py rename to consai_game/consai_game/world_model/evaluation/evaluation.py index 4e2d0f36..50b89513 100644 --- a/consai_game/consai_game/evaluation/evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/evaluation.py @@ -19,13 +19,11 @@ from dataclasses import dataclass -from consai_game.evaluation.kick_target_evaluation import KickTargetEvaluation -from consai_game.evaluation.evaluation_meta_data import EvaluationMetaData +from consai_game.world_model.evaluation.relative_position_evaluation import RelativePositionEvaluation @dataclass class Evaluation: - """試合全体の状態を統合的に保持するデータクラス.""" + """評価に関する関数やクラスを統合的に保持するデータクラス.""" - kick_target: KickTargetEvaluation = KickTargetEvaluation() - meta: EvaluationMetaData = EvaluationMetaData() + relative_position: RelativePositionEvaluation = RelativePositionEvaluation() 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/kick_target_model.py b/consai_game/consai_game/world_model/kick_target_model.py index e7ddc5d3..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,22 +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 consai_game.evaluation.robot_evaluation import _is_robot_inside_pass_area, _obstacle_exists -from copy import deepcopy +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tool @dataclass @@ -99,12 +96,12 @@ def update( self.best_pass_target = self._search_pass_robot(ball=ball_model, robots=robots_model, search_ours=False) - # def _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: - # """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" - # for robot in robots.values(): - # if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): - # return True - # return False + def _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: + """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" + for robot in robots.values(): + if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): + return True + return False def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: """各シュートターゲットの成功率を計算し, リストを更新する関数.""" @@ -183,23 +180,23 @@ def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=Fa return self.shoot_target_list[0] return last_shoot_target_list[0] - # def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: - # """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" - # if robot.pos.x < 0.0: - # return False + def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: + """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + if robot.pos.x < 0.0: + return False - # upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) - # lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._half_width)) + upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) + lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 + 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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: """各パスターゲットの成功率を計算し, リストを返す関数.""" @@ -213,19 +210,19 @@ def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_our for robot in robots.our_visible_robots.values(): score = 0 if ( - _obstacle_exists( + self._obstacle_exists( target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE ) and search_ours ): score = 0 - elif _obstacle_exists( + elif self._obstacle_exists( target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE ): score = 0 elif tool.get_distance(ball.pos, robot.pos) < 0.5: score = 0 - elif _is_robot_inside_pass_area(ball, robot, Field.half_width) is False: + elif self._is_robot_inside_pass_area(ball, robot) is False: score = 0 else: # ボールとパスを受けるロボットの距離 diff --git a/consai_game/consai_game/world_model/world_model.py b/consai_game/consai_game/world_model/world_model.py index 4629d76c..4c3a2a3f 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -22,11 +22,12 @@ 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 from consai_game.world_model.robots_model import RobotsModel -# from consai_game.world_model.kick_target_model import KickTargetModel +from consai_game.world_model.kick_target_model import KickTargetModel from consai_game.world_model.game_config_model import GameConfigModel from consai_game.world_model.threats_model import ThreatsModel from consai_game.world_model.world_meta_model import WorldMetaModel @@ -44,7 +45,9 @@ class WorldModel: ball_position: BallPositionModel = BallPositionModel(field, field_points) robot_activity: RobotActivityModel = RobotActivityModel() ball_activity: BallActivityModel = BallActivityModel() - # kick_target: KickTargetModel = KickTargetModel() + kick_target: KickTargetModel = KickTargetModel() game_config: GameConfigModel = GameConfigModel() threats: ThreatsModel = ThreatsModel(field, field_points) meta: WorldMetaModel = WorldMetaModel() + + evaluation: Evaluation = Evaluation() 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 7c6a3a99..4ac11ad8 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,10 +148,10 @@ def update(self) -> None: field_points=self.world_model.field_points, ) # 最適なシュートターゲットを更新 - # self.world_model.kick_target.update( - # self.world_model.ball, - # self.world_model.robots, - # ) + self.world_model.kick_target.update( + self.world_model.ball, + self.world_model.robots, + ) # 敵ロボットの驚異度を更新 self.world_model.threats.update( ball=self.world_model.ball, @@ -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.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 From d23481527aa1229865a92a00c8d7d98f0ee4cf66 Mon Sep 17 00:00:00 2001 From: uchikun2493 Date: Wed, 13 Aug 2025 14:47:49 +0900 Subject: [PATCH 03/13] =?UTF-8?q?=E3=83=A2=E3=82=B8=E3=83=A5=E3=83=BC?= =?UTF-8?q?=E3=83=AB=E5=89=8A=E9=99=A4=E3=81=AB=E4=BC=B4=E3=81=86=E4=BD=99?= =?UTF-8?q?=E8=A8=88=E3=81=AAimport=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai_game/consai_game/main.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/consai_game/consai_game/main.py b/consai_game/consai_game/main.py index 83455d06..cfda420c 100755 --- a/consai_game/consai_game/main.py +++ b/consai_game/consai_game/main.py @@ -31,7 +31,6 @@ from consai_game.core.play.play_node import PlayNode from consai_game.core.tactic.agent_scheduler_node import AgentSchedulerNode from consai_game.visualization.visualize_msg_publisher_node import VisualizeMsgPublisherNode -from consai_game.evaluation.evaluation_provider_node import EvaluationProviderNode from consai_game.world_model.world_model_provider_node import WorldModelProviderNode @@ -70,9 +69,6 @@ def main(): goalie_id=args.goalie, invert=args.invert, ) - evaluation_provider_node = EvaluationProviderNode( - update_hz=UPDATE_HZ, - ) # TODO: agent_numをplay_nodeから取得したい agent_scheduler_node = AgentSchedulerNode(update_hz=UPDATE_HZ, team_is_yellow=team_is_yellow, agent_num=11) play_node.set_update_role_callback(agent_scheduler_node.set_roles) @@ -83,7 +79,6 @@ def main(): executor = MultiThreadedExecutor() executor.add_node(play_node) executor.add_node(world_model_provider_node) - executor.add_node(evaluation_provider_node) executor.add_node(agent_scheduler_node) executor.add_node(vis_msg_publisher_node) From 0d70d3cc5d0501455aabc7d165ceeb17e2a25ce6 Mon Sep 17 00:00:00 2001 From: uchikun2493 Date: Wed, 13 Aug 2025 15:01:44 +0900 Subject: [PATCH 04/13] =?UTF-8?q?kick=5Ftarget=E3=82=92evaluation=E3=81=AB?= =?UTF-8?q?=E7=A7=BB=E6=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../tactic/composite/composite_defense.py | 10 +- .../tactic/composite/composite_offense.py | 11 +- .../world_model/evaluation/evaluation.py | 2 + .../evaluation/kick_target_evaluation.py | 281 ++++++++++++++++++ .../world_model/world_model_provider_node.py | 4 +- 5 files changed, 296 insertions(+), 12 deletions(-) create mode 100644 consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py diff --git a/consai_game/consai_game/tactic/composite/composite_defense.py b/consai_game/consai_game/tactic/composite/composite_defense.py index 2b072327..e6fdca94 100644 --- a/consai_game/consai_game/tactic/composite/composite_defense.py +++ b/consai_game/consai_game/tactic/composite/composite_defense.py @@ -76,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 d8613c64..11e63019 100644 --- a/consai_game/consai_game/tactic/composite/composite_offense.py +++ b/consai_game/consai_game/tactic/composite/composite_offense.py @@ -183,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 @@ -202,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/world_model/evaluation/evaluation.py b/consai_game/consai_game/world_model/evaluation/evaluation.py index 50b89513..85fb030d 100644 --- a/consai_game/consai_game/world_model/evaluation/evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/evaluation.py @@ -19,6 +19,7 @@ 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 @@ -26,4 +27,5 @@ class Evaluation: """評価に関する関数やクラスを統合的に保持するデータクラス.""" + kick_target: KickTargetEvaluation = KickTargetEvaluation() relative_position: RelativePositionEvaluation = RelativePositionEvaluation() 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..86dd3014 --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py @@ -0,0 +1,281 @@ +# 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 Robot, RobotsModel + +from consai_msgs.msg import State2D + +from consai_tools.geometry import geometry_tools as tool + + +@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: + """キックターゲットを評価するクラス.""" + + def __init__(self): + """KickTargetEvaluationの初期化関数.""" + self.hysteresis_distance = 0.3 + + # shoot_targetの位置と成功率を保持するリスト + self.shoot_target_list: list[ShootTarget] = [] + self._goal_pos_list = [ShootTarget()] + + # pass_targetの位置と成功率を保持するリスト + self.pass_target_list: list[PassTarget] = [] + + self._half_width = 4.5 + + 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 _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: + """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" + for robot in robots.values(): + if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): + return True + return False + + def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: + """各シュートターゲットの成功率を計算し, リストを更新する関数.""" + TOLERANCE = robots.robot_radius # ロボット半径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 60 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 20 # スコア計算時のシュートターゲットの最大角度スコア + MAX_GOALIE_LEAVE_SCORE = 20 # スコア計算時のシュートターゲットがgoalieからどれくらい離れているかの最大スコア + + # 相手の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 ( + self._obstacle_exists( + target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + ) + and search_ours + ): + target.success_rate = score + elif self._obstacle_exists( + target=target.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE + ): + target.success_rate = score + else: + # ボールからの角度（目標方向がゴール方向と合っているか） + angle = abs(tool.get_angle(ball.pos, target.pos)) + score += max( + 0, MAX_ANGLE_SCORE - np.rad2deg(angle) * MAX_ANGLE_SCORE / 60 + ) # 小さい角度（正面）ほど高得点とし、60度以上角度がついていれば0点 + + # 距離（近いほうが成功率が高そう） + distance = tool.get_distance(ball.pos, target.pos) + score += max( + 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 6 + ) # ディフェンスエリア外から6m以内ならOK + + if goalie_pos is None: + score += 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) + * 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: + """ボールからの直線上にロボットがいないシュート位置を返す関数.""" + RATE_MARGIN = 50 # ヒステリシスのためのマージン + 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 + RATE_MARGIN: + return self.shoot_target_list[0] + return last_shoot_target_list[0] + + def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: + """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" + if robot.pos.x < 0.0: + return False + + upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) + lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: + """各パスターゲットの成功率を計算し, リストを返す関数.""" + TOLERANCE = robots.robot_radius * 2 # ロボット直径 + MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る + MAX_DISTANCE_SCORE = 55 # スコア計算時のシュートターゲットの最大スコア + MAX_ANGLE_SCORE = 45 # スコア計算時のシュートターゲットの最大角度スコア + + pass_target_list: list[PassTarget] = [] + + for robot in robots.our_visible_robots.values(): + score = 0 + if ( + self._obstacle_exists( + target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + ) + and search_ours + ): + score = 0 + elif self._obstacle_exists( + target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE + ): + score = 0 + elif tool.get_distance(ball.pos, robot.pos) < 0.5: + score = 0 + elif self._is_robot_inside_pass_area(ball, robot) is False: + score = 0 + else: + # ボールとパスを受けるロボットの距離 + distance = tool.get_distance(ball.pos, robot.pos) + score += max( + 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 4 + ) # ディフェンスエリア外から4m以内ならOK + # ボールからの角度（目標方向がロボット方向と合っているか） + angle = abs(tool.get_angle(ball.pos, robot.pos)) + score += max(0, 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 - 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/world_model_provider_node.py b/consai_game/consai_game/world_model/world_model_provider_node.py index 4ac11ad8..b802c0bf 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,7 +148,7 @@ 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, ) @@ -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を元にゲーム設定を更新する.""" From 839b7a27bc0f4b29564820f10230b9e89963f59f Mon Sep 17 00:00:00 2001 From: uchikun2493 Date: Wed, 13 Aug 2025 16:03:16 +0900 Subject: [PATCH 05/13] =?UTF-8?q?ball=5Factivity=5Fmodel=E3=81=8B=E3=82=89?= =?UTF-8?q?=E4=BA=88=E6=B8=AC=E3=81=99=E3=82=8B=E3=82=B3=E3=83=BC=E3=83=89?= =?UTF-8?q?=E3=82=92=E5=88=87=E3=82=8A=E9=9B=A2=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model/ball_activity_model.py | 70 +++----------- .../world_model/perception/__init__.py | 0 .../world_model/perception/ball_prediction.py | 92 +++++++++++++++++++ 3 files changed, 107 insertions(+), 55 deletions(-) create mode 100644 consai_game/consai_game/world_model/perception/__init__.py create mode 100644 consai_game/consai_game/world_model/perception/ball_prediction.py 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 a0c046cd..0e37c36b 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.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.prediction_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 prediction_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/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_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, + ) From ae937499d6e8eb32bc3d9faba0e0730ed9c24f4d Mon Sep 17 00:00:00 2001 From: uchikun2493 Date: Thu, 14 Aug 2025 10:44:38 +0900 Subject: [PATCH 06/13] =?UTF-8?q?fix:=20=E3=83=9C=E3=83=BC=E3=83=AB?= =?UTF-8?q?=E3=81=AE=E4=BA=88=E6=B8=AC=E4=BD=8D=E7=BD=AE=E3=81=AE=E6=9B=B4?= =?UTF-8?q?=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai_game/consai_game/world_model/ball_activity_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0e37c36b..c45f319a 100644 --- a/consai_game/consai_game/world_model/ball_activity_model.py +++ b/consai_game/consai_game/world_model/ball_activity_model.py @@ -113,7 +113,7 @@ def update( self.ball_is_moving = self.is_ball_moving(ball) # ボールの予測位置を更新する - self.ball_prediction.next_ball_pos(ball) + self.next_ball_pos = self.ball_prediction.next_ball_pos(ball) # 最終的なボール状態を更新する self.update_ball_state() From e8ec1bb9e4c4907f91b7d08c237e26afc56c4f3f Mon Sep 17 00:00:00 2001 From: agich073 Date: Thu, 14 Aug 2025 16:49:18 +0900 Subject: [PATCH 07/13] =?UTF-8?q?threats=5Fmodel=E3=81=AE=E7=A7=BB?= =?UTF-8?q?=E6=A4=8D=E3=81=A8perception=E3=81=AB=E3=83=AD=E3=83=9C?= =?UTF-8?q?=E3=83=83=E3=83=88=E3=81=AE=E4=BD=8D=E7=BD=AE=E3=81=AB=E9=96=A2?= =?UTF-8?q?=E3=81=99=E3=82=8B=E8=A9=95=E4=BE=A1=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model/evaluation/evaluation.py | 5 + .../evaluation/kick_target_evaluation.py | 39 ++---- .../evaluation/threats_evaluation.py | 123 ++++++++++++++++++ .../world_model/perception/ball_decision.py | 74 +++++++++++ .../world_model/perception/perception.py | 32 +++++ .../world_model/perception/robot_decision.py | 110 ++++++++++++++++ .../consai_game/world_model/world_model.py | 2 +- 7 files changed, 353 insertions(+), 32 deletions(-) create mode 100644 consai_game/consai_game/world_model/evaluation/threats_evaluation.py create mode 100644 consai_game/consai_game/world_model/perception/ball_decision.py create mode 100644 consai_game/consai_game/world_model/perception/perception.py create mode 100644 consai_game/consai_game/world_model/perception/robot_decision.py diff --git a/consai_game/consai_game/world_model/evaluation/evaluation.py b/consai_game/consai_game/world_model/evaluation/evaluation.py index 85fb030d..46f9e2bb 100644 --- a/consai_game/consai_game/world_model/evaluation/evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/evaluation.py @@ -21,11 +21,16 @@ 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 ThreatsModel +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_model: ThreatsModel = ThreatsModel(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 index 86dd3014..74f7c641 100644 --- a/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py @@ -13,7 +13,7 @@ # limitations under the License. """ -キックターゲットを管理するモジュール. +キックターゲットを管理し予測するモジュール. シュートを試みるための最適なターゲット位置を計算し, キックターゲットの成功率を更新する. """ @@ -33,6 +33,8 @@ from consai_tools.geometry import geometry_tools as tool +from consai_game.world_model.perception.robot_decision import RobotDecision + @dataclass class ShootTarget: @@ -96,13 +98,6 @@ def update( self.best_pass_target = self._search_pass_robot(ball=ball_model, robots=robots_model, search_ours=False) - def _obstacle_exists(self, target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: - """ターゲット位置に障害物（ロボット）が存在するかを判定する関数.""" - for robot in robots.values(): - if tool.is_on_line(pose=robot.pos, line_pose1=ball.pos, line_pose2=target, tolerance=tolerance): - return True - return False - def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: """各シュートターゲットの成功率を計算し, リストを更新する関数.""" TOLERANCE = robots.robot_radius # ロボット半径 @@ -121,13 +116,13 @@ def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours for target in self._goal_pos_list: score = 0 if ( - self._obstacle_exists( + RobotDecision.obstacle_exists( target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE ) and search_ours ): target.success_rate = score - elif self._obstacle_exists( + elif RobotDecision.obstacle_exists( target=target.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE ): target.success_rate = score @@ -180,24 +175,6 @@ def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=Fa return self.shoot_target_list[0] return last_shoot_target_list[0] - def _is_robot_inside_pass_area(self, ball: BallModel, robot: Robot) -> bool: - """味方ロボットがパスを出すロボットとハーフライン両サイドを結んでできる五角形のエリア内にいるかを判別する関数""" - if robot.pos.x < 0.0: - return False - - upper_side_slope, upper_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, self._half_width)) - lower_side_slope, lower_side_intercept, flag = tool.get_line_parameter(ball.pos, Point(0.0, -self._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 make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[PassTarget]: """各パスターゲットの成功率を計算し, リストを返す関数.""" TOLERANCE = robots.robot_radius * 2 # ロボット直径 @@ -210,19 +187,19 @@ def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_our for robot in robots.our_visible_robots.values(): score = 0 if ( - self._obstacle_exists( + RobotDecision.obstacle_exists( target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE ) and search_ours ): score = 0 - elif self._obstacle_exists( + elif RobotDecision.obstacle_exists( target=robot.pos, ball=ball, robots=robots.their_visible_robots, tolerance=TOLERANCE ): score = 0 elif tool.get_distance(ball.pos, robot.pos) < 0.5: score = 0 - elif self._is_robot_inside_pass_area(ball, robot) is False: + elif RobotDecision.is_robot_inside_pass_area(ball, robot) is False: score = 0 else: # ボールとパスを受けるロボットの距離 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..f54a5f09 --- /dev/null +++ b/consai_game/consai_game/world_model/evaluation/threats_evaluation.py @@ -0,0 +1,123 @@ +#!/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 List, Dict + +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 ThreatsModel: + def __init__(self, field: Field, field_points: FieldPoints): + self.threats: List[Threat] = [] + self._field = field + self._field_points = field_points + self._prev_scores: Dict[int, float] = {} # ロボットIDごとの前回のスコア + self._alpha = 0.1 # ローパスフィルターの係数（0-1、小さいほど変化が遅い） + + 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/perception/ball_decision.py b/consai_game/consai_game/world_model/perception/ball_decision.py new file mode 100644 index 00000000..af20336d --- /dev/null +++ b/consai_game/consai_game/world_model/perception/ball_decision.py @@ -0,0 +1,74 @@ +# 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 dataclasses import dataclass +from enum import Enum, auto +from typing import Optional + +from consai_tools.geometry import geometry_tools as tools + +from consai_game.world_model.ball_model import BallModel +from consai_game.world_model.robots_model import Robot, RobotsModel +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 + + +class BallDecision: + """ ボールの位置や動きを判定するクラス. """ + + def is_ball_moving(self, ball: BallModel) -> bool: + """ボールが動いているかを判定するメソッド.""" + + # ボールが動いたと判断する距離 + # ここが小さすぎると、ノイズによって動いた判定になってしまう + BALL_MOVING_DIST_THRESHOLD = 0.3 + MOVING_VELOCITY_THRESHOLD = 0.1 # ボールが動いているとみなす速度の閾値 + + if not ball.is_visible: + return False + + vel_norm = tools.get_norm(ball.vel) + + if vel_norm < 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 > BALL_MOVING_DIST_THRESHOLD: + # 一定距離以上離れたら、動いたと判定してキャプチャした位置をリセット + self.last_ball_pos_to_detect_moving = None + return True + + # 一定距離移動してなければ、ボールは止まっていると判断 + return False 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..c307f7bc --- /dev/null +++ b/consai_game/consai_game/world_model/perception/perception.py @@ -0,0 +1,32 @@ +#!/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() \ No newline at end of file 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..c0c2f834 --- /dev/null +++ b/consai_game/consai_game/world_model/perception/robot_decision.py @@ -0,0 +1,110 @@ +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 + + +class RobotDecision: + """ロボットやボールの位置関係を判定するクラス.""" + + def obstacle_exists(target: State2D, ball: BallModel, robots: dict[int, Robot], 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_backside( + 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( + 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(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/world_model.py b/consai_game/consai_game/world_model/world_model.py index 4c3a2a3f..4ac92c97 100644 --- a/consai_game/consai_game/world_model/world_model.py +++ b/consai_game/consai_game/world_model/world_model.py @@ -50,4 +50,4 @@ class WorldModel: threats: ThreatsModel = ThreatsModel(field, field_points) meta: WorldMetaModel = WorldMetaModel() - evaluation: Evaluation = Evaluation() + evaluation: Evaluation = Evaluation(field, field_points) From 1772b4c8774ff9a4dc6f80413a0d736251eccddb Mon Sep 17 00:00:00 2001 From: agich073 Date: Thu, 14 Aug 2025 16:59:20 +0900 Subject: [PATCH 08/13] =?UTF-8?q?lint=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- consai_game/consai_game/world_model/perception/perception.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/consai_game/consai_game/world_model/perception/perception.py b/consai_game/consai_game/world_model/perception/perception.py index c307f7bc..5292c7be 100644 --- a/consai_game/consai_game/world_model/perception/perception.py +++ b/consai_game/consai_game/world_model/perception/perception.py @@ -23,10 +23,11 @@ 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() \ No newline at end of file + robot_decision: RobotDecision = RobotDecision() From 112f6e5fa7877ccab59ccdea38b9ad7b69bf0e92 Mon Sep 17 00:00:00 2001 From: agich073 Date: Thu, 14 Aug 2025 17:04:59 +0900 Subject: [PATCH 09/13] =?UTF-8?q?lint=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model/evaluation/kick_target_evaluation.py | 2 +- .../world_model/perception/ball_decision.py | 12 +----------- 2 files changed, 2 insertions(+), 12 deletions(-) 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 index 74f7c641..b12d52ac 100644 --- a/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py @@ -27,7 +27,7 @@ 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 consai_game.world_model.robots_model import RobotsModel from consai_msgs.msg import State2D diff --git a/consai_game/consai_game/world_model/perception/ball_decision.py b/consai_game/consai_game/world_model/perception/ball_decision.py index af20336d..27c47c4b 100644 --- a/consai_game/consai_game/world_model/perception/ball_decision.py +++ b/consai_game/consai_game/world_model/perception/ball_decision.py @@ -20,24 +20,14 @@ """ from copy import deepcopy -from dataclasses import dataclass -from enum import Enum, auto -from typing import Optional from consai_tools.geometry import geometry_tools as tools from consai_game.world_model.ball_model import BallModel -from consai_game.world_model.robots_model import Robot, RobotsModel -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 class BallDecision: - """ ボールの位置や動きを判定するクラス. """ + """ボールの位置や動きを判定するクラス.""" def is_ball_moving(self, ball: BallModel) -> bool: """ボールが動いているかを判定するメソッド.""" From cf8404b30826facd89cecbea60e03f880add9a67 Mon Sep 17 00:00:00 2001 From: agich073 Date: Fri, 15 Aug 2025 10:07:24 +0900 Subject: [PATCH 10/13] =?UTF-8?q?robot=5Factivity=5Fmodel=E3=82=92?= =?UTF-8?q?=E7=A7=BB=E6=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model/perception/robot_decision.py | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/consai_game/consai_game/world_model/perception/robot_decision.py b/consai_game/consai_game/world_model/perception/robot_decision.py index c0c2f834..efb5a7ad 100644 --- a/consai_game/consai_game/world_model/perception/robot_decision.py +++ b/consai_game/consai_game/world_model/perception/robot_decision.py @@ -8,6 +8,26 @@ from consai_tools.geometry import geometry_tools as tools +from consai_msgs.msg import MotionCommand + +from dataclasses import dataclass + + +@dataclass +class ReceiveScore: + """ボールをどれだけ受け取りやすいかを保持するデータクラス.""" + + robot_id: int = 0 + intercept_time: float = float("inf") # あと何秒後にボールを受け取れるか + + +@dataclass +class OurRobotsArrived: + """自ロボットが目標位置に到達したか保持するデータクラス.""" + + robot_id: int = 0 + arrived: bool = False + class RobotDecision: """ロボットやボールの位置関係を判定するクラス.""" @@ -108,3 +128,33 @@ def is_ball_front(robot_pos: State2D, ball_pos: State2D, target_pos: State2D) -> if abs(tr_ball_pos.y) > side_dist_threshold: return False return True + + def update_our_robots_arrived(self, our_visible_robots: dict[int, Robot], commands: list[MotionCommand]) -> bool: + """各ロボットが目標位置に到達したか判定する関数.""" + + # ロボットが目標位置に到着したと判定する距離[m] + DIST_ROBOT_TO_DESIRED_THRESHOLD = 0.1 + + # 初期化 + self.our_robots_arrived_list = [] + # エラー処理 + if len(our_visible_robots) == 0 or len(commands) == 0: + return + + # 更新 + for command in commands: + if command.robot_id not in our_visible_robots.keys(): + continue + + robot = our_visible_robots[command.robot_id] + robot_pos = robot.pos + desired_pose = command.desired_pose + # ロボットと目標位置の距離を計算 + dist_robot_to_desired = tools.get_distance(robot_pos, desired_pose) + # 目標位置に到達したか判定結果をリストに追加 + self.our_robots_arrived_list.append( + OurRobotsArrived( + robot_id=robot.robot_id, + arrived=dist_robot_to_desired < DIST_ROBOT_TO_DESIRED_THRESHOLD, + ) + ) From 5fad5e1331617aab7a69885bb42757b188ac4571 Mon Sep 17 00:00:00 2001 From: agich073 Date: Fri, 15 Aug 2025 17:34:40 +0900 Subject: [PATCH 11/13] =?UTF-8?q?tactic=E3=81=AE=E7=A7=BB=E6=A4=8D,=20?= =?UTF-8?q?=E5=AE=9A=E6=95=B0=E3=81=AE=E6=8A=BD=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../evaluation/kick_target_evaluation.py | 54 +++++++------ .../evaluation/threats_evaluation.py | 16 +++- .../world_model/perception/ball_decision.py | 81 +++++++++++++++++-- .../world_model/perception/robot_decision.py | 50 ++++++++++-- 4 files changed, 159 insertions(+), 42 deletions(-) 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 index b12d52ac..00cdce93 100644 --- a/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/kick_target_evaluation.py @@ -56,9 +56,21 @@ class PassTarget: 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の初期化関数.""" - self.hysteresis_distance = 0.3 # shoot_targetの位置と成功率を保持するリスト self.shoot_target_list: list[ShootTarget] = [] @@ -67,10 +79,9 @@ def __init__(self): # pass_targetの位置と成功率を保持するリスト self.pass_target_list: list[PassTarget] = [] - self._half_width = 4.5 - def update_field_pos_list(self, field: Field) -> None: """フィールドの位置候補を更新する関数.""" + quarter_width = field.half_goal_width / 2 one_eighth_width = field.half_goal_width / 4 @@ -84,7 +95,7 @@ def update_field_pos_list(self, field: Field) -> None: ShootTarget(pos=Point(field.half_length, -(quarter_width + one_eighth_width))), ] - self._half_width = field.half_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( @@ -100,11 +111,6 @@ def update( def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours: bool) -> list[ShootTarget]: """各シュートターゲットの成功率を計算し, リストを更新する関数.""" - TOLERANCE = robots.robot_radius # ロボット半径 - MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る - MAX_DISTANCE_SCORE = 60 # スコア計算時のシュートターゲットの最大スコア - MAX_ANGLE_SCORE = 20 # スコア計算時のシュートターゲットの最大角度スコア - MAX_GOALIE_LEAVE_SCORE = 20 # スコア計算時のシュートターゲットがgoalieからどれくらい離れているかの最大スコア # 相手のgoalieの位置でシュートターゲットのスコア計算 goalie_pos = None @@ -117,37 +123,37 @@ def _update_shoot_scores(self, ball: BallModel, robots: RobotsModel, search_ours score = 0 if ( RobotDecision.obstacle_exists( - target=target.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + 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=TOLERANCE + 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, MAX_ANGLE_SCORE - np.rad2deg(angle) * MAX_ANGLE_SCORE / 60 + 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, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 6 + 0, self.MAX_DISTANCE_SCORE - (distance - self.MARGIN) * self.MAX_DISTANCE_SCORE / 6 ) # ディフェンスエリア外から6m以内ならOK if goalie_pos is None: - score += MAX_GOALIE_LEAVE_SCORE + 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) - * MAX_GOALIE_LEAVE_SCORE + * self.MAX_GOALIE_LEAVE_SCORE / (robots.robot_radius * 6) ) target.success_rate = int(score) @@ -158,7 +164,7 @@ def _sort_kick_targets_by_success_rate(self, targets: list[ShootTarget]) -> list def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=False) -> ShootTarget: """ボールからの直線上にロボットがいないシュート位置を返す関数.""" - RATE_MARGIN = 50 # ヒステリシスのためのマージン + 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() @@ -171,16 +177,12 @@ def _search_shoot_pos(self, ball: BallModel, robots: RobotsModel, search_ours=Fa return self.shoot_target_list[0] # ヒステリシス処理 - if self.shoot_target_list[0].success_rate > last_shoot_target_list[0].success_rate + RATE_MARGIN: + 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]: """各パスターゲットの成功率を計算し, リストを返す関数.""" - TOLERANCE = robots.robot_radius * 2 # ロボット直径 - MARGIN = 1.8 # ディフェンスエリアの距離分マージンを取る - MAX_DISTANCE_SCORE = 55 # スコア計算時のシュートターゲットの最大スコア - MAX_ANGLE_SCORE = 45 # スコア計算時のシュートターゲットの最大角度スコア pass_target_list: list[PassTarget] = [] @@ -188,13 +190,13 @@ def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_our score = 0 if ( RobotDecision.obstacle_exists( - target=robot.pos, ball=ball, robots=robots.our_visible_robots, tolerance=TOLERANCE + 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=TOLERANCE + 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: @@ -205,14 +207,14 @@ def make_pass_target_list(self, ball: BallModel, robots: RobotsModel, search_our # ボールとパスを受けるロボットの距離 distance = tool.get_distance(ball.pos, robot.pos) score += max( - 0, MAX_DISTANCE_SCORE - (distance - MARGIN) * MAX_DISTANCE_SCORE / 4 + 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, MAX_ANGLE_SCORE - np.rad2deg(angle) * 0.5) # 小さい角度ほど高得点 + 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 - MARGIN) * 10) # ボールからディフェンスエリアまで2m以内だったら減点 + score -= max(0, 20 - (distance - self.MARGIN) * 10) # ボールからディフェンスエリアまで2m以内だったら減点 pass_target_list.append( PassTarget( robot_id=robot.robot_id, diff --git a/consai_game/consai_game/world_model/evaluation/threats_evaluation.py b/consai_game/consai_game/world_model/evaluation/threats_evaluation.py index f54a5f09..da6b7bcd 100644 --- a/consai_game/consai_game/world_model/evaluation/threats_evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/threats_evaluation.py @@ -22,28 +22,36 @@ """ from dataclasses import dataclass -from typing import List, Dict +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 ThreatsModel: +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ごとの前回のスコア - self._alpha = 0.1 # ローパスフィルターの係数（0-1、小さいほど変化が遅い） def _apply_low_pass_filter(self, robot_id: int, new_score: float) -> float: """ローパスフィルターを適用してスコアを平滑化する @@ -60,7 +68,7 @@ def _apply_low_pass_filter(self, robot_id: int, new_score: float) -> float: return new_score # 前回のスコアと新しいスコアを重み付けして結合 - filtered_score = self._prev_scores[robot_id] * (1 - self._alpha) + new_score * self._alpha + filtered_score = self._prev_scores[robot_id] * (1 - self.ALPHA) + new_score * self.ALPHA self._prev_scores[robot_id] = filtered_score return filtered_score diff --git a/consai_game/consai_game/world_model/perception/ball_decision.py b/consai_game/consai_game/world_model/perception/ball_decision.py index 27c47c4b..df817494 100644 --- a/consai_game/consai_game/world_model/perception/ball_decision.py +++ b/consai_game/consai_game/world_model/perception/ball_decision.py @@ -25,24 +25,38 @@ 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: """ボールが動いているかを判定するメソッド.""" - # ボールが動いたと判断する距離 - # ここが小さすぎると、ノイズによって動いた判定になってしまう - BALL_MOVING_DIST_THRESHOLD = 0.3 - MOVING_VELOCITY_THRESHOLD = 0.1 # ボールが動いているとみなす速度の閾値 - if not ball.is_visible: return False vel_norm = tools.get_norm(ball.vel) - if vel_norm < MOVING_VELOCITY_THRESHOLD: + 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) @@ -55,10 +69,63 @@ def is_ball_moving(self, ball: BallModel) -> bool: # 停止時のボール位置からの移動距離 move_distance = tools.get_distance(ball.pos, self.last_ball_pos_to_detect_moving) - if move_distance > BALL_MOVING_DIST_THRESHOLD: + 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/robot_decision.py b/consai_game/consai_game/world_model/perception/robot_decision.py index efb5a7ad..5a45ae47 100644 --- a/consai_game/consai_game/world_model/perception/robot_decision.py +++ b/consai_game/consai_game/world_model/perception/robot_decision.py @@ -32,7 +32,12 @@ class OurRobotsArrived: class RobotDecision: """ロボットやボールの位置関係を判定するクラス.""" - def obstacle_exists(target: State2D, ball: BallModel, robots: dict[int, Robot], tolerance) -> bool: + 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(): @@ -132,9 +137,6 @@ def is_ball_front(robot_pos: State2D, ball_pos: State2D, target_pos: State2D) -> def update_our_robots_arrived(self, our_visible_robots: dict[int, Robot], commands: list[MotionCommand]) -> bool: """各ロボットが目標位置に到達したか判定する関数.""" - # ロボットが目標位置に到着したと判定する距離[m] - DIST_ROBOT_TO_DESIRED_THRESHOLD = 0.1 - # 初期化 self.our_robots_arrived_list = [] # エラー処理 @@ -155,6 +157,44 @@ def update_our_robots_arrived(self, our_visible_robots: dict[int, Robot], comman self.our_robots_arrived_list.append( OurRobotsArrived( robot_id=robot.robot_id, - arrived=dist_robot_to_desired < DIST_ROBOT_TO_DESIRED_THRESHOLD, + arrived=dist_robot_to_desired < 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 From fbcccb7f9ff6105e6628ba27acc4e79fd743ac00 Mon Sep 17 00:00:00 2001 From: agich073 Date: Fri, 15 Aug 2025 17:41:38 +0900 Subject: [PATCH 12/13] =?UTF-8?q?threats=5Fevaluation=E3=81=8C=E5=8B=95?= =?UTF-8?q?=E3=81=8F=E3=82=88=E3=81=86=E3=81=AB=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../consai_game/visualization/visualize_msg_publisher_node.py | 4 ++-- consai_game/consai_game/world_model/evaluation/evaluation.py | 4 ++-- .../consai_game/world_model/world_model_provider_node.py | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) 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/evaluation/evaluation.py b/consai_game/consai_game/world_model/evaluation/evaluation.py index 46f9e2bb..19652d39 100644 --- a/consai_game/consai_game/world_model/evaluation/evaluation.py +++ b/consai_game/consai_game/world_model/evaluation/evaluation.py @@ -21,7 +21,7 @@ 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 ThreatsModel +from consai_game.world_model.evaluation.threats_evaluation import ThreatsEvaluation from consai_game.world_model.field_model import Field, FieldPoints @@ -33,4 +33,4 @@ class Evaluation: field_points: FieldPoints = FieldPoints.create_field_points(field) kick_target: KickTargetEvaluation = KickTargetEvaluation() relative_position: RelativePositionEvaluation = RelativePositionEvaluation() - threats_model: ThreatsModel = ThreatsModel(field, field_points) + threats_evaluation: ThreatsEvaluation = ThreatsEvaluation(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 b802c0bf..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 @@ -153,7 +153,7 @@ def update(self) -> None: 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, ) From 92164d1b06660f94af5d3dc2e20a2c8e666a5b89 Mon Sep 17 00:00:00 2001 From: agich073 Date: Wed, 20 Aug 2025 16:58:02 +0900 Subject: [PATCH 13/13] =?UTF-8?q?=E9=87=8D=E8=A4=87=E3=81=97=E3=81=9F?= =?UTF-8?q?=E9=96=A2=E6=95=B0=E3=81=AE=E5=89=8A=E9=99=A4,=20update=5Four?= =?UTF-8?q?=5Frobots=5Farrived=E3=81=AE=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model/perception/robot_decision.py | 148 ++++++++++-------- 1 file changed, 83 insertions(+), 65 deletions(-) diff --git a/consai_game/consai_game/world_model/perception/robot_decision.py b/consai_game/consai_game/world_model/perception/robot_decision.py index 5a45ae47..2a72f179 100644 --- a/consai_game/consai_game/world_model/perception/robot_decision.py +++ b/consai_game/consai_game/world_model/perception/robot_decision.py @@ -10,7 +10,7 @@ from consai_msgs.msg import MotionCommand -from dataclasses import dataclass +from dataclasses import dataclass, field @dataclass @@ -29,6 +29,79 @@ class OurRobotsArrived: 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: """ロボットやボールの位置関係を判定するクラス.""" @@ -66,23 +139,6 @@ def is_robot_inside_pass_area(ball: BallModel, robot: Robot) -> bool: return False return True - def is_robot_backside( - 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( robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float ) -> bool: @@ -111,55 +167,17 @@ def is_robot_on_kick_line( return True - def is_ball_front(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 - - def update_our_robots_arrived(self, our_visible_robots: dict[int, Robot], commands: list[MotionCommand]) -> bool: - """各ロボットが目標位置に到達したか判定する関数.""" - - # 初期化 - self.our_robots_arrived_list = [] - # エラー処理 - if len(our_visible_robots) == 0 or len(commands) == 0: - return - - # 更新 + 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 our_visible_robots.keys(): + if command.robot_id not in robots: continue - - robot = our_visible_robots[command.robot_id] - robot_pos = robot.pos - desired_pose = command.desired_pose - # ロボットと目標位置の距離を計算 - dist_robot_to_desired = tools.get_distance(robot_pos, desired_pose) - # 目標位置に到達したか判定結果をリストに追加 - self.our_robots_arrived_list.append( - OurRobotsArrived( - robot_id=robot.robot_id, - arrived=dist_robot_to_desired < self.DIST_ROBOT_TO_DESIRED_THRESHOLD, - ) - ) + 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: