Apollo3D/Apollo3D_SE
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

repair some bugs and path planning 0.2.0 version

Browse Source
This commit is contained in:
徐学颢
2026-03-24 20:42:18 +08:00
parent bc288cc2ee
commit b49185a04a
2 changed files with 49 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
agent/agent.py
View File

@@ -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
)

81
agent/path_planner.py
View File

@@ -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. 转换坐标