def main():
global manual_command, stop
thread_fly = None
tello = Tello()
# Remove most of the log lines
logger = logging.getLogger('djitellopy')
logger.setLevel(logging.CRITICAL)
tello.connect()
try:
tello.streamon()
thread_fly = Thread(target=fly, args=[tello])
thread_fly.start()
while not stop:
manual_command = input(
"Pulsa una tecla de dirección o escribe un comando ('despegar','aterrizar','giro horario', 'giro antihorario', 'salir'): "
)
except:
error = sys.exc_info()[0]
print("Unexpected error:", error)
finally:
stop = True
if not thread_fly is None:
thread_fly.join()
tello.land()
tello.streamoff()
tello.end()
def run(CommandsQueue):
move_distance = 30
idle_sleep = 2 #sec
video_ready_sleep = 7 #sec
is_airborn = False
global is_active
tello = Tello()
tello.connect()
tello.set_speed(10)
droneVid = threading.Thread(target=drone_video, args=(tello, ))
droneVid.start()
time.sleep(video_ready_sleep)
while is_active:
#execute next command from the que
if not CommandsQueue.empty():
command = CommandsQueue.get()
timeStamp = str(datetime.datetime.now())
print('Command for Drone: ' + str(command[0]) + ' at time ' +
timeStamp)
if command[0] == Commands.up:
if is_airborn:
tello.move_up(move_distance)
else:
tello.takeoff()
is_airborn = True
# elif command[0] == Commands.idle:
# time.sleep(idle_sleep)
elif command[0] == Commands.up and is_airborn == True:
tello.move_up(move_distance)
elif command[0] == Commands.down and is_airborn == True:
tello.move_down(move_distance)
elif command[0] == Commands.forward and is_airborn == True:
tello.move_forward(move_distance)
elif command[0] == Commands.back and is_airborn == True:
tello.move_back(move_distance)
elif command[0] == Commands.left and is_airborn == True:
tello.move_left(move_distance)
elif command[0] == Commands.right and is_airborn == True:
tello.move_right(move_distance)
elif command[0] == Commands.flip and is_airborn == True:
tello.flip_back()
elif command[0] == Commands.stop:
is_active = False
break
# else:
# time.sleep(idle_sleep)
tello.land()
is_airborn = False
droneVid.join()
tello.end()
def main():
tello = Tello()
tello.connect()
tello.streamoff()
tello.streamon()
frontend = FrontEnd(tello)
right = 0
while(right<2):
right = frontend.run(right)
print("right is now hsdawqbvqwuja : {}".format(right))
frontend.clear()
print("found the end of the code")
tello.land()
tello.end()
class FrontEnd(object):
def __init__(self, window_width, window_height):
# Init pygame
pygame.init()
# Creat pygame window
self.screen = pygame.display.set_mode([window_width,
window_height]) # [960, 720]
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# create update timer
#pygame.time.set_timer(pygame.USEREVENT + 1, 50)
def run(self):
pygame.display.set_caption("Starting frame reading...")
frame_read = self.tello.get_frame_read()
pygame.display.set_caption("Receiving Tello video stream")
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.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()
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def main():
tello = Tello()
tello.connect()
print(f"battery level: {tello.get_battery()}")
print(f"temp: {tello.get_temperature()}")
tello.takeoff()
tello.move_left(50)
tello.move_right(50)
tello.rotate_clockwise(360)
print(f"Waiting for 1 secs")
time.sleep(1)
tello.flip_left()
tello.land()
tello.end()
class missions(object):
def __init__(self):
pygame.init()
pygame.display.set_caption("Tello Feed")
self.screen = pygame.display.set_mode([960, 720])
self.tello = Tello()
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
eye_cascade = cv2.CascadeClassifier('haarcascade_eye.xml')
face_cascade = cv2.CascadeClassifier(
'haarcascade_frontalface_default.xml')
while True:
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
for (x, y, w, h) in faces:
cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
roi_gray = gray[y:y + h, x:x + w]
roi_color = frame[y:y + h, x:x + w]
eyes = eye_cascade.detectMultiScale(roi_gray)
for (ex, ey, ew, eh) in eyes:
cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh),
(0, 255, 0), 2)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / 60)
self.tello.end()
def main():
print("Configuring tello Drone")
tello = Tello()
if not tello.connect():
print("Tello not connected")
return
if not tello.streamon():
print("Could not start video stream")
return
while (True):
frame = tello.get_frame_read()
cv2.imshow("DetectionResults", frame.frame)
raw_key = cv2.waitKey(1)
if raw_key == 27: #esc
break
cv2.destroyAllWindows()
tello.end()
def demo():
# tello.send_rc_control(left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity)
input("Press enter to go!")
tello = Tello()
tello.connect()
time.sleep(1)
tello.takeoff()
time.sleep(1)
# tello.move_up(100)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
# tello.flip_left()
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.move_forward(100)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.move_right(100)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.rotate_clockwise(180)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.move_right(100)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.move_forward(100)
time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.rotate_clockwise(180)
time.sleep(1)
# tello.flip_right()
# time.sleep(1)
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.land()
print(f"Tello: height {tello.get_height()} battery {tello.get_battery()} temperature {tello.get_temperature()}")
tello.end()
print("Power off! :-)")
def demo():
# tello.send_rc_control(left_right_velocity, for_back_velocity, up_down_velocity, yaw_velocity)
tello = Tello()
tello.connect()
time.sleep(1)
print("Take off...")
tello.takeoff()
input()
do(tello, forward=50)
input()
do(tello, forward=-0)
input()
do(tello, forward=-50)
input()
do(tello, forward=-0)
print("Landing...")
tello.land()
tello.end()
print(
f"Tello: battery {tello.get_battery()} temperature {tello.get_temperature()}"
)
print("Power off! :-)")
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.width = 640
self.height = 480
self.screen = pygame.display.set_mode([self.width, self.height])
# create queue for data communications
self.data_queue=queue.Queue()
# Init Tello object that interacts with the Tello drone
self.tello = Tello(self.data_queue)
# 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
# Variables for drone's states
self.battery = 0
self.angles = [0., 0., 0., 0.]
# Direction queue for navigation
self.dir_queue=queue.Queue()
self.dir_queue.queue.clear()
# Bool variables for setting functions
self.send_rc_control = False
self.calibrate = False
self.getPoints = False
self.resetPoints = False
self.save = False
self.getOrigin = False
# Creating video queue
self.video_queue = queue.Queue()
self.video_queue.queue.clear()
self.END_event = threading.Event()
self.END_event.clear()
self.videoWrite = WriteVideo(self.video_queue, FPS, self.END_event)
# Run video writer in the background
thread_vid = threading.Thread(target=self.videoWrite.writer)
thread_vid.daemon = True
thread_vid.start()
# Data collection event
self.getCoords_event = threading.Event()
self.getCoords_event.clear()
# Navigate between markers
self.navigate_event = threading.Event()
self.navigate_event.clear()
# Camera class
self.cam = Camera(S_prog, self.dir_queue, 'camcalib.npz',
self.getCoords_event, self.navigate_event, self.END_event)
# 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()
directions = np.zeros(4)
should_stop = False
while not should_stop:
img=cv2.resize(frame_read.frame, (960,720))
# Read from drone state queue
if not self.data_queue.empty():
pitch, roll, yaw, tof, bat = self.data_queue.get()
self.data_queue.queue.clear()
self.battery = bat
self.angles_tof = [pitch, roll, yaw, tof]
#print([pitch, roll, yaw, tof])
# Calibrate drone camera
if self.calibrate:
img = self.cam.calibrator(img)
# Detect ArUco markers
img = self.cam.aruco(img, self.getPoints, self.resetPoints, self.angles_tof)
# Reset measurements
if self.resetPoints:
self.resetPoints=False
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.update(directions)
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
# Save image on 'M' press
if self.save:
timestr = time.strftime("%Y%m%d_%H%M%S")
cv2.imwrite("images/"+timestr+".jpg", img)
self.save = False
# Navigation started, points and video capture
if self.getCoords_event.is_set():
self.video_queue.put(np.copy(img))
# Write battery percentage
img = self.cam.writeBattery(img, self.battery)
img=cv2.resize(img, (640,480))
# Resize pyGame window
if (img.shape[1] != self.width) or (img.shape[0] != self.height):
self.width = img.shape[1]
self.height = img.shape[0]
self.screen=pygame.display.set_mode((self.width, self.height))
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(img, 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()
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_k: # camera calibration
if self.calibrate:
self.calibrate = False
else:
self.calibrate = True
elif key == pygame.K_c: # get aruco marker points
if self.getPoints:
self.getPoints=False
else:
self.getPoints = True
self.resetPoints = True
elif key == pygame.K_m: # save image
self.save = True
elif key == pygame.K_o: # start navigation
if self.navigate_event.is_set():
self.navigate_event.clear()
else:
self.navigate_event.set()
elif key == pygame.K_x: # end video
self.END_event.set()
self.getPoints = False
def update(self, dirs):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
if self.navigate_event.is_set() and not self.dir_queue.empty():
# Auto navigation, read directions queue
x, y, z, yaw = self.dir_queue.get()
self.tello.send_rc_control(int(x), int(y), int(z), int(yaw))
else:
# Clear directions queue to avoid storing old data
self.dir_queue.queue.clear()
# Control tello manually
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity,
self.yaw_velocity)
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()
# 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.imgCount = 0
self.send_rc_control = False
self.c = 0
self.g = 0
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
self.set_var = 0
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:
for event in pygame.event.get():
#if event.type == USEREVENT + 1:
#pass
#self.update()
if 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])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
#frame = cv2.cvtColor(frame_read.frame)
img_c = 0
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, self.imgCount), frame)
cv2.imwrite("{}/tellocap{}.jpg".format(ddir1, img_c), frame)
self.imgCount = self.imgCount + 1
x_axis = 0
y_axis = 0
z_axis = 0
self.g = 0
command_arr = []
print("yooooo start")
try:
with open(
r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv"
) as f:
rows = csv.reader(f)
for command in rows:
command_arr.append(command)
print(command_arr)
except:
print("1st error")
try:
if (int(command_arr[2][0]) == 2):
print("land")
self.tello.land()
self.set_var = 0
self.g = 1
elif (int(command_arr[1][0]) == 1):
print("takeoff")
self.tello.takeoff()
self.set_var = 1
self.g = 1
with open(
r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv",
'w') as fl:
writer = csv.writer(fl, delimiter=',')
fl.truncate(0)
fl.close()
except:
print("error speech")
if (self.g == 0):
if (self.set_var == 1):
try:
with open(r"C:\Users\hp\Desktop\\ginnovators.csv",
'r') as csvfile:
# creating a csv reader object
csvreader = csv.reader(csvfile)
# extracting field names through first row
for row in csvreader:
values = row
print(values)
x_axis = int(values[0])
y_axis = int(values[1])
z_axis = int(values[2])
except:
print("csv error")
try:
if (-50 < x_axis < 50):
if (-38 < y_axis < 38):
if (-7 < z_axis < 18):
self.tello.send_rc_control(0, 0, 0, 0)
else:
if (z_axis > 18):
self.for_back_velocity = -30
self.tello.send_rc_control(
0, self.for_back_velocity, 0, 0)
elif (z_axis < -7):
self.for_back_velocity = 30
self.tello.send_rc_control(
0, self.for_back_velocity, 0, 0)
else:
if (y_axis > 38):
self.up_down_velocity = 30
self.tello.send_rc_control(
0, 0, self.up_down_velocity, 0)
elif (y_axis < -38):
self.up_down_velocity = -30
self.tello.send_rc_control(
0, 0, self.up_down_velocity, 0)
else:
if (x_axis > 50):
self.yaw_velocity = -20
self.tello.send_rc_control(
0, 0, 0, self.yaw_velocity)
elif (x_axis < -50):
self.yaw_velocity = 20
self.tello.send_rc_control(
0, 0, 0, self.yaw_velocity)
except:
print("error")
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / 25)
# 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.set_var = 1
self.send_rc_control = True
elif key == pygame.K_l: # land
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 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")
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 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()
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 getTakeOff(self):
return self.hasTakenOff
def setTakeoff(self, hasTakenOff):
self.hasTakenOff = hasTakenOff
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
###################################################################
##Drone Project Defined variables
self.hasTakenOff = False
self.specifiedTarget = known_face_names[0]
self.targetSeen = False
self.targetLeftSide = 320 # set to default
self.targetRightSide = 640
###################################################################
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
detector = dlib.get_frontal_face_detector()
spinCounter = 0
while not should_stop:
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])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
#dets = detector(frame)
#for det in dets:
# cv2.rectangle(frame, (det.left(), det.top()), (det.right(), det.bottom()), color=(0,255,0), thickness=3)
# Resize frame of video to 1/4 size for faster face recognition processing
small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
# Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
rgb_small_frame = small_frame[:, :, ::-1]
# Only process every other frame of video to save time
face_locations = face_recognition.face_locations(rgb_small_frame)
face_encodings = face_recognition.face_encodings(
rgb_small_frame, face_locations)
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(
known_face_encodings, face_encoding)
name = "Unknown"
# If a match was found in known_face_encodings, just use the first one.
if True in matches:
first_match_index = matches.index(True)
name = known_face_names[
first_match_index] # This might not actually be correct, given it only uses the first one
face_names.append(name)
for (top, right, bottom,
left), name in zip(face_locations, face_names):
# Scale back up face locations since the frame we detected in was scaled to 1/4 size
top *= 4
right *= 4
bottom *= 4
left *= 4
# Draw a box around the face
cv2.rectangle(frame, (left, top), (right, bottom),
(0, 0, 255), 2)
# Draw a label with a name below the face
cv2.rectangle(frame, (left, bottom - 35), (right, bottom),
(0, 0, 255), cv2.FILLED)
font = cv2.FONT_HERSHEY_DUPLEX
cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0,
(255, 255, 255), 1)
# Updates when the target is seen
if (name == self.specifiedTarget):
self.targetSeen = True
self.targetLeftSide = left
self.targetRightSide = right
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
# Circles right until face detected
#while (targetSeen != True and self.hasTakenOff):
#self.tello.rotate_clockwise(self, 10)
#self.keydown(self, pygame.K_a)
if (self.targetSeen != True and self.hasTakenOff):
self.yaw_velocity = turnSpeed
spinCounter = spinCounter + 1
if (spinCounter > (5 * FPS)):
self.tello.land()
self.send_rc_control = False
self.tello.end()
#break ## ends loop and land drone
else:
if (self.targetLeftSide < 320):
#self.tello.rotate_clockwise(self, 10)
# self.keydown(self, pygame.K_a)
self.yaw_velocity = turnSpeed
# rotate left, target outside of the middle 1/3
elif (self.targetRightSide > 640):
#self.tello.rotate_counter_clockwise(self, 10)
# self.keydown(self, pygame.K_d)
self.yaw_velocity = -turnSpeed
else:
self.yaw_velocity = 0
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.hasTakenOff = True
self.send_rc_control = True
elif key == pygame.K_l: # land
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 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 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.
- B: Go to Ground
- K: Emergency Land
- Q: Emergency Motor Kill
- F: Face Follow mode
"""
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.mode = None
self.send_rc_control = False
self.yolo = Yolo()
self.yolo.initializeModel()
self.tracker = tracker = cv2.TrackerCSRT().create()
self.locked = False
self.locked_frame = None
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
logger.info("Game Initialized")
def run(self):
if not self.tello.connect():
print("Tello not connected")
logger.error("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
logger.error("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")
logger.error("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
logger.error("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == USEREVENT + 1:
if self.mode != None:
frame_read.frame = self.get_update(frame_read.frame)
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])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
cv2.putText(
img=frame,
text="Height : {}".format(self.tello.get_tello_status().h),
org=(0, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.15 * 5,
color=(255, 255, 255),
)
cv2.putText(
img=frame,
text="Battery : {}".format(self.tello.get_tello_status().bat),
org=(0, 70),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.15 * 5,
color=(255, 255, 255),
)
cv2.putText(
img=frame,
text="Temp : {} - {}".format(
self.tello.get_tello_status().temph,
self.tello.get_tello_status().templ,
),
org=(0, 90),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.15 * 5,
color=(255, 255, 255),
)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
time.sleep((1 / FPS))
pygame.display.update()
self.tello.get_tello_status()
# 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
elif key == pygame.K_f:
if self.mode == "Aquire lock" or self.mode == "Follow":
logger.info("Back to normal mode")
self.mode = None
self.locked = False
self.locked_frame = None
else:
logger.info("Aquiring lock")
self.mode = "Aquire lock"
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()
logger.info("Take off")
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
logger.info("Land")
self.send_rc_control = False
elif key == pygame.K_b: # go to ground
self.tello.go_to_ground()
logger.info("Got to Ground")
self.send_rc_control = False
elif key == pygame.K_k: # Manual land and kill motors
self.tello.emergency_land()
logger.info("Emmergency land")
self.send_rc_control = False
# elif key ==pygame.K_q:
# self.tello.emergency()
# logger.info("Kill motors")
# 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,
)
def get_update(self, frame_read):
if self.mode == "Aquire lock":
return self.aquire_lock(frame_read)
if self.mode == "Follow":
return self.follow(frame_read)
def face_follow(self, frame_read):
cv2.cvtColor(frame_read, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(frame_read, 1.3, 5)
frame_x = frame_read.shape[1]
frame_y = frame_read.shape[0]
face_center_x = 0
for (x, y, w, h) in faces:
cv2.rectangle(frame_read, (x, y), (x + w, y + h), (255, 0, 0), 2)
face_center_x = x + (w / 2) - frame_x
break
if face_center_x > 200:
self.yaw_velocity = -S
if face_center_x < -200:
self.yaw_velocity = S
else:
self.yaw_velocity = 0
logger.info("Frame_x: {} \tface_center: {} \tyaw_vel: {}".format(
frame_x, face_center_x, self.yaw_velocity))
return frame_read
def aquire_lock(self, frame):
bbox, _, _ = self.yolo.detect(frame, "person")
if len(bbox) > 0:
self.locked = True
self.locked_frame = [int(i) for i in bbox[0]]
logger.info("Lock Aquired : {}".format(self.locked_frame))
self.mode = "Follow"
self.tracker.init(
frame,
(
self.locked_frame[0],
self.locked_frame[1],
self.locked_frame[0] + self.locked_frame[2],
self.locked_frame[1] + self.locked_frame[3],
),
)
return self.follow(frame)
return frame
def follow(self, frame_read):
ok, self.locked_frame = self.tracker.update(frame_read)
self.locked_frame = [int(i) for i in self.locked_frame]
frame_shape = (frame_read.shape[1], frame_read.shape[0])
logger.info("Locked Frame : {}".format(self.locked_frame))
if ok:
self.calcMovementVector(frame_shape, self.locked_frame)
cv2.rectangle(
frame_read,
(int(self.locked_frame[0]), self.locked_frame[1]),
(
self.locked_frame[0] + self.locked_frame[2],
self.locked_frame[1] + self.locked_frame[3],
),
(255, 125, 0),
2,
)
else:
self.mode = "Aquire Lock"
self.locked = False
self.locked_frame = None
return frame_read
def calcMovementVector(self, frame_shape, frame):
frame_center = (int(frame_shape[0] / 2), int(frame_shape[1] / 2))
x_mov = (frame[0] + frame[2] / 2) - frame_center[0]
logger.info("X mov : {}".format(x_mov))
if x_mov > 50 or x_mov < -50:
self.yaw_velocity = int(S * x_mov / frame_center[0])
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 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)
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.
- P: enter/exit follow person mode.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 360])
# create object detector
coco_frozen_graph_path = './ssd_mobilenet_v2_coco_2018_03_29/frozen_inference_graph.pb'
coco_labels_path = './ssd_mobilenet_v2_coco_2018_03_29/mscoco_label_map.pbtxt'
self.coco_obj_detector = Object_Detector(coco_frozen_graph_path,
coco_labels_path,
debug_mode=True)
# create object detector
face_frozen_graph_path = './ssd_face_detection/frozen_inference_graph_face.pb'
face_labels_path = './ssd_face_detection/face_label_map.pbtxt'
self.face_obj_detector = Object_Detector(face_frozen_graph_path,
face_labels_path,
debug_mode=True)
#create tracker object
self.tracker = Object_Tracker(class_id=1)
# create depth detector
self.depth_detector = Depth_Detector()
self.object_depth_detector = Object_Depth_Detector()
#create planner
self.planner = Planner()
self.face_bounding_box = [
0.3, 0.4, 0.5, 0.6, 0.1, 0.2
] #[min_y, min_x, max_y, max_x, min_size, max_size]
self.person_bounding_box = [
0.45, 0.45, 0.55, 0.55, 0.3, 0.5
] #[min_y, min_x, max_y, max_x, min_size, max_size]
# 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.follow_human = 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:
print('Battery: ' + str(self.tello.get_battery()) + '%')
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])
current_frame = frame_read.frame
#get depth map
# depth_map, depth_img = self.depth_detector.detect_depth(current_frame)
#detect objects
output_dict, img = self.face_obj_detector.detect_objects(
current_frame)
#track object
box = self.tracker.track_object(current_frame, output_dict)
bounding_box = self.face_bounding_box
# if box is None:#not found a face for n frames (where n is defined in tracker constructor)
# #so use coco object detector instead to look for a person
# #detect objects
# output_dict, img = self.coco_obj_detector.detect_objects(current_frame)
# #track object
# box = self.tracker.track_object(current_frame, output_dict)
# bounding_box = self.person_bounding_box
#get objects depth
# depth = self.object_depth_detector.get_depth(box, depth_map)
depth = None # temp
to_show = concat_images(img, current_frame)
commands = self.planner.follow_object(box, depth, bounding_box, 60)
if self.follow_human:
for c in commands:
key, speed = c
self.make_move(key, speed)
frame = cv2.cvtColor(to_show, 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()
# time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def make_move(self, key, speed):
if key == 'FORWARD': # set forward velocity
self.for_back_velocity = speed
elif key == 'BACK': # set backward velocity
self.for_back_velocity = -speed
elif key == 'LEFT': # set left velocity
self.left_right_velocity = -speed
elif key == 'RIGHT': # set right velocity
self.left_right_velocity = speed
elif key == 'UP': # set up velocity
self.up_down_velocity = speed
elif key == 'DOWN': # set down velocity
self.up_down_velocity = -speed
elif key == 'CLOCKWISE': # set yaw clockwise velocity
self.yaw_velocity = -speed
elif key == 'ANTICLOCKWISE': # set yaw counter clockwise velocity
self.yaw_velocity = speed
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
elif key == pygame.K_p:
self.follow_human = not self.follow_human
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
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):
""" 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()
# Create pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([SCREEN_WIDTH, SCREEN_HEIGHT])
# Init Tello object that interacts with the Tello drone
if WEBCAM:
self.tello = MockTello()
else:
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(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:
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])
frame = frame_read.frame
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
# frame = np.flipud(frame)
gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
faces = faceCascade.detectMultiScale(gray,
scaleFactor=1.1,
minNeighbors=5,
minSize=(30, 30),
flags=cv2.CASCADE_SCALE_IMAGE)
# frame = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB)
print(len(faces))
for (x, y, w, h) in faces:
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
if len(faces) > 0:
center_x = x + w / 2
center_y = y + h / 2
self.yaw_velocity = S if center_x < SCREEN_WIDTH / 2 else -S
self.up_down_velocity = S if center_y < SCREEN_HEIGHT / 2 else -S
else:
self.yaw_velocity = 0
self.up_down_velocity = 0
frame = np.rot90(frame)
frame = pygame.surfarray.make_surface(frame)
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
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):
""" 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("Video FPV Tello")
self.screen = pygame.display.set_mode([960, 720])
# Creat pygame fuente
myFont = pygame.font.Font(None, 30)
self.mitexto = myFont.render("prueba Pantalla", 0, (200, 60, 80))
# 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(USEREVENT + 1, 50)
def run(self):
if not self.tello.connect():
print("Drone no conectado")
return
if not self.tello.set_speed(self.speed):
print("No es posible menos velocidad")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("no se pudo parar el etreaming")
return
if not self.tello.streamon():
print("no se pudo iniciar el etreaming")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
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])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
self.screen.blit(self.mitexto, (100, 100))
time.sleep(1 / FPS)
# Face detecction
gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray,
scaleFactor=1.3,
minNeighbors=4,
minSize=(30, 30))
for (x, y, w, h) in faces:
# print (x,y,w,h)
color = (255, 0, 0) #BGR color
stroke = 2
end_coord_x = x + w
end_coord_y = y + h
cv2.rectangle(frame_read.frame, (x, y),
(end_coord_x, end_coord_y), color, stroke)
pygame.display.update()
print('Response: ' + str(self.tello.send_read_command('battery?')))
# 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
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):
""" 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
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
self.telloPose = np.zeros((1,3))
self.telloEulerAngles = np.zeros((1,3))
self.rcOut=np.zeros(4)
self.poseQueue = np.zeros((7,3))
self.supreQueue = np.zeros((7,3))
self.flag = 0
self.telloPoseVariance = np.zeros(3)
self.telloPoseMean = np.zeros(3)
self.tello.TIME_BTW_RC_CONTROL_COMMANDS = 20
self.R = np.zeros((3,3))
# self.telloPose = np.array([])
# self.telloEulerAngles = EulerAngles
def run(self):
if not self.tello.connect():
print("Tello not connected")
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
Height = 100
while not should_stop:
if frame_read.stopped:
frame_read.stop()
break
frameBGR = np.copy(frame_read.frame)
# frameBGR = np.rot90(frameBGR)
# frameBGR = np.flipud(frameBGR)
frame2use = im.resize(frameBGR,width=720)
key = cv2.waitKey(1) & 0xFF;
K = np.array([[7.092159469231584126e+02,0.000000000000000000e+00,3.681653710406367850e+02],[0.000000000000000000e+00,7.102890453175559742e+02,2.497677007139825491e+02],[0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00]])
dist = np.array([-1.428750372096417864e-01,-3.544750945429044758e-02,1.403740315118516459e-03,-2.734988255518019593e-02,1.149084393996809700e-01])
K_inv = np.linalg.inv(K)
h , w = frame2use.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h))
mapx,mapy = cv2.initUndistortRectifyMap(K,dist,None,newcameramtx,(w,h),5)
dst = cv2.remap(frame2use,mapx,mapy,cv2.INTER_LINEAR)
x,y,w,h = roi
dst = dst[y:y+h,x:x+w]
# print("ROI: ",x,y,w,h)
cv2.imshow("Orignal",frame2use)
cv2.imshow("rectified",dst)
# gray = cv2.cvtColor(frame2use, cv2.COLOR_BGR2GRAY)
gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
detector = apriltag.Detector()
result = detector.detect(gray)
if len(result) != 0:
self.flag = 1
else :
self.flag = 0
self.FindPose(result,K_inv)
if self.flag == 1:
varN = np.linalg.norm(self.telloPoseVariance)
print "varN",varN
Pose = self.telloPoseMean
xEr = 0 - Pose[0]
yEr = 0 - Pose[1]
zEr = Height - Pose[2]
ErrorN = np.linalg.norm([xEr,yEr,zEr])
print "Height",Height
print "ErrorN",ErrorN
if varN < 370 and key == ord("e"):
if varN < 120 and ErrorN < 10:
Height = Height -5
print "#######################################################################"
if Height <30:
self.tello.land()
kp = 3
MtnCmd = np.matmul(1,[kp*xEr,kp*yEr,kp*zEr])
self.rcOut = [MtnCmd[0],MtnCmd[1],MtnCmd[2],0]
if self.rcOut[0] > 60:
self.rcOut[0] = 60
elif self.rcOut[0] < -60:
self.rcOut[0] = -60
if self.rcOut[1] > 60:
self.rcOut[1] = 60
elif self.rcOut[1] < -60:
self.rcOut[1] = -60
if self.rcOut[2] > 60:
self.rcOut[2] = 60
elif self.rcOut[2] < -60:
self.rcOut[2] = -60
# elif self.rcOut[1] > 60:
# self.rcOut[1] = 60
# elif self.rcOut[1] < -60:
# self.rcOut[1] = -60
else :
if key == ord("w"):
self.rcOut[1] = 50
elif key == ord("a"):
self.rcOut[0] = -50
elif key == ord("s"):
self.rcOut[1] = -50
elif key == ord("d"):
self.rcOut[0] = 50
elif key == ord("u"):
self.rcOut[2] = 50
elif key == ord("j"):
self.rcOut[2] = -50
else:
self.rcOut = [0,0,0,0]
else:
if key == ord("w"):
self.rcOut[1] = 50
elif key == ord("a"):
self.rcOut[0] = -50
elif key == ord("s"):
self.rcOut[1] = -50
elif key == ord("d"):
self.rcOut[0] = 50
elif key == ord("u"):
self.rcOut[2] = 50
elif key == ord("j"):
self.rcOut[2] = -50
else:
self.rcOut = [0,0,0,0]
print self.rcOut
self.tello.send_rc_control(int(self.rcOut[0]),int(self.rcOut[1]),int(self.rcOut[2]),int(self.rcOut[3]))
self.rcOut = [0,0,0,0]
if key == ord("q"):
break
if key == ord("t"):
self.tello.takeoff()
if key == ord("l"):
self.tello.land()
Height = 100
if cv2.waitKey(1) & 0xFF == ord('q'):
break
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def FindPose(self,result,K_inv):
if self.flag == 1:
center = result[0].center
H = np.array(result[0].homography)
# print H
h1h2h3 = np.matmul(K_inv,H)
h1T = h1h2h3[:,0]
h2T = h1h2h3[:,1]
h3T = h1h2h3[:,2]
h1Xh2T = np.cross(h1T,h2T)
h1_h2_h1Xh2T = np.array([h1T,h2T,h1Xh2T])
h1_h2_h1Xh2 = np.transpose(h1_h2_h1Xh2T)
u, s, vh = np.linalg.svd(h1_h2_h1Xh2, full_matrices=True)
uvh = np.matmul(u,vh)
det_OF_uvh = np.linalg.det(uvh)
M = np.array([[1,0,0],[0,1,0],[0,0,det_OF_uvh]])
T = h3T/np.linalg.norm(h1T) # Translation Matrix
T = T*100/17.5
r = np.matmul(u,M)
R = np.matmul(r,vh) # Rotation matrix
T = T
self.R = R
T_t = np.reshape(T,(3,1))
neg_Rt_T = -1*np.dot(R.T,T_t)
f = np.array([[0,0,0,1]])
if neg_Rt_T[2,0] < 0:
flag = -1
else:
flag = 1
neg_Rt_T[2,0] = neg_Rt_T[2,0]*flag
neg_Rt_T[0,0] = neg_Rt_T[0,0]*(-1)
Pose = neg_Rt_T.T
EulerAngles = rotationMatrixToEulerAngles(R.T)
self.telloPose = Pose
self.telloEulerAngles = EulerAngles
self.poseQueue = np.roll(self.poseQueue,1,axis = 0)
self.poseQueue[0,:] = [Pose[0,0],Pose[0,1],Pose[0,2]]
self.telloPoseVariance = np.var(self.poseQueue,axis=0)
self.telloPoseMean = np.mean(self.poseQueue,axis = 0)
# print "PoseQueue",self.poseQueue
print "PoseMean",self.telloPoseMean
# print"telloPoseVariance",self.telloPoseVariance
# Pose,EulerAngles
return
def main():
# init global vars
global gesture_buffer
global gesture_id
global battery_status
# Argument parsing
args = get_args()
KEYBOARD_CONTROL = args.is_keyboard
WRITE_CONTROL = False
in_flight = False
# Camera preparation
tello = Tello()
#print(dir(tello))
tello.connect()
tello.set_speed(speed["slow"])
print("\n\n" + tello.get_speed() + "\n\n")
tello.streamon()
cap_drone = tello.get_frame_read()
cap_webcam = cv.VideoCapture(0)
# Init Tello Controllers
gesture_controller = TelloGestureController(tello)
keyboard_controller = TelloKeyboardController(tello)
gesture_detector = GestureRecognition(args.use_static_image_mode,
args.min_detection_confidence,
args.min_tracking_confidence)
gesture_buffer = GestureBuffer(buffer_len=args.buffer_len)
def tello_control(key, keyboard_controller, gesture_controller):
global gesture_buffer
if KEYBOARD_CONTROL:
keyboard_controller.control(key)
else:
gesture_controller.gesture_control(gesture_buffer)
def tello_battery(tello):
global battery_status
battery_status = tello.get_battery()
# FPS Measurement
cv_fps_calc = CvFpsCalc(buffer_len=10)
mode = 0
number = -1
battery_status = -1
tello.move_down(20)
while True:
fps = cv_fps_calc.get()
# Process Key (ESC: end)
key = cv.waitKey(1) & 0xff
if key == 27: # ESC
break
elif key == 32: # Space
if not in_flight:
# Take-off drone
tello.takeoff()
in_flight = True
elif in_flight:
# Land tello
tello.land()
in_flight = False
elif key == ord('k'):
mode = 0
KEYBOARD_CONTROL = True
WRITE_CONTROL = False
tello.send_rc_control(0, 0, 0, 0) # Stop moving
elif key == ord('g'):
KEYBOARD_CONTROL = False
elif key == ord('n'):
mode = 1
WRITE_CONTROL = True
KEYBOARD_CONTROL = True
if WRITE_CONTROL:
number = -1
if 48 <= key <= 57: # 0 ~ 9
number = key - 48
# Camera capture
image_drone = cap_drone.frame
image = cap_webcam.read()[1]
try:
debug_image, gesture_id = gesture_detector.recognize(
image, number, mode)
gesture_buffer.add_gesture(gesture_id)
# Start control thread
threading.Thread(target=tello_control,
args=(
key,
keyboard_controller,
gesture_controller,
)).start()
threading.Thread(target=tello_battery, args=(tello, )).start()
debug_image = gesture_detector.draw_info(debug_image, fps, mode,
number)
battery_str_postion = (5, 100) # dustin webcam
#battery_str_postion = (5, 720 - 5) # drone camera
# Battery status and image rendering
cv.putText(debug_image, "Battery: {}".format(battery_status),
battery_str_postion, cv.FONT_HERSHEY_SIMPLEX, 1,
(0, 0, 255), 2)
modeStr = "gestures"
if KEYBOARD_CONTROL:
modeStr = "keyboard"
cv.putText(debug_image, modeStr + " control", (5, 150),
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv.imshow('Webcam Gesture Recognition', debug_image)
cv.imshow('Tello drone camera', image_drone)
except:
print("exception")
tello.land()
tello.end()
cv.destroyAllWindows()
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
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
self.telloPose = np.zeros((1, 3))
self.telloEulerAngles = np.zeros((1, 3))
self.rcOut = np.zeros(4)
self.poseQueue = np.zeros((7, 3))
self.supreQueue = np.zeros((7, 3))
self.flag = 0
self.telloPoseVariance = np.zeros(3)
self.telloPoseMean = np.zeros(3)
self.tello.TIME_BTW_RC_CONTROL_COMMANDS = 20
self.R = np.zeros((3, 3))
# self.telloPose = np.array([])
# self.telloEulerAngles = EulerAngles
def run(self):
if not self.tello.connect():
print("Tello not connected")
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
Height = 100
while not should_stop:
if frame_read.stopped:
frame_read.stop()
break
frameBGR = np.copy(frame_read.frame)
# frameBGR = np.rot90(frameBGR)
# frameBGR = np.flipud(frameBGR)
frame2use = im.resize(frameBGR, width=720)
frame = frame2use
kernel = np.ones((5, 5), np.uint8) #param 1
blurred = cv2.GaussianBlur(frame, (7, 7), 0) #param 1
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
h, s, v = cv2.split(hsv)
dilS = cv2.dilate(s, kernel, iterations=1)
newS = dilS - s
newS = cv2.equalizeHist(newS)
# newS = cv2.GaussianBlur(newS, (11, 11), 0)
dilV = cv2.dilate(v, kernel, iterations=1) #param 1
newV = dilV - v
newV = cv2.equalizeHist(newV)
dilH = cv2.dilate(h, kernel, iterations=1)
newH = dilH - h
newH = cv2.equalizeHist(newH)
sabKaAnd = cv2.bitwise_or(newS, newV)
kernel2 = np.ones((3, 3), np.uint8) #param 1
sabKaAnd = cv2.erode(sabKaAnd, kernel2, iterations=1) #param 1
sabKaAnd = cv2.erode(sabKaAnd, kernel2, iterations=1) #param 1
sabKaAnd = cv2.dilate(sabKaAnd, kernel2, iterations=1) #param 1
sabKaAnd = cv2.GaussianBlur(sabKaAnd, (11, 11), 0)
maskSab = cv2.inRange(sabKaAnd, 120, 255) #param 1****
maskSab = cv2.erode(maskSab, kernel2, iterations=1)
maskSab = cv2.dilate(maskSab, kernel2, iterations=1)
maskSab = cv2.bitwise_and(maskSab, newV)
maskSab = cv2.equalizeHist(maskSab)
maskSab = cv2.inRange(maskSab, 190, 255) # param *****
kernel2 = np.ones((2, 2), np.uint8) #param ****
maskSab = cv2.erode(maskSab, kernel2, iterations=1)
maskSab = cv2.dilate(maskSab, kernel2, iterations=1)
cv2.imshow("fff", frame)
mask = maskSab
# Contours detection
_, contours, _ = cv2.findContours(mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
oldArea = 300
for cnt in contours:
area = cv2.contourArea(cnt)
approx = cv2.approxPolyDP(cnt,
0.012 * cv2.arcLength(cnt, True),
True) # 0.012 param
x = approx.ravel()[0]
y = approx.ravel()[1]
if area > 300: #param
# if len(approx) == 3:
# cv2.putText(frame, "Triangle", (x, y), font, 1, (0, 0, 0))
if len(approx) == 4:
(cx, cy), (MA, ma), angle = cv2.fitEllipse(cnt)
ar = MA / ma
# area = cv2.contourArea(cnt)
hull = cv2.convexHull(cnt)
hull_area = cv2.contourArea(hull)
solidity = float(area) / hull_area
# print "Angle",angle
# print "solidity",solidity
# print "ar",ar
if solidity > 0.9 and ar < 0.4:
if area > oldArea:
cv2.drawContours(frame, [approx], 0, (0, 0, 0),
5)
cv2.circle(frame, (int(cx), int(cy)), 3,
(0, 0, 255), -1)
cv2.putText(frame, "Rectangle" + str(angle),
(x, y), font, 1, (0, 0, 0))
cntMain = approx
rect = order_points(cntMain)
print "rect", rect
Pose = PoseEstimation(rect)
pX = Pose[0, 0]
pY = Pose[0, 1]
pZ = Pose[0, 2]
Pose[0, 0] = pZ
Pose[0, 1] = -pX
Pose[0, 2] = -pY
self.telloPose = np.transpose(Pose)
self.poseQueue = np.roll(self.poseQueue,
1,
axis=0)
self.poseQueue[0, :] = [
Pose[0, 0], Pose[0, 1], Pose[0, 2]
]
self.telloPoseVariance = np.var(self.poseQueue,
axis=0)
self.telloPoseMean = np.mean(self.poseQueue,
axis=0)
self.flag = 1
# print "PoseQueue",self.poseQueue
print "PoseMean", self.telloPoseMean
# print "telloPoseVariance" , self.telloPoseVariance
varN = np.linalg.norm(self.telloPoseVariance)
print "varN", varN
oldArea = area
cv2.imshow("Frame", frame)
cv2.imshow("Mask", mask)
key = cv2.waitKey(1) & 0xFF
K = np.array([[
6.981060802052014651e+02, 0.000000000000000000e+00,
3.783628172155137577e+02
],
[
0.000000000000000000e+00,
6.932839845949604296e+02,
2.823973488087042369e+02
],
[
0.000000000000000000e+00,
0.000000000000000000e+00,
1.000000000000000000e+00
]])
dist = np.array([
-1.128288663079663086e-02, 1.551794079596884035e-02,
3.003426614702892333e-03, 1.319203673619398672e-03,
1.086713281720452368e-01
])
K_inv = np.linalg.inv(K)
h, w = frame2use.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
K, dist, (w, h), 1, (w, h))
mapx, mapy = cv2.initUndistortRectifyMap(K, dist, None,
newcameramtx, (w, h), 5)
dst = cv2.remap(frame2use, mapx, mapy, cv2.INTER_LINEAR)
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
# print("ROI: ",x,y,w,h)
cv2.imshow("Orignal", frame2use)
cv2.imshow("rectified", dst)
# gray = cv2.cvtColor(frame2use, cv2.COLOR_BGR2GRAY)
gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
if self.flag == 1:
varN = np.linalg.norm(self.telloPoseVariance)
# print "varN",varN
Pose = self.telloPoseMean
xEr = 450 - Pose[0]
yEr = 0 - Pose[1]
zEr = 0 - Pose[2]
ErrorN = np.linalg.norm([xEr, yEr, zEr])
if key == ord("e"):
kp = 0.35
MtnCmd = np.array([kp * xEr, kp * yEr, kp * zEr])
MtnCmd[0] = -1 * MtnCmd[0]
self.rcOut = [MtnCmd[1], MtnCmd[0], MtnCmd[2], 0]
if self.rcOut[0] > 35:
self.rcOut[0] = 35
elif self.rcOut[0] < -35:
self.rcOut[0] = -35
if self.rcOut[1] > 35:
self.rcOut[1] = 35
elif self.rcOut[1] < -35:
self.rcOut[1] = -35
if self.rcOut[2] > 35:
self.rcOut[2] = 35
elif self.rcOut[2] < -35:
self.rcOut[2] = -35
else:
if key == ord("w"):
self.rcOut[1] = 50
elif key == ord("a"):
self.rcOut[0] = -50
elif key == ord("s"):
self.rcOut[1] = -50
elif key == ord("d"):
self.rcOut[0] = 50
elif key == ord("u"):
self.rcOut[2] = 50
elif key == ord("j"):
self.rcOut[2] = -50
else:
self.rcOut = [0, 0, 0, 0]
else:
if key == ord("w"):
self.rcOut[1] = 50
elif key == ord("a"):
self.rcOut[0] = -50
elif key == ord("s"):
self.rcOut[1] = -50
elif key == ord("d"):
self.rcOut[0] = 50
elif key == ord("u"):
self.rcOut[2] = 50
elif key == ord("j"):
self.rcOut[2] = -50
else:
self.rcOut = [0, 0, 0, 0]
# print self.rcOut
self.tello.send_rc_control(int(self.rcOut[0]), int(self.rcOut[1]),
int(self.rcOut[2]), int(self.rcOut[3]))
self.rcOut = [0, 0, 0, 0]
if key == ord("q"):
break
if key == ord("t"):
self.tello.takeoff()
if key == ord("l"):
self.tello.land()
Height = 100
if cv2.waitKey(1) & 0xFF == ord('q'):
break
time.sleep(1 / FPS)
self.flag = 0
# Call it always before finishing. I deallocate resources.
self.tello.end()
from djitellopy import Tello
import cv2
import time
tello = Tello()
tello.connect()
cmd = raw_input("What would you like to do?\n")
while cmd != 'q':
if cmd == "t":
tello.takeoff()
elif cmd == 'l':
tello.move_left(50)
elif cmd == 'r':
tello.move_right(50)
elif cmd == '45':
tello.rotate_counter_clockwise(45)
else:
tello.land()
cmd = raw_input("Next cmd\n")
tello.land()
tello.end()
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
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
def run(self):
if not self.tello.connect():
print("Tello not connected")
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
name = "outImg"
i = 0
while not should_stop:
if frame_read.stopped:
frame_read.stop()
break
frameBGR = np.copy(frame_read.frame)
# frameBGR = np.rot90(frameBGR)
# frameBGR = np.flipud(frameBGR)
frame2use = im.resize(frameBGR, width=720)
key = cv2.waitKey(1) & 0xFF
cv2.imshow("lkgs", frame2use)
if key == ord("c"):
new = name + str(i) + ".jpg"
print new
i = i + 1
cv2.imwrite(new, frame2use)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
if cv2.waitKey(1) & 0xFF == ord('q'):
break
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
# frame = np.rot90(frame)
# frame = np.flipud(frame)
# a = frameGray.dtype
# print a
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
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()
# 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(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:
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])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = pygame.surfarray.make_surface(frame)
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
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 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
