for vertex in fence['Vertices']:
vertex3d = [*vertex, Home[2]]
localFence.append(ConvertLLA2NED(Home, vertex3d))
localFence.append(localFence[0])
localFenceList.append(localFence)
#traj.InputTrafficData(1,[37.10233259,-76.38832752,5.0],[270,2.04*0.5144,0])
#traj.InputTrafficData(2,[37.10206582,-76.38683474,5.0],[90,2.04*0.5144,0])
t1 = time.time()
traj.FindPath("plan0", Home, Goal, Vel, Velf)
t2 = time.time()
print("Computation time:", t2 - t1)
waypoints = traj.GetFlightPlan("plan0", )
if len(waypoints) <= 1:
print("No path computed")
fp = [[wp.time, wp.latitude, wp.longitude, wp.altitude] for wp in waypoints]
tcp = [[*wp.tcp] for wp in waypoints]
tcpValues = [[*wp.tcpValue] for wp in waypoints]
n, e, d, ptn, pte, ptd = plotTcpPlan(fp, tcp, tcpValues, local=False)
minc1 = min(n)
minc2 = min(e)
minc = np.min([minc1, minc2]) - 5
maxc1 = max(n)
maxc2 = max(e)
maxc = np.max([maxc1, maxc2]) + 5
