def main():
tello = Tello()
tello.connect()
print(f"battery level: {tello.get_battery()}")
print(f"temp: {tello.get_temperature()}")
tello.takeoff()
tello.move_left(50)
tello.move_right(50)
tello.rotate_clockwise(360)
print(f"Waiting for 1 secs")
time.sleep(1)
tello.flip_left()
tello.land()
tello.end()
class DroneUI(object):
def __init__(self):
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.mode = PMode.NONE # Can be '', 'FIND', 'OVERRIDE' or 'FOLLOW'
self.send_rc_control = False
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
if args.cat == 'any':
print('Using CatDetectionModel')
self.model = CatDetectionModel(0.5)
else:
print('Using MyCatsDetectionModel ({})'.format(args.cat))
self.model = MyCatsDetectionModel(0.5)
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
DETECT_ENABLED = False # Set to true to automatically start in follow mode
self.mode = PMode.NONE
self.tello.get_battery()
# Safety Zone X
szX = args.saftey_x
# Safety Zone Y
szY = args.saftey_y
if args.debug: print("DEBUG MODE ENABLED!")
while not should_stop:
frame_time_start = time.time()
# self.update() # Moved to the end before sleep to have more accuracy
if frame_read.stopped:
frame_read.stop()
self.update() ## Just in case
break
print('---')
# TODO: Analize if colors have to be tweaked
frame = cv2.flip(frame_read.frame,
0) # Vertical flip due to the mirror
frameRet = frame.copy()
vid = self.tello.get_video_capture()
imgCount += 1
#time.sleep(1 / FPS)
# Listen for key presses
k = cv2.waitKey(20)
try:
if chr(k) in 'ikjluoyhp': OVERRIDE = True
except:
...
if k == ord('e'):
DETECT_ENABLED = True
elif k == ord('d'):
DETECT_ENABLED = False
# Press T to take off
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
if k == ord('s') and self.send_rc_control == True:
self.mode = PMode.FIND
DETECT_ENABLED = True # To start following with autopilot
OVERRIDE = False
print('Switch to spiral mode')
# This is temporary, follow mode should start automatically
if k == ord('f') and self.send_rc_control == True:
DETECT_ENABLED = True
OVERRIDE = False
print('Switch to follow mode')
# Press L to land
if k == ord('g'):
self.land_and_set_none()
# self.update() ## Just in case
# break
# Press Backspace for controls override
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
# Quit the software
if k == 27:
should_stop = True
self.update() ## Just in case
break
autoK = -1
if k == -1 and self.mode == PMode.FIND:
if not OVERRIDE:
autoK = next_auto_key()
if autoK == -1:
self.mode = PMode.NONE
print('Queue empty! no more autokeys')
else:
print('Automatically pressing ', chr(autoK))
key_to_process = autoK if k == -1 and self.mode == PMode.FIND and OVERRIDE == False else k
if self.mode == PMode.FIND and not OVERRIDE:
#frame ret will get the squares drawn after this operation
if self.process_move_key_andor_square_bounce(
key_to_process, frame, frameRet) == False:
# If the queue is empty and the object hasn't been found, land and finish
self.land_and_set_none()
#self.update() # Just in case
break
else:
self.process_move_key(key_to_process)
dCol = (0, 255, 255)
#detected = False
if not OVERRIDE and self.send_rc_control and DETECT_ENABLED:
self.detect_subjects(frame, frameRet, szX, szY)
show = ""
if OVERRIDE:
show = "MANUAL"
dCol = (255, 255, 255)
elif self.mode == PMode.FOLLOW or self.mode == PMode.FLIP:
show = "FOUND!!!"
elif self.mode == PMode.FIND:
show = "Finding.."
mode_label = 'Mode: {}'.format(self.mode)
# Draw the distance choosen
cv2.putText(frameRet, mode_label, (32, 664),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
cv2.putText(frameRet, show, (32, 600), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# Display the resulting frame
cv2.imshow('FINDER DRONE', frameRet)
if (self.mode == PMode.FLIP):
self.flip()
# OVERRIDE = True
self.update(
) # Moved here instead of beginning of loop to have better accuracy
frame_time = time.time() - frame_time_start
sleep_time = 1 / FPS - frame_time
if sleep_time < 0:
sleep_time = 0
print('SLEEEP TIME NEGATIVE FOR FRAME {} ({}s).. TURNING IT 0'.
format(imgCount, frame_time))
if args.save_session and self.send_rc_control == True: # To avoid recording before takeoff
self.create_frame_files(frame, frameRet, imgCount)
time.sleep(sleep_time)
# On exit, print the battery
self.tello.get_battery()
# When everything done, release the capture
# cv2.destroyWindow('FINDER DRONE')
# cv2.waitKey(0)
cv2.destroyAllWindows()
# Call it always before finishing. I deallocate resources.
self.tello.end()
def create_frame_files(self, frame, frameRet, imgCount):
def create_frame_file(image, subdir, print_log=False):
global ddir
path = ddir + '/' + subdir
if not os.path.exists(path): os.makedirs(path)
filename = "{}/tellocap{}.jpg".format(path, imgCount)
if print_log: print('Created {}'.format(filename))
cv2.imwrite(filename, image)
create_frame_file(frame, 'raw')
create_frame_file(frameRet, 'output', True)
def flip(self):
print('Flip!')
self.left_right_velocity = self.for_back_velocity = 0
self.update()
time.sleep(self.tello.TIME_BTW_COMMANDS * 2)
if not args.debug:
self.tello.flip_left()
#self.tello.flip_right()
# The following 2 lines allow going back to follow mode
self.mode = PMode.FOLLOW
global onFoundAction
onFoundAction = PMode.FOLLOW # So it doesn't flip over and over
def land_and_set_none(self):
if not args.debug:
print("------------------Landing--------------------")
self.tello.land()
self.send_rc_control = False
self.mode = PMode.NONE # TODO: Consider calling reset
def oq_discard_keys(self, keys_to_pop):
oq = globals.mission.operations_queue
keys_to_pop += 'p'
while len(oq) > 0:
oqkey = oq[0]['key']
if oqkey in keys_to_pop:
print('Removing {} from queue'.format(oqkey))
oq.popleft()
else:
break
def process_move_key_andor_square_bounce(self, k, frame, frameRet=None):
self.process_move_key(k) # By default use key direction
(hor_dir, ver_dir) = get_squares_push_directions(frame, frameRet)
print('(hor_dir, ver_dir): ({}, {})'.format(hor_dir, ver_dir))
oq = globals.mission.operations_queue
print('operations_queue len: ', len(oq))
keys_to_pop = ''
if ver_dir == 'forward':
self.for_back_velocity = int(S)
if k != ord('i'): print('Square pushing forward')
keys_to_pop += 'k'
elif ver_dir == 'back':
self.for_back_velocity = -int(S)
if k != ord('k'): print('Square pushing back')
keys_to_pop += 'i'
if hor_dir == 'right':
self.left_right_velocity = int(S)
if k != ord('l'): print('Square pushing right')
keys_to_pop += 'j'
elif hor_dir == 'left':
self.left_right_velocity = -int(S)
if k != ord('j'): print('Square pushing left')
keys_to_pop += 'l'
if (len(keys_to_pop) > 0):
self.oq_discard_keys(keys_to_pop)
return (len(oq) > 0)
def process_move_key(self, k):
# i & k to fly forward & back
if k == ord('i'):
self.for_back_velocity = int(S)
elif k == ord('k'):
self.for_back_velocity = -int(S)
else:
self.for_back_velocity = 0
# o & u to pan left & right
if k == ord('o'):
self.yaw_velocity = int(S)
elif k == ord('u'):
self.yaw_velocity = -int(S)
else:
self.yaw_velocity = 0
# y & h to fly up & down
if k == ord('y'):
self.up_down_velocity = int(S)
elif k == ord('h'):
self.up_down_velocity = -int(S)
else:
self.up_down_velocity = 0
# l & j to fly left & right
if k == ord('l'):
self.left_right_velocity = int(S)
elif k == ord('j'):
self.left_right_velocity = -int(S)
else:
self.left_right_velocity = 0
# p to keep still
if k == ord('p'):
print('pressing p')
def show_save_detection(self, frame, frameRet, firstDetection):
output_filename_det_full = "{}/detected_full.jpg".format(ddir)
cv2.imwrite(output_filename_det_full, frameRet)
print('Created {}'.format(output_filename_det_full))
(x, y, w, h) = firstDetection['box']
add_to_borders = 100
(xt, yt) = (x + w + add_to_borders, y + h + add_to_borders)
(x, y) = (max(0, x - add_to_borders), max(0, y - add_to_borders))
# subframeRet = frameRet[y:yt, x:xt].copy()
subframe = frame[y:yt, x:xt].copy()
def show_detection():
output_filename_det_sub = "{}/detected_sub.jpg".format(ddir)
cv2.imwrite(output_filename_det_sub, subframe)
print('Created {}'.format(output_filename_det_sub))
# Shows detection in a window. If it doesn't exist yet, waitKey
waitForKey = cv2.getWindowProperty('Detected',
0) < 0 # True for first time
cv2.imshow('Detected', subframe)
if waitForKey: cv2.waitKey(0)
Timer(0.5, show_detection).start()
def detect_subjects(self, frame, frameRet, szX, szY):
detections = self.model.detect(frameRet)
# print('detections: ', detections)
self.model.drawDetections(frameRet, detections)
class_wanted = 0 if args.cat == 'any' else self.model.LABELS.index(
args.cat)
detection = next(
filter(lambda d: d['classID'] == class_wanted, detections), None)
isSubjectDetected = not detection is None
if isSubjectDetected:
print('{} FOUND!!!!!!!!!!'.format(self.model.LABELS[class_wanted]))
#if self.mode != onFoundAction: # To create it only the first time
self.mode = onFoundAction
# if we've given rc controls & get object coords returned
# if self.send_rc_control and not OVERRIDE:
if self.mode == PMode.FOLLOW:
self.follow(detection, frameRet, szX, szY)
self.show_save_detection(frame, frameRet, detection)
elif self.mode == onFoundAction:
# if there are no objects detected, don't do anything
print("CAT NOT DETECTED NOW")
return isSubjectDetected
def follow(self, detection, frameRet, szX, szY):
print('Following...')
# These are our center dimensions
(frame_h, frame_w) = frameRet.shape[:2]
cWidth = int(frame_w / 2)
cHeight = int(frame_h / 2)
(x, y, w, h) = detection['box']
# end coords are the end of the bounding box x & y
end_cord_x = x + w
end_cord_y = y + h
# This is not face detection so we don't need offset
UDOffset = 0
# these are our target coordinates
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# This calculates the vector from the object to the center of the screen
vTrue = np.array((cWidth, cHeight))
vTarget = np.array((targ_cord_x, targ_cord_y))
vDistance = vTrue - vTarget
if True or not args.debug:
if vDistance[0] < -szX:
# Right
self.left_right_velocity = S
elif vDistance[0] > szX:
# Left
self.left_right_velocity = -S
else:
self.left_right_velocity = 0
# for up & down
if vDistance[1] > szY:
self.for_back_velocity = S
elif vDistance[1] < -szY:
self.for_back_velocity = -S
else:
self.for_back_velocity = 0
# Draw the center of screen circle, this is what the drone tries to match with the target coords
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
# Draw the target as a circle
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10, (0, 255, 0), 2)
# Draw the safety zone
obStroke = 2
cv2.rectangle(frameRet, (targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY), (0, 255, 0),
obStroke)
# Draw the estimated drone vector position in relation to object bounding box
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
print('Sending speeds to tello. H: {} V: {}'.format(
self.left_right_velocity, self.for_back_velocity))
if not args.debug:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class TelloCV(object):
"""
TelloTracker builds keyboard controls on top of TelloPy as well
as generating images from the video stream and enabling opencv support
"""
def __init__(self):
self.prev_flight_data = None
self.record = False
self.tracking = False
self.keydown = False
self.date_fmt = '%Y-%m-%d_%H%M%S'
self.speed = 50
self.go_speed = 80
self.drone = Tello()
self.init_drone()
self.init_controls()
self.battery = self.drone.get_battery()
self.frame_read = self.drone.get_frame_read()
self.forward_time = 0
self.forward_flag = True
self.takeoff_time = 0
self.command_time = 0
self.command_flag = False
# trackingimport libh264decoder a color
# green_lower = (30, 50, 50)
# green_upper = (80, 255, 255)
# red_lower = (0, 50, 50)
# red_upper = (20, 255, 255)
blue_lower = np.array([0, 0, 0])
upper_blue = np.array([255, 255, 180])
bh_lower = (180, 30, 100)
bh_upper = (275, 50, 100)
self.track_cmd = ""
self.tracker = Tracker(960,
720,
blue_lower, upper_blue)
self.speed_list = [5, 10, 15, 20, 25]
self.frame_center = 480, 360
self.error = 60
def init_drone(self):
"""Connect, uneable streaming and subscribe to events"""
# self.drone.log.set_level(2)
self.drone.connect()
self.drone.streamon()
def on_press(self, keyname):
"""handler for keyboard listener"""
if self.keydown:
return
try:
self.keydown = True
keyname = str(keyname).strip('\'')
print('+' + keyname)
if keyname == 'Key.esc':
self.drone.quit()
exit(0)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(self.speed)
else:
key_handler(self.speed)
except AttributeError:
print('special key {0} pressed'.format(keyname))
def on_release(self, keyname):
"""Reset on key up from keyboard listener"""
self.keydown = False
keyname = str(keyname).strip('\'')
print('-' + keyname)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(0)
else:
key_handler(0)
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': lambda speed: self.drone.move_forward(speed),
's': lambda speed: self.drone.move_back(speed),
'a': lambda speed: self.drone.move_left(speed),
'd': lambda speed: self.drone.move_right(speed),
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.rotate_counter_clockwise(speed),
'Key.right': lambda speed: self.drone.rotate_clockwise(speed),
'Key.up': lambda speed: self.drone.move_up(speed),
'Key.down': lambda speed: self.drone.move_down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
# 'Key.tab': self.drone.takeoff(60),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
# self.key_listener.join()
def process_frame(self):
"""convert frame to cv2 image and show"""
# print("TRACKING START")
frame = self.frame_read.frame
# self.drone.move_up(self.speed)
# image = self.write_hud(image)
# if self.record:
# self.record_vid(frame)
return frame
def move_up(self):
self.drone.move_up(self.speed)
def take_off(self):
self.drone.takeoff()
def go(self):
self.drone.move_forward(self.go_speed)
def move_left(self):
self.drone.move_left(270) # speed 테스트해서 조절하기
def go_window9(self):
self.drone.move_forward()
def rotate_right(self):
self.drone.rotate_clockwise()
def rotate_left(self):
self.drone.rotate_counter_clockwise()
def landing(self):
self.drone.land()
def write_hud(self, frame):
"""Draw drone info, tracking and record on frame"""
stats = self.prev_flight_data.split('|')
stats.append("Tracking:" + str(self.tracking))
if self.drone.zoom:
stats.append("VID")
else:
stats.append("PIC")
if self.record:
diff = int(time.time() - self.start_time)
mins, secs = divmod(diff, 60)
stats.append("REC {:02d}:{:02d}".format(mins, secs))
for idx, stat in enumerate(stats):
text = stat.lstrip()
cv2.putText(frame, text, (0, 30 + (idx * 30)),
cv2.FONT_HERSHEY_SIMPLEX,
1.0, (255, 0, 0), lineType=30)
return frame
def toggle_recording(self, speed):
"""Handle recording keypress, creates output stream and file"""
if speed == 0:
return
self.record = not self.record
if self.record:
datename = [os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)]
self.out_name = '{}/Pictures/tello-{}.mp4'.format(*datename)
print("Outputting video to:", self.out_name)
self.out_file = av.open(self.out_name, 'w')
self.start_time = time.time()
self.out_stream = self.out_file.add_stream(
'mpeg4', self.vid_stream.rate)
self.out_stream.pix_fmt = 'yuv420p'
self.out_stream.width = self.vid_stream.width
self.out_stream.height = self.vid_stream.height
if not self.record:
print("Video saved to ", self.out_name)
self.out_file.close()
self.out_stream = None
def record_vid(self, frame):
"""
convert frames to packets and write to file
"""
new_frame = av.VideoFrame(
width=frame.width, height=frame.height, format=frame.format.name)
for i in range(len(frame.planes)):
new_frame.planes[i].update(frame.planes[i])
pkt = None
try:
pkt = self.out_stream.encode(new_frame)
except IOError as err:
print("encoding failed: {0}".format(err))
if pkt is not None:
try:
self.out_file.mux(pkt)
except IOError:
print('mux failed: ' + str(pkt))
def take_picture(self, speed):
"""Tell drone to take picture, image sent to file handler"""
if speed == 0:
return
self.drone.take_picture()
def palm_land(self, speed):
"""Tell drone to land"""
if speed == 0:
return
self.drone.palm_land()
def toggle_tracking(self, speed):
""" Handle tracking keypress"""
if speed == 0: # handle key up event
return
self.tracking = not self.tracking
print("tracking:", self.tracking)
return
def toggle_zoom(self, speed):
"""
In "video" mode the self.drone sends 1280x720 frames.
In "photo" mode it sends 2592x1936 (952x720) frames.
The video will always be centered in the window.
In photo mode, if we keep the window at 1280x720 that gives us ~160px on
each side for status information, which is ample.
Video mode is harder because then we need to abandon the 16:9 display size
if we want to put the HUD next to the video.
"""
if speed == 0:
return
self.drone.set_video_mode(not self.drone.zoom)
def flight_data_handler(self, event, sender, data):
"""Listener to flight data from the drone."""
text = str(data)
if self.prev_flight_data != text:
self.prev_flight_data = text
def handle_flight_received(self, event, sender, data):
"""Create a file in ~/Pictures/ to receive image from the drone"""
path = '%s/Pictures/tello-%s.jpeg' % (
os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt))
with open(path, 'wb') as out_file:
out_file.write(data)
print('Saved photo to %s' % path)
def enable_mission_pads(self):
self.drone.enable_mission_pads()
def disable_mission_pads(self):
self.drone.disable_mission_pads()
def go_xyz_speed_mid(self, x, y, z, speed, mid):
self.drone.go_xyz_speed_mid(x, y, z, speed, mid)
# if function return True, set drone center to object's center
def track_mid(self, x, y):
midx, midy = 480, 360
distance_x = abs(midx - x)
distance_y = abs(midy - y)
print(x, y, distance_x, distance_y)
move_done = True
if y > midy + self.error + 15:
self.drone.move_down(20)
move_done = False
elif y < midy - self.error + 5:
self.drone.move_up(20)
move_done = False
elif x < midx - self.error:
self.drone.move_left(20)
move_done = False
elif x > midx + self.error:
self.drone.move_right(20)
move_done = False
return move_done
def track_x(self, x, left_count, right_count):
midx = 480
move_done = True
if x < midx - 100:
self.drone.move_left(20)
left_count += 1
move_done = False
elif x > midx + 100:
self.drone.move_right(20)
right_count += 1
move_done = False
return move_done
def go_slow(self):
self.drone.move_forward(30)
def go_fast(self):
self.drone.move_forward(200)
class 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 DroneControl():
def __init__(self):
self.drone = Tello() # Instantiate a Tello Object
self.drone.connect() # Connect to the drone
self.drone.streamoff() # In case stream never exited before
self.drone.streamon() # Turn on drone camera stream
self.timer = 0 # Timing for printing statements
self.flying = False # Keep track of flying state
# How many video frames have been requested
self.frame_count = 0
self.init_frames = 50 # Begin flying only after init_frames
self.myThread = threading.Thread(target=self.fly)
def __del__(self):
self.drone.streamoff()
def get_state(self):
self.drone.get_battery()
self.drone.get_speed_x
self.drone.get_speed_y
self.drone.get_speed_z
self.drone.get_height
self.drone.get_flight_time
self.drone.get_temperature
self.drone.get_yaw
self.drone.get_roll
self.drone.get_pitch
self.drone.get_barometer
self.drone.get_acceleration_x
self.drone.get_acceleration_y
self.drone.get_acceleration_z
self.drone.get_distance_tof
self.drone.query_wifi_signal_noise_ratio
####################################################
############## ADD YOUR CODE HERE ##################
####################################################
def fly(self):
#time.sleep(5)
#self.get_state()
#time.sleep(5)
#self.drone.takeoff()
#time.sleep(5)
#self.drone.move_left(50)
#time.sleep(5)
#self.drone.rotate_clockwise(90)
#time.sleep(5)
#self.drone.rotate_clockwise(180)
#time.sleep(5)
#self.drone.land()
#################################################
##################Challenge 1####################
#################################################
time.sleep(5)
self.get_state()
time.sleep(5)
self.drone.takeoff()
time.sleep(5)
self.drone.move_left(50)
time.sleep(5)
self.drone.move_back(50)
time.sleep(5)
self.drone.move_right(50)
time.sleep(5)
self.drone.move_forward(50)
time.sleep(5)
self.drone.land()
#################################################
###################Challenge 2###################
#################################################
time.sleep(5)
self.drone.takeoff()
time.sleep(10)
self.drone.move_up(40)
time.sleep(5)
self.drone.move_forward(50)
time.sleep(5)
self.drone.rotate_counter_clockwise(45)
time.sleep(5)
self.drone.move_forward(50)
time.sleep(5)
self.drone.rotate_counter_clockwise(70)
time.sleep(5)
self.drone.flip_left()
time.sleep(5)
self.drone.move_down(25)
time.sleep(5)
self.drone.flip_right()
time.sleep(5)
self.drone.flip_right()
time.sleep(5)
self.drone.move_back(35)
time.sleep(5)
self.drone.flip_back()
time.sleep(5)
self.get_state()
time.sleep(5)
curve_xyz_speed(-50, -50, 0, -200, -200, 0, 10)
time.sleep(5)
self.rotate_clockwise(115)
time.sleep(5)
self.drone.land()
####################################################
############## ADD YOUR CODE HERE ##################
####################################################
def get_frame(self):
# only begin flying once a video feed is established
self.frame_count += 1
if self.flying == False and self.frame_count > self.init_frames:
self.flying = True
# self.fly()
self.myThread.start()
# Grab a frame and resize it
frame_read = self.drone.get_frame_read()
if frame_read.stopped:
return
frame = cv2.resize(frame_read.frame, (360, 240))
# encode OpenCV raw frame to jpeg
ret, jpeg = cv2.imencode('.jpg', frame)
return jpeg.tobytes()
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()
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Create pygame window
pygame.display.set_caption("LaserCat View - Battery: %s" %
self.tello.get_battery())
self.screen = pygame.display.set_mode([960, 720])
# 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
update_bat_counter = 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)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
update_bat_counter += 1
if (update_bat_counter > 249):
pygame.display.set_caption("LaserCat View - Battery: %s" %
self.tello.get_battery())
update_bat_counter = 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.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
elif key == pygame.K_f: #flip
self.tello.flip_left()
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)
# TO GO UP IN THE BEGINNING
if startCounter == 0:
me.takeoff()
time.sleep(5)
me.move_back(160)
time.sleep(5)
me.rotate_counter_clockwise(90)
time.sleep(5)
me.flip_forward()
time.sleep(5)
me.flip_forward()
time.sleep(5)
me.flip_right()
time.sleep(5)
me.flip_left()
time.sleep(5)
me.flip_back()
time.sleep(5)
me.flip_back()
#me.move_forward(195)
#time.sleep(5)
#me.rotate_clockwise(90)
#time.sleep(5)
#me.move_forward(100)
#time.sleep(5)
#me.rotate_counter_clockwise(90)
#time.sleep(5)
#me.move_forward(100)
#
#time.sleep(5)
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
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 action_to_do(
self, fingers, orientation,
results): #use the variable results for the hand tracking control
#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]:
self.action = "flip forward"
self.tello.flip_forward()
elif fingers == [0, 1, 1, 0, 0]:
self.action = "flip back"
self.tello.flip_back()
elif fingers == [1, 0, 0, 0, 0]:
self.action = "flip right"
self.tello.flip_right()
elif fingers == [0, 0, 0, 0, 1]:
self.action = "flip left"
self.tello.flip_left()
elif fingers == [0, 1, 1, 1, 0]:
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)
elif fingers == [0, 0, 1, 0, 0]:
self.action = " :( "
self.tello.land()
else:
self.action = " "
elif orientation == "right hand": #Thumb on the right = right hand!
if fingers == [0, 1, 0, 0, 0]:
self.action = "Move up"
self.tello.move_up(20)
elif fingers == [1, 0, 0, 0, 0]:
self.action = "Move left"
self.tello.move_left(20)
elif fingers == [0, 0, 0, 0, 1]:
self.action = "Move right"
self.tello.move_right(20)
elif fingers == [0, 1, 1, 0, 0]:
self.action = "Move down"
self.tello.move_down(20)
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
# 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)
#self.battery = self.tello.query_battery()
#if 75 <= self.battery <= 100:
#color = (0, 150, 0) # Green (BGR)
#elif 50 <= self.battery < 75:
#color = (0, 255, 255) # Yellow
#elif 0 <= self.battery < 50:
#color = (0, 0, 255) # Red
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)
cv2.putText(
image,
f'Action: {str(self.action)}',
(10, 70),
cv2.FONT_HERSHEY_PLAIN,
3,
(100, 100, 255),
3,
)
#cv2.putText(image, f'Battery: {str(self.battery)}%', (10, 450), cv2.FONT_HERSHEY_PLAIN, 3, color, 3, )
cv2.imshow("image", image)
if cv2.waitKey(5) & 0xFF == 27:
break
def main_interface(self):
self.tello_startup()
self.tello.takeoff()
self.detection_loop()
tello = Tello()
tello.connect()
try:
# Despegue
tello.takeoff()
time.sleep(wait_time_sec)
# Se mueve 40cm a la izquierda y luego 40cm a la derecha
tello.move_left(40)
time.sleep(wait_time_sec)
tello.move_right(40)
time.sleep(wait_time_sec)
# Hacer un gripo de 360º sobre si mismo a baja velocidad
i = 0
while i < 4:
tello.rotate_clockwise(90)
time.sleep(wait_time_sec)
i = i + 1
# Hacer uno de los flips que vinen preprogramados
tello.flip_left()
time.sleep(wait_time_sec)
finally:
# Aterrizar
tello.land()
