62 lines
1.7 KiB
Python
62 lines
1.7 KiB
Python
import numpy as np
|
||
|
||
class ReplayPolicy:
|
||
"""
|
||
严格回放策略：根据专家轨迹数据，逐帧回放车辆状态
|
||
"""
|
||
|
||
def __init__(self, expert_trajectory, vehicle_id):
|
||
"""
|
||
Args:
|
||
expert_trajectory: 专家轨迹字典，包含 positions, headings, velocities, valid
|
||
vehicle_id: 车辆ID（用于调试）
|
||
"""
|
||
self.trajectory = expert_trajectory
|
||
self.vehicle_id = vehicle_id
|
||
self.current_step = 0
|
||
|
||
def act(self, observation=None):
|
||
"""
|
||
返回动作：在回放模式下返回空动作
|
||
实际状态由环境直接设置
|
||
"""
|
||
return [0.0, 0.0]
|
||
|
||
def get_target_state(self, step):
|
||
"""
|
||
获取指定时间步的目标状态
|
||
|
||
Args:
|
||
step: 时间步
|
||
|
||
Returns:
|
||
dict: 包含 position, heading, velocity 的字典，如果无效则返回 None
|
||
"""
|
||
if step >= len(self.trajectory['valid']):
|
||
return None
|
||
|
||
if not self.trajectory['valid'][step]:
|
||
return None
|
||
|
||
return {
|
||
'position': self.trajectory['positions'][step],
|
||
'heading': self.trajectory['headings'][step],
|
||
'velocity': self.trajectory['velocities'][step]
|
||
}
|
||
|
||
def is_finished(self, step):
|
||
"""
|
||
判断轨迹是否已经播放完毕
|
||
|
||
Args:
|
||
step: 当前时间步
|
||
|
||
Returns:
|
||
bool: 如果轨迹已播放完或当前步无效，返回 True
|
||
"""
|
||
# 超出轨迹长度
|
||
if step >= len(self.trajectory['valid']):
|
||
return True
|
||
|
||
# 当前步及之后都无效
|
||
return not any(self.trajectory['valid'][step:]) |