class DroneCamera():
def __init__(self):
self.drone = Tello() # Instantiate a Tello Object
self.drone.connect() # Connect to the drone
self.drone.streamoff() # In case stream never exited before
self.drone.streamon() # Turn on drone camera stream
time.sleep(5) # Give the stream time to start up
self.timer = 0 # Timing for printing statements
def __del__(self):
self.drone.streamoff()
def get_frame(self):
# Grab a frame and resize it
frame_read = self.drone.get_frame_read()
if frame_read.stopped:
return None
frame = cv2.resize(frame_read.frame, (360, 240))
# Print battery status to the log every 10 seconds
if (time.time() - self.timer > 10):
self.timer = time.time()
self.drone.get_battery()
# encode OpenCV raw frame to jpeg
ret, jpeg = cv2.imencode('.jpg', frame)
return jpeg.tobytes()
def start_tello():
'''
Connects to drone and sets velocities and speed to 0
Cycles drone stream
'''
#makes drone and connects to it
drone = Tello()
drone.connect()
#sets all drones velocity to 0
drone.forward_backward_velocity = 0
drone.left_right_velocity = 0
drone.up_down_velocity = 0
drone.yaw_velocity = 0
drone.speed = 0
#cycles drone stream off and on
drone.streamoff()
drone.streamon()
#prints drone's battery at start
time.sleep(5)
print(drone.get_battery())
return drone
class FrontEnd(object):
def __init__(self):
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.h_pid = PID(0.1, 0.00001, 0.01)
self.v_pid = PID(0.5, 0.00001, 0.01)
self.dist_pid = PID(0.1, 0.00001, 0.01)
self.send_rc_control = False
def run(self, args):
run(self, args, lerp, ddir, face_cascade)
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity,
self.yaw_velocity)
def initializeTello():
# METHOD: CONNECT TO TELLO
myDrone = Tello() # Create a Tello object
myDrone.connect() # Connect PC to Tello ()
# Set all velocity and speed of drone to 0
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
myDrone.speed = 0
print(myDrone.get_battery()) # Print out percentage of battery
myDrone.streamoff() # Turn off video stream of drone
myDrone.streamon() # Turn on video stream of drone
print("battery: ", myDrone.get_battery())
return myDrone
class Drone(object):
def __init__(self):
self.drone = None
self.drone_speed = 25
self.connect_drone()
def __del__(self):
global tello_conn
print("tello destroyed")
self.disconnect_drone()
tello_conn.close()
def begin_scan(self):
i = 18
sleep(1)
while i != 0:
self.drone.rotate_clockwise(21)
sleep(2)
self.drone.move_up(23)
sleep(3)
self.drone.move_down(23)
i -= 1
sleep(3)
def stream_image_to_pipe(self):
camera = picamera.PiCamera()
camera.resolution = (640, 480)
output_path = vars(ap.parse_args())['output']
filename = os.path.join(output_path, 'outpy.h264')
print('Save video to: ', filename)
camera.start_recording(filename)
while True:
camera.wait_recording(1)
camera.stop_recording()
print("Created video file")
def connect_drone(self):
self.drone = Tello()
if not self.drone.connect():
raise RuntimeError("drone not connected")
self.drone.streamon()
def disconnect_drone(self):
self.drone.streamoff()
self.drone = None
def scan(self, triangulation_type):
self.drone.takeoff()
sleep(3)
Thread(target=self.stream_image_to_pipe, daemon=True).start()
self.begin_scan(triangulation_type)
print("battery after scan:" + self.drone.get_battery())
self.drone.land()
sleep(3)
def initializeTello():
tccdrone = Tello()
tccdrone.connect()
tccdrone.for_back_velocity = 0
tccdrone.left_right_velocity = 0
tccdrone.up_down_velocity = 0
tccdrone.yaw_velocity = 0
tccdrone.speed = 0
print(tccdrone.get_battery())
tccdrone.streamoff()
tccdrone.streamon()
return tccdrone
def initializeTello():
JwTello = Tello()
JwTello.connect()
JwTello.for_back_velocity = 0
JwTello.left_right_velocity = 0
JwTello.up_down_velocity = 0
JwTello.yaw_velocity = 0
JwTello.speed = 0
print(JwTello.get_battery())
JwTello.streamoff()
JwTello.streamon()
return JwTello
def initializeDrone():
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
print(drone.get_battery())
drone.streamoff()
drone.streamon()
return drone
def initialize(self):
myDrone = Tello()
myDrone.connect()
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
myDrone.speed = 0
print(myDrone.get_battery())
myDrone.streamoff()
myDrone.streamon()
return myDrone
def intializeTello():
# CONNECT TO TELLO
myDrone = Tello()
myDrone.connect()
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
myDrone.speed = 0
print(myDrone.get_battery())
myDrone.streamoff()
myDrone.streamon()
return myDrone
def initTello():
tello = Tello()
tello.connect()
tello.for_back_velocity = 0
tello.left_right_velocity = 0
tello.up_down_velocity = 0
tello.yaw_velocity = 0
tello.speed = 0
print(tello.get_battery())
# tello.streamoff()
tello.streamon()
tello.takeoff()
return tello
def main():
# 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()
########################
me.takeoff()
flyCurve(me, 160, 50)
time.sleep(5)
me.land()
def initTello():
myDrone = Tello()
myDrone.connect()
#set velicties to 0
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
myDrone.speed = 0
# get battery status
print(myDrone.get_battery())
# turn off stream
myDrone.streamoff()
# stream on
myDrone.streamon()
return myDrone
def __init__(self):
self.width = 640
self.height = 480
me = Tello()
me.connect()
me.forward_backward_velocity = 0
me.left_right_velocity = 0
me.up_down_velocity = 0
me.yaw_velocity = 0
me.speed = 0
battery = me.get_battery()
if battery <= 15:
print("[ERROR] - battery under 15% ")
sys.exit(0)
me.streamoff()
me.streamon()
self.me = me
self.takeoff = False
self.frame = None
class hand_tello_control:
def __init__(self):
self.action = " "
self.mp_drawing = mp.solutions.drawing_utils
self.mp_drawing_styles = mp.solutions.drawing_styles
self.mp_hands = mp.solutions.hands
self.action_done = True
self.ffoward = 1
self.fback = 1
self.fright = 1
self.fleft = 1
self.fsquare = 1
self.fmiddle = 1
self.fno = 1
self.fland = 1
def tello_startup(self):
# For Tello input:
self.tello = Tello() # Starts the tello object
self.tello.connect() # Connects to the drone
def define_orientation(self, results):
if results.multi_hand_landmarks[0].landmark[
4].x < results.multi_hand_landmarks[0].landmark[17].x:
orientation = "right hand"
else:
orientation = "left hand"
return orientation
def follow_hand(self, results):
normalizedLandmark = results.multi_hand_landmarks[0].landmark[
9] # Normalizes the lowest middle-finger coordinate for hand tracking
pixelCoordinatesLandmark = self.mp_drawing._normalized_to_pixel_coordinates(
normalizedLandmark.x, normalizedLandmark.y, 255, 255
) #Tracks the coordinates of the same landmark in a 255x255 grid
print(pixelCoordinatesLandmark)
if pixelCoordinatesLandmark == None: # If hand goes out of frame, stop following
self.tello.send_rc_control(0, 0, 0, 0)
return
centerRange = [12,
12] #Range for detecting commands in the x and y axis.
centerPoint = [128, 128] #Theoretical center of the image
xCorrection = pixelCoordinatesLandmark[0] - centerPoint[0]
yCorrection = pixelCoordinatesLandmark[1] - centerPoint[1]
xSpeed = 0
ySpeed = 0
if xCorrection > centerRange[0] or xCorrection < -centerRange[
0]: #If the hand is outside the acceptable range, changes the current speed to compensate.
xSpeed = xCorrection // 3
if yCorrection > centerRange[1] or yCorrection < -centerRange[1]:
ySpeed = -yCorrection // 3
self.tello.send_rc_control(xSpeed, 0, ySpeed, 0)
time.sleep(0.5)
self.tello.send_rc_control(0, 0, 0, 0)
def action_to_do(
self, fingers, orientation,
results): #use the variable results for the hand tracking control
if self.action_done == True:
self.ffoward = 1
self.fback = 1
self.fright = 1
self.fleft = 1
self.fsquare = 1
self.fmiddle = 1
self.fno = 1
self.fland = 1
self.action_done = False
#Left hand controls tricks, right hand controls movement
if orientation == "left hand": #Thumb on the left = left hand!
if fingers == [0, 1, 0, 0, 0]:
if self.ffoward >= 15:
self.action = "flip forward"
self.tello.flip_forward()
self.action_done = True
self.ffoward = self.ffoward + 1
elif fingers == [0, 1, 1, 0, 0] and self.battery == True:
if self.fback >= 15:
self.action = "flip back"
self.tello.flip_back()
self.action_done = True
self.fback = self.fback + 1
elif fingers == [1, 0, 0, 0, 0] and self.battery == True:
if self.fright >= 15:
self.action = "flip right"
self.tello.flip_right()
self.action_done = True
self.fright = self.fright + 1
elif fingers == [0, 0, 0, 0, 1] and self.battery == True:
if self.fleft >= 15:
self.action = "flip left"
self.tello.flip_left()
self.action_done = True
self.fleft = self.fleft + 1
elif fingers == [0, 1, 1, 1, 0]:
if self.fsquare >= 15:
self.action = "Square"
self.tello.move_left(20)
self.tello.move_up(40)
self.tello.move_right(40)
self.tello.move_down(40)
self.tello.move_left(20)
self.action_done = True
self.fsquare = self.fsquare + 1
elif fingers == [0, 0, 1, 0, 0]:
if self.fmiddle >= 15:
self.action = " :( "
self.tello.land()
self.action_done = True
self.fmiddle = self.fmiddle + 1
elif ((self.battery == False) and
(fingers == [1, 0, 0, 0, 0] or fingers == [0, 1, 0, 0, 0]
or fingers == [0, 0, 0, 0, 1])): #not avaiable to do tricks
if self.fno >= 15:
self.tello.rotate_clockwise(45)
self.tello.rotate_counter_clockwise(90)
self.tello.rotate_clockwise(45)
self.action_done = True
self.fno = self.fno + 1
else:
self.action = " "
elif orientation == "right hand": #Thumb on the right = right hand!
if fingers == [1, 1, 1, 1, 1]:
self.action = "Follow"
self.follow_hand(results)
elif fingers == [1, 0, 0, 0, 0]:
if self.fland >= 15:
self.action = "Land"
self.tello.land()
self.action_done = True
self.fland = self.fland + 1
else:
self.action = " "
def fingers_position(self, results, orientation):
# [thumb, index, middle finger, ring finger, pinky]
# 0 for closed, 1 for open
fingers = [0, 0, 0, 0, 0]
if (results.multi_hand_landmarks[0].landmark[4].x >
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "right hand":
fingers[0] = 0
if (results.multi_hand_landmarks[0].landmark[4].x <
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "right hand":
fingers[0] = 1
if (results.multi_hand_landmarks[0].landmark[4].x >
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "left hand":
fingers[0] = 1
if (results.multi_hand_landmarks[0].landmark[4].x <
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "left hand":
fingers[0] = 0
fingermarkList = [8, 12, 16, 20]
i = 1
for k in fingermarkList:
if results.multi_hand_landmarks[0].landmark[
k].y > results.multi_hand_landmarks[0].landmark[k - 2].y:
fingers[i] = 0
else:
fingers[i] = 1
i = i + 1
return fingers
def detection_loop(self):
with self.mp_hands.Hands(model_complexity=0,
min_detection_confidence=0.75,
min_tracking_confidence=0.5) as hands:
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while True:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
self.battery = self.tello.get_battery()
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
results = hands.process(image)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
if results.multi_hand_landmarks:
action = " "
for hand_landmarks in results.multi_hand_landmarks:
self.mp_drawing.draw_landmarks(
image, hand_landmarks,
self.mp_hands.HAND_CONNECTIONS,
self.mp_drawing_styles.
get_default_hand_landmarks_style(),
self.mp_drawing_styles.
get_default_hand_connections_style())
orientation = self.define_orientation(results)
fingers = self.fingers_position(results, orientation)
#print(fingers)
self.action_to_do(fingers, orientation, results)
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_l:
self.tello.land()
if event.key == pg.K_t:
self.tello.takeoff()
if event.key == pg.K_b:
print("A bateria esta em ",
self.tello.get_battery(), "%")
if event.key == pg.K_m:
return 0
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
def key_control(self):
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while True:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_w:
self.tello.move_forward(20)
if event.key == pg.K_a:
self.tello.move_left(20)
if event.key == pg.K_s:
self.tello.move_back(20)
if event.key == pg.K_d:
self.tello.move_right(20)
if event.key == pg.K_q:
self.tello.rotate_counter_clockwise(20)
if event.key == pg.K_e:
self.tello.rotate_clockwise(20)
if event.key == pg.K_SPACE:
self.tello.move_up(20)
if event.key == pg.K_LCTRL:
self.tello.move_down(20)
if event.key == pg.K_b:
print("A bateria esta em ", self.tello.get_battery(),
"%")
if event.key == pg.K_m:
return 0
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
def main_interface(self):
telloMode = -1
self.tello_startup()
pg.init()
win = pg.display.set_mode((500, 500))
pg.display.set_caption("Test")
#self.tello.takeoff()
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while telloMode != 0:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
if telloMode == 1:
self.key_control()
telloMode = -1
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
elif telloMode == 2:
self.detection_loop()
telloMode = -1
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
elif telloMode == 0:
self.tello.land()
telloMode = -1
print("Obrigado por voar hoje")
elif telloMode != -1 and telloMode != 1 and telloMode != 2:
print("valor invalido!")
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_1:
telloMode = 1
if event.key == pg.K_2:
telloMode = 2
if event.key == pg.K_0:
telloMode = 0
if event.key == pg.K_l:
self.tello.land()
if event.key == pg.K_t:
self.tello.takeoff()
if event.key == pg.K_b:
print("A bateria esta em ", self.tello.get_battery(),
"%")
#Simply import of "panoramaModule.py" and you can use each function by calling it with name of the drone inside arguments. from djitellopy import Tello import cv2 import time import panoramaModule tello = Tello() tello.connect() print(tello.get_battery()) tello.takeoff() tello.move_up(500) panoramaModule.panorama_half_clockwise(tello) tello.land()
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations (yaw)
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
def control_manual(self):
key = input('entraste en control manual')
time.sleep(2)
if key == '':
print
return
else:
if key == 'w':
print('entraste al comando')
self.tello.move_forward(30)
elif key == 's':
self.tello.move_down(30)
elif key == 'a':
self.tello.move_left(30)
elif key == ord('d'):
self.tello.move_right(30)
elif key == ord('e'):
self.tello.rotate_clockwise(30)
elif key == ord('q'):
self.tello.rotate_counter_clockwise(30)
elif key == ord('r'):
self.tello.move_up(30)
elif key == ord('f'):
self.tello.move_down(30)
print('termino control manual')
def run(self):
self.tello.connect()
self.tello.set_speed(self.speed)
# In case streaming is on. This happens when we quit this program without the escape key.
self.tello.streamoff()
self.tello.streamon()
#self.tello.takeoff()
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1:
self.update()
elif event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == pygame.KEYUP:
self.keyup(event.key)
if frame_read.stopped:
break
self.screen.fill([0, 0, 0])
frame = frame_read.frame
text = "Battery: {}%".format(self.tello.get_battery())
cv2.putText(frame, text, (5, 720 - 5), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
#x=threading.Thread(target=self.control_manual)
#x.start()
#x.join()
#time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
not self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class DroneUI(object):
def __init__(self):
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.mode = PMode.NONE # Can be '', 'FIND', 'OVERRIDE' or 'FOLLOW'
self.send_rc_control = False
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
if args.cat == 'any':
print('Using CatDetectionModel')
self.model = CatDetectionModel(0.5)
else:
print('Using MyCatsDetectionModel ({})'.format(args.cat))
self.model = MyCatsDetectionModel(0.5)
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
DETECT_ENABLED = False # Set to true to automatically start in follow mode
self.mode = PMode.NONE
self.tello.get_battery()
# Safety Zone X
szX = args.saftey_x
# Safety Zone Y
szY = args.saftey_y
if args.debug: print("DEBUG MODE ENABLED!")
while not should_stop:
frame_time_start = time.time()
# self.update() # Moved to the end before sleep to have more accuracy
if frame_read.stopped:
frame_read.stop()
self.update() ## Just in case
break
print('---')
# TODO: Analize if colors have to be tweaked
frame = cv2.flip(frame_read.frame,
0) # Vertical flip due to the mirror
frameRet = frame.copy()
vid = self.tello.get_video_capture()
imgCount += 1
#time.sleep(1 / FPS)
# Listen for key presses
k = cv2.waitKey(20)
try:
if chr(k) in 'ikjluoyhp': OVERRIDE = True
except:
...
if k == ord('e'):
DETECT_ENABLED = True
elif k == ord('d'):
DETECT_ENABLED = False
# Press T to take off
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
if k == ord('s') and self.send_rc_control == True:
self.mode = PMode.FIND
DETECT_ENABLED = True # To start following with autopilot
OVERRIDE = False
print('Switch to spiral mode')
# This is temporary, follow mode should start automatically
if k == ord('f') and self.send_rc_control == True:
DETECT_ENABLED = True
OVERRIDE = False
print('Switch to follow mode')
# Press L to land
if k == ord('g'):
self.land_and_set_none()
# self.update() ## Just in case
# break
# Press Backspace for controls override
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
# Quit the software
if k == 27:
should_stop = True
self.update() ## Just in case
break
autoK = -1
if k == -1 and self.mode == PMode.FIND:
if not OVERRIDE:
autoK = next_auto_key()
if autoK == -1:
self.mode = PMode.NONE
print('Queue empty! no more autokeys')
else:
print('Automatically pressing ', chr(autoK))
key_to_process = autoK if k == -1 and self.mode == PMode.FIND and OVERRIDE == False else k
if self.mode == PMode.FIND and not OVERRIDE:
#frame ret will get the squares drawn after this operation
if self.process_move_key_andor_square_bounce(
key_to_process, frame, frameRet) == False:
# If the queue is empty and the object hasn't been found, land and finish
self.land_and_set_none()
#self.update() # Just in case
break
else:
self.process_move_key(key_to_process)
dCol = (0, 255, 255)
#detected = False
if not OVERRIDE and self.send_rc_control and DETECT_ENABLED:
self.detect_subjects(frame, frameRet, szX, szY)
show = ""
if OVERRIDE:
show = "MANUAL"
dCol = (255, 255, 255)
elif self.mode == PMode.FOLLOW or self.mode == PMode.FLIP:
show = "FOUND!!!"
elif self.mode == PMode.FIND:
show = "Finding.."
mode_label = 'Mode: {}'.format(self.mode)
# Draw the distance choosen
cv2.putText(frameRet, mode_label, (32, 664),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
cv2.putText(frameRet, show, (32, 600), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# Display the resulting frame
cv2.imshow('FINDER DRONE', frameRet)
if (self.mode == PMode.FLIP):
self.flip()
# OVERRIDE = True
self.update(
) # Moved here instead of beginning of loop to have better accuracy
frame_time = time.time() - frame_time_start
sleep_time = 1 / FPS - frame_time
if sleep_time < 0:
sleep_time = 0
print('SLEEEP TIME NEGATIVE FOR FRAME {} ({}s).. TURNING IT 0'.
format(imgCount, frame_time))
if args.save_session and self.send_rc_control == True: # To avoid recording before takeoff
self.create_frame_files(frame, frameRet, imgCount)
time.sleep(sleep_time)
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
# cv2.destroyWindow('FINDER DRONE')
# cv2.waitKey(0)
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def create_frame_files(self, frame, frameRet, imgCount):
def create_frame_file(image, subdir, print_log=False):
global ddir
path = ddir + '/' + subdir
if not os.path.exists(path): os.makedirs(path)
filename = "{}/tellocap{}.jpg".format(path, imgCount)
if print_log: print('Created {}'.format(filename))
cv2.imwrite(filename, image)
create_frame_file(frame, 'raw')
create_frame_file(frameRet, 'output', True)
def flip(self):
print('Flip!')
self.left_right_velocity = self.for_back_velocity = 0
self.update()
time.sleep(self.tello.TIME_BTW_COMMANDS * 2)
if not args.debug:
self.tello.flip_left()
#self.tello.flip_right()
# The following 2 lines allow going back to follow mode
self.mode = PMode.FOLLOW
global onFoundAction
onFoundAction = PMode.FOLLOW # So it doesn't flip over and over
def land_and_set_none(self):
if not args.debug:
print("------------------Landing--------------------")
self.tello.land()
self.send_rc_control = False
self.mode = PMode.NONE # TODO: Consider calling reset
def oq_discard_keys(self, keys_to_pop):
oq = globals.mission.operations_queue
keys_to_pop += 'p'
while len(oq) > 0:
oqkey = oq[0]['key']
if oqkey in keys_to_pop:
print('Removing {} from queue'.format(oqkey))
oq.popleft()
else:
break
def process_move_key_andor_square_bounce(self, k, frame, frameRet=None):
self.process_move_key(k) # By default use key direction
(hor_dir, ver_dir) = get_squares_push_directions(frame, frameRet)
print('(hor_dir, ver_dir): ({}, {})'.format(hor_dir, ver_dir))
oq = globals.mission.operations_queue
print('operations_queue len: ', len(oq))
keys_to_pop = ''
if ver_dir == 'forward':
self.for_back_velocity = int(S)
if k != ord('i'): print('Square pushing forward')
keys_to_pop += 'k'
elif ver_dir == 'back':
self.for_back_velocity = -int(S)
if k != ord('k'): print('Square pushing back')
keys_to_pop += 'i'
if hor_dir == 'right':
self.left_right_velocity = int(S)
if k != ord('l'): print('Square pushing right')
keys_to_pop += 'j'
elif hor_dir == 'left':
self.left_right_velocity = -int(S)
if k != ord('j'): print('Square pushing left')
keys_to_pop += 'l'
if (len(keys_to_pop) > 0):
self.oq_discard_keys(keys_to_pop)
return (len(oq) > 0)
def process_move_key(self, k):
# i & k to fly forward & back
if k == ord('i'):
self.for_back_velocity = int(S)
elif k == ord('k'):
self.for_back_velocity = -int(S)
else:
self.for_back_velocity = 0
# o & u to pan left & right
if k == ord('o'):
self.yaw_velocity = int(S)
elif k == ord('u'):
self.yaw_velocity = -int(S)
else:
self.yaw_velocity = 0
# y & h to fly up & down
if k == ord('y'):
self.up_down_velocity = int(S)
elif k == ord('h'):
self.up_down_velocity = -int(S)
else:
self.up_down_velocity = 0
# l & j to fly left & right
if k == ord('l'):
self.left_right_velocity = int(S)
elif k == ord('j'):
self.left_right_velocity = -int(S)
else:
self.left_right_velocity = 0
# p to keep still
if k == ord('p'):
print('pressing p')
def show_save_detection(self, frame, frameRet, firstDetection):
output_filename_det_full = "{}/detected_full.jpg".format(ddir)
cv2.imwrite(output_filename_det_full, frameRet)
print('Created {}'.format(output_filename_det_full))
(x, y, w, h) = firstDetection['box']
add_to_borders = 100
(xt, yt) = (x + w + add_to_borders, y + h + add_to_borders)
(x, y) = (max(0, x - add_to_borders), max(0, y - add_to_borders))
# subframeRet = frameRet[y:yt, x:xt].copy()
subframe = frame[y:yt, x:xt].copy()
def show_detection():
output_filename_det_sub = "{}/detected_sub.jpg".format(ddir)
cv2.imwrite(output_filename_det_sub, subframe)
print('Created {}'.format(output_filename_det_sub))
# Shows detection in a window. If it doesn't exist yet, waitKey
waitForKey = cv2.getWindowProperty('Detected',
0) < 0 # True for first time
cv2.imshow('Detected', subframe)
if waitForKey: cv2.waitKey(0)
Timer(0.5, show_detection).start()
def detect_subjects(self, frame, frameRet, szX, szY):
detections = self.model.detect(frameRet)
# print('detections: ', detections)
self.model.drawDetections(frameRet, detections)
class_wanted = 0 if args.cat == 'any' else self.model.LABELS.index(
args.cat)
detection = next(
filter(lambda d: d['classID'] == class_wanted, detections), None)
isSubjectDetected = not detection is None
if isSubjectDetected:
print('{} FOUND!!!!!!!!!!'.format(self.model.LABELS[class_wanted]))
#if self.mode != onFoundAction: # To create it only the first time
self.mode = onFoundAction
# if we've given rc controls & get object coords returned
# if self.send_rc_control and not OVERRIDE:
if self.mode == PMode.FOLLOW:
self.follow(detection, frameRet, szX, szY)
self.show_save_detection(frame, frameRet, detection)
elif self.mode == onFoundAction:
# if there are no objects detected, don't do anything
print("CAT NOT DETECTED NOW")
return isSubjectDetected
def follow(self, detection, frameRet, szX, szY):
print('Following...')
# These are our center dimensions
(frame_h, frame_w) = frameRet.shape[:2]
cWidth = int(frame_w / 2)
cHeight = int(frame_h / 2)
(x, y, w, h) = detection['box']
# end coords are the end of the bounding box x & y
end_cord_x = x + w
end_cord_y = y + h
# This is not face detection so we don't need offset
UDOffset = 0
# these are our target coordinates
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# This calculates the vector from the object to the center of the screen
vTrue = np.array((cWidth, cHeight))
vTarget = np.array((targ_cord_x, targ_cord_y))
vDistance = vTrue - vTarget
if True or not args.debug:
if vDistance[0] < -szX:
# Right
self.left_right_velocity = S
elif vDistance[0] > szX:
# Left
self.left_right_velocity = -S
else:
self.left_right_velocity = 0
# for up & down
if vDistance[1] > szY:
self.for_back_velocity = S
elif vDistance[1] < -szY:
self.for_back_velocity = -S
else:
self.for_back_velocity = 0
# Draw the center of screen circle, this is what the drone tries to match with the target coords
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
# Draw the target as a circle
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2)
# Draw the safety zone
obStroke = 2
cv2.rectangle(frameRet, (targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY), (0, 255, 0),
obStroke)
# Draw the estimated drone vector position in relation to object bounding box
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
print('Sending speeds to tello. H: {} V: {}'.format(
self.left_right_velocity, self.for_back_velocity))
if not args.debug:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class TelloCV(object):
"""
TelloTracker builds keyboard controls on top of TelloPy as well
as generating images from the video stream and enabling opencv support
"""
def __init__(self):
self.prev_flight_data = None
self.record = False
self.tracking = False
self.keydown = False
self.date_fmt = '%Y-%m-%d_%H%M%S'
self.speed = 50
self.go_speed = 80
self.drone = Tello()
self.init_drone()
self.init_controls()
self.battery = self.drone.get_battery()
self.frame_read = self.drone.get_frame_read()
self.forward_time = 0
self.forward_flag = True
self.takeoff_time = 0
self.command_time = 0
self.command_flag = False
# trackingimport libh264decoder a color
# green_lower = (30, 50, 50)
# green_upper = (80, 255, 255)
# red_lower = (0, 50, 50)
# red_upper = (20, 255, 255)
blue_lower = np.array([0, 0, 0])
upper_blue = np.array([255, 255, 180])
bh_lower = (180, 30, 100)
bh_upper = (275, 50, 100)
self.track_cmd = ""
self.tracker = Tracker(960,
720,
blue_lower, upper_blue)
self.speed_list = [5, 10, 15, 20, 25]
self.frame_center = 480, 360
self.error = 60
def init_drone(self):
"""Connect, uneable streaming and subscribe to events"""
# self.drone.log.set_level(2)
self.drone.connect()
self.drone.streamon()
def on_press(self, keyname):
"""handler for keyboard listener"""
if self.keydown:
return
try:
self.keydown = True
keyname = str(keyname).strip('\'')
print('+' + keyname)
if keyname == 'Key.esc':
self.drone.quit()
exit(0)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(self.speed)
else:
key_handler(self.speed)
except AttributeError:
print('special key {0} pressed'.format(keyname))
def on_release(self, keyname):
"""Reset on key up from keyboard listener"""
self.keydown = False
keyname = str(keyname).strip('\'')
print('-' + keyname)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(0)
else:
key_handler(0)
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': lambda speed: self.drone.move_forward(speed),
's': lambda speed: self.drone.move_back(speed),
'a': lambda speed: self.drone.move_left(speed),
'd': lambda speed: self.drone.move_right(speed),
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.rotate_counter_clockwise(speed),
'Key.right': lambda speed: self.drone.rotate_clockwise(speed),
'Key.up': lambda speed: self.drone.move_up(speed),
'Key.down': lambda speed: self.drone.move_down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
# 'Key.tab': self.drone.takeoff(60),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
# self.key_listener.join()
def process_frame(self):
"""convert frame to cv2 image and show"""
# print("TRACKING START")
frame = self.frame_read.frame
# self.drone.move_up(self.speed)
# image = self.write_hud(image)
# if self.record:
# self.record_vid(frame)
return frame
def move_up(self):
self.drone.move_up(self.speed)
def take_off(self):
self.drone.takeoff()
def go(self):
self.drone.move_forward(self.go_speed)
def move_left(self):
self.drone.move_left(270) # speed 테스트해서 조절하기
def go_window9(self):
self.drone.move_forward()
def rotate_right(self):
self.drone.rotate_clockwise()
def rotate_left(self):
self.drone.rotate_counter_clockwise()
def landing(self):
self.drone.land()
def write_hud(self, frame):
"""Draw drone info, tracking and record on frame"""
stats = self.prev_flight_data.split('|')
stats.append("Tracking:" + str(self.tracking))
if self.drone.zoom:
stats.append("VID")
else:
stats.append("PIC")
if self.record:
diff = int(time.time() - self.start_time)
mins, secs = divmod(diff, 60)
stats.append("REC {:02d}:{:02d}".format(mins, secs))
for idx, stat in enumerate(stats):
text = stat.lstrip()
cv2.putText(frame, text, (0, 30 + (idx * 30)),
cv2.FONT_HERSHEY_SIMPLEX,
1.0, (255, 0, 0), lineType=30)
return frame
def toggle_recording(self, speed):
"""Handle recording keypress, creates output stream and file"""
if speed == 0:
return
self.record = not self.record
if self.record:
datename = [os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)]
self.out_name = '{}/Pictures/tello-{}.mp4'.format(*datename)
print("Outputting video to:", self.out_name)
self.out_file = av.open(self.out_name, 'w')
self.start_time = time.time()
self.out_stream = self.out_file.add_stream(
'mpeg4', self.vid_stream.rate)
self.out_stream.pix_fmt = 'yuv420p'
self.out_stream.width = self.vid_stream.width
self.out_stream.height = self.vid_stream.height
if not self.record:
print("Video saved to ", self.out_name)
self.out_file.close()
self.out_stream = None
def record_vid(self, frame):
"""
convert frames to packets and write to file
"""
new_frame = av.VideoFrame(
width=frame.width, height=frame.height, format=frame.format.name)
for i in range(len(frame.planes)):
new_frame.planes[i].update(frame.planes[i])
pkt = None
try:
pkt = self.out_stream.encode(new_frame)
except IOError as err:
print("encoding failed: {0}".format(err))
if pkt is not None:
try:
self.out_file.mux(pkt)
except IOError:
print('mux failed: ' + str(pkt))
def take_picture(self, speed):
"""Tell drone to take picture, image sent to file handler"""
if speed == 0:
return
self.drone.take_picture()
def palm_land(self, speed):
"""Tell drone to land"""
if speed == 0:
return
self.drone.palm_land()
def toggle_tracking(self, speed):
""" Handle tracking keypress"""
if speed == 0: # handle key up event
return
self.tracking = not self.tracking
print("tracking:", self.tracking)
return
def toggle_zoom(self, speed):
"""
In "video" mode the self.drone sends 1280x720 frames.
In "photo" mode it sends 2592x1936 (952x720) frames.
The video will always be centered in the window.
In photo mode, if we keep the window at 1280x720 that gives us ~160px on
each side for status information, which is ample.
Video mode is harder because then we need to abandon the 16:9 display size
if we want to put the HUD next to the video.
"""
if speed == 0:
return
self.drone.set_video_mode(not self.drone.zoom)
def flight_data_handler(self, event, sender, data):
"""Listener to flight data from the drone."""
text = str(data)
if self.prev_flight_data != text:
self.prev_flight_data = text
def handle_flight_received(self, event, sender, data):
"""Create a file in ~/Pictures/ to receive image from the drone"""
path = '%s/Pictures/tello-%s.jpeg' % (
os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt))
with open(path, 'wb') as out_file:
out_file.write(data)
print('Saved photo to %s' % path)
def enable_mission_pads(self):
self.drone.enable_mission_pads()
def disable_mission_pads(self):
self.drone.disable_mission_pads()
def go_xyz_speed_mid(self, x, y, z, speed, mid):
self.drone.go_xyz_speed_mid(x, y, z, speed, mid)
# if function return True, set drone center to object's center
def track_mid(self, x, y):
midx, midy = 480, 360
distance_x = abs(midx - x)
distance_y = abs(midy - y)
print(x, y, distance_x, distance_y)
move_done = True
if y > midy + self.error + 15:
self.drone.move_down(20)
move_done = False
elif y < midy - self.error + 5:
self.drone.move_up(20)
move_done = False
elif x < midx - self.error:
self.drone.move_left(20)
move_done = False
elif x > midx + self.error:
self.drone.move_right(20)
move_done = False
return move_done
def track_x(self, x, left_count, right_count):
midx = 480
move_done = True
if x < midx - 100:
self.drone.move_left(20)
left_count += 1
move_done = False
elif x > midx + 100:
self.drone.move_right(20)
right_count += 1
move_done = False
return move_done
def go_slow(self):
self.drone.move_forward(30)
def go_fast(self):
self.drone.move_forward(200)
class FrontEnd(object):
def __init__(self):
# 드론과 상호작용하는 Tello 객체
self.tello = Tello()
# 드론의 속도 (-100~100)
#수직, 수평 속도
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# 실행 함수
def run(self):
#드론이 연결이 되지 않으면 함수 종료
if not self.tello.connect():
print("Tello not connected")
return
#drone의 제한속도가 적절하지 않은 경우
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# 프로그램을 비정상적인 방법으로 종료를 시도하여 비디오 화면이 꺼지지 않은 경우 종료.
if not self.tello.streamoff():
print("Could not stop video stream")
return
# 비디오가 켜지지않는 경우 종료.
if not self.tello.streamon():
print("Could not start video stream")
return
#프레임 단위로 인식
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
oSpeed = args.override_speed
tDistance = args.distance
self.tello.get_battery()
# X축 안전 범위
szX = args.saftey_x
# Y축 안전 범위
szY = args.saftey_y
#디버깅 모드
if args.debug:
print("DEBUG MODE ENABLED!")
#비행을 멈취야할 상황이 주어지지 않은 경우
while not should_stop:
self.update()
#프레임 입력이 멈췄을 경우 while문 탈출
if frame_read.stopped:
frame_read.stop()
break
theTime = str(datetime.datetime.now()).replace(':', '-').replace(
'.', '_')
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frameRet = frame_read.frame
vid = self.tello.get_video_capture()
#저장할 경우
if args.save_session:
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount),
frameRet)
frame = np.rot90(frame)
imgCount += 1
time.sleep(1 / FPS)
# 키보드 입력을 기다림
k = cv2.waitKey(20)
# 0을 눌러서 거리를 0으로 설정
if k == ord('0'):
if not OVERRIDE:
print("Distance = 0")
tDistance = 0
# 1을 눌러서 거리를 1으로 설정
if k == ord('1'):
if OVERRIDE:
oSpeed = 1
else:
print("Distance = 1")
tDistance = 1
# 2을 눌러서 거리를 2으로 설정
if k == ord('2'):
if OVERRIDE:
oSpeed = 2
else:
print("Distance = 2")
tDistance = 2
# 3을 눌러서 거리를 3으로 설정
if k == ord('3'):
if OVERRIDE:
oSpeed = 3
else:
print("Distance = 3")
tDistance = 3
# 4을 눌러서 거리를 4으로 설정
if k == ord('4'):
if not OVERRIDE:
print("Distance = 4")
tDistance = 4
# 5을 눌러서 거리를 5으로 설정
if k == ord('5'):
if not OVERRIDE:
print("Distance = 5")
tDistance = 5
# 6을 눌러서 거리를 6으로 설정
if k == ord('6'):
if not OVERRIDE:
print("Distance = 6")
tDistance = 6
# T를 눌러서 이륙
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
# L을 눌러서 착륙
if k == ord('l'):
if not args.debug:
print("Landing")
self.tello.land()
self.send_rc_control = False
# Backspace를 눌러서 명령을 덮어씀
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
if OVERRIDE:
# S & W 눌러서 앞 & 뒤로 비행
if k == ord('w'):
self.for_back_velocity = int(S * oSpeed)
elif k == ord('s'):
self.for_back_velocity = -int(S * oSpeed)
else:
self.for_back_velocity = 0
# a & d 를 눌러서 왼쪽 & 오른쪽으로 회전
if k == ord('d'):
self.yaw_velocity = int(S * oSpeed)
elif k == ord('a'):
self.yaw_velocity = -int(S * oSpeed)
else:
self.yaw_velocity = 0
# Q & E 를 눌러서 위 & 아래로 비행
if k == ord('e'):
self.up_down_velocity = int(S * oSpeed)
elif k == ord('q'):
self.up_down_velocity = -int(S * oSpeed)
else:
self.up_down_velocity = 0
# c & z 를 눌러서 왼쪽 & 오른쪽으로 비행
if k == ord('c'):
self.left_right_velocity = int(S * oSpeed)
elif k == ord('z'):
self.left_right_velocity = -int(S * oSpeed)
else:
self.left_right_velocity = 0
# 프로그램 종료
if k == 27:
should_stop = True
break
gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray,
scaleFactor=1.5,
minNeighbors=2)
# 대상 크기
tSize = faceSizes[tDistance]
# 중심 차원들
cWidth = int(dimensions[0] / 2)
cHeight = int(dimensions[1] / 2)
noFaces = len(faces) == 0
# 컨트롤을 얻고, 얼굴 좌표 등을 얻으면
if self.send_rc_control and not OVERRIDE:
for (x, y, w, h) in faces:
#
roi_gray = gray[y:y + h,
x:x + w] #(ycord_start, ycord_end)
roi_color = frameRet[y:y + h, x:x + w]
# 얼굴 상자 특성 설정
fbCol = (255, 0, 0) #BGR 0-255
fbStroke = 2
# 끝 좌표들은 x와 y를 제한하는 박스의 끝에 존재
end_cord_x = x + w
end_cord_y = y + h
end_size = w * 2
# 목표 좌표들
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# 얼굴에서 화면 중심까지의 벡터를 계산
vTrue = np.array((cWidth, cHeight, tSize))
vTarget = np.array((targ_cord_x, targ_cord_y, end_size))
vDistance = vTrue - vTarget
#
if not args.debug:
# 회전
if vDistance[0] < -szX:
self.yaw_velocity = S
# self.left_right_velocity = S2
elif vDistance[0] > szX:
self.yaw_velocity = -S
# self.left_right_velocity = -S2
else:
self.yaw_velocity = 0
# 위 & 아래 (상승/하강)
if vDistance[1] > szY:
self.up_down_velocity = S
elif vDistance[1] < -szY:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
F = 0
if abs(vDistance[2]) > acc[tDistance]:
F = S
# 앞, 뒤
if vDistance[2] > 0:
self.for_back_velocity = S + F
elif vDistance[2] < 0:
self.for_back_velocity = -S - F
else:
self.for_back_velocity = 0
# 얼굴 테두리 박스를 그림
cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y),
fbCol, fbStroke)
# 목표를 원으로 그림
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10,
(0, 255, 0), 2)
# 안전 구역을 그림
cv2.rectangle(frameRet,
(targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY),
(0, 255, 0), fbStroke)
# 드론의 얼굴 상자로부터의 상대적 벡터 위치를 구함.
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255),
2)
# 인식되는 얼굴이 없으면 아무것도 안함.
if noFaces:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
print("NO TARGET")
# 화면의 중심을 그림. 드론이 목표 좌표와 맞추려는 대상이 됨.
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)),
tDistance + 1 / 7)
if OVERRIDE:
show = "OVERRIDE: {}".format(oSpeed)
dCol = (255, 255, 255)
else:
show = "AI: {}".format(str(tDistance))
# 선택된 거리를 그림
cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# 결과 프레임을 보여줌.
cv2.imshow(f'Tello Tracking...', frameRet)
# 종료시에 배터리를 출력
self.tello.get_battery()
# 전부 완료되면 캡쳐를 해제함.
cv2.destroyAllWindows()
# 종료 전에 항상 호출. 자원들을 해제함.
self.tello.end()
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class FrontEnd(object):
def __init__(self):
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.safe_zone = args.SafeZone
self.oSpeed = args.override_speed
self.send_rc_control = False
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
should_stop = False
imgCount = 0
OVERRIDE = False
self.tello.get_battery()
result = cv2.VideoWriter('Prueba_Tello.avi',
cv2.VideoWriter_fourcc(*'MJPG'), 10,
dimensions)
vid = self.tello.get_video_capture()
if args.debug:
print("DEBUG MODE ENABLED!")
while not should_stop:
#self.update()
#time.sleep(1 / FPS)
# Listen for key presses
k = cv2.waitKey(20)
self.Set_Action(k, OVERRIDE)
if OVERRIDE:
self.Action_OVERRRIDE(k)
# Quit the software
if k == 27:
should_stop = True
break
if vid.isOpened():
ret, image = vid.read()
imgCount += 1
if imgCount % 2 != 0:
continue
if ret:
image = imutils.resize(image,
width=min(350, image.shape[1]))
# Detecting all the regions
# in the Image that has a
# pedestrians inside it
(regions, weigths) = hog.detectMultiScale(image,
winStride=(4, 4),
padding=(4, 4),
scale=1.1)
# Safety Zone Z
szZ = Safe_Zones[self.safe_zone][2]
# Safety Zone X
szX = Safe_Zones[self.safe_zone][0]
# Safety Zone Y
szY = Safe_Zones[self.safe_zone][1]
center = (int(image.shape[1] / 2), int(image.shape[0] / 2))
# if we've given rc controls & get body coords returned
if self.send_rc_control and not OVERRIDE:
if len(regions) > 0:
max_index = np.where(weigths == np.amax(weigths))
(x, y, w, h) = regions[max_index[0][0]]
cv2.rectangle(image, (x, y), (x + w, y + h),
(0, 0, 255), 2)
# points
person = (int(x + w / 2), int(y + h / 2))
distance = (person[0] - center[0],
center[1] - person[1])
theta0 = 86.6
B = image.shape[1]
person_width = w
# z
theta1 = theta0 * (2 * abs(distance[0]) +
person_width) / (2 * B)
z = (2 * abs(distance[0]) + person_width) / (
2 * math.tan(math.radians(abs(theta1))))
distance = (int(distance[0]), int(distance[1]),
int(z))
if not args.debug:
# for turning
self.update()
if distance[0] < -szX:
self.yaw_velocity = S
# self.left_right_velocity = S2
elif distance[0] > szX:
self.yaw_velocity = -S
# self.left_right_velocity = -S2
else:
self.yaw_velocity = 0
# for up & down
if distance[1] > szY:
self.up_down_velocity = S
elif distance[1] < -szY:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
F = 0
if abs(distance[2]) > acc[self.safe_zone]:
F = S
# for forward back
if distance[2] > szZ:
self.for_back_velocity = S + F
elif distance[2] < szZ:
self.for_back_velocity = -S - F
else:
self.for_back_velocity = 0
cv2.line(image, center,
(center[0] + distance[0], center[1]),
(255, 0, 0))
cv2.line(image,
(center[0] + distance[0], center[1]),
person, (0, 255, 0))
cv2.circle(image, center, 3, (0, 255, 0))
cv2.circle(image, person, 3, (0, 0, 255))
cv2.putText(
image,
"d:" + str(distance),
(0, 20),
2,
0.7,
(0, 0, 0),
)
# if there are no body detected, don't do anything
else:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
print("NO TARGET")
dCol = lerp(np.array((0, 0, 255)), np.array(
(255, 255, 255)), self.safe_zone + 1 / 7)
if OVERRIDE:
show = "OVERRIDE: {}".format(self.oSpeed)
dCol = (255, 255, 255)
else:
show = "AI: {}".format(str(self.safe_zone))
cv2.rectangle(
image,
(int(center[0] - szX / 2), int(center[1] - szY / 2)),
(int(center[0] + szX / 2), int(center[1] + szY / 2)),
(255, 0, 0), 1)
# Showing the output Image
image = cv2.resize(image,
dimensions,
interpolation=cv2.INTER_AREA)
# Write the frame into the
# file 'Prueba_Tello.avi'
result.write(image)
# Draw the distance choosen
cv2.putText(image, show, (32, 664),
cv2.FONT_HERSHEY_SIMPLEX, 1, dCol, 2)
# Display the resulting frame
cv2.imshow(f'Tello Tracking...', image)
else:
break
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
cv2.destroyAllWindows()
result.release()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
def Action_OVERRRIDE(self, k):
# S & W to fly forward & back
if k == ord('w'):
self.for_back_velocity = int(S * oSpeed)
elif k == ord('s'):
self.for_back_velocity = -int(S * oSpeed)
else:
self.for_back_velocity = 0
# a & d to pan left & right
if k == ord('d'):
self.yaw_velocity = int(S * self.oSpeed)
elif k == ord('a'):
self.yaw_velocity = -int(S * self.oSpeed)
else:
self.yaw_velocity = 0
# Q & E to fly up & down
if k == ord('e'):
self.up_down_velocity = int(S * self.oSpeed)
elif k == ord('q'):
self.up_down_velocity = -int(S * self.oSpeed)
else:
self.up_down_velocity = 0
# c & z to fly left & right
if k == ord('c'):
self.left_right_velocity = int(S * self.oSpeed)
elif k == ord('z'):
self.left_right_velocity = -int(S * self.oSpeed)
else:
self.left_right_velocity = 0
return
def Set_Action(self, k, OVERRIDE):
# Press 0 to set distance to 0
if k == ord('0'):
if not OVERRIDE:
print("Distance = 0")
self.safe_zone = 0
# Press 1 to set distance to 1
if k == ord('1'):
if OVERRIDE:
self.oSpeed = 1
else:
print("Distance = 1")
self.safe_zone = 1
# Press 2 to set distance to 2
if k == ord('2'):
if OVERRIDE:
self.oSpeed = 2
else:
print("Distance = 2")
self.safe_zone = 2
# Press 3 to set distance to 3
if k == ord('3'):
if OVERRIDE:
self.oSpeed = 3
else:
print("Distance = 3")
self.safe_zone = 3
# Press 4 to set distance to 4
if k == ord('4'):
if not OVERRIDE:
print("Distance = 4")
self.safe_zone = 4
# Press 5 to set distance to 5
if k == ord('5'):
if not OVERRIDE:
print("Distance = 5")
self.safe_zone = 5
# Press 6 to set distance to 6
if k == ord('6'):
if not OVERRIDE:
print("Distance = 6")
self.safe_zone = 6
# Press T to take off
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
# Press L to land
if k == ord('l'):
if not args.debug:
print("Landing")
self.tello.land()
self.send_rc_control = False
# Press Backspace for controls override
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
sys.exit()
if not tello.set_speed(speed):
print("Not set speed to lowest possible")
sys.exit()
# In case streaming is on. This happens when we quit this program without the escape key.
if not tello.streamoff():
print("Could not stop video stream")
sys.exit()
if not tello.streamon():
print("Could not start video stream")
sys.exit()
print("Current battery is " + tello.get_battery())
tello.takeoff()
time.sleep(9)
tello.move_up(82)
time.sleep(2)
frame_read = tello.get_frame_read()
stop = False
face_cascade = cv2.CascadeClassifier("face detector.xml")
while not stop:
frame = frame_read.frame
tello = Tello()
tello.connect()
time.sleep(1)
keepRecording = True
tello.streamon()
frame_read = tello.get_frame_read()
def videoRecorder():
height, width, _ = frame_read.frame.shape
video = cv2.VideoWriter('video.avi', cv2.VideoWriter_fourcc(*'XVID'), 30,
(width, height))
while keepRecording:
video.write(frame_read.frame)
time.sleep(1 / 30)
video.release()
recorder = Thread(target=videoRecorder)
recorder.start()
tello.get_battery()
tello.takeoff()
time.sleep(100)
tello.move_up(100)
tello.rotate_clockwise(360)
tello.land()
keepRecording = False
recorder.join()
class FrontEnd(object):
def __init__(self):
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
oSpeed = args.override_speed
tDistance = args.distance
self.tello.get_battery()
# Safety Zone X
szX = args.saftey_x
# Safety Zone Y
szY = args.saftey_y
if args.debug:
print("DEBUG MODE ENABLED!")
while not should_stop:
self.update()
if frame_read.stopped:
frame_read.stop()
break
theTime = str(datetime.datetime.now()).replace(':', '-').replace(
'.', '_')
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frameRet = frame_read.frame
vid = self.tello.get_video_capture()
if args.save_session:
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount),
frameRet)
frame = np.rot90(frame)
imgCount += 1
time.sleep(1 / FPS)
# Listen for key presses
k = cv2.waitKey(20)
# Press 0 to set distance to 0
if k == ord('0'):
if not OVERRIDE:
print("Distance = 0")
tDistance = 0
# Press 1 to set distance to 1
if k == ord('1'):
if OVERRIDE:
oSpeed = 1
else:
print("Distance = 1")
tDistance = 1
# Press 2 to set distance to 2
if k == ord('2'):
if OVERRIDE:
oSpeed = 2
else:
print("Distance = 2")
tDistance = 2
# Press 3 to set distance to 3
if k == ord('3'):
if OVERRIDE:
oSpeed = 3
else:
print("Distance = 3")
tDistance = 3
# Press 4 to set distance to 4
if k == ord('4'):
if not OVERRIDE:
print("Distance = 4")
tDistance = 4
# Press 5 to set distance to 5
if k == ord('5'):
if not OVERRIDE:
print("Distance = 5")
tDistance = 5
# Press 6 to set distance to 6
if k == ord('6'):
if not OVERRIDE:
print("Distance = 6")
tDistance = 6
# Press T to take off
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
# Press L to land
if k == ord('l'):
if not args.debug:
print("Landing")
self.tello.land()
self.send_rc_control = False
# Press Backspace for controls override
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
if OVERRIDE:
# S & W to fly forward & back
if k == ord('w'):
self.for_back_velocity = int(S * oSpeed)
elif k == ord('s'):
self.for_back_velocity = -int(S * oSpeed)
else:
self.for_back_velocity = 0
# a & d to pan left & right
if k == ord('d'):
self.yaw_velocity = int(S * oSpeed)
elif k == ord('a'):
self.yaw_velocity = -int(S * oSpeed)
else:
self.yaw_velocity = 0
# Q & E to fly up & down
if k == ord('e'):
self.up_down_velocity = int(S * oSpeed)
elif k == ord('q'):
self.up_down_velocity = -int(S * oSpeed)
else:
self.up_down_velocity = 0
# c & z to fly left & right
if k == ord('c'):
self.left_right_velocity = int(S * oSpeed)
elif k == ord('z'):
self.left_right_velocity = -int(S * oSpeed)
else:
self.left_right_velocity = 0
# Quit the software
if k == 27:
should_stop = True
break
gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray,
scaleFactor=1.5,
minNeighbors=2)
# Target size
tSize = faceSizes[tDistance]
# These are our center dimensions
cWidth = int(dimensions[0] / 2)
cHeight = int(dimensions[1] / 2)
noFaces = len(faces) == 0
# if we've given rc controls & get face coords returned
if self.send_rc_control and not OVERRIDE:
for (x, y, w, h) in faces:
#
roi_gray = gray[y:y + h,
x:x + w] #(ycord_start, ycord_end)
roi_color = frameRet[y:y + h, x:x + w]
# setting Face Box properties
fbCol = (255, 0, 0) #BGR 0-255
fbStroke = 2
# end coords are the end of the bounding box x & y
end_cord_x = x + w
end_cord_y = y + h
end_size = w * 2
# these are our target coordinates
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# This calculates the vector from your face to the center of the screen
vTrue = np.array((cWidth, cHeight, tSize))
vTarget = np.array((targ_cord_x, targ_cord_y, end_size))
vDistance = vTrue - vTarget
#
if not args.debug:
# for turning
if vDistance[0] < -szX:
self.yaw_velocity = S
# self.left_right_velocity = S2
elif vDistance[0] > szX:
self.yaw_velocity = -S
# self.left_right_velocity = -S2
else:
self.yaw_velocity = 0
# for up & down
if vDistance[1] > szY:
self.up_down_velocity = S
elif vDistance[1] < -szY:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
F = 0
if abs(vDistance[2]) > acc[tDistance]:
F = S
# for forward back
if vDistance[2] > 0:
self.for_back_velocity = S + F
elif vDistance[2] < 0:
self.for_back_velocity = -S - F
else:
self.for_back_velocity = 0
# Draw the face bounding box
cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y),
fbCol, fbStroke)
# Draw the target as a circle
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10,
(0, 255, 0), 2)
# Draw the safety zone
cv2.rectangle(frameRet,
(targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY),
(0, 255, 0), fbStroke)
# Draw the estimated drone vector position in relation to face bounding box
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255),
2)
# if there are no faces detected, don't do anything
if noFaces:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
print("NO TARGET")
# Draw the center of screen circle, this is what the drone tries to match with the target coords
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)),
tDistance + 1 / 7)
if OVERRIDE:
show = "OVERRIDE: {}".format(oSpeed)
dCol = (255, 255, 255)
else:
show = "AI: {}".format(str(tDistance))
# Draw the distance choosen
cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# Display the resulting frame
cv2.imshow(f'Tello Tracking...', frameRet)
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class DroneObject:
#initialize the object
#Set default state to IdleState()
#Create Tello object from the API
#set default values for coordinate, FPS, distance, and tilt
def __init__(self):
self.state = IdleState()
self.tello = Tello()
self.coordinate = (0, 0)
self.FPS = 30
self.distance = 30 #pre defined, to be changed later
self.tilt = 0
#Generates pass events to tellodrone class
def on_event(self, event):
self.state = self.state.on_event(event)
#setter for member variables
def set_parameter (self, x,y, dist, tilt):
self.coordinate = (x,y)
self.distance = dist
self.tilt = tilt
#for take off the drone, called when the drone is in takeoff state
#when take off is complete, trigger event for track
def take_off(self):
self.tello.takeoff()
for i in range (0,3):
print("taking off %d /3" % (i+1))
time.sleep(1)
self.on_event("track")
#for landing the drone, called when the drone is in landingstate
#when landing is complete, trigger event for idle
def land(self):
self.tello.land()
for i in range (0,3):
print("landing %d / 3" % (i+1))
time.sleep(1)
self.on_event("idle")
#for tracking the drone, called when the drone is in track state
#when tilt is active, it will prioritize the turning motion over the other motions
#controls shifting, moving up and down, forward backwards, and turning
def track(self):
if(self.tilt <= 0.95 and self.tilt != 0):
self.tello.rotate_clockwise(int((1-self.tilt)*100))
time.sleep(0.05)
elif(self.tilt >= 1.05):
self.tello.rotate_counter_clockwise(int((self.tilt-1)*100))
time.sleep(0.05)
else:
if (self.distance > 60):
forward = int((self.distance - 60))
if ((forward < 20)):
self.tello.move_forward(20)
else:
self.tello.move_forward(forward)
time.sleep(0.05)
elif (self.distance < 50):
backward = int(abs(self.distance - 50))
if ((backward < 20)):
self.tello.move_back(20)
else:
self.tello.move_back(backward)
time.sleep(0.05)
if (self.coordinate[0] < 400 and self.coordinate[0] >= 0):
self.tello.move_left(20)
time.sleep(0.05)
elif (self.coordinate[0] < 959 and self.coordinate[0] >= 559):
self.tello.move_right(20)
time.sleep(0.05)
if (self.coordinate[1] > 0 and self.coordinate[1] <= 200):
self.tello.move_up(20)
time.sleep(0.05)
elif (self.coordinate[1] >= 519 and self.coordinate[1] < 719):
self.tello.move_down(20)
time.sleep(0.05)
#For setting up the drone
#Establish connection
#Initialize streamming
#Display battery
def setup(self):
if not self.tello.connect():
print("Tello not connected")
sys.exit()
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
sys.exit()
if not self.tello.streamon():
print("Could not start video stream")
sys.exit()
print("Current battery is " + self.tello.get_battery())
self.tello.streamon()
frame_read = self.tello.get_frame_read()
#For calling corresponding functions based on their state
def action(self):
print("current state is" , self.state)
print(str(self.state))
if (str(self.state) == "TakeOffState"):
print("take off!")
self.take_off()
elif (str(self.state) == "LandState"):
print("land")
self.land()
if (str(self.state)== "TrackState"):
self.track()
else:
return #idle state or undefined state do nothing
return
class Drone:
def __init__(self, speed, forward_backward_speed, steering_speed, up_down_speed, save_session, save_path, distance,
safety_x, safety_y, safety_z, face_recognition, face_path):
# Initialize Drone
self.tello = Tello()
# Initialize Person Detector
self.detector = detectFaces()
self.face_recognition = face_recognition
self.face_encoding = self.detector.getFaceEncoding(cv2.imread(face_path))
self.speed = speed
self.fb_speed = forward_backward_speed
self.steering_speed = steering_speed
self.ud_speed = up_down_speed
self.save_session = save_session
self.save_path = save_path
self.distance = distance
self.safety_x = safety_x
self.safety_y = safety_y
self.safety_z = safety_z
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.flight_mode = False
self.send_rc_control = False
self.dimensions = (960, 720)
if self.save_session:
os.makedirs(self.save_path, exist_ok=True)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
elif not self.tello.set_speed(self.speed):
print('Not set speed to lowest possible')
return
elif not self.tello.streamoff():
print("Could not stop video stream")
return
elif not self.tello.streamon():
print("Could not start video stream")
return
self.tello.get_battery()
frame_read = self.tello.get_frame_read()
count = 0
while True:
if frame_read.stopped:
frame_read.stop()
break
# Listen for key presses
k = cv2.waitKey(20)
if k == 8:
self.flight_mode = not self.flight_mode
elif k == 27:
break
elif k == ord('t'):
self.tello.takeoff()
self.send_rc_control = True
elif k == ord('l'):
self.tello.land()
self.send_rc_control = False
# read frame
frameBGR = frame_read.frame
if self.save_path:
cv2.imwrite(f'{self.save_path}/{count}.jpg', frameBGR)
count += 1
x_min, y_min, x_max, y_max = 0, 0, 0, 0
# detect person and get coordinates
if self.face_recognition:
boundingBoxes = self.detector.detectMultiple(frameBGR)
for x_mi, y_mi, x_ma, y_ma in boundingBoxes:
image = frameBGR[y_mi:y_ma, x_mi:x_ma]
if image.shape[0] != 0 and image.shape[1] != 0:
face_encoding = self.detector.getFaceEncoding(image)
dist = np.linalg.norm(self.face_encoding - face_encoding)
if dist < 0.95:
x_min, y_min, x_max, y_max = x_mi, y_mi, x_ma, y_ma
cv2.rectangle(frameBGR, (x_mi, y_mi), (x_ma, y_ma), (0, 255, 0), 2)
else:
cv2.rectangle(frameBGR, (x_mi, y_mi), (x_ma, y_ma), (255, 0, 0), 2)
else:
x_min, y_min, x_max, y_max = self.detector.detectSingle(frameBGR)
cv2.rectangle(frameBGR, (x_min, y_min), (x_max, y_max), (255, 0, 0), 2)
if not self.flight_mode and self.send_rc_control and x_max != 0 and y_max != 0:
# these are our target coordinates
targ_cord_x = int((x_min + x_max) / 2)
targ_cord_y = int((y_min + y_max) / 2)
# This calculates the vector from your face to the center of the screen
vTrue = np.array((int(self.dimensions[0]/2), int(self.dimensions[1]/2), sizes[self.distance]))
vTarget = np.array((targ_cord_x, targ_cord_y, (x_max-x_min)*2))
vDistance = vTrue - vTarget
# turn drone
if vDistance[0] < -self.safety_x:
self.yaw_velocity = self.steering_speed
elif vDistance[0] > self.safety_x:
self.yaw_velocity = -self.steering_speed
else:
self.yaw_velocity = 0
# for up & down
if vDistance[1] > self.safety_y:
self.up_down_velocity = self.ud_speed
elif vDistance[1] < -self.safety_y:
self.up_down_velocity = -self.ud_speed
else:
self.up_down_velocity = 0
# forward & backward
if vDistance[2] > self.safety_z:
self.for_back_velocity = self.fb_speed
elif vDistance[2] < self.safety_z:
self.for_back_velocity = -self.fb_speed
else:
self.for_back_velocity = 0
# always set left_right_velocity to 0
self.left_right_velocity = 0
# Draw the target as a circle
cv2.circle(frameBGR, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2)
# Draw the safety zone
cv2.rectangle(frameBGR, (targ_cord_x - self.safety_x, targ_cord_y - self.safety_y), (targ_cord_x + self.safety_x, targ_cord_y + self.safety_y),
(0, 255, 0), 2)
elif not self.flight_mode and self.send_rc_control and x_max==0 and y_max==0:
self.for_back_velocity = 0
self.left_right_velocity = 0
self.yaw_velocity = 0
self.up_down_velocity = 0
elif self.flight_mode and self.send_rc_control:
# fligh forward and back
if k == ord('w'):
self.for_back_velocity = self.speed
elif k == ord('s'):
self.for_back_velocity = -self.speed
else:
self.for_back_velocity = 0
# fligh left & right
if k == ord('d'):
self.left_right_velocity = self.speed
elif k == ord('a'):
self.left_right_velocity = -self.speed
else:
self.left_right_velocity = 0
# fligh up & down
if k == 38:
self.up_down_velocity = self.speed
elif k == 40:
self.up_down_velocity = -self.speed
else:
self.up_down_velocity = 0
# turn
if k == 37:
self.yaw_velocity = self.speed
elif k == 39:
self.yaw_velocity = -self.speed
else:
self.yaw_velocity = 0
if self.send_rc_control:
# Send velocities to Drone
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity,
self.yaw_velocity)
cv2.imshow('Tello Drone', frameBGR)
# Destroy cv2 windows and end drone connection
cv2.destroyAllWindows()
self.tello.end()
width = 640 height = 480 deadZone = 100 startCounter = 0 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() frameWidth = width frameHeight = height # cap = cv2.VideoCapture(1) # cap.set(3, frameWidth) # cap.set(4, frameHeight) # cap.set(10,200) global imgContour global dir
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
self.faceCascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_alt2.xml')
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.show_stats = False
self.send_rc_control = False
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
frameRet = frame_read.frame
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.update()
elif event.type == QUIT:
should_stop = True
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == KEYUP:
self.keyup(event.key)
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY)
faces = self.faceCascade.detectMultiScale(gray, scaleFactor=1.5, minNeighbors=2)
for (x, y, w, h) in faces:
fbCol = (255, 0, 0) #BGR 0-255
fbStroke = 2
end_cord_x = x + w
end_cord_y = y + h
cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y), fbCol, fbStroke)
if self.show_stats:
cv2.putText(frameRet, "Batt: " + str(self.tello.get_battery()),(0,32),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2)
cv2.putText(frameRet, "Wifi: " + str(self.tello.get_wifi()),(0,64),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2)
cv2.putText(frameRet, "Faces: " + str(len(faces)),(0,96),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,255),2)
frameRet = cv2.cvtColor(frameRet,cv2.COLOR_BGR2RGB)
frameRet = np.rot90(frameRet)
frameRet = np.flipud(frameRet)
frame = pygame.surfarray.make_surface(frameRet)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw counter clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
elif key == pygame.K_h: # stats
self.show_stats = not self.show_stats
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity,
self.yaw_velocity)
###############################Parameters###################################
fSpeed = 117 / 10 #foreward speed in cm/s 117 cm in 10 secs (15cm/s)
aSpeed = 360 / 10 #angular speed in cm/s 360 degs in 10 secs (50cm/s)
interval = 0.25
dInterval = fSpeed * interval #gives distance everytime we move 1 unit
aInterval = aSpeed * interval #Gives angle every time we move 1 unit
############Dont Change Unless You Kinda Know What You're Doing.############
x, y = 500, 500
angle = 0 #angle
yaw = 0
km.init()
drone = Tello()
drone.connect()
print(drone.get_battery())
points = [(0, 0), (0, 0)]
#getting keyinp using the provided module
def getKeyInput():
leftRight, foreBack, upNDown, yawVel = 0, 0, 0, 0
speed = 15
aSpeed = 50
global x, y, angle, yaw
distance = 0 #distance
#Controls with WASD;
if km.getKey("w"):
foreBack = speed #Foreward
class DroneControl(object):
def __init__(self):
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
# Autonomous mode: Navigate autonomously
self.autonomous = True
# Enroll mode: Try to find new faces
self.enroll_mode = False
# Create arrays of known face encodings and their names
self.known_face_encodings = []
self.known_face_names = []
# Logic used for navigation
self.face_locations = None
self.face_encodings = None
self.target_name = ""
self.has_face = False
self.detect_faces = True
self.wait = 0
self.load_all_faces()
# Video frame for Streaming
self.frame_available = None
if not self.tello.connect():
print("Tello not connected")
raise Exception("Tello not connected")
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
raise Exception("Not set speed to lowest possible")
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
raise Exception("Could not stop video stream")
if not self.tello.streamon():
print("Could not start video stream")
raise Exception("Could not start video stream")
self.loop()
def loop(self):
self.update_rc_control()
if self.tello.get_frame_read().stopped:
self.tello.get_frame_read().stop()
self.shutdown()
video_frame = self.tello.get_frame_read().frame
# Resize the frame
capture_divider = 0.5
recognition_frame = cv2.resize(video_frame, (0, 0), fx=capture_divider, fy=capture_divider) #BGR is used, not RGB
# Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
# recognition_frame = bgr_recognition_frame[:, :, ::-1]
# Copy of the frame for capturing faces
if self.enroll_mode:
capture_frame = video_frame.copy()
# Only detect faces every other frame
if self.detect_faces:
self.face_locations = face_recognition.face_locations(recognition_frame)
self.face_encodings = face_recognition.face_encodings(recognition_frame, self.face_locations)
self.detect_faces = False
else:
self.detect_faces = True
# Navigate Autonomously
if self.autonomous:
# Loop through detected faces
for (top, right, bottom, left), name in self.identify_faces(self.face_locations, self.face_encodings):
# Scale back up face locations since the frame we detected in was scaled to 1/4 size
top = int(top * 1/capture_divider)
right = int(right * 1/capture_divider)
bottom = int(bottom * 1/capture_divider)
left = int(left * 1/capture_divider)
x = left
y = top
w = right - left
h = bottom - top
# Draw a box around the face
cv2.rectangle(video_frame, (left, top), (right, bottom), (0, 0, 255), 2)
if name is not unknown_face_name:
# Draw a label with a name below the face
font = cv2.FONT_HERSHEY_DUPLEX
cv2.rectangle(video_frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED)
cv2.putText(video_frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1)
print(self.target_name + '-' + name)
if self.enroll_mode:
if name is unknown_face_name:
target_reached = self.approach_target(video_frame, top,right, bottom, left)
if target_reached:
newUUID = uuid.uuid4()
newFacePath = "new_faces/{}.png".format(newUUID)
roi = capture_frame[y:y+h, x:x+w]
cv2.imwrite(newFacePath, roi)
if self.load_face(newFacePath, str(newUUID)):
shutil.copy2(newFacePath, "known_faces/{}.png".format(newUUID))
break
elif (self.target_name and name == self.target_name) or ((not self.target_name) and name != unknown_face_name):
target_reached = self.approach_target(video_frame, top,right, bottom, left)
break
# No Faces / Face lost
if len(self.face_encodings) == 0:
# Wait for a bit if the stream has collapsed
if self.wait >= detection_wait_interval:
self.wait = 0
if self.has_face:
# Go back and down to try to find the face again
self.has_face = False
self.up_down_velocity = -30
self.for_back_velocity = -20
else:
# Turn to find any face
self.yaw_velocity = 25
self.up_down_velocity = 0
self.for_back_velocity = 0
else:
self.wait += 1
# Show video stream
self.frame_available = video_frame
#cv2.imshow("Tello Drone Delivery", video_frame)
def shutdown(self):
# On exit, print the battery
print(self.get_battery())
# When everything done, release the capture
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def update_rc_control(self):
""" Update routine. Send velocities to Tello."""
self.tello.send_rc_control(
self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
def get_battery(self):
""" Get Tello battery state """
battery = self.tello.get_battery()
if battery:
return battery[:2]
else:
return False
def load_face(self, file, name):
""" Load and enroll a face from the File System """
face_img = face_recognition.load_image_file(file)
face_encodings = face_recognition.face_encodings(face_img)
if len(face_encodings) > 0:
self.known_face_encodings.append(face_encodings[0])
self.known_face_names.append(name)
return True
return False
def load_all_faces(self):
""" Load and enroll all faces from the known_faces folder, then clear out the new_faces folder """
for face in os.listdir("known_faces/"):
print(face[:-4])
self.load_face("known_faces/" + face, face[:-4])
for file in os.listdir("new_faces/"):
os.remove("new_faces/" + file)
def identify_faces(self, face_locations, face_encodings):
""" Identify known faces from face encodings """
face_names = []
for face_encoding in face_encodings:
# See if the face is a match for the known face(s)
matches = face_recognition.compare_faces(self.known_face_encodings, face_encoding)
name = unknown_face_name
# Use the known face with the smallest distance to the new face
face_distances = face_recognition.face_distance(self.known_face_encodings, face_encoding)
best_match_index = np.argmin(face_distances)
if matches[best_match_index]:
name = self.known_face_names[best_match_index]
face_names.append(name)
return zip(face_locations, face_names)
def approach_target(self, video_frame, top, right, bottom, left):
""" The main navigation algorithm """
x = left
y = top
w = right - left
h = bottom - top
self.has_face = True
# The Center point of our Target
target_point_x = int(left + (w/2))
target_point_y = int(top + (h/2))
# Draw the target point
cv2.circle(video_frame,
(target_point_x, target_point_y),
10, (0, 255, 0), 2)
# The Center Point of the drone's view
heading_point_x = int(dimensions[0] / 2)
heading_point_y = int(dimensions[1] / 2)
# Draw the heading point
cv2.circle(
video_frame,
(heading_point_x, heading_point_y),
5, (0, 0, 255), 2)
target_reached = heading_point_x >= (target_point_x - tolerance_x) \
and heading_point_x <= (target_point_x + tolerance_x) \
and heading_point_y >= (target_point_y - tolerance_y) \
and heading_point_y <= (target_point_y + tolerance_y)
# Draw the target zone
cv2.rectangle(
video_frame,
(target_point_x - tolerance_x, target_point_y - tolerance_y),
(target_point_x + tolerance_x, target_point_y + tolerance_y),
(0, 255, 0) if target_reached else (0, 255, 255), 2)
close_enough = (right-left) > depth_box_size * 2 - depth_tolerance \
and (right-left) < depth_box_size * 2 + depth_tolerance \
and (bottom-top) > depth_box_size * 2 - depth_tolerance \
and (bottom-top) < depth_box_size * 2 + depth_tolerance
# Draw the target zone
cv2.rectangle(
video_frame,
(target_point_x - depth_box_size, target_point_y - depth_box_size),
(target_point_x + depth_box_size, target_point_y + depth_box_size),
(0, 255, 0) if close_enough else (255, 0, 0), 2)
if not target_reached:
target_offset_x = target_point_x - heading_point_x
target_offset_y = target_point_y - heading_point_y
self.yaw_velocity = round(v_yaw_pitch * map_values(target_offset_x, -dimensions[0], dimensions[0], -1, 1))
self.up_down_velocity = -round(v_yaw_pitch * map_values(target_offset_y, -dimensions[1], dimensions[1], -1, 1))
print("YAW SPEED {} UD SPEED {}".format(self.yaw_velocity, self.up_down_velocity))
if not close_enough:
depth_offset = (right - left) - depth_box_size * 2
if (right - left) > depth_box_size * 1.5 and not target_reached:
self.for_back_velocity = 0
else:
self.for_back_velocity = -round(v_for_back * map_values(depth_offset, -depth_box_size, depth_box_size, -1, 1))
else:
self.for_back_velocity = 0
return target_reached and close_enough
def take_off(self):
self.tello.takeoff()
def land(self):
self.tello.land()
def set_autonomous(self, autonomous): self.autonomous = autonomous
def set_enroll_mode(self, enroll_mode): self.enroll_mode = enroll_mode
def set_target_name(self, target_name): self.target_name = target_name
def set_for_back_velocity(self, for_back_velocity): self.for_back_velocity = for_back_velocity
def set_left_right_velocity(self, left_right_velocity): self.left_right_velocity = left_right_velocity
def set_up_down_velocity(self, up_down_velocity): self.up_down_velocity = up_down_velocity
def set_yaw_velocity(self, yaw_velocity): self.yaw_velocity = yaw_velocity
def get_video_frame(self):
video_frame = self.frame_available
self.frame_available = drone
return video_frame