#List of waypoints
waypoints = []
# waypoints.append(msg_ned(10.,10., -100.))
# waypoints.append(msg_ned(90.,90., -100.))
# waypoints.append(msg_ned(80.,10., -100.))
# waypoints.append(msg_ned(-40.,-50., -100.))
# waypoints.append(msg_ned(30.,30., -100.))
# waypoints.append(msg_ned(30.,30., -150.))
# waypoints.append(msg_ned(20.,20., -120.))
# waypoints.append(msg_ned(25.,25., -100.))
# waypoints.append(msg_ned(40.,25., -100.))
# waypoints.append(msg_ned(40.,0., -100.))
waypoints.append(msg_ned(25., -25., -140.))
waypoints.append(msg_ned(20.,20., -100.))
waypoints.append(msg_ned(80.,20., -140.))
waypoints.append(msg_ned(60., 60., -140.))
waypoints.append(msg_ned(-60., 60., -140.))
waypoints.append(msg_ned(-80., -60., -140.))
waypoints.append(msg_ned(50., 60., -140.))
waypoints.append(msg_ned(25., -25., -140.))
waypoints.append(msg_ned(25., 20., -100.))
# Create rrt object
rrt = RRT(obstaclesList, boundariesList, clearance, maxDistance, maxIncline, maxRelChi, iterations, resolution, scaleHeight, animating)
# Solve the path
solvedPath = rrt.findFullPath(waypoints)
# Make sure shows plot even when not debugging??
