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(pygame.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 == 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:
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 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
elif key == pygame.K_b: # get battery
print(self.tello.get_battery())
elif key == pygame.K_c: # send command from terminal
command = input(
'Enter command (speed?time?battery?tof?height?baro?temp?wifi?sn?sdk?altitude?)'
)
print(self.tello.send_read_command(command))
elif key == pygame.K_1:
print('flip right')
self.tello.flip('r')
elif key == pygame.K_2:
print('flip left')
self.tello.flip('l')
elif key == pygame.K_3:
print('flip forward')
self.tello.flip('f')
elif key == pygame.K_4:
print('flip back')
self.tello.flip('b')
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)
continue
if global_coords[0] == -1:
global_coords[0] = 0
global_coords[1] = 0
global_coords[2] = 2
cv2.circle(frame, (global_coords[0], global_coords[1]),
global_coords[2], (0, 255, 0), 2)
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == 27:
break
drone.land()
drone_cam.stop()
detect_thread.join()
#vid_process.join() # waits for video process to be killed
#detect_process.terminate() # then kills detection process
#vid_thread.join()
#detect_thread.join()
'''
final_drone_cam_memory = shared_memory.SharedMemory(name="drone_cam_object")
final_drone_cam_array = np.ndarray((1,), dtype=np.object, buffer=final_drone_cam_memory.buf) # load camera in
final_drone_cam_array[0].stop() # stop camera
final_drone_cam_memory.close() # close camera storage
final_drone_cam_memory.unlink() # delete camera storage
class DroneProcessor:
FINISH_DRAWING_HOLD_TIME_S = 2
def __init__(self, max_area_cm=100, starting_move_up_cm=50, min_length_between_points_cm=5,
max_speed=30):
"""
:param max_area_cm: Maximum length that drone can move from starting point in both axes.
:param starting_move_up_cm: How many cms should drone go up after the takeoff
:param min_length_between_points_cm: Minimum length between points, to reduce number of points from detection.
"""
self.max_area = max_area_cm
self.min_length_between_points_cm = min_length_between_points_cm
self.max_speed = max_speed
self.tello = Tello()
self.tello.connect()
self.tello.streamon()
self.tello.takeoff()
self.tello.move_up(starting_move_up_cm)
self.tello_ping_thread = Thread(target=self.ping_tello)
self.should_stop_pinging_tello = False
def get_last_frame(self):
return self.tello.get_frame_read().frame
def finish_drawing(self):
"""
Finish drawing, by stopping drone in air for a while and then force it to land. Disable video streaming.
:return:
"""
self.tello.send_rc_control(0, 0, 0, 0)
time.sleep(self.FINISH_DRAWING_HOLD_TIME_S)
self.tello.land()
self.tello.streamoff()
def ping_tello(self):
"""
Ping tello to prevent it from landing while drawing.
:return:
"""
while True:
time.sleep(1)
self.tello.send_command_with_return("command")
print(f"Battery level: {self.tello.get_battery()}")
if self.should_stop_pinging_tello:
break
def start_pinging_tello(self):
"""
Starts thread that pings Tello drone, to prevent it from landing while drawing
:return:
"""
self.tello_ping_thread.start()
def stop_pinging_tello(self):
"""
Stop pinging tello to make it available to control
:return:
"""
self.should_stop_pinging_tello = True
self.tello_ping_thread.join()
def rescale_points(self, point_list, is_int=False):
"""
Rescale points from 0-1 range to range defined by max_area.
:param point_list:
:param is_int:
:return: Points rescaled to max_area
"""
temp_list = []
for point in point_list:
temp_point = []
for coordinate in point:
coordinate = coordinate * self.max_area
if is_int:
temp_point.append(int(coordinate))
else:
temp_point.append(coordinate)
temp_list.append(temp_point)
return temp_list
def discrete_path(self, rescaled_points):
"""
Reduce number of points in list, so the difference between next points needs to be at least
min_length_between_points_cm
:param rescaled_points:
:return:
"""
last_index = -1
length = 0
while length < self.min_length_between_points_cm:
last_index -= 1
length = distance(rescaled_points[-1], rescaled_points[last_index])
last_index = len(rescaled_points) + last_index
discrete_path = [rescaled_points[0]]
actual_point = 0
for ind, point in enumerate(rescaled_points):
if ind > last_index:
discrete_path.append(rescaled_points[-1])
break
if distance(rescaled_points[actual_point], point) > 5:
discrete_path.append(point)
actual_point = ind
return discrete_path
def reproduce_discrete_path_by_drone(self, discrete_path):
"""
Converts discrete path to velocity commands and sends them to drone, so the drone reproduce the path
:param discrete_path: list of [x, y] points, which represents distance in each axis between previous point in
list
:return:
"""
for current_point in discrete_path:
ang = np.arctan2(current_point[0], current_point[1])
x_speed = int(np.sin(ang) * self.max_speed)
y_speed = -int(np.cos(ang) * self.max_speed)
euclidean_distance = (current_point[0] ** 2 + current_point[1] ** 2) ** 0.5
move_time = euclidean_distance / self.max_speed
self.tello.send_rc_control(x_speed, 0, y_speed, 0)
time.sleep(move_time)
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 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.
"""
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 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.
"""
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()
# 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.
- 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()
# 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 = 60
self.send_rc_control = False
# create update timer
pygame.time.set_timer(USEREVENT + 1, 2)
pygame.time.set_timer(USEREVENT + 2, 10)
# Run thread to find box in frame
print('init done')
def run(self):
global frame_glob
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('output.avi', fourcc, 60.0, (960, 720))
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
print('loop started')
last_yaw=0
box_cnt = 0
frame_glob = []
frame = []
while not should_stop:
frame = frame_read.frame
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.update()
if event.type == USEREVENT + 2:
frame_glob = frame
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])
box_list = []
try:
box_list, actions_list = box_q.get_nowait()
box = box_list[0]
actions = actions_list[0]
except Exception:
pass
box_x = 0
box_y = 0
if len(box_list) > 0:
box_cnt = 0
area = (box.xmax - box.xmin) * (box.ymax - box.ymin)
area_p = (area / 691200.) * 100.0
box_x = int((box.xmax - box.xmin) / 2) + box.xmin
box_y = int((box.ymax - box.ymin) / 2) + box.ymin
draw_boxes(frame, box_list, config['model']['labels'], obj_thresh)
done = bool(get_dist(box_x, box_y) < 100 and (15 < area_p < 50))
#If not done keep setting speeds
if not done:
self.yaw_velocity = -int((actions[0])/2)
last_yaw = self.yaw_velocity
self.up_down_velocity = int((actions[1])/2)
if area_p < 25:
self.for_back_velocity = 30
elif area_p > 50:
self.for_back_velocity = -30
else:
self.for_back_velocity = 0
frame = cv2.circle(frame, (box_x, box_y), 5, (255, 0, 0), -1)
else:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
#if no box is detected set the last yaw command so the drone goes into search.
else:
box_cnt += 1
if box_cnt > 10:
self.yaw_velocity = last_yaw
else:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
frame = cv2.circle(frame, (480, 360), 5,(0, 0, 255),-1)
out.write(frame)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
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()
out.release()
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)
def reset_speed(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(0, 0, 0, 0)
class TelloDrone():
def __init__(self):
"""
Get the DJITelloPy Tello object to use for communication
with the drone.
speed: general speed for non rc control calls (10-100)
"""
self.tello = Tello()
self.left_right_velocity = 0
self.forward_backward_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.has_target = False
self.battery_level = 0
self.last_battery_check = 0
self.speed = 20 # Lowest
def cleanup(self):
"""Call this function if the script has to be stopped for whatever
reason to make sure everything is exited cleanly"""
self.end_drone()
cv2.destroyAllWindows()
def end_drone(self):
"""Lands drone and ends drone interaction"""
self.stop_moving()
self.tello.streamoff()
if self.tello.is_flying:
self.tello.land()
self.tello.end()
def update_battery_level(self):
if time.time() - self.last_battery_check >= TIME_BTW_BATTERY_CHECKS:
self.battery_level = self.tello.get_battery()
self.last_battery_check = time.time()
def update_drone_velocities_if_flying(self):
"""Move drone based on velocities only if drone is flying"""
if not self.tello.is_flying:
return
self.tello.send_rc_control(self.left_right_velocity,
self.forward_backward_velocity,
self.up_down_velocity,
self.yaw_velocity)
def update_drone_velocities(self):
"""Move drone based on velocities"""
self.tello.send_rc_control(self.left_right_velocity,
self.forward_backward_velocity,
self.up_down_velocity,
self.yaw_velocity)
def stop_moving(self):
"""Freeze in place"""
time.sleep(self.tello.TIME_BTW_RC_CONTROL_COMMANDS) # Delay makes sure it is sent
self.left_right_velocity = 0
self.forward_backward_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.update_drone_velocities()
def scan_surroundings(self):
"""Start looking around"""
self.yaw_velocity = SCAN_VELOCITY
self.update_drone_velocities_if_flying()
def find_target(self):
# Returns False for now
return False
def handle_user_keypress(self):
"""Check for pressed keys.
't': Take off
'l': Land
'Esc': Exit
Returns:
key (int): unicode value of key that was pressed, or -1 if
none were pressed"""
key = cv2.waitKey(20)
if key == ord('t'):
print_warning("Drone taking off...")
self.tello.takeoff()
elif key == ord('l'):
print_warning("Landing drone...")
self.stop_moving()
self.tello.land()
elif key == ESC_KEY:
print_warning("Stopping drone...")
elif key == NO_KEY:
if not self.has_target:
self.scan_surroundings()
return key
def initialize_before_run(self):
"""Set up drone before we bring it to life"""
if not self.tello.connect():
print_error("Failed to set drone to SDK mode")
sys.exit(-1)
print_status("Connected to Drone")
if not self.tello.set_speed(self.speed):
print_error("Failed to set initial drone speed")
print_status(f"Drone speed set to {self.speed} cm/s ({self.speed}%)")
# Make sure stream is off first
self.tello.streamoff()
self.tello.streamon() # Not sure why DJITelloPy doesn't return here
self.battery_level = self.tello.get_battery()
print_status(f"Drone battery is at {self.battery_level}%")
# Reset velocities
self.update_drone_velocities()
print_status(f"All drone velocities initialized to 0")
def run(self, user_interface=True):
"""Bring an autonomous being to life"""
self.initialize_before_run()
frame_read = self.tello.get_frame_read()
while True:
self.update_drone_velocities_if_flying()
if frame_read.stopped:
frame_read.stop()
break
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame_ret = frame_read.frame
tello_cam = self.tello.get_video_capture()
time.sleep(1 / FPS)
key_pressed = self.handle_user_keypress()
if key_pressed == ESC_KEY:
break
# Show battery level
self.update_battery_level()
cv2.putText(frame_ret,f"Battery: {self.battery_level}%",(32,664),cv2.FONT_HERSHEY_SIMPLEX,1,(0, 0, 255), thickness=2)
cv2.imshow(f'Tello Tracking...', frame_ret)
self.cleanup() # Clean exit
from djitellopy import Tello
myDrone = Tello()
myDrone.connect()
# Read datas from JSON file.
f = open('waypoints.json', 'r')
path_dict = json.load(f)
path_wp = path_dict['wp']
path_pos = path_dict['pos']
print(path_wp)
print(path_pos)
# Follow the instructions
myDrone.takeoff()
for instruction in path_wp:
angle = instruction['angle_deg']
length = instruction['dist_cm']
if (angle < 0):
myDrone.rotate_counter_clockwise(-angle)
else:
myDrone.rotate_clockwise(angle)
myDrone.move_forward(length)
myDrone.land()
# d.rotate_clockwise(90)
# d.move_forward(20)
# d.land()
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):
""" 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
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):
""" 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()
class TelloCV(object):
"""
TelloTracker builds keyboard controls on top of TelloPy as well
as generating images from the video stream and enabling opencv support
"""
def __init__(self):
self.prev_flight_data = None
self.record = False
self.tracking = False
self.keydown = False
self.date_fmt = '%Y-%m-%d_%H%M%S'
self.speed = 50
self.go_speed = 80
self.drone = Tello()
self.init_drone()
self.init_controls()
self.battery = self.drone.get_battery()
self.frame_read = self.drone.get_frame_read()
self.forward_time = 0
self.forward_flag = True
self.takeoff_time = 0
self.command_time = 0
self.command_flag = False
# trackingimport libh264decoder a color
# green_lower = (30, 50, 50)
# green_upper = (80, 255, 255)
# red_lower = (0, 50, 50)
# red_upper = (20, 255, 255)
blue_lower = np.array([0, 0, 0])
upper_blue = np.array([255, 255, 180])
bh_lower = (180, 30, 100)
bh_upper = (275, 50, 100)
self.track_cmd = ""
self.tracker = Tracker(960,
720,
blue_lower, upper_blue)
self.speed_list = [5, 10, 15, 20, 25]
self.frame_center = 480, 360
self.error = 60
def init_drone(self):
"""Connect, uneable streaming and subscribe to events"""
# self.drone.log.set_level(2)
self.drone.connect()
self.drone.streamon()
def on_press(self, keyname):
"""handler for keyboard listener"""
if self.keydown:
return
try:
self.keydown = True
keyname = str(keyname).strip('\'')
print('+' + keyname)
if keyname == 'Key.esc':
self.drone.quit()
exit(0)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(self.speed)
else:
key_handler(self.speed)
except AttributeError:
print('special key {0} pressed'.format(keyname))
def on_release(self, keyname):
"""Reset on key up from keyboard listener"""
self.keydown = False
keyname = str(keyname).strip('\'')
print('-' + keyname)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(0)
else:
key_handler(0)
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': lambda speed: self.drone.move_forward(speed),
's': lambda speed: self.drone.move_back(speed),
'a': lambda speed: self.drone.move_left(speed),
'd': lambda speed: self.drone.move_right(speed),
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.rotate_counter_clockwise(speed),
'Key.right': lambda speed: self.drone.rotate_clockwise(speed),
'Key.up': lambda speed: self.drone.move_up(speed),
'Key.down': lambda speed: self.drone.move_down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
# 'Key.tab': self.drone.takeoff(60),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
# self.key_listener.join()
def process_frame(self):
"""convert frame to cv2 image and show"""
# print("TRACKING START")
frame = self.frame_read.frame
# self.drone.move_up(self.speed)
# image = self.write_hud(image)
# if self.record:
# self.record_vid(frame)
return frame
def move_up(self):
self.drone.move_up(self.speed)
def take_off(self):
self.drone.takeoff()
def go(self):
self.drone.move_forward(self.go_speed)
def move_left(self):
self.drone.move_left(270) # speed 테스트해서 조절하기
def go_window9(self):
self.drone.move_forward()
def rotate_right(self):
self.drone.rotate_clockwise()
def rotate_left(self):
self.drone.rotate_counter_clockwise()
def landing(self):
self.drone.land()
def write_hud(self, frame):
"""Draw drone info, tracking and record on frame"""
stats = self.prev_flight_data.split('|')
stats.append("Tracking:" + str(self.tracking))
if self.drone.zoom:
stats.append("VID")
else:
stats.append("PIC")
if self.record:
diff = int(time.time() - self.start_time)
mins, secs = divmod(diff, 60)
stats.append("REC {:02d}:{:02d}".format(mins, secs))
for idx, stat in enumerate(stats):
text = stat.lstrip()
cv2.putText(frame, text, (0, 30 + (idx * 30)),
cv2.FONT_HERSHEY_SIMPLEX,
1.0, (255, 0, 0), lineType=30)
return frame
def toggle_recording(self, speed):
"""Handle recording keypress, creates output stream and file"""
if speed == 0:
return
self.record = not self.record
if self.record:
datename = [os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)]
self.out_name = '{}/Pictures/tello-{}.mp4'.format(*datename)
print("Outputting video to:", self.out_name)
self.out_file = av.open(self.out_name, 'w')
self.start_time = time.time()
self.out_stream = self.out_file.add_stream(
'mpeg4', self.vid_stream.rate)
self.out_stream.pix_fmt = 'yuv420p'
self.out_stream.width = self.vid_stream.width
self.out_stream.height = self.vid_stream.height
if not self.record:
print("Video saved to ", self.out_name)
self.out_file.close()
self.out_stream = None
def record_vid(self, frame):
"""
convert frames to packets and write to file
"""
new_frame = av.VideoFrame(
width=frame.width, height=frame.height, format=frame.format.name)
for i in range(len(frame.planes)):
new_frame.planes[i].update(frame.planes[i])
pkt = None
try:
pkt = self.out_stream.encode(new_frame)
except IOError as err:
print("encoding failed: {0}".format(err))
if pkt is not None:
try:
self.out_file.mux(pkt)
except IOError:
print('mux failed: ' + str(pkt))
def take_picture(self, speed):
"""Tell drone to take picture, image sent to file handler"""
if speed == 0:
return
self.drone.take_picture()
def palm_land(self, speed):
"""Tell drone to land"""
if speed == 0:
return
self.drone.palm_land()
def toggle_tracking(self, speed):
""" Handle tracking keypress"""
if speed == 0: # handle key up event
return
self.tracking = not self.tracking
print("tracking:", self.tracking)
return
def toggle_zoom(self, speed):
"""
In "video" mode the self.drone sends 1280x720 frames.
In "photo" mode it sends 2592x1936 (952x720) frames.
The video will always be centered in the window.
In photo mode, if we keep the window at 1280x720 that gives us ~160px on
each side for status information, which is ample.
Video mode is harder because then we need to abandon the 16:9 display size
if we want to put the HUD next to the video.
"""
if speed == 0:
return
self.drone.set_video_mode(not self.drone.zoom)
def flight_data_handler(self, event, sender, data):
"""Listener to flight data from the drone."""
text = str(data)
if self.prev_flight_data != text:
self.prev_flight_data = text
def handle_flight_received(self, event, sender, data):
"""Create a file in ~/Pictures/ to receive image from the drone"""
path = '%s/Pictures/tello-%s.jpeg' % (
os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt))
with open(path, 'wb') as out_file:
out_file.write(data)
print('Saved photo to %s' % path)
def enable_mission_pads(self):
self.drone.enable_mission_pads()
def disable_mission_pads(self):
self.drone.disable_mission_pads()
def go_xyz_speed_mid(self, x, y, z, speed, mid):
self.drone.go_xyz_speed_mid(x, y, z, speed, mid)
# if function return True, set drone center to object's center
def track_mid(self, x, y):
midx, midy = 480, 360
distance_x = abs(midx - x)
distance_y = abs(midy - y)
print(x, y, distance_x, distance_y)
move_done = True
if y > midy + self.error + 15:
self.drone.move_down(20)
move_done = False
elif y < midy - self.error + 5:
self.drone.move_up(20)
move_done = False
elif x < midx - self.error:
self.drone.move_left(20)
move_done = False
elif x > midx + self.error:
self.drone.move_right(20)
move_done = False
return move_done
def track_x(self, x, left_count, right_count):
midx = 480
move_done = True
if x < midx - 100:
self.drone.move_left(20)
left_count += 1
move_done = False
elif x > midx + 100:
self.drone.move_right(20)
right_count += 1
move_done = False
return move_done
def go_slow(self):
self.drone.move_forward(30)
def go_fast(self):
self.drone.move_forward(200)
class hand_tello_control:
def __init__(self):
self.action = " "
self.mp_drawing = mp.solutions.drawing_utils
self.mp_drawing_styles = mp.solutions.drawing_styles
self.mp_hands = mp.solutions.hands
self.action_done = True
self.ffoward = 1
self.fback = 1
self.fright = 1
self.fleft = 1
self.fsquare = 1
self.fmiddle = 1
self.fno = 1
self.fland = 1
def tello_startup(self):
# For Tello input:
self.tello = Tello() # Starts the tello object
self.tello.connect() # Connects to the drone
def define_orientation(self, results):
if results.multi_hand_landmarks[0].landmark[
4].x < results.multi_hand_landmarks[0].landmark[17].x:
orientation = "right hand"
else:
orientation = "left hand"
return orientation
def follow_hand(self, results):
normalizedLandmark = results.multi_hand_landmarks[0].landmark[
9] # Normalizes the lowest middle-finger coordinate for hand tracking
pixelCoordinatesLandmark = self.mp_drawing._normalized_to_pixel_coordinates(
normalizedLandmark.x, normalizedLandmark.y, 255, 255
) #Tracks the coordinates of the same landmark in a 255x255 grid
print(pixelCoordinatesLandmark)
if pixelCoordinatesLandmark == None: # If hand goes out of frame, stop following
self.tello.send_rc_control(0, 0, 0, 0)
return
centerRange = [12,
12] #Range for detecting commands in the x and y axis.
centerPoint = [128, 128] #Theoretical center of the image
xCorrection = pixelCoordinatesLandmark[0] - centerPoint[0]
yCorrection = pixelCoordinatesLandmark[1] - centerPoint[1]
xSpeed = 0
ySpeed = 0
if xCorrection > centerRange[0] or xCorrection < -centerRange[
0]: #If the hand is outside the acceptable range, changes the current speed to compensate.
xSpeed = xCorrection // 3
if yCorrection > centerRange[1] or yCorrection < -centerRange[1]:
ySpeed = -yCorrection // 3
self.tello.send_rc_control(xSpeed, 0, ySpeed, 0)
time.sleep(0.5)
self.tello.send_rc_control(0, 0, 0, 0)
def action_to_do(
self, fingers, orientation,
results): #use the variable results for the hand tracking control
if self.action_done == True:
self.ffoward = 1
self.fback = 1
self.fright = 1
self.fleft = 1
self.fsquare = 1
self.fmiddle = 1
self.fno = 1
self.fland = 1
self.action_done = False
#Left hand controls tricks, right hand controls movement
if orientation == "left hand": #Thumb on the left = left hand!
if fingers == [0, 1, 0, 0, 0]:
if self.ffoward >= 15:
self.action = "flip forward"
self.tello.flip_forward()
self.action_done = True
self.ffoward = self.ffoward + 1
elif fingers == [0, 1, 1, 0, 0] and self.battery == True:
if self.fback >= 15:
self.action = "flip back"
self.tello.flip_back()
self.action_done = True
self.fback = self.fback + 1
elif fingers == [1, 0, 0, 0, 0] and self.battery == True:
if self.fright >= 15:
self.action = "flip right"
self.tello.flip_right()
self.action_done = True
self.fright = self.fright + 1
elif fingers == [0, 0, 0, 0, 1] and self.battery == True:
if self.fleft >= 15:
self.action = "flip left"
self.tello.flip_left()
self.action_done = True
self.fleft = self.fleft + 1
elif fingers == [0, 1, 1, 1, 0]:
if self.fsquare >= 15:
self.action = "Square"
self.tello.move_left(20)
self.tello.move_up(40)
self.tello.move_right(40)
self.tello.move_down(40)
self.tello.move_left(20)
self.action_done = True
self.fsquare = self.fsquare + 1
elif fingers == [0, 0, 1, 0, 0]:
if self.fmiddle >= 15:
self.action = " :( "
self.tello.land()
self.action_done = True
self.fmiddle = self.fmiddle + 1
elif ((self.battery == False) and
(fingers == [1, 0, 0, 0, 0] or fingers == [0, 1, 0, 0, 0]
or fingers == [0, 0, 0, 0, 1])): #not avaiable to do tricks
if self.fno >= 15:
self.tello.rotate_clockwise(45)
self.tello.rotate_counter_clockwise(90)
self.tello.rotate_clockwise(45)
self.action_done = True
self.fno = self.fno + 1
else:
self.action = " "
elif orientation == "right hand": #Thumb on the right = right hand!
if fingers == [1, 1, 1, 1, 1]:
self.action = "Follow"
self.follow_hand(results)
elif fingers == [1, 0, 0, 0, 0]:
if self.fland >= 15:
self.action = "Land"
self.tello.land()
self.action_done = True
self.fland = self.fland + 1
else:
self.action = " "
def fingers_position(self, results, orientation):
# [thumb, index, middle finger, ring finger, pinky]
# 0 for closed, 1 for open
fingers = [0, 0, 0, 0, 0]
if (results.multi_hand_landmarks[0].landmark[4].x >
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "right hand":
fingers[0] = 0
if (results.multi_hand_landmarks[0].landmark[4].x <
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "right hand":
fingers[0] = 1
if (results.multi_hand_landmarks[0].landmark[4].x >
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "left hand":
fingers[0] = 1
if (results.multi_hand_landmarks[0].landmark[4].x <
results.multi_hand_landmarks[0].landmark[3].x
) and orientation == "left hand":
fingers[0] = 0
fingermarkList = [8, 12, 16, 20]
i = 1
for k in fingermarkList:
if results.multi_hand_landmarks[0].landmark[
k].y > results.multi_hand_landmarks[0].landmark[k - 2].y:
fingers[i] = 0
else:
fingers[i] = 1
i = i + 1
return fingers
def detection_loop(self):
with self.mp_hands.Hands(model_complexity=0,
min_detection_confidence=0.75,
min_tracking_confidence=0.5) as hands:
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while True:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
self.battery = self.tello.get_battery()
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
results = hands.process(image)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
if results.multi_hand_landmarks:
action = " "
for hand_landmarks in results.multi_hand_landmarks:
self.mp_drawing.draw_landmarks(
image, hand_landmarks,
self.mp_hands.HAND_CONNECTIONS,
self.mp_drawing_styles.
get_default_hand_landmarks_style(),
self.mp_drawing_styles.
get_default_hand_connections_style())
orientation = self.define_orientation(results)
fingers = self.fingers_position(results, orientation)
#print(fingers)
self.action_to_do(fingers, orientation, results)
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_l:
self.tello.land()
if event.key == pg.K_t:
self.tello.takeoff()
if event.key == pg.K_b:
print("A bateria esta em ",
self.tello.get_battery(), "%")
if event.key == pg.K_m:
return 0
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
def key_control(self):
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while True:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_w:
self.tello.move_forward(20)
if event.key == pg.K_a:
self.tello.move_left(20)
if event.key == pg.K_s:
self.tello.move_back(20)
if event.key == pg.K_d:
self.tello.move_right(20)
if event.key == pg.K_q:
self.tello.rotate_counter_clockwise(20)
if event.key == pg.K_e:
self.tello.rotate_clockwise(20)
if event.key == pg.K_SPACE:
self.tello.move_up(20)
if event.key == pg.K_LCTRL:
self.tello.move_down(20)
if event.key == pg.K_b:
print("A bateria esta em ", self.tello.get_battery(),
"%")
if event.key == pg.K_m:
return 0
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
def main_interface(self):
telloMode = -1
self.tello_startup()
pg.init()
win = pg.display.set_mode((500, 500))
pg.display.set_caption("Test")
#self.tello.takeoff()
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
self.tello.streamoff(
) # Ends the current stream, in case it's still opened
self.tello.streamon() # Starts a new stream
while telloMode != 0:
frame_read = self.tello.get_frame_read(
) # Stores the current streamed frame
image = frame_read.frame
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Draw the hand annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
if telloMode == 1:
self.key_control()
telloMode = -1
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
elif telloMode == 2:
self.detection_loop()
telloMode = -1
print("Para controlar pelo teclado, digite 1")
print("Para controlar com a mao, digite 2")
print("Para sair, digite 0")
elif telloMode == 0:
self.tello.land()
telloMode = -1
print("Obrigado por voar hoje")
elif telloMode != -1 and telloMode != 1 and telloMode != 2:
print("valor invalido!")
for event in pg.event.get():
if event.type == pg.KEYDOWN:
if event.key == pg.K_1:
telloMode = 1
if event.key == pg.K_2:
telloMode = 2
if event.key == pg.K_0:
telloMode = 0
if event.key == pg.K_l:
self.tello.land()
if event.key == pg.K_t:
self.tello.takeoff()
if event.key == pg.K_b:
print("A bateria esta em ", self.tello.get_battery(),
"%")
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 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
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 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()
me.streamon()
while True:
# GET THE IMAGE FROM TELLO
frame_read = me.get_frame_read()
myFrame = frame_read.frame
img = cv2.resize(myFrame, (width, height))
# TO GO UP IN THE BEGINNING
if startCounter == 0:
me.takeoff()
time.sleep(8)
me.rotate_clockwise(90)
time.sleep(3)
me.move_left(25)
time.sleep(3)
me.land()
startCounter = 1
# # SEND VELOCITY VALUES TO TELLO
# if me.send_rc_control:
# me.send_rc_control(me.left_right_velocity, me.for_back_velocity, me.up_down_velocity, me.yaw_velocity)
# DISPLAY IMAGE
cv2.imshow("MyResult", img)
# WAIT FOR THE 'Q' BUTTON TO STOP
if cv2.waitKey(1) & 0xFF == ord('g'):
me.land()
break
class DroneObject:
#initialize the object
#Set default state to IdleState()
#Create Tello object from the API
#set default values for coordinate, FPS, distance, and tilt
def __init__(self):
self.state = IdleState()
self.tello = Tello()
self.coordinate = (0, 0)
self.FPS = 30
self.distance = 30 #pre defined, to be changed later
self.tilt = 0
#Generates pass events to tellodrone class
def on_event(self, event):
self.state = self.state.on_event(event)
#setter for member variables
def set_parameter (self, x,y, dist, tilt):
self.coordinate = (x,y)
self.distance = dist
self.tilt = tilt
#for take off the drone, called when the drone is in takeoff state
#when take off is complete, trigger event for track
def take_off(self):
self.tello.takeoff()
for i in range (0,3):
print("taking off %d /3" % (i+1))
time.sleep(1)
self.on_event("track")
#for landing the drone, called when the drone is in landingstate
#when landing is complete, trigger event for idle
def land(self):
self.tello.land()
for i in range (0,3):
print("landing %d / 3" % (i+1))
time.sleep(1)
self.on_event("idle")
#for tracking the drone, called when the drone is in track state
#when tilt is active, it will prioritize the turning motion over the other motions
#controls shifting, moving up and down, forward backwards, and turning
def track(self):
if(self.tilt <= 0.95 and self.tilt != 0):
self.tello.rotate_clockwise(int((1-self.tilt)*100))
time.sleep(0.05)
elif(self.tilt >= 1.05):
self.tello.rotate_counter_clockwise(int((self.tilt-1)*100))
time.sleep(0.05)
else:
if (self.distance > 60):
forward = int((self.distance - 60))
if ((forward < 20)):
self.tello.move_forward(20)
else:
self.tello.move_forward(forward)
time.sleep(0.05)
elif (self.distance < 50):
backward = int(abs(self.distance - 50))
if ((backward < 20)):
self.tello.move_back(20)
else:
self.tello.move_back(backward)
time.sleep(0.05)
if (self.coordinate[0] < 400 and self.coordinate[0] >= 0):
self.tello.move_left(20)
time.sleep(0.05)
elif (self.coordinate[0] < 959 and self.coordinate[0] >= 559):
self.tello.move_right(20)
time.sleep(0.05)
if (self.coordinate[1] > 0 and self.coordinate[1] <= 200):
self.tello.move_up(20)
time.sleep(0.05)
elif (self.coordinate[1] >= 519 and self.coordinate[1] < 719):
self.tello.move_down(20)
time.sleep(0.05)
#For setting up the drone
#Establish connection
#Initialize streamming
#Display battery
def setup(self):
if not self.tello.connect():
print("Tello not connected")
sys.exit()
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
sys.exit()
if not self.tello.streamon():
print("Could not start video stream")
sys.exit()
print("Current battery is " + self.tello.get_battery())
self.tello.streamon()
frame_read = self.tello.get_frame_read()
#For calling corresponding functions based on their state
def action(self):
print("current state is" , self.state)
print(str(self.state))
if (str(self.state) == "TakeOffState"):
print("take off!")
self.take_off()
elif (str(self.state) == "LandState"):
print("land")
self.land()
if (str(self.state)== "TrackState"):
self.track()
else:
return #idle state or undefined state do nothing
return
class 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):
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)
from djitellopy import Tello tello = Tello() tello.connect() tello.takeoff() tello.move_left(100) tello.rotate_clockwise(90) tello.move_forward(100) tello.land()
def demo():
# Setup for arucodes
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters_create()
# Initialise
tello = Tello()
tello.connect()
# Wait for video stream
ok = tello.streamon()
frame_read = tello.get_frame_read()
while not frame_read.grabbed:
frame_read = tello.get_frame_read()
img = frame_read.frame
cv2.imshow("video", img)
k = cv2.waitKey(10) & 0xff
# Take off
print(
f"Take off. Height: {tello.get_height()}, Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}"
)
# Start the main loop
keep_going = True
left_right_velocity = 0
forward_back = 0
up_down = 0
rotate = 0
while keep_going:
frame_read = tello.get_frame_read()
if frame_read.grabbed:
# Get frame
img = frame_read.frame
height, width, channels = img.shape
# Check for arucodes
corners, ids, rejectedImgPoints = aruco.detectMarkers(
img, aruco_dict, parameters=parameters)
arucodes = AruCode.parse_ids_and_corners(ids, corners)
delta = 10
up_down = 0
rotate = 0
forward_back = 0
if 0 in arucodes.keys():
cv2.rectangle(img, (0, 0), (width, height), (255, 0, 0),
100) # Color us BGR
keep_going = False
elif 1 in arucodes.keys():
cv2.rectangle(img, (0, 0), (width, height), (0, 0, 0),
100) # Color us BGR
followme = arucodes[1]
x_percentile = int(100 * followme.x / width)
y_percentile = int(100 * followme.y / height)
if y_percentile < 40:
cv2.rectangle(img, (0, 0), (width, 0), (0, 255, 0),
100) # Color us BGR
print("move up", time.time())
up_down = 30
elif y_percentile > 60:
cv2.rectangle(img, (0, height), (width, height),
(0, 255, 0), 100) # Color us BGR
print("move down", time.time())
up_down = -30
if x_percentile < 40:
cv2.rectangle(img, (0, 0), (0, height), (0, 255, 0),
100) # Color us BGR
print("rotate ccw", time.time())
rotate = -40
elif x_percentile > 60:
cv2.rectangle(img, (width, 0), (width, height),
(0, 255, 0), 100) # Color us BGR
print("rotate cw", time.time())
rotate = 40
if followme.diagonal < 150:
cv2.circle(img, (int(width / 2), int(height / 2)), 50,
(0, 255, 0), -1)
print("forward", followme.w, time.time())
forward_back = 40
else:
print("stop", time.time())
forward_back = 0
else: # No valid arucode found
cv2.rectangle(img, (0, 0), (width, height), (0, 0, 255),
100) # Color us BGR
# Preview the image on screen
cv2.imshow("video", img)
# Wait 30 milliseconds for a keyboard press
k = cv2.waitKey(10) & 0xff
# If ESC key pressed, close
if k == 27:
keep_going = False
elif k == ord('0'):
keep_going = False
elif k == ord('1'):
tello.takeoff()
elif k == ord('+') or k == ord('='):
up_down = 50
elif k == ord('-'):
up_down = -50
elif k == ord('s'):
forward_back = 0
left_right_velocity = 0
rotate = 0
up_down = 0
elif k == ord('a'):
rotate = -90
elif k == ord('d'):
rotate = 90
elif k == ord('w'):
forward_back = 100
elif k == ord('x'):
forward_back = -100
elif k == ord('q'):
left_right_velocity = -100
elif k == ord('e'):
left_right_velocity = 100
# Send to RC
print(
f"tello: forward: {forward_back}, up: {up_down}, yaw: {rotate}"
)
tello.send_rc_control(left_right_velocity=left_right_velocity,
forward_backward_velocity=forward_back,
up_down_velocity=up_down,
yaw_velocity=rotate)
print(
f"Landing. Height: {tello.get_height()}, Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}"
)
try:
tello.land()
tello.end()
except:
print("Unable to land without error")
print(
f"Power off. Battery: {tello.get_battery()}, Temp: {tello.get_temperature()}"
)
cv2.destroyAllWindows()
