Writeup

Instructions: Provide a Writeup / README that includes all the rubric points and how you addressed each one. You can submit your writeup as markdown or pdf.

This README file. If you're seeing this, this is my writeup. :P

Explain the Starter Code

Instructions: Test that motion_planning.py is a modified version of backyard_flyer_solution.py for simple path planning. Verify that both scripts work. Then, compare them side by side and describe in words how each of the modifications implemented in motion_planning.py is functioning.

The starter code, first connects to the mavlink via the default port 5760. Both scripts did work on the MacBook and Windows environments.The drone lifted off, and did the same (or similar) square route as was done for the Backyard Flyer, then landed at (or close to) the starting point.

Differences between the previous project, Backyard Flyer, and this project, Motion Planning

The states are now set to use auto() values. Which, Python provides a nice helper function that can automate the values for the enumerations. See https://docs.python.org/3/library/enum.html#using-automatic-values for more information.

In backyard flyer:

class States(Enum):
    MANUAL = 0
    ARMING = 1
    TAKEOFF = 2
    WAYPOINT = 3
    LANDING = 4
    DISARMING = 5

Now in the motion planning:

from enum import Enum, auto

class States(Enum):
    MANUAL = auto()
    ARMING = auto()
    TAKEOFF = auto()
    WAYPOINT = auto()
    LANDING = auto()
    DISARMING = auto()
    PLANNING = auto()

The above code, if I'm understandign it correctly, simply does an auto-increment of the values, starting with 0. Therefore, the initial enumerations will have the same values as provided in the hard-coded Backyard Flyer code. What's also new here, is the PLANNING phase.

Another difference is the Backyard Flyer had super().start() inside of the start() function, then the Motion Planning changes it to self.connection.start() in the same function. It appears they do the same thing, the latter making use of the initilzed connection setup:

def __init__(self, connection):
    super().__init__(connection)

New functions in Motion Planning:

def send_waypoints(self):
    print("Sending waypoints to simulator ...")
    data = msgpack.dumps(self.waypoints)
    self.connection._master.write(data)

When the waypoints are sent to the simulator/drone, the waypoints are serialized so the drone can receive the data as a smaller packet size, for faster processing. See https://github.com/msgpack/msgpack-python.

def plan_path(self):
    self.flight_state = States.PLANNING
    print("Searching for a path ...")
    TARGET_ALTITUDE = 5
    SAFETY_DISTANCE = 5
    ...

The bulk of the new code comes in the above function, plan_path(). What happens here, or what is to occur within this function? The goal here is to generate a grid and path to submit to the simulator/drone. The colliders data is loaded into memory, the grid and offsets are generated by the create_grid() function found in planning_utils.py file.

The grid start and goal locations are defined, then a path is generated using the A* and heuristics privded in the planning_utils.py code. The paths are converted to waypoints, and finally, submitted to the drone/simulator. The waypoints are serialized (or optimized) for the drone/simulator to handle the data better in the send_waypoints() function.

Implementing Your Path Planning Algorithm - My Code

motion_planning.py altered file from the original provided by the project git repository. This contains the changes made for the plan_path() function.

planning_utils.py also provided by the git repository. Here, the actions for NW, NE, SW, and SE were added, along with setting the costs to np.sqrt(2).

The code implemented to handle the diagonal motions

The Action class was updated to include the NORTH_WEST, NORTH_EAST, SOUTH_WEST, SOUTH_EAST actions, along with their cost of np.sqrt(2):

NORTH_WEST = (-1, -1, np.sqrt(2))
NORTH_EAST = (-1, 1, np.sqrt(2))
SOUTH_WEST = (1, -1, np.sqrt(2))
SOUTH_EAST = (1, 1, np.sqrt(2))

Then in the valid_actions() function, the check was modified to include the NW, NE, SW, and SE acceptable movement:

if x - 1 < 0:
    # eliminate NORTH as we're at the edge
    valid_actions.remove(Action.NORTH)
else:
    if grid[x - 1, y] == 1:
        valid_actions.remove(Action.NORTH)
    if grid[x - 1, y + 1] == 1:
        valid_actions.remove(Action.NORTH_EAST)
    if grid[x - 1, y - 1] == 1:
        valid_actions.remove(Action.NORTH_WEST)
if x + 1 > n:
    # eliminate SOUTH because we're at the edge
    valid_actions.remove(Action.SOUTH)
else:
    if grid[x + 1, y] == 1:
        valid_actions.remove(Action.SOUTH)
    if grid[x + 1, y + 1] == 1:
        valid_actions.remove(Action.SOUTH_EAST)
    if grid[x + 1, y - 1] == 1:
        valid_actions.remove(Action.SOUTH_WEST)
# since the other code above takes care of SW, SE, NW, and NE, no need to repeat that below
if y - 1 < 0 or grid[x, y - 1] == 1:
    valid_actions.remove(Action.WEST)
if y + 1 > m or grid[x, y + 1] == 1:
    valid_actions.remove(Action.EAST)

From the above, it does appear the drone does a nifty little diagonal motion. Not sure if that's the correct result, but, it does move in the correct direction. Sure, there is a cleaner way to handle the above code, but, for now, it's fairly readable, to know what it's doing.

my_utils.py contains the collinearity check and path pruning functions that were brought over from the course exercises:

The collinearity_check() function concatenates three points, and uses the numpy's linear algorithm to detect if the three points form a straight line, if they do, then we know we can eliminated the 2nd point.

def collinearity_check(p1, p2, p3, epsilon=1e-6):
    m = np.concatenate((p1, p2, p3), 0)
    det = np.linalg.det(m)
    return abs(det) < epsilon

The point() function simple converts the path values into a point shape that the collinearity check can use.

def point(p):
    return np.array([p[0], p[1], 1.]).reshape(1, -1)

The motion planning code calls this prune_path() function to look for points that can be cleared off from the path before submitting all paths/waypoints to the simulator/drone. If the combination of the three points submitted to the collinearty check function form a straight line, then the second path point is removed from the path list.

def prune_path(path):
    pruned_path = [p for p in path]
    i = 0
    while i < len(pruned_path) - 2:
        p1 = point(pruned_path[i])
        p2 = point(pruned_path[i + 1])
        p3 = point(pruned_path[i + 2])

        # If the 3 points are in a line remove the 2nd point.
        # The 3rd point now becomes and 2nd point and the check 
        # is redone with a new third point on the next iteration.
        if collinearity_check(p1, p2, p3):
            # Something subtle here but we can mutate
            # `pruned_path` freely because the length
            # of the list is check on every iteration.
            pruned_path.remove(pruned_path[i + 1])
        else:
            i += 1
    return pruned_path

The result returned by the prune_path() function should be a much smaller list of paths than originally started with - unless of course, the original path has a small amount of points to check.

Bonus code attempt

Heading was added to this project. In the motion_planning.py file, there is a heading section that is commented out - since it's not part of the project requirements:

# Add bearing to waypoints
waypoints = adjust_bearing(waypoints)
# print('waypoints with bearing: ', waypoints[0])

In the file my_utils.py, the following was added to handle adjusting the bearing:

# set bearing
def adjust_bearing(waypoints):
    for idx in range(len(waypoints)):
        # skip the first waypoint
        if idx > 0:
            previous_waypoint = waypoints[idx -1]
            current_waypoint = waypoints[idx]
            current_waypoint[3] = np.arctan2((current_waypoint[1] - previous_waypoint[1]), (current_waypoint[0] - previous_waypoint[0]))
    return waypoints

Executing the flight

Note: On my windows laptop, it seems the whole planning takes a lot longer to process - which makes sense - when using long distances.

In windows

1. Open Anaconda Prompt
2. Change directory to where the project project files are located
3. Run the command: activate fcnd
4. Start the Simulator and select Motion Planning for the simulation
5. Set the drone to "Armed" status, manually move the drone to any location away from the center of the map. Then land the drone. This will be the starting point for the motion planner.
6. Run the command: python motion_planning.py

The drone should lift-off and following along a waypoint path that should end up back where the dron originally was set by default when the simulator was loaded.