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:, :]