(7)函数pure_pursuit_control_wrapper(self)的功能是通过纯追踪模型实现一步车辆控制,存储关键信息,包括当前速度、位置和参考速度,应用油门-转向-刹车控制,同时检查安全距离和交通信号灯状态,最终返回是否到达终点。
def pure_pursuit_control_wrapper(self):
'''
对车辆应用一步控制,存储后续使用的关键信息
Returns
-------
end_trajectory : bool
车辆是否到达终点
'''
# 获取当前速度和车辆位置(包括(x,y)坐标和偏航角)
curr_speed = self.env.get_forward_speed(self.model_uniquename)
vehicle_pos_2d = self.env.get_transform_2d(self.model_uniquename)
# 存储当前速度和位置信息
self.speed.append(curr_speed)
self.curr_speeds.append(curr_speed)
# 在调试模式下绘制真实轨迹
if self.debug_vehicle:
sel