backyard_flyer.py

import argparse
import time
from enum import Enum

import numpy as np

from udacidrone import Drone
from udacidrone.connection import MavlinkConnection, WebSocketConnection  # noqa: F401
from udacidrone.messaging import MsgID


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


class BackyardFlyer(Drone):

    def __init__(self, connection):
        super().__init__(connection)
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        # initialize waypoints to navigate
        self.all_waypoints = self.calculate_box()

        # TODO: Register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

    def local_position_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.LOCAL_POSITION` is received and self.local_position contains new data
        """
        if self.flight_state == States.TAKEOFF:
            altitude = -1 * self.local_position[2]
            if altitude > 0.95 * self.target_position[2]:
                # Transition to WAYPOINT state after reaching 
                # certain altitude after takeoff
                self.waypoint_transition()
        if self.flight_state == States.WAYPOINT:
            # My solution: Working fine
            # north_diff = abs(self.local_position[0] - self.target_position[0])
            # east_diff = abs(self.local_position[1] - self.target_position[1])
            # altitude = -1.0 * self.target_position[2]
            # alt_diff = abs(altitude - self.local_position[2])
            # # check if a waypoint is reached
            # if (north_diff < 0.2 and east_diff < 0.2 and alt_diff < 0.2):
            #     # once at a waypoint, treat as if the 
            #     # quad rotor just took off from ground
            #     self.flight_state = States.TAKEOFF
            # Solution
            if self.flight_state == States.WAYPOINT:
                if np.linalg.norm(self.target_position[:2] - self.local_position[:2]) < 1.0:
                    self.flight_state = States.TAKEOFF

    def velocity_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.LOCAL_VELOCITY` is received and self.local_velocity contains new data
        """
        if self.flight_state == States.LANDING:
            if abs(self.local_position[2]) < 0.1:
                self.disarming_transition()

    def state_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.STATE` is received and self.armed and self.guided contain new data
        """
        if not self.in_mission:
            return
        if self.flight_state == States.MANUAL:
            self.arming_transition()
        if self.flight_state == States.ARMING:
            self.takeoff_transition()
        if self.flight_state == States.DISARMING:
            self.manual_transition()

    def calculate_box(self):
        """TODO: Fill out this method

        1. Return waypoints to fly following vertices of a square
            1--------2
            |        |
            |        |
            0--------3
        """
        box = []
        # add first visted point last since pop() is
        # LIFO -- Last In First Out
        box.append([0, 0, 3])  # 0 (last waypoint)
        box.append([0, 10, 3])  # 3
        box.append([10, 10, 3])  # 2
        box.append([10, 0, 3])  # 1 (first waypoint)

        return box

    def arming_transition(self):
        """TODO: Fill out this method

        1. Take control of the drone
        2. Pass an arming command
        3. Set the home location to current position
        4. Transition to the ARMING state
        """
        print("arming transition")
        self.take_control()
        self.arm()
        # set home
        global_pos = self.global_position
        # Check if a valid global_position attribute is returned.
        # If not, print out a message and transition to manual so that
        # the invoker can run the script again. This happends sometimes
        # and it's a known bug in the FCND quad rotor simulator
        if np.sum(global_pos) == 0:
            print("Could not set home position since "
                  "call to self.global_position() returned [0, 0, 0]."
                  "Please re-run the script.")
            self.manual_transition()
        print("Setting home postion.")
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])
        print("Home postion set to (GPS):")
        print(self.global_position)
        print("Home postion set to (local):")
        print(self.local_position)
        # set appropriate state
        self.flight_state = States.ARMING

    def takeoff_transition(self):
        """TODO: Fill out this method

        1. Set target_position altitude to 3.0m
        2. Command a takeoff to 3.0m
        3. Transition to the TAKEOFF state
        """
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        # set appropriate state
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        """TODO: Fill out this method

        1. Command the next waypoint position
        2. Transition to WAYPOINT state
        """
        print("waypoint transition")
        if self.all_waypoints:
            waypoint = self.all_waypoints.pop()
            print(f'next waypoint: {waypoint}')
            north, east, altitude = waypoint
            print(north, east, altitude)
            self.target_position[0] = north
            self.target_position[1] = east
            self.target_position[2] = altitude
            self.cmd_position(north, east, altitude, 0)
            self.flight_state = States.WAYPOINT
        else:
            self.landing_transition()

    def landing_transition(self):
        """TODO: Fill out this method

        1. Command the drone to land
        2. Transition to the LANDING state
        """
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        """TODO: Fill out this method

        1. Command the drone to disarm
        2. Transition to the DISARMING state
        """
        print("disarm transition")
        self.disarm()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        """This method is provided

        1. Release control of the drone
        2. Stop the connection (and telemetry log)
        3. End the mission
        4. Transition to the MANUAL state
        """
        print("manual transition")

        self.release_control()
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        """This method is provided

        1. Open a log file
        2. Start the drone connection
        3. Close the log file
        """
        print("Creating log file")
        self.start_log("Logs", "NavLog.txt")
        print("starting connection")
        self.connection.start()
        print("Closing log file")
        self.stop_log()


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument('--port', type=int, default=5760, help='Port number')
    parser.add_argument('--host', type=str, default='127.0.0.1',
                        help="host address, i.e. '127.0.0.1'")
    args = parser.parse_args()

    conn = MavlinkConnection('tcp:{0}:{1}'.format(
        args.host, args.port), threaded=False, PX4=False)
    #conn = WebSocketConnection('ws://{0}:{1}'.format(args.host, args.port))
    drone = BackyardFlyer(conn)
    time.sleep(2)
    drone.start()