From b49185a04a2b97e5212f2c1d794da3ebbfa6969c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BE=90=E5=AD=A6=E9=A2=A2?= Date: Tue, 24 Mar 2026 20:42:18 +0800 Subject: [PATCH] repair some bugs and path planning 0.2.0 version --- agent/agent.py | 15 +++++--- agent/path_planner.py | 81 +++++++++++++++++++++---------------------- 2 files changed, 49 insertions(+), 47 deletions(-) diff --git a/agent/agent.py b/agent/agent.py index bca9972..0ca485a 100644 --- a/agent/agent.py +++ b/agent/agent.py @@ -99,9 +99,10 @@ class Agent: """ Basic example of a behavior: moves the robot toward the goal while handling the ball. """ - their_goal_pos = self.agent.world.field.get_their_goal_position()[:2] + their_goal_pos = np.array(self.agent.world.field.get_their_goal_position()[:2]) ball_pos = self.agent.world.ball_pos[:2] my_pos = self.agent.world.global_position[:2] + next_target = None ball_to_goal = their_goal_pos - ball_pos bg_norm = np.linalg.norm(ball_to_goal) @@ -132,20 +133,24 @@ class Agent: if not aligned or not behind_ball: self.path_planner.set_target(carry_ball_pos) current_time = self.agent.world.server_time - next_target = self.path_planner.update(my_pos, current_time=current_time) if next_target is not None else carry_ball_pos + next_target = self.path_planner.update(my_pos, current_time=current_time) + if next_target is None: + next_target = carry_ball_pos self.agent.skills_manager.execute( "Walk", - target_2d=next_target, + target_2d=carry_ball_pos, is_target_absolute=True, orientation=None if np.linalg.norm(my_pos - carry_ball_pos) > 2 else desired_orientation ) else: self.path_planner.set_target(their_goal_pos) current_time = self.agent.world.server_time - next_target = self.path_planner.update(my_pos, current_time=current_time) if next_target is not None else their_goal_pos + next_target = self.path_planner.update(my_pos, current_time=current_time) + if next_target is None: + next_target = their_goal_pos self.agent.skills_manager.execute( "Walk", - target_2d=next_target, + target_2d=their_goal_pos, is_target_absolute=True, orientation=desired_orientation ) diff --git a/agent/path_planner.py b/agent/path_planner.py index d9af8b5..6dc4a00 100644 --- a/agent/path_planner.py +++ b/agent/path_planner.py @@ -103,57 +103,26 @@ class PathPlanner: for j in range(self.ny): y = self._grid_to_world_y(j) if abs(y) > goal_half_width: - self.cost_map[i, j] = -3 + self.static_cost_map[i, j] = -3 # 右侧边界：x > field_half_len 的区域，保留球门开口 for i in range(self._world_to_grid_x(field_half_len + 0.001), self.nx): for j in range(self.ny): y = self._grid_to_world_y(j) if abs(y) > goal_half_width: - self.cost_map[i, j] = -3 + self.static_cost_map[i, j] = -3 # 上下边界 for i in range(self.nx): for j in [0, self.ny-1]: - self.cost_map[i, j] = -3 + self.static_cost_map[i, j] = -3 # 可选：如果需要在边界内留出线宽，可额外处理 # 4. 门柱（作为硬墙） goal_post_positions = [ - (field_half_len+1, goal_half_width+1), # 右侧上柱 - (field_half_len+1, goal_half_width), - (field_half_len+1, goal_half_width-1), - (field_half_len, goal_half_width-1), - (field_half_len, goal_half_width), - (field_half_len, goal_half_width+1), - (field_half_len-1, goal_half_width+1), - (field_half_len-1, goal_half_width), - (field_half_len-1, goal_half_width-1), - (field_half_len+1, -goal_half_width+1), # 右侧下柱 - (field_half_len+1, -goal_half_width), - (field_half_len+1, -goal_half_width-1), - (field_half_len, -goal_half_width-1), - (field_half_len, -goal_half_width), - (field_half_len, -goal_half_width+1), - (field_half_len-1, -goal_half_width+1), - (field_half_len-1, -goal_half_width), - (field_half_len-1, -goal_half_width-1), - (-field_half_len+1, goal_half_width+1), # 左侧上柱 - (-field_half_len+1, goal_half_width), - (-field_half_len+1, goal_half_width-1), - (-field_half_len, goal_half_width-1), - (-field_half_len, goal_half_width), - (-field_half_len, goal_half_width+1), - (-field_half_len-1, goal_half_width+1), - (-field_half_len-1, goal_half_width), - (-field_half_len-1, goal_half_width-1), - (-field_half_len+1, -goal_half_width+1), # 左侧下柱 - (-field_half_len+1, -goal_half_width), - (-field_half_len+1, -goal_half_width-1), - (-field_half_len, -goal_half_width-1), - (-field_half_len, -goal_half_width), - (-field_half_len, -goal_half_width+1), - (-field_half_len-1, -goal_half_width+1), - (-field_half_len-1, -goal_half_width), - (-field_half_len-1, -goal_half_width-1), + + (field_half_len, goal_half_width), # 右侧上柱 + (field_half_len, -goal_half_width), # 右侧下柱 + (-field_half_len, goal_half_width), # 左侧上柱 + (-field_half_len, -goal_half_width), # 左侧下柱 ] for px, py in goal_post_positions: ix, iy = self._world_to_grid(px, py) @@ -164,7 +133,7 @@ class PathPlanner: if not (0 <= nx < self.nx and 0 <= ny < self.ny): continue dist = np.hypot(dx * self.res, dy * self.res) - if dist <= self.post_radius: + if dist <= post_radius: self.static_cost_map[nx, ny] = -3 # ---------- 获取动态障碍物（对手球员） ---------- @@ -179,6 +148,16 @@ class PathPlanner: continue # 跳过未看到的球员 return opponents + def _get_teammate_positions(self) -> List[np.ndarray]: + """从 FieldLandmarks 获取所有队友球员的全局位置""" + teammates = [] + for player in self.world.our_team_players: + if player.last_seen_time is not None and (self.world.server_time - player.last_seen_time) < 1.0: + teammates.append(player.position[:2]) # 只使用 x, y + else: + # 长时间未看到的球员不添加到避障列表中 + continue # 跳过未看到的球员 + return teammates # ---------- 动态障碍物添加到代价地图 ---------- def _apply_opponents_to_map(self, cost_map: np.ndarray, opponents: List[np.ndarray]): """ @@ -208,6 +187,22 @@ class PathPlanner: if cost_map[nx, ny] >= 0: cost_map[nx, ny] += 10.0 * (1.0 - (dist - effective_radius) / buffer_width) + def _apply_teammate_to_map(self, cost_map, teammates): + soft_radius = self.robot_radius + 0.2 # 允许稍微接近 + for tm in teammates: + ix, iy = self._world_to_grid(tm[0], tm[1]) + rad_cells = int(soft_radius / self.res) + 1 + for dx in range(-rad_cells, rad_cells+1): + for dy in range(-rad_cells, rad_cells+1): + nx, ny = ix+dx, iy+dy + if not (0 <= nx < self.nx and 0 <= ny < self.ny): + continue + dist = np.hypot(dx*self.res, dy*self.res) + if dist <= soft_radius: + # 不设为硬墙，只加较小代价 + if cost_map[nx, ny] >= 0: + cost_map[nx, ny] += 5.0 # 代价比对手小 + # ---------- 启发函数 ---------- def _heuristic(self, ix1: int, iy1: int, ix2: int, iy2: int) -> float: """对角线距离（允许8方向移动）""" @@ -268,9 +263,11 @@ class PathPlanner: :param timeout_ms: 超时时间（毫秒） :return: 路径点列表（世界坐标），若失败返回空列表 """ - # 1. 获取对手并构建动态代价地图 - opponents = self._get_opponent_positions() + # 1. 获取队友并构建动态代价地图 + teammates = self._get_teammate_positions() cost_map = self.static_cost_map.copy() + self._apply_teammate_to_map(cost_map, teammates) + opponents = self._get_opponent_positions() self._apply_opponents_to_map(cost_map, opponents) # 2. 转换坐标