1. Load the 2.5D map in the colliders.csv file describing the environment.
2. Discretize the environment into a grid or graph representation.
3. Define the start and goal locations.
4. Perform a search using A* or other search algorithm.
5. Use a collinearity test or ray tracing method (like Bresenham) to remove unnecessary waypoints.
6. Return waypoints in local ECEF coordinates (format for self.all_waypoints is [N, E, altitude, heading], where the drone’s start location corresponds to [0, 0, 0, 0].
7. Write it up.
8. Congratulations! Your Done!

•   motion_planning.py

•   planning_utils.py

The grid representation of the 2.5D map and the path generated by the A* algorithm is as below :

And here is a short demo of the drone flying through the simulator :

The full simulatoin video can be found at Youtube