Results visualization with the simulator here.
The target of this project is to plan a path through an urban environment between a start and a goal location using the A* algorithm.
A finite state machine using event-driven programming will be used for autonomously flying a drone.
The communication with the drone is done using MAVLink.
Autonomous control will be done using an event-driven state machine. Each callback function checks against transition criteria dependent on the current state. If the transition criteria are met, it will transition to the next state and pass along any required commands to the drone.
Design of the SW pipeline:
- 2.5D map download in the
colliders.csv
file describing the environment. - Environment discretization into a grid representation.
- Start and goal locations definition.
- Perform a search using A* algorithm.
- Perform the collinearity test to remove unnecessary waypoints.
- Waypoints in local ECEF coordinates ([N, E, altitude, heading] format).
Download the Motion-Planning simulator for this project that's appropriate for your operating system from the simulator releases respository.
If you haven't already, set up your Python environment and get all the relevant packages installed using Anaconda following instructions in this repository
git clone https://github.com/udacity/FCND-Motion-Planning
The file colliders.csv
contains the 2.5D map of the simulator environment.
First start up the simulator, then at the command line:
source activate fcnd # if you haven't already sourced your Python environment, do so now.
python motion_planning_grid.py
The planning_utils.py delivers the python function useful for the motion planning :
-
Class Action : valid movement actions that can take the drone from the current position. An action is represented by a 3 element tuple :
- the two first values are the delta of the action relative to the current grid position (N/E/S/W).
- the third value is the cost of performing the action.
-
Valid_actions : deliver the list of valid actions given a grid and current node.
-
A_star : Algorithm, which calculate the path from the start point to the goal point.
-
Heuristic : calculation of the euclidean distance (and the Manhattan distance) between a start point and a goal point.
-
Create_grid : 2D configuration space based on given obstacle data, drone altitude and safety distance arguments.
-
Point : 3D point implementation in a array.
-
Collinearity check : determinant calculation of a matrix containing the points. If the determinant is less that the epsilon threshold, then the points are collinear.
-
Prune_path : deletion of the unneeded waypoints. Use of the collinearity_check to know if the points are in the in a linear line.
-
Plot_route : visualization of the start point, the goal point and the waypoints. The function is called before and after the prune activity.
The motion_planning.py is the implementation of the Motion Planning algorithm. The grid search and the A* algorithm will be used.
These two classes are already implemented in the last project FCND-Backyard-Flyer, with a simple rectangular route without obstacle.
- States Class : Flight states implemented in the finite State Machine
- MotionPlanning Class : Main class of the programm. The code is implemented in this function.
The basic functions (transition and callback fonction) present in the finite State Machine (asynchronous graph) are in the picture below:
The state named "PLANNING" has been added between the state "ARMING" and "TAKEOFF".
The SW Pipeline of the state "PLANNING" with his function Plan_Path is here documented :
1- Load the 2.5D Map
2- Environment discretization
3- Start and Goal localization
4- A* Search algorithm
5- Collinearity test to remove unnecessary waypoints
6- Convert Path to waypoints
The results are presented here :
The diagnostic informations are :
-
the local start and goal + the north and the east offset,
-
the cost for the path,
-
the lenght of the path and the lenght of the prunned path,
-
the time to provide the path,
-
waypoints coordinate with the format [N, E, altitude, heading].