Course notes: Tello basics

Preamble

Notes from Easy Programming of Tello Drone | Python OpenCV Object Tracking

See also

Courses

Required Files

Required Packages

  • djitellopy
  • opencv-python
  • numpy

Notes

Basic code

# TelloMain2.py

# TelloMain.py

# From [Easy Programming of Tello Drone | Python OpenCV Object Tracking](https://www.youtube.com/watch?v=vDOkUHNdmKs)

from djitellopy import Tello
import cv2
import time

# ###################################################################################################################
width = 320
height = 240
startCounter = 0  # 0 to fly, 1 to not fly (camera only)
# ###################################################################################################################


# Connect to Tello

me = Tello()
me.connect()
me.for_back_velocity = 0
me.left_right_velocity = 0
me.up_down_velocity = 0
me.yaw_velocity = 0
me.speed = 0

print(me.get_battery())

me.streamoff()
me.streamon()

while True:
    # Get the image from Tello
    frame_read = me.get_frame_read()
    myFrame = frame_read.frame
    img = cv2.resize(myFrame, (width, height))

    # Go up in the beginning
    if startCounter == 0:
        me.takeoff()
        time.sleep(8)
        me.rotate_clockwise(90)
        time.sleep(3)
        me.move_left(35)
        time.sleep(3)
        me.land()

        startCounter = 1

    # Send velovity values to Tello
    if me.send_rc_control:
        me.send_rc_control(me.left_right_velocity, me.for_back_velocity, me.up_down_velocity, me.yaw_velocity)

    # Display image
    cv2.imshow("My Result", img)

    # Wait for the "q" ket to stop
    if cv2.waitKey(0) & 0xFF == ord('q'):
        me.land()
        break

Colour tracker code

# ColourObjectTracking.py

import cv2
import numpy as np

framewidth = 640
frameheight = 480
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, framewidth)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, frameheight)

deadzone = 100
# global imgContour

def empty(a):
    pass


cv2.namedWindow('HSV')
cv2.resizeWindow('HSV', 640, 240)
cv2.createTrackbar('HUE Min', 'HSV', 19, 179, empty)
cv2.createTrackbar('HUE Max', 'HSV', 35, 179, empty)
cv2.createTrackbar('SAT Min', 'HSV', 107, 255, empty)
cv2.createTrackbar('SAT Max', 'HSV', 255, 255, empty)
cv2.createTrackbar('VAL Min', 'HSV', 89, 255, empty)
cv2.createTrackbar('VAL Max', 'HSV', 255, 255, empty)

cv2.namedWindow('Parameters')
cv2.resizeWindow('Parameters', 640, 240)
cv2.createTrackbar('Threshold1', 'Parameters', 166, 255, empty)
cv2.createTrackbar('Threshold2', 'Parameters', 171, 255, empty)
cv2.createTrackbar('Area', 'Parameters', 3750, 30000, empty)


def stackImages(scale, imgArray):
    rows = len(imgArray)
    cols = len(imgArray[0])
    rowsAvailable = isinstance(imgArray[0], list)
    width = imgArray[0][0].shape[1]
    height = imgArray[0][0].shape[0]
    if rowsAvailable:
        for x in range(0, rows):
            for y in range(0, cols):
                if imgArray[x][y].shape[:2] == imgArray[0][0].shape[:2]:
                    imgArray[x][y] = cv2.resize(imgArray[x][y], (0, 0), None, scale, scale)
                else:
                    imgArray[x][y] = cv2.resize(imgArray[x][y], (imgArray[0][0].shape[1], imgArray[0][0].shape[0]),
                                                None, scale, scale)
                if len(imgArray[x][y].shape) == 2:
                    imgArray[x][y] = cv2.cvtColor(imgArray[x][y], cv2.COLOR_GRAY2BGR)
        imageBlank = np.zeros((height, width, 3), np.uint8)
        hor = [imageBlank] * rows
        hor_con = [imageBlank] * rows
        for x in range(0, rows):
            hor[x] = np.hstack(imgArray[x])
        ver = np.vstack(hor)
    else:
        for x in range(0, rows):
            if imgArray[x].shape[:2] == imgArray[0].shape[:2]:
                imgArray[x] = cv2.resize(imgArray[x], (0, 0), None, scale, scale)
            else:
                imgArray[x] = cv2.resize(imgArray[x], (imgArray[0].shape[1], imgArray[0].shape[0]), None, scale, scale)
                if len(imgArray[x].shape) == 2:
                    imgArray[x] = cv2.cvtColor(imgArray[x], cv2.COLOUR_GRAY2BGR)
        hor = np.hstack(imgArray)
        ver = hor
    return ver


def getContours(img, imgContour):
    contours, hierarhy = cv2.findContours(img, cv2.cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    for cnt in contours:
        area = cv2.contourArea(cnt)
        areaMin = cv2.getTrackbarPos("Area", "Parameters")
        if area > areaMin:
            cv2.drawContours(imgContour, cnt, -1, (255, 0, 255), 7)
            perimeter = cv2.arcLength(cnt, True)
            approx = cv2.pproxPolyDP(cnt, 0.02 * perimeter, True)
            print(len(approx))
            x, y, w, h = cv2.boundingRect(approx)
            cv2.rectangle(imgContour, (x, y), (x + w, y + h), (0, 255, 0), 5)

            cv2.putText(imgContour, "Points:" + str(len(approx)), (x + w + 20, y + 20), cv2.FONT_HERSHEY_COMPLEX, 0.7,
                        (0, 255, 0), 2)
            cv2.putText(imgContour, "Area:" + str(int(area)), (x + w + 20, y + 45), cv2.FONT_HERSHEY_COMPLEX, 0.7,
                        (0, 255, 0), 2)
            cv2.putText(imgContour, " " + str(int(x)) + " " + str(int(y)), (x - 20, y - 45), cv2.FONT_HERSHEY_COMPLEX,
                        0.7, (0, 255, 0), 2)

            cx = int(x + (w / 2))
            cy = int(y + (h / 2))

            if cx < int(framewidth / 2) - deadzone:
                cv2.putText(imgContour, " GO LEFT ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
            if cx < int(framewidth / 2) + deadzone:
                cv2.putText(imgContour, " GO RIGHT ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
            if cx < int(frameheight / 2) - deadzone:
                cv2.putText(imgContour, " GO UP ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
            if cx < int(frameheight / 2) + deadzone:
                cv2.putText(imgContour, " GO DOWN ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)

            cv2.line(imgContour, (int(framewidth / 2), int(frameheight / 2)), (cx, cy), (0, 0, 255), 3)


def display(img):
    cv2.line(img, (int(framewidth / 2) - deadzone, 0), (int(framewidth / 2) - deadzone, frameheight), (255, 255, 0), 3)
    cv2.line(img, (int(framewidth / 2) + deadzone, 0), (int(framewidth / 2) + deadzone, frameheight), (255, 255, 0), 3)

    cv2.circle(img, (int(framewidth / 2), int(frameheight / 2)), 5, (0, 0, 255), 5)
    cv2.line(img, (0, int(frameheight / 2) - deadzone), (framewidth, int(frameheight / 2) - deadzone), (255, 255, 0), 3)
    cv2.line(img, (0, int(frameheight / 2) + deadzone), (framewidth, int(frameheight / 2) + deadzone), (255, 255, 0), 3)


while True:
    _, img = cap.read()
    imgContour = img.copy()
    imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    h_min = cv2.getTrackbarPos("HUE Min", "HSV")
    h_max = cv2.getTrackbarPos("HUE Max", "HSV")
    s_min = cv2.getTrackbarPos("SAT Min", "HSV")
    s_max = cv2.getTrackbarPos("SAT Max", "HSV")
    v_min = cv2.getTrackbarPos("VAL Min", "HSV")
    v_max = cv2.getTrackbarPos("VAL Max", "HSV")
    print(h_min)

    lower = np.array([h_min, s_min, v_min])
    upper = np.array([h_max, s_max, v_max])
    mask = cv2.inRange(imgHSV, lower, upper)
    result = cv2.bitwise_and(img, img, mask=mask)
    mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)

    imgBlur = cv2.GaussianBlur(result, (7, 7), 1)
    imgGray = cv2.cvtColor(imgBlur, cv2.COLOR_BGR2GRAY)
    threshold1 = cv2.getTrackbarPos("Threshold1", "Parameters")
    threshold2 = cv2.getTrackbarPos("Threshold2", "Parameters")
    imgCanny = cv2.Canny(imgGray, threshold1, threshold2)
    kernel = np.ones((5, 5))
    imgDil = cv2.dilate(imgCanny, kernel, iterations=1)
    getContours(imgDil, imgContour)
    display(imgContour)

    stack = stackImages(0.7, ([img, result], [imgDil, imgContour]))

    cv2.imshow('Stacked Horiz', stack)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

 

Full code

# ObjectTrackingTello.py


# ColourObjectTracking.py
from djitellopy import Tello
import cv2
import numpy as np

# ###################################################################################################################
width = 640
height = 480
startCounter = 0  # 0 to fly, 1 to not fly (camera only)
deadzone = 100
bNotDrone = True
# ###################################################################################################################


framewidth = 640
frameheight = 480
if bNotDrone:
    cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, framewidth)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, frameheight)
    cap.set(10, 200)
else:
    # Connect to Tello
    me = Tello()
    me.connect()
    me.for_back_velocity = 0
    me.left_right_velocity = 0
    me.up_down_velocity = 0
    me.yaw_velocity = 0
    me.speed = 0

    print(me.get_battery())
    me.streamoff()
    me.streamon()

global imgContour
global dir


def empty(a):
    pass


cv2.namedWindow('HSV')
cv2.resizeWindow('HSV', 640, 240)
cv2.createTrackbar('HUE Min', 'HSV', 19, 179, empty)
cv2.createTrackbar('HUE Max', 'HSV', 35, 179, empty)
cv2.createTrackbar('SAT Min', 'HSV', 107, 255, empty)
cv2.createTrackbar('SAT Max', 'HSV', 255, 255, empty)
cv2.createTrackbar('VAL Min', 'HSV', 89, 255, empty)
cv2.createTrackbar('VAL Max', 'HSV', 255, 255, empty)

cv2.namedWindow('Parameters')
cv2.resizeWindow('Parameters', 640, 240)
cv2.createTrackbar('Threshold1', 'Parameters', 166, 255, empty)
cv2.createTrackbar('Threshold2', 'Parameters', 171, 255, empty)
cv2.createTrackbar('Area', 'Parameters', 3750, 30000, empty)


def stackImages(scale, imgArray):
    rows = len(imgArray)
    cols = len(imgArray[0])
    rowsAvailable = isinstance(imgArray[0], list)
    width = imgArray[0][0].shape[1]
    height = imgArray[0][0].shape[0]
    if rowsAvailable:
        for x in range(0, rows):
            for y in range(0, cols):
                if imgArray[x][y].shape[:2] == imgArray[0][0].shape[:2]:
                    imgArray[x][y] = cv2.resize(imgArray[x][y], (0, 0), None, scale, scale)
                else:
                    imgArray[x][y] = cv2.resize(imgArray[x][y], (imgArray[0][0].shape[1], imgArray[0][0].shape[0]),
                                                None, scale, scale)
                if len(imgArray[x][y].shape) == 2:
                    imgArray[x][y] = cv2.cvtColor(imgArray[x][y], cv2.COLOR_GRAY2BGR)
        imageBlank = np.zeros((height, width, 3), np.uint8)
        hor = [imageBlank] * rows
        hor_con = [imageBlank] * rows
        for x in range(0, rows):
            hor[x] = np.hstack(imgArray[x])
        ver = np.vstack(hor)
    else:
        for x in range(0, rows):
            if imgArray[x].shape[:2] == imgArray[0].shape[:2]:
                imgArray[x] = cv2.resize(imgArray[x], (0, 0), None, scale, scale)
            else:
                imgArray[x] = cv2.resize(imgArray[x], (imgArray[0].shape[1], imgArray[0].shape[0]), None, scale, scale)
                if len(imgArray[x].shape) == 2:
                    imgArray[x] = cv2.cvtColor(imgArray[x], cv2.COLOUR_GRAY2BGR)
        hor = np.hstack(imgArray)
        ver = hor
    return ver


def getContours(img, imgContour):
    contours, hierarhy = cv2.findContours(img, cv2.cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    for cnt in contours:
        area = cv2.contourArea(cnt)
        areaMin = cv2.getTrackbarPos("Area", "Parameters")
        if area > areaMin:
            cv2.drawContours(imgContour, cnt, -1, (255, 0, 255), 7)
            perimeter = cv2.arcLength(cnt, True)
            approx = cv2.pproxPolyDP(cnt, 0.02 * perimeter, True)
            print(len(approx))
            x, y, w, h = cv2.boundingRect(approx)
            cv2.rectangle(imgContour, (x, y), (x + w, y + h), (0, 255, 0), 5)

            cv2.putText(imgContour, "Points:" + str(len(approx)), (x + w + 20, y + 20), cv2.FONT_HERSHEY_COMPLEX, 0.7,
                        (0, 255, 0), 2)
            cv2.putText(imgContour, "Area:" + str(int(area)), (x + w + 20, y + 45), cv2.FONT_HERSHEY_COMPLEX, 0.7,
                        (0, 255, 0), 2)
            cv2.putText(imgContour, " " + str(int(x)) + " " + str(int(y)), (x - 20, y - 45), cv2.FONT_HERSHEY_COMPLEX,
                        0.7, (0, 255, 0), 2)

            cx = int(x + (w / 2))
            cy = int(y + (h / 2))

            if cx < int(framewidth / 2) - deadzone:
                cv2.putText(imgContour, " GO LEFT ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
                dir = 1
            if cx < int(framewidth / 2) + deadzone:
                cv2.putText(imgContour, " GO RIGHT ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
                dir = 2
            if cx < int(frameheight / 2) - deadzone:
                cv2.putText(imgContour, " GO UP ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
                dir = 3
            if cx < int(frameheight / 2) + deadzone:
                cv2.putText(imgContour, " GO DOWN ", (20, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 3)
                cv2.rectangle(imgContour, (0, int(frameheight / 2) - deadzone),
                              (int(framewidth / 2) - deadzone, int(frameheight / 2) + deadzone), (0, 0, 255),
                              cv2.FILLED)
                dir = 4
            else:
                dir = 0

            cv2.line(imgContour, (int(framewidth / 2), int(frameheight / 2)), (cx, cy), (0, 0, 255), 3)
        else:
            dir = 0


def display(img):
    cv2.line(img, (int(framewidth / 2) - deadzone, 0), (int(framewidth / 2) - deadzone, frameheight), (255, 255, 0), 3)
    cv2.line(img, (int(framewidth / 2) + deadzone, 0), (int(framewidth / 2) + deadzone, frameheight), (255, 255, 0), 3)

    cv2.circle(img, (int(framewidth / 2), int(frameheight / 2)), 5, (0, 0, 255), 5)
    cv2.line(img, (0, int(frameheight / 2) - deadzone), (framewidth, int(frameheight / 2) - deadzone), (255, 255, 0), 3)
    cv2.line(img, (0, int(frameheight / 2) + deadzone), (framewidth, int(frameheight / 2) + deadzone), (255, 255, 0), 3)


while True:
    if bNotDrone:
        _, img = cap.read()
    else:
        # Get the image from Tello
        frame_read = me.get_frame_read()
        myFrame = frame_read.frame
        img = cv2.resize(myFrame, (width, height))

    imgContour = img.copy()
    imgHSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    h_min = cv2.getTrackbarPos("HUE Min", "HSV")
    h_max = cv2.getTrackbarPos("HUE Max", "HSV")
    s_min = cv2.getTrackbarPos("SAT Min", "HSV")
    s_max = cv2.getTrackbarPos("SAT Max", "HSV")
    v_min = cv2.getTrackbarPos("VAL Min", "HSV")
    v_max = cv2.getTrackbarPos("VAL Max", "HSV")
    print(h_min)

    lower = np.array([h_min, s_min, v_min])
    upper = np.array([h_max, s_max, v_max])
    mask = cv2.inRange(imgHSV, lower, upper)
    result = cv2.bitwise_and(img, img, mask=mask)
    mask = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)

    imgBlur = cv2.GaussianBlur(result, (7, 7), 1)
    imgGray = cv2.cvtColor(imgBlur, cv2.COLOR_BGR2GRAY)
    threshold1 = cv2.getTrackbarPos("Threshold1", "Parameters")
    threshold2 = cv2.getTrackbarPos("Threshold2", "Parameters")
    imgCanny = cv2.Canny(imgGray, threshold1, threshold2)
    kernel = np.ones((5, 5))
    imgDil = cv2.dilate(imgCanny, kernel, iterations=1)
    getContours(imgDil, imgContour)
    display(imgContour)

    if not bNotDrone:
        # ######## Flight
        if startCounter == 0:
            me.takeoff()
            startCounter = 1

        if dir == 1:
            me.yaw_velocity = -1  # -60
        elif dir == 2:
            me.yaw_velocity = 1  # 60
        elif dir == 3:
            me.up_down_velocity = 40  # 60
        elif dir == 4:
            me.up_down_velocity = -40  # -50?  or -60
        else:
            me.left_right_velocity = 0
            me.for_back_velocity = 0
            me.up_down_velocity = 0
            me.yaw_velocity = 0
        # Send velocity values to Tello
        if me.send_rc_control():
            me.send_rc_control(me.left_right_velocity, me.for_back_velocity, me.up_down_velocity, me.yaw_velocity)
        print(dir)

    stack = stackImages(0.7, ([img, result], [imgDil, imgContour]))

    cv2.imshow('Stacked Horiz', stack)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        if not bNotDrone:
            me.land()
        break

if bNotDrone:
    cap.release()
cv2.destroyAllWindows()

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s