34f74c3005
头部追踪： - 球可见时 he1/he2 追踪球方位，球不可见时左右扫描 - 在技能执行后、commit_motor_targets_pd 前覆盖头部电机目标 球位置过时检测： - world.py 新增 ball_age property 和阈值常量 - 球消失 ≥3s 时外场球员原地旋转搜索，门将不再基于过期信息拦截 最近球员独占： - 每个 agent 自行判断是否离球最近（视觉队友位置 + 0.5m 死区防抖） - 最近球员执行 carry_ball，其他球员走到编号对应的支援站位 定位球基础处理： - THEIR_KICK 时外场球员退到中圈半径外防守位，面朝球 - OUR_KICK 时最近球员上前踢球，其他人进攻支援位 决策层重构： - update_current_behavior 外场分支拆分为 playmode_group 判断 - 新增 _outfield_decide / _am_i_nearest_to_ball / support_position / _defensive_set_piece
86 lines
3.4 KiB
Python
86 lines
3.4 KiB
Python
from world.commons.field import Field
|
|
import numpy as np
|
|
from world.commons.other_robot import OtherRobot
|
|
from world.commons.field import FIFAField, HLAdultField, Soccer7vs7Field
|
|
from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum
|
|
|
|
|
|
class World:
|
|
"""
|
|
Represents the current simulation world, containing all relevant
|
|
information about the environment, the ball, and the robots.
|
|
"""
|
|
|
|
MAX_PLAYERS_PER_TEAM = 7
|
|
|
|
def __init__(self, agent, team_name: str, number: int, field_name: str):
|
|
"""
|
|
Initializes the world state.
|
|
|
|
Args:
|
|
agent: Reference to the agent that owns this world.
|
|
team_name (str): The name of the agent's team.
|
|
number (int): The player's number within the team.
|
|
field_name (str): The name of the field to initialize
|
|
(e.g., 'fifa' or 'hl_adult').
|
|
"""
|
|
|
|
from agent.base_agent import Agent # type hinting
|
|
|
|
self.agent: Agent = agent
|
|
self.team_name: str = team_name
|
|
self.number: int = number
|
|
self.playmode: PlayModeEnum = PlayModeEnum.NOT_INITIALIZED
|
|
self.playmode_group: PlayModeGroupEnum = PlayModeGroupEnum.NOT_INITIALIZED
|
|
self.is_left_team: bool = None
|
|
self.game_time: float = None
|
|
self.server_time: float = None
|
|
self.score_left: int = None
|
|
self.score_right: int = None
|
|
self.their_team_name: str = None
|
|
self.last_server_time: float = None
|
|
self._global_cheat_position: np.ndarray = np.zeros(3)
|
|
self.global_position: np.ndarray = np.zeros(3)
|
|
self.ball_pos: np.ndarray = np.zeros(3)
|
|
self.ball_last_pos: np.ndarray = np.zeros(3)
|
|
self.ball_last_update_time: float = None
|
|
self.ball_velocity_2d: np.ndarray = np.zeros(2)
|
|
self.ball_speed: float = 0.0
|
|
self.is_ball_pos_updated: bool = False
|
|
self.our_team_players: list[OtherRobot] = [OtherRobot() for _ in range(self.MAX_PLAYERS_PER_TEAM)]
|
|
self.their_team_players: list[OtherRobot] = [OtherRobot(is_teammate=False) for _ in
|
|
range(self.MAX_PLAYERS_PER_TEAM)]
|
|
self.field: Field = self.__initialize_field(field_name=field_name)
|
|
|
|
def update(self) -> None:
|
|
"""
|
|
Updates the world state
|
|
"""
|
|
self.playmode_group = PlayModeGroupEnum.get_group_from_playmode(
|
|
playmode=self.playmode, is_left_team=self.is_left_team
|
|
)
|
|
|
|
# ------------------------------------------------------------------
|
|
# Ball freshness
|
|
# ------------------------------------------------------------------
|
|
BALL_FRESH_THRESHOLD: float = 0.6 # < 0.6s: reliable
|
|
BALL_STALE_THRESHOLD: float = 3.0 # > 3.0s: completely lost
|
|
|
|
@property
|
|
def ball_age(self) -> float:
|
|
"""Seconds since the ball position was last updated by vision."""
|
|
if self.ball_last_update_time is None or self.server_time is None:
|
|
return float("inf")
|
|
return self.server_time - self.ball_last_update_time
|
|
|
|
def is_fallen(self) -> bool:
|
|
return self.global_position[2] < 0.3
|
|
|
|
def __initialize_field(self, field_name: str) -> Field:
|
|
if field_name in ('hl_adult', 'hl_adult_2020', 'hl_adult_2019',):
|
|
return HLAdultField(world=self)
|
|
elif field_name in ('sim3d_7vs7'):
|
|
return Soccer7vs7Field(world=self)
|
|
else:
|
|
return FIFAField(world=self)
|