def update_global_path_by_dist(env: Env): def is_close(x: float, y: float) -> bool: xe, ye = env.state[:2] return (x - xe) ** 2 + (y - ye) ** 2 <= 6 ** 2 i = 0 while not is_close(*env.path[i]): # print('Passed a point in global path.') i += 1 if i + 10 > len(env.path): print("ERROR: No point in global path is close enough") return print(f"Skipped {i} points in global path, remaining {len(env.path) - i}") env.path = env.path[i:, :]
def update_global_path(env: Env): def line_behind_vehicle(x: float, y: float) -> float: p: state_t = env.state # yaw = p[2] p0, p1 = env.path[0:2] yaw = np.arctan2(p1[1] - p0[1], p1[0] - p0[0]) return (x - p[0]) * np.cos(yaw) + (y - p[1]) * np.sin(yaw) def is_behind(x: float, y: float) -> bool: return line_behind_vehicle(x, y) < 0 while is_behind(*env.path[0]): print('Passed a point in global path.') env.path = env.path[1:, :]