@@ -1,520 +0,0 @@
import numpy as np
from heapq import heappush , heappop
from typing import List , Tuple , Optional
from world . world import World # 假设你的世界类
from world . commons . field_landmarks import FieldLandmarks # 假设你的地标类
import time
class PathPlanner :
"""
足球机器人全局路径规划器（
- 静态地图：边界硬墙，球门内部可通行，门柱硬墙
- 动态障碍物：对手球员（扩大半径 + 缓冲代价）
- 支持多目标点队列，自动切换
- 不预测对手运动，仅使用当前帧位置
"""
def __init__ ( self , world , grid_resolution : float = 0.2 ) :
"""
:param world: 世界对象，需包含 field_landmarks 和 global_position 属性
:param grid_resolution: 栅格分辨率（米/格）
"""
self . world = world
self . res = grid_resolution
# 球场参数（基于 Sim3D7vs7SoccerField）
self . field_half_len = 27.5 # 球场半长（不含球门深度）
self . field_half_width = 18.0 # 球场半宽
self . goal_width = 3.66 # 球门宽度
self . goal_depth = 1.0 # 球门深度
self . goal_half_width = self . goal_width / 2.0
self . post_radius = 0.05 # 门柱半径
# 机器人物理参数
self . robot_radius = 0.2 # 机器人半径（米）
self . safety_margin = 0.2 # 避障安全余量（代替预测）
# 栅格尺寸
# x 范围扩展以包含球门内部：[-field_half_len - goal_depth, field_half_len + goal_depth]
self . x_min = - self . field_half_len - self . goal_depth
self . x_max = self . field_half_len + self . goal_depth
self . y_min = - self . field_half_width
self . y_max = self . field_half_width
self . nx = int ( ( self . x_max - self . x_min ) / self . res ) + 1
self . ny = int ( ( self . y_max - self . y_min ) / self . res ) + 1
# 静态代价地图（初始化后不再改变）
self . static_cost_map = np . zeros ( ( self . nx , self . ny ) , dtype = np . float32 )
self . _init_static_map ( )
# 多目标点管理
self . waypoints : List [ np . ndarray ] = [ ] # 目标点列表
self . current_wp_idx : int = 0
self . current_path : List [ np . ndarray ] = [ ] # 当前规划的完整路径（世界坐标）
self . replan_interval : float = 1.0 # 重新规划频率（ ）
self . last_replan_time : float = 0.0
self . arrival_threshold : float = 0.3 # 到达目标的距离阈值（米）
# ---------- 坐标转换辅助 ----------
def _world_to_grid ( self , x : float , y : float ) - > Tuple [ int , int ] :
""" 世界坐标 -> 栅格坐标（边界裁剪） """
ix = int ( ( x - self . x_min ) / self . res )
iy = int ( ( y - self . y_min ) / self . res )
ix = max ( 0 , min ( ix , self . nx - 1 ) )
iy = max ( 0 , min ( iy , self . ny - 1 ) )
return ix , iy
def _grid_to_world ( self , ix : int , iy : int ) - > Tuple [ float , float ] :
""" 栅格坐标 -> 世界坐标（中心点） """
x = ix * self . res + self . x_min
y = iy * self . res + self . y_min
return x , y
def _world_to_grid_x ( self , x : float ) - > int :
return int ( ( x - self . x_min ) / self . res )
def _world_to_grid_y ( self , y : float ) - > int :
return int ( ( y - self . y_min ) / self . res )
def _grid_to_world_x ( self , ix : int ) - > float :
return ix * self . res + self . x_min
def _grid_to_world_y ( self , iy : int ) - > float :
return iy * self . res + self . y_min
# ---------- 静态地图生成 ----------
def _init_static_map ( self ) :
""" 根据 Sim3D7vs7SoccerField 参数生成静态代价地图 """
# 球场参数
field_half_len = 27.5 # 球场半长（不含球门深度）
field_half_width = 18.0
goal_width = 3.66
goal_depth = 1.0
goal_half_width = goal_width / 2.0
post_radius = 0.05
# 1. 初始化全地图为 0（ ）
self . static_cost_map . fill ( 0.0 )
# 2. 边界硬墙
# 左侧边界：
for i in range ( self . _world_to_grid_x ( - field_half_len - 0.001 ) , - 1 , - 1 ) :
for j in range ( self . ny ) :
y = self . _grid_to_world_y ( j )
if abs ( y ) > goal_half_width :
self . static_cost_map [ i , j ] = - 3
# 右侧边界：
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 . static_cost_map [ i , j ] = - 3
# 上下边界
for i in range ( self . nx ) :
for j in [ 0 , self . ny - 1 ] :
self . static_cost_map [ i , j ] = - 3
# 可选：如果需要在边界内留出线宽，可额外处理
# 4. 门柱（作为硬墙）
goal_post_positions = [
( 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 )
rad_cells = int ( post_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 < = post_radius :
self . static_cost_map [ nx , ny ] = - 3
# ---------- 获取动态障碍物（对手球员） ----------
def _get_opponent_positions ( self ) - > List [ np . ndarray ] :
""" 从 FieldLandmarks 获取所有对手球员的全局位置 """
opponents = [ ]
for player in self . world . their_team_players :
if player . last_seen_time is not None and ( self . world . server_time - player . last_seen_time ) < 1.0 :
opponents . append ( player . position [ : 2 ] ) # 只使用 x, y
else :
# 长时间未看到的球员不添加到避障列表中
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 ] ) :
"""
在动态代价地图上添加对手障碍物：
- 硬半径内：-3（ ）
- 缓冲区内：正代价（鼓励远离）
"""
effective_radius = self . robot_radius + self . safety_margin
radius_cells = int ( effective_radius / self . res ) + 1
buffer_width = 0.2 # 缓冲区宽度（米）
for opp in opponents :
if opp is None :
continue
ox , oy = opp [ 0 ] , opp [ 1 ]
ix , iy = self . _world_to_grid ( ox , oy )
for dx in range ( - radius_cells , radius_cells + 1 ) :
for dy in range ( - radius_cells , radius_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 < = effective_radius :
cost_map [ nx , ny ] = - 3 # 硬墙
elif dist < = effective_radius + buffer_width :
# 缓冲区内增加代价（线性衰减）
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 :
""" 对角线距离（ ） """
dx = abs ( ix1 - ix2 )
dy = abs ( iy1 - iy2 )
return ( dx + dy ) - 0.585786 * min ( dx , dy ) # sqrt(2)-1 ≈ 0.414
# ---------- 直线检测 ----------
def _line_is_free ( self , start : np . ndarray , end : np . ndarray , opponents : List [ np . ndarray ] ) - > bool :
""" 检查线段是否与任何对手（扩大后）相交 """
effective_radius = self . robot_radius + self . safety_margin
ax , ay = start
bx , by = end
abx = bx - ax
aby = by - ay
len_sq = abx * abx + aby * aby
if len_sq == 0 :
return True
for opp in opponents :
if opp is None :
continue
ox , oy = opp [ 0 ] , opp [ 1 ]
# 计算投影参数 t
t = ( ( ox - ax ) * abx + ( oy - ay ) * aby ) / len_sq
if t < 0 :
t = 0
elif t > 1 :
t = 1
closest_x = ax + t * abx
closest_y = ay + t * aby
dist = np . hypot ( closest_x - ox , closest_y - oy )
if dist < = effective_radius :
return False
return True
# ---------- 路径重构 ----------
def _reconstruct_path ( self , node : Tuple [ int , int ] , parent : dict , start : np . ndarray ) - > List [ np . ndarray ] :
""" 从父字典重构路径（世界坐标） """
path = [ ]
cur = node
while cur in parent :
x , y = self . _grid_to_world ( cur [ 0 ] , cur [ 1 ] )
path . append ( np . array ( [ x , y ] ) )
cur = parent [ cur ]
path . append ( start )
path . reverse ( )
return path
# ---------- A* 规划 ----------
def plan ( self , start : np . ndarray , target : Optional [ np . ndarray ] = None ,
go_to_goal : bool = False , timeout_ms : float = 10.0 ) - > List [ np . ndarray ] :
"""
A* 路径规划
:param start: 起点 (x, y)
:param target: 目标点（当 go_to_goal=False 时使用）
:param go_to_goal: 是否前往对方球门
:param timeout_ms: 超时时间（毫秒）
:return: 路径点列表（世界坐标），若失败返回空列表
"""
# 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. 转换坐标
sx , sy = self . _world_to_grid ( start [ 0 ] , start [ 1 ] )
if go_to_goal :
# 目标点为球门线上 y=0 附近的格子
goal_x = self . field_half_len
goal_y = 0.0
tx , ty = self . _world_to_grid ( goal_x , goal_y )
else :
if target is None :
raise ValueError ( " target must be provided when go_to_goal=False " )
tx , ty = self . _world_to_grid ( target [ 0 ] , target [ 1 ] )
# 3. 【关键修改】强制将目标点格子设为 -1（ ）
if go_to_goal :
# 球门线上所有格子都设为 -1（ ）
goal_line_cell = self . _world_to_grid_x ( self . field_half_len )
y_min_cell = self . _world_to_grid_y ( - self . goal_half_width )
y_max_cell = self . _world_to_grid_y ( self . goal_half_width )
for j in range ( y_min_cell , y_max_cell + 1 ) :
if 0 < = goal_line_cell < self . nx and 0 < = j < self . ny :
cost_map [ goal_line_cell , j ] = - 1
else :
if 0 < = tx < self . nx and 0 < = ty < self . ny :
cost_map [ tx , ty ] = - 1
# 4. 快速直线检测（可选，可提高效率）
if go_to_goal :
end_point = np . array ( [ goal_x , goal_y ] )
else :
end_point = target
if self . _line_is_free ( start , end_point , opponents ) :
# 直线无碰撞，直接返回
return [ start , end_point ]
# 5. A* 初始化
open_set = [ ]
closed = np . zeros ( ( self . nx , self . ny ) , dtype = bool )
g = np . full ( ( self . nx , self . ny ) , np . inf )
f = np . full ( ( self . nx , self . ny ) , np . inf )
parent = { } # (ix, iy) -> (pix, piy)
g [ sx , sy ] = 0.0
f [ sx , sy ] = self . _heuristic ( sx , sy , tx , ty )
heappush ( open_set , ( f [ sx , sy ] , sx , sy ) )
# 记录最佳节点（用于超时/不可达回退）
best_node = ( sx , sy )
best_h = self . _heuristic ( sx , sy , tx , ty )
start_time = time . time ( )
iterations = 0
# 邻居方向（ ）
dirs = [ ( - 1 , - 1 , 1.414 ) , ( - 1 , 0 , 1.0 ) , ( - 1 , 1 , 1.414 ) ,
( 0 , - 1 , 1.0 ) , ( 0 , 1 , 1.0 ) ,
( 1 , - 1 , 1.414 ) , ( 1 , 0 , 1.0 ) , ( 1 , 1 , 1.414 ) ]
while open_set :
iterations + = 1
# 超时检查
if iterations % 32 == 0 and ( time . time ( ) - start_time ) * 1000 > timeout_ms :
# 超时，返回最佳节点路径（如果有）
if best_node != ( sx , sy ) :
return self . _reconstruct_path ( best_node , parent , start )
else :
return [ ]
_ , cx , cy = heappop ( open_set )
if closed [ cx , cy ] :
continue
closed [ cx , cy ] = True
# 更新最佳节点（基于启发式距离）
h = self . _heuristic ( cx , cy , tx , ty )
if h < best_h :
best_h = h
best_node = ( cx , cy )
# 到达目标
if ( cx , cy ) == ( tx , ty ) or ( go_to_goal and cost_map [ cx , cy ] == - 1 ) :
return self . _reconstruct_path ( ( cx , cy ) , parent , start )
# 扩展邻居
for dx , dy , step_cost in dirs :
nx , ny = cx + dx , cy + dy
if not ( 0 < = nx < self . nx and 0 < = ny < self . ny ) :
continue
if closed [ nx , ny ] :
continue
cell_cost = cost_map [ nx , ny ]
if cell_cost == - 3 : # 硬墙
continue
move_cost = step_cost + max ( 0.0 , cell_cost )
tentative_g = g [ cx , cy ] + move_cost
if tentative_g < g [ nx , ny ] :
parent [ ( nx , ny ) ] = ( cx , cy )
g [ nx , ny ] = tentative_g
f [ nx , ny ] = tentative_g + self . _heuristic ( nx , ny , tx , ty )
heappush ( open_set , ( f [ nx , ny ] , nx , ny ) )
# open 集为空，不可达
if best_node != ( sx , sy ) :
return self . _reconstruct_path ( best_node , parent , start )
else :
return [ ]
# ---------- 多目标点管理 ----------
def set_waypoints ( self , waypoints : List [ np . ndarray ] ) :
"""
设置目标点序列（世界坐标）。如果某个元素为 None，
"""
self . waypoints = [ np . array ( wp ) if wp is not None else None for wp in waypoints ]
self . current_wp_idx = 0
self . current_path = [ ]
self . last_replan_time = 0.0
def get_next_target ( self ) - > Optional [ np . ndarray ] :
""" 返回当前需要前往的目标点（世界坐标） """
if self . current_wp_idx > = len ( self . waypoints ) :
return None
wp = self . waypoints [ self . current_wp_idx ]
if wp is None :
# 前往球门
return np . array ( [ self . field_half_len , 0.0 ] )
return wp
def advance_to_next_target ( self ) :
""" 标记当前目标已完成，切换到下一个 """
if self . current_wp_idx < len ( self . waypoints ) :
self . current_wp_idx + = 1
self . current_path = [ ] # 清空旧路径
def update ( self , current_pos : np . ndarray , current_time : float ) - > Optional [ np . ndarray ] :
"""
更新路径规划，返回下一个需要前往的路径点。
:param current_pos: 机器人当前世界坐标 (x, y)
:param current_time: 当前时间（秒），用于可选的重规划频率控制
:return: 下一个路径点（世界坐标），若无有效目标则返回 None
"""
# 1. 获取当前需要前往的目标点
target = self . get_next_target ( )
if target is None :
# 没有剩余目标，停止移动
return None
# 2. 到达检测：如果已有路径且路径终点距离机器人很近，则认为已到达当前目标
if len ( self . current_path ) > = 2 :
last_point = self . current_path [ - 1 ]
if np . linalg . norm ( last_point - current_pos ) < self . arrival_threshold :
# 当前目标已完成，切换到下一个目标
self . advance_to_next_target ( )
target = self . get_next_target ( )
if target is None :
return None
# 清空旧路径，强制下次重规划到新目标
self . current_path = [ ]
# 3. 路径有效性检查（仅当存在有效路径时）
path_valid = ( len ( self . current_path ) > = 2 ) and self . _is_path_still_valid ( current_pos )
# 4. 判断是否需要重新规划
# 条件1： （ ）
# 条件2：
need_replan = ( len ( self . current_path ) < 2 ) or not path_valid
if need_replan :
# 重新规划到当前目标
new_path = self . plan ( current_pos ,
target = target if target is not None else None ,
go_to_goal = ( target is None ) ,
timeout_ms = 10.0 )
if new_path and len ( new_path ) > 1 :
self . current_path = new_path
self . last_replan_time = current_time
else :
# 当前目标不可达（规划失败），跳过它，尝试下一个
self . advance_to_next_target ( )
# 递归调用，继续处理下一个目标（避免深度过大，但目标数量有限）
return self . update ( current_pos , current_time )
# 5. 返回下一个路径点（路径的第二个点，第一个点为机器人当前位置的近似）
if len ( self . current_path ) > = 2 :
return self . current_path [ 1 ]
else :
return None
def _is_path_still_valid ( self , current_pos : np . ndarray , lookahead_dist : float = 3.0 ) - > bool :
"""
检查从机器人当前位置开始的剩余路径是否仍无碰撞（仅检查前方 lookahead_dist 米内）。
:param current_pos: 机器人当前世界坐标 (x, y)
:param lookahead_dist: 检查的前向距离（米），默认 3.0
:return: 如果路径在前方范围内无碰撞返回 True，
"""
if len ( self . current_path ) < 2 :
return False
# 获取对手最新位置
opponents = self . _get_opponent_positions ( )
# 累积距离
accumulated_dist = 0.0
# 从机器人当前位置到路径第二个点的第一段
start = current_pos
end = self . current_path [ 1 ]
seg_dist = np . linalg . norm ( end - start )
if not self . _line_is_free ( start , end , opponents ) :
return False
accumulated_dist + = seg_dist
# 如果第一段就已经超过阈值，直接返回 True（ ）
if accumulated_dist > = lookahead_dist :
return True
# 继续检查后续路径段，直到累积距离超过阈值
for i in range ( 1 , len ( self . current_path ) - 1 ) :
start = self . current_path [ i ]
end = self . current_path [ i + 1 ]
seg_dist = np . linalg . norm ( end - start )
if not self . _line_is_free ( start , end , opponents ) :
return False
accumulated_dist + = seg_dist
if accumulated_dist > = lookahead_dist :
break
return True
def set_target ( self , target : np . ndarray , force : bool = False ) :
"""
设置单目标点（世界坐标）。
:param target: 新目标点
:param force: 是否强制更新（即使目标相同或距离很近）
"""
# 获取当前有效目标（如果存在）
current_target = self . get_next_target ( )
if current_target is not None and not force :
# 计算新目标与当前目标的欧氏距离
dist = np . linalg . norm ( target - current_target )
if dist < 0.2 : # 阈值可调（例如 0.2 米）
# 目标没有显著变化，不更新
return
# 目标变化显著，或强制更新
self . waypoints = [ target ]
self . current_wp_idx = 0
self . current_path = [ ] # 清空旧路径，触发重规划
self . last_replan_time = 0.0
