Feature/stationary obstacle avoidance

[cmspencer109]

Complete redesign of the original avoidance algorithm to use the Informed RRT* algorithm.

3 files are included, path_finder.py and helpers.py are the main important files. plotter.py is optional but very handy for visualizing the results.

An example of the usage is in the bottom of path_finder.py. To use it just call path_finder.solve() and pass in data for the boundary, obstacles, and waypoints in the original json format (list of dictionaries). Optional parameters are available for plotting.

Calling solve() takes a complete list of waypoints > 2 and returns an unobstructed route with possibly more waypoints than before (depending on number of obstructions). This function can be used for solving a path between only two waypoints, or a large list of waypoints. It returns a list of tuples in the format of (latitude, longitude, altitude).

The algorithm does not work in full 3 dimensions. It solves a path in 2d space and then adds altitudes later. Altitude for original waypoints is known (from the given json data). Altitude for new points is calculated by taking the percentage of distance the new point is from the starting point and multiplying it by the difference in height between the start and goal points. This results in an even ascent or descent between start and goal points (if the start and goal points are different altitudes, otherwise all points will be a constant height).