world_modelとtacticからの評価関数の抜き出し

consai_game/consai_game/tactic/composite/composite_defense.py

@@ -12,22 +12,29 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""
-条件に応じてディフェンス動作やキックやパスを切り替えるTactic
-"""
+"""条件に応じてディフェンス動作やキックやパスを切り替えるTactic."""
 
 from consai_game.core.tactic.tactic_base import TacticBase
 from consai_game.core.tactic.composite_tactic_base import CompositeTacticBase
 from consai_game.tactic.kick import Kick
 from consai_game.tactic.receive import Receive
 from consai_game.world_model.world_model import WorldModel
+
 from consai_tools.geometry import geometry_tools as tool
 
 from consai_msgs.msg import MotionCommand
 
 
 class CompositeDefense(CompositeTacticBase):
+    """条件に応じてディフェンス動作を切り替えるTactic."""
+
     def __init__(self, tactic_default: TacticBase, do_receive: bool = True):
+        """
+        コンストラクタ.
+
+        使用するTacticを指定と各変数の初期化を実行.
+        """
+
         super().__init__(
             tactic_shoot=Kick(is_pass=False),
             tactic_pass=Kick(is_pass=True),
@@ -69,16 +76,16 @@ def run(self, world_model: WorldModel) -> MotionCommand:
     def control_the_ball(self, world_model: WorldModel) -> MotionCommand:
         """ボールを制御するためのTacticを実行する関数."""
 
-        if world_model.kick_target.best_shoot_target.success_rate > 50:
+        if world_model.evaluation.kick_target.best_shoot_target.success_rate > 50:
             # シュートできる場合
-            self.tactic_shoot.target_pos = world_model.kick_target.best_shoot_target.pos
+            self.tactic_shoot.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos
             return self.run_sub_tactic(self.tactic_shoot, world_model)
 
-        elif world_model.kick_target.best_pass_target.success_rate > 30:
+        elif world_model.evaluation.kick_target.best_pass_target.success_rate > 30:
             # パスできる場合
-            self.tactic_pass.target_pos = world_model.kick_target.best_pass_target.robot_pos
+            self.tactic_pass.target_pos = world_model.evaluation.kick_target.best_pass_target.robot_pos
             return self.run_sub_tactic(self.tactic_pass, world_model)
 
         # シュート成功率が一番高いところに向かってパスする(クリア)
-        self.tactic_pass.target_pos = world_model.kick_target.best_shoot_target.pos
+        self.tactic_pass.target_pos = world_model.evaluation.kick_target.best_shoot_target.pos
         return self.run_sub_tactic(self.tactic_pass, world_model)

consai_game/consai_game/tactic/composite/composite_offense.py

@@ -12,29 +12,34 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""
-条件に応じてキックやパスを切り替えるTactic
-"""
+"""条件に応じてキックやパスを切り替えるTactic."""
+
 import copy
+import math
+from typing import Optional, Set
+
 from consai_game.core.tactic.tactic_base import TacticBase
 from consai_game.core.tactic.composite_tactic_base import CompositeTacticBase
 from consai_game.tactic.kick import Kick
 from consai_game.tactic.receive import Receive
-from consai_game.world_model.world_model import WorldModel
 from consai_game.tactic.steal_ball import StealBall
-
+from consai_game.world_model.world_model import WorldModel
 
 from consai_msgs.msg import MotionCommand
 from consai_msgs.msg import State2D
+
 from consai_tools.geometry import geometry_tools as tools
-import math
-from typing import Optional, Set
 
 
 class SharedInfo:
     """CompositeOffenseの情報を共有するクラス"""
 
     def __init__(self):
+        """
+        コンストラクタ.
+
+        メンバ変数の初期化を実行.
+        """
         self.assigned_robot_ids: Set[int] = set()  # CompositeOffenseを担当しているロボットのID
         self.update_conter: int = 0  # 内部用更新カウンター
         self.can_control_ball_id: Optional[bool] = None  # ボールを操作できるロボットのID
@@ -113,9 +118,17 @@ def ball_receiving_candidates(self, world_model: WorldModel) -> tuple[Optional[i
 
 
 class CompositeOffense(CompositeTacticBase):
+    """条件に応じてキックやパスを切り替えるTactic."""
+
     shared_info = SharedInfo()
 
     def __init__(self, tactic_default: TacticBase, is_setplay=False, force_pass=False, kick_score_threshold=30):
+        """
+        コンストラクタ.
+
+        使用するtacticやメンバ変数の初期化を実行.
+        """
+
         super().__init__(
             tactic_shoot=Kick(is_pass=False, is_setplay=is_setplay),
             tactic_pass=Kick(is_pass=True, is_setplay=is_setplay),
@@ -139,6 +152,7 @@ def reset(self, robot_id: int) -> None:
         self.shared_info.register_robot(robot_id)
 
     def exit(self):
+        """Exit the tactic and unregister the robot ID."""
         super().exit()
 
         # ロボットのIDを登録解除する
@@ -169,26 +183,27 @@ 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
 
             return self.run_sub_tactic(self.tactic_pass, world_model)
 
         # 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:

consai_game/consai_game/tactic/dribble.py

@@ -156,7 +156,9 @@ def run(self, world_model: WorldModel) -> MotionCommand:
             dist_robot_to_ball=dist_robot_to_ball,
             dist_ball_to_target=dist_ball_to_target,
             dribble_diff_angle=np.rad2deg(dribble_diff_angle),
-            ball_is_front=self.ball_is_front(ball_pos=ball_pos, robot_pos=robot_pos),
+            ball_is_front=world_model.evaluation.relative_position.is_ball_front(
+                robot_pos=robot_pos, ball_pos=ball_pos, target_pos=self.target_pos
+            ),
         )
 
         command = MotionCommand()
@@ -178,28 +180,6 @@ def run(self, world_model: WorldModel) -> MotionCommand:
         self.append_machine_state_to_name()  # デバッグのため、状態を名前に追加
         return command
 
-    def ball_is_front(self, ball_pos: State2D, robot_pos: State2D) -> bool:
-        """ボールがロボットの前にあるかどうかを判定する."""
-        FRONT_DIST_THRESHOLD = 0.15  # 正面方向にどれだけ離れることを許容するか
-        SIDE_DIST_THRESHOLD = 0.05  # 横方向にどれだけ離れることを許容するか
-
-        # ロボットを中心に、ターゲットを+x軸とした座標系を作る
-        trans = tool.Trans(robot_pos, tool.get_angle(robot_pos, self.target_pos))
-        tr_ball_pos = trans.transform(ball_pos)
-
-        # ボールがロボットの後ろにある
-        if tr_ball_pos.x < 0:
-            return False
-
-        # ボールが正面から離れすぎている
-        if tr_ball_pos.x > FRONT_DIST_THRESHOLD:
-            return False
-
-        # ボールが横方向に離れすぎている
-        if abs(tr_ball_pos.y) > SIDE_DIST_THRESHOLD:
-            return False
-        return True
-
     def approach_to_ball(self, command: MotionCommand, ball_pos: State2D) -> MotionCommand:
         """ボールに近づくコマンドを返す."""
         APPROACH_DIST = 0.09  # ボールに近づく距離。ロボットの半径と同じくらいにする

consai_game/consai_game/tactic/kick.py

@@ -14,18 +14,20 @@
 
 """キック動作に関するTacticを定義するモジュール."""
 
-import numpy as np
 import copy
-from consai_msgs.msg import MotionCommand, State2D
-
-from consai_tools.geometry import geometry_tools as tool
 
 from consai_game.world_model.world_model import WorldModel
 from consai_game.core.tactic.tactic_base import TacticBase
 from consai_game.utils.generate_dummy_ball_position import generate_dummy_ball_position
 
+from consai_msgs.msg import MotionCommand, State2D
+
+from consai_tools.geometry import geometry_tools as tool
+
 from transitions.extensions import GraphMachine
 
+import numpy as np
+
 
 class KickStateMachine(GraphMachine):
     """状態遷移マシン."""
@@ -136,9 +138,11 @@ def run(self, world_model: WorldModel) -> MotionCommand:
             width_threshold = 0.03
 
         self.machine.update(
-            robot_is_backside=self.robot_is_backside(robot_pos, ball_pos, self.final_target_pos),
-            robot_is_on_kick_line=self.robot_is_on_kick_line(
-                robot_pos, ball_pos, self.final_target_pos, width_threshold=width_threshold
+            robot_is_backside=world_model.evaluation.relative_position.is_robot_backside(
+                robot_pos, ball_pos, self.final_target_pos, self.ANGLE_BALL_TO_ROBOT_THRESHOLD
+            ),
+            robot_is_on_kick_line=world_model.evaluation.relative_position.is_robot_on_kick_line(
+                robot_pos, ball_pos, self.final_target_pos, width_threshold
             ),
         )
 
@@ -201,47 +205,6 @@ def run(self, world_model: WorldModel) -> MotionCommand:
         self.append_machine_state_to_name()  # デバッグのため、状態を名前に追加
         return command
 
-    def robot_is_backside(self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D) -> bool:
-        """ボールからターゲットを見て、ロボットが後側に居るかを判定する."""
-        # ボールからターゲットへの座標系を作成
-        trans = tool.Trans(ball_pos, tool.get_angle(ball_pos, target_pos))
-        tr_robot_pos = trans.transform(robot_pos)
-
-        # ボールから見たロボットの位置の角度
-        # ボールの後方にいれば角度は90度以上
-        tr_ball_to_robot_angle = tool.get_angle(State2D(x=0.0, y=0.0), tr_robot_pos)
-
-        if abs(tr_ball_to_robot_angle) > np.deg2rad(self.ANGLE_BALL_TO_ROBOT_THRESHOLD):
-            return True
-        return False
-
-    def robot_is_on_kick_line(
-        self, robot_pos: State2D, ball_pos: State2D, target_pos: State2D, width_threshold: float
-    ) -> bool:
-        """ボールからターゲットまでの直線上にロボットが居るかを判定する.
-
-        ターゲットまでの距離が遠いと、角度だけで狙いを定めるのは難しいため、位置を使って判定する.
-        """
-        MINIMAL_THETA_THRESHOLD = 45  # 最低限満たすべきロボットの角度
-
-        # ボールからターゲットへの座標系を作成
-        trans = tool.Trans(ball_pos, tool.get_angle(ball_pos, target_pos))
-        tr_robot_pos = trans.transform(robot_pos)
-        tr_robot_theta = trans.transform_angle(robot_pos.theta)
-
-        # ボールより前にロボットが居る場合
-        if tr_robot_pos.x > 0.0:
-            return False
-
-        # ターゲットを向いていない
-        if abs(tr_robot_theta) > np.deg2rad(MINIMAL_THETA_THRESHOLD):
-            return False
-
-        if abs(tr_robot_pos.y) > width_threshold:
-            return False
-
-        return True
-
     def move_to_backside_pose(
         self, ball_pos: State2D, robot_pos: State2D, target_pos: State2D, distance: float
     ) -> State2D:
@@ -290,6 +253,6 @@ def pass_power(self, ball_pos: State2D, target_pos: State2D, world_model: WorldM
             return MAX_KICK_POWER
         else:
             # 線形補間
-            return MIN_PASS_POWER + (MAX_KICK_POWER - MIN_PASS_POWER) * (
-                distance_to_target - MIN_PASS_DISTANCE
-            ) / (MAX_PASS_DISTANCE - MIN_PASS_DISTANCE)
+            return MIN_PASS_POWER + (MAX_KICK_POWER - MIN_PASS_POWER) * (distance_to_target - MIN_PASS_DISTANCE) / (
+                MAX_PASS_DISTANCE - MIN_PASS_DISTANCE
+            )

consai_game/consai_game/visualization/visualize_msg_publisher_node.py

@@ -44,15 +44,15 @@ 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(
             self.ball_activity_to_vis_msg(activity=world_model.ball_activity, ball=world_model.ball)
         )
 
         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)