Hello Tello Pilot!
Join our DJI Tello community & remove this banner.
Sign up

Tello Counting object using SSD and control with gamepad F310

danisafidah

New member
Joined
Jan 22, 2020
Messages
1
Reaction score
0
I have an project to in tello, im inspired from OpenCV People Counter - PyImageSearch and i use module from damiafuentes/DJITelloPy. But I have problem when control analog in gamepad its doesnt work. Can anyone help me?.
Python:
import cv2
import time
from pyimagesearch.centroidtracker import CentroidTracker
from pyimagesearch.trackableobject import TrackableObject
from imutils.video import VideoStream
from imutils.video import FPS
import numpy as np
import argparse
import imutils
import time
import dlib
import pygame.key
import pygame
import keyboard
import os
from tello import Tello
#from multiprocessing import Process
from time import sleep
import threading



#terhubung ke Tello

#gamepad.get_axis
#drone = tellopy.Tello()
drone = Tello()
drone.connect()
drone.for_back_velocity = 0
drone.left_right_velocity = 0
drone.up_down_velocity = 0
drone.yaw_velocity = 0
drone.speed = 0
buttons = None
#class semua:

def video():
    #global drone
    #t =Tello()
    
    
    
    #print(drone.get_battery())



    #drone.streamoff()
    #drone.streamon()

    width = 800 # WIDTH OF THE IMAGE
    height = 600  # HEIGHT OF THE IMAGE

    startCounter =0   #  0 FOR FIGHT 1 FOR TESTING

    # construct the argument parse and parse the arguments
    ap = argparse.ArgumentParser()
    ap.add_argument("-p", "--prototxt", required=True,
        help="path to Caffe 'deploy' prototxt file")
    ap.add_argument("-m", "--model", required=True,
        help="path to Caffe pre-trained model")
    ap.add_argument("-i", "--input", type=str,
        help="path to optional input video file")
    ap.add_argument("-o", "--output", type=str,
        help="path to optional output video file")
    ap.add_argument("-c", "--confidence", type=float, default=0.4,
        help="minimum probability to filter weak detections")
    ap.add_argument("-s", "--skip-frames", type=int, default=30,
        help="# of skip frames between detections")
    args = vars(ap.parse_args())

    # initialize the list of class labels MobileNet SSD was trained to
    # detect
    CLASSES = ["background", "aeroplane", "bicycle", "bird", "boat",
        "bottle", "bus", "car", "cat", "chair", "cow", "diningtable",
        "dog", "horse", "motorbike", "person", "pottedplant", "sheep",
        "sofa", "train", "tvmonitor"]

    # load our serialized model from disk
    print("[INFO] loading model...")
    #net = cv2.dnn.readNetFromCaffe(args["model"], args["prototxt"])

    net = cv2.dnn.readNetFromCaffe(args["prototxt"], args["model"])

    #mendapatkan referensi untuk streaming di Tello
    if not args.get("input", False):
        print("[INFO] starting video stream...")
        #vs = drone.streamon().start()
        vs = drone.streamon()
        time.sleep(2.0)

    # otherwise, grab a reference to the video file
    else:
        print("[INFO] opening video file...")
        vs = cv2.VideoCapture(args["input"])

    # initialize the video writer (we'll instantiate later if need be)
    writer = None

    # instantiate our centroid tracker, then initialize a list to store
    # each of our dlib correlation trackers, followed by a dictionary to
    # map each unique object ID to a TrackableObject
    ct = CentroidTracker(maxDisappeared=40, maxDistance=50)
    trackers = []
    trackableObjects = {}

    # initialize the total number of frames processed thus far, along
    # with the total number of objects that have moved either up or down
    totalFrames = 0
    totalDown = 0
    #totalUp = 0

    # start the frames per second throughput estimator
    fps = FPS().start()
    frame = drone.get_frame_read()
    

    #drone.takeoff()
    #drone.move_up(30)
    #drone.move_down(30)
    #drone.land
    global buttons

    while True :
        #Get Image or video from Tello
        #frame = drone.get_frame_read()
        
        myFrame = frame.frame
        #img = cv2.resize(myFrame, (width, height))
        #img = np.ascontiguousarray(img)
    # frameku = cv2.resize(myFrame,  (width, height))

        frameku = imutils.resize(myFrame, width = 800)

        text1 = "Battery: {}%".format(drone.get_battery())
        cv2.putText(frameku, text1, (500,500-5),
            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)

        text2 = "Pemanfaatan Quadcopter DJI Tello Untuk Mendeteksi Dan Menghitung Jumlah Mobil Dengan Menggunakan Bahasa Python "
        cv2.putText(frameku, text2, (5,50),
            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)

        


    
        
    
        frame = frame[1] if args.get("input", False) else frame
        

        # if we are viewing a video and we did not grab a frame then we
        # have reached the end of the video
        if args["input"] is not None and frame is None:
            break

        # the frame from BGR to RGB for dlib
        rgb = cv2.cvtColor(myFrame, cv2.COLOR_BGR2RGB)

        #frame = imutils.resize(frame, width=500)
        # if the frame dimensions are empty, set them
        if width is None or height is None:
            (height, width) = frameku.shape[:2]

        


        # # 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)

        # DISPLAY IMAGE
        #cv2.imshow("PictFromTello", img)

        # if we are supposed to be writing a video to disk, initialize
        # the writer
        if args["output"] is not None and writer is None:
            fourcc = cv2.VideoWriter_fourcc(*"MJPG")
            writer = cv2.VideoWriter(args["output"], fourcc, 30,
                (width, height), True)
        
        # initialize the current status along with our list of bounding
        # box rectangles returned by either (1) our object detector or
        # (2) the correlation trackers
        status = "Waiting"
        rects = []

        # check to see if we should run a more computationally expensive
        # object detection method to aid our tracker
        if totalFrames % args["skip_frames"] == 0:
            # set the status and initialize our new set of object trackers
            status = "Detecting"
            trackers = []
            # convert the frame to a blob and pass the blob through the
            # network and obtain the detections
            blob = cv2.dnn.blobFromImage(myFrame, 0.007843, (width, height), 127.5)
            net.setInput(blob)
            detections = net.forward()

            # loop over the detections
            for i in np.arange(0, detections.shape[2]):
                # extract the confidence (i.e., probability) associated
                # with the prediction
                confidence = detections[0, 0, i, 2]

                # filter out weak detections by requiring a minimum
                # confidence
                if confidence > args["confidence"]:
                    # extract the index of the class label from the
                    # detections list
                    idx = int(detections[0, 0, i, 1])

                    # if the class label is not a person, ignore it
                    if CLASSES[idx] != "car":
                        continue

                    # compute the (x, y)-coordinates of the bounding box
                    # for the object
                    box = detections[0, 0, i, 3:7] * np.array([width, height, width, height])
                    (startX, startY, endX, endY) = box.astype("int")


                    # construct a dlib rectangle object from the bounding
                    # box coordinates and then start the dlib correlation
                    # tracker
                    tracker = dlib.correlation_tracker()
                    rect = dlib.rectangle(startX, startY, endX, endY)
                    #rect.is_empty() == False

                    tracker.start_track(rgb, rect)

                    pos = tracker.get_position()
                    
                    # add the tracker to our list of trackers so we can
                    # utilize it during skip frames
                    trackers.append(tracker)

        # otherwise, we should utilize our object *trackers* rather than
        # object *detectors* to obtain a higher frame processing throughput
        else:
            # loop over the trackers
            for tracker in trackers:
                # set the status of our system to be 'tracking' rather
                # than 'waiting' or 'detecting'
                status = "Tracking"

                # update the tracker and grab the updated position
                tracker.update(rgb)
                pos = tracker.get_position()

                # unpack the position object
                startX = int(pos.left())
                startY = int(pos.top())
                endX = int(pos.right())
                endY = int(pos.bottom())

                # add the bounding box coordinates to the rectangles list
                rects.append((startX, startY, endX, endY))
        
        
        # draw a horizontal line in the center of the frame -- once an
        # object crosses this line we will determine whether they were
        # moving 'up' or 'down'
        cv2.line(frameku, (width // 2, 0), (width // 2, height), (0, 255, 255), 2)


        # use the centroid tracker to associate the (1) old object
        # centroids with (2) the newly computed object centroids
        objects = ct.update(rects)

        #me-looping object tracking
        for (objectID, centroid) in objects.items():
            # check to see if a trackable object exists for the current
            # object ID
            to = trackableObjects.get(objectID, None)

            # if there is no existing trackable object, create one
            if to is None:
                to = TrackableObject(objectID, centroid)
            
            # otherwise, there is a trackable object so we can utilize it
            # to determine direction
            else:
                # the difference between the y-coordinate of the *current*
                # centroid and the mean of *previous* centroids will tell
                # us in which direction the object is moving (negative for
                # 'up' and positive for 'down')
                y = [c[1] for c in to.centroids]
                direction = centroid[1] - np.mean(y)
                to.centroids.append(centroid)

                # check to see if the object has been counted or not
                if not to.counted:
                    # if the direction is negative (indicating the object
                    # is moving up) AND the centroid is above the center
                    # line, count the object
                    if direction < 0 and centroid[1] < height // 2:
                        totalDown += 1
                        to.counted = True
                    # if the direction is positive (indicating the object
                    # is moving down) AND the centroid is below the
                    # center line, count the object
                    if direction > 0 and centroid[1] > height // 2:
                        totalDown += 1
                        to.counted = True
            
            
            # store the trackable object in our dictionary
            trackableObjects[objectID] = to

            # draw both the ID of the object and the centroid of the
            # object on the output frame
            text = "Mobil {}".format(objectID)
            cv2.putText(frameku, text, (centroid[0] - 10, centroid[1] - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
            cv2.circle(frameku, (centroid[0], centroid[1]), 4, (0, 255, 0), -1)
            
            
            # construct a tuple of information we will be displaying on the
        # frame
        info = [
            #("Jumlah Mobil", totalUp),
            ("Jumlah Mobil", totalDown),
            ("Status", status),
        ]

        # loop over the info tuples and draw them on our frame
        for (i, (k, v)) in enumerate(info):
            text = "{}: {}".format(k, v)
            cv2.putText(frameku, text, (10, height - ((i * 20) + 20)),
                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
        
        # check to see if we should write the frame to disk
        if writer is not None:
            writer.write(myFrame)

        #os.system("python3 Joystick_and_Video.py")

        # show the output frame
        cv2.imshow("Output Pengujian", frameku)
        key = cv2.waitKey(1) & 0xFF

        #if startCounter == 0:
        # drone.takeoff()
            #time.sleep(8)
            #drone.rotate_clockwise(90)
            #time.sleep(3)
            #drone.move_left(35)
            #time.sleep(3)
        # drone.land()
            #startCounter = 1

        #if key == ord('b') :
            #print(drone.get_battery())

        if key == ord('x'):
            drone.takeoff()
        
        if key == ord('w'):
            drone.move_forward(30)

        if key == ord('s'):
            drone.move_backward(30)

        if key == ord('d'):
            drone.move_right(30)

        if key == ord('a'):
            drone.move_left(30)

        if key == ord('u'):
            drone.move_up(50)

        if key == ord('j'):
            drone.move_down(50)

        # WAIT FOR THE 'Q' BUTTON TO STOP
        if key == ord('z'):
            drone.land()
            break
        
        # increment the total number of frames processed thus far and
        # then update the FPS counter
        totalFrames += 1
        fps.update()

        sleep(.02)

        # stop the timer and display FPS information
    fps.stop()
    print("[INFO] elapsed time: {:.2f}".format(fps.elapsed()))
    print("[INFO] approx. FPS: {:.2f}".format(fps.fps()))

    # check to see if we need to release the video writer pointer
    if writer is not None:
        writer.release()

    # if we are not using a video file, stop the camera video stream
    if not args.get("input", False):
        vs = drone.end()
        #vs.stop()

    # otherwise, release the video file pointer
    else:
        vs.release()



    # close any open windows
    cv2.destroyAllWindows()


def joystick():

    #drone = tellopy.Tello()
    #global drone

    pygame.init()

    #joystick = None

    joystick_count = pygame.joystick.get_count()

    if joystick_count == 0:
        print("sorry")
        

    else:
        gamepad = pygame.joystick.Joystick(0)
        gamepad.init()
        

    while True:
        pygame.event.get()

        if joystick_count != 0:

            rightsticky = gamepad.get_axis(1)
            leftsticky = gamepad.get_axis(4)

            rightstickx = gamepad.get_axis(0)
            leftstickx = gamepad.get_axis(3)
            Start = gamepad.get_button(7)
            Back = gamepad.get_button(6)
            x, y = gamepad.get_hat(0)

            X = gamepad.get_button(2)
            Y = gamepad.get_button(3)
            A = gamepad.get_button(0)
            B = gamepad.get_button(1)

        if Start == 1:
            print('Start pressed')
            drone.takeoff()
        
        if Back == 1:
            drone.land()
            print('Back pressed')

        if Y == 1:
            drone.move_forward(50)
            #print('Maju')

        if A == 1:
            drone.move_backward(50)
            #print('Mundur')

        if B == 1:
            drone.move_right(50)
            #print('kanan')

        if X == 1:
            drone.move_left(50)
            #print('kiri')



        if abs(leftsticky) > .05:
        
            drone.set_pitch(-leftsticky)
        else:
            drone.set_pitch(0)
        if abs(rightsticky) > .05:
        
            drone.set_throttle(-rightsticky)
        else:
            drone.set_throttle(0)
            
        if abs(leftstickx) > .05:
            print("call")
            drone.set_roll(leftstickx)
        else:
            drone.set_roll(0)
            
        if abs(rightstickx) > .05:
            drone.set_yaw(rightstickx)
        else:
            drone.set_yaw(0)
        
        sleep(.02)

#def main():
    
    #joystick.video()

if __name__ == '__main__':
    
    proc1 = threading.Thread(target=video)
    proc1.start()

    proc2 = threading.Thread(target=joystick)
    proc2.start()

    proc2.join()
 

New Posts

Members online

No members online now.

Forum statistics

Threads
5,759
Messages
40,277
Members
17,398
Latest member
avia_explorer

New Posts