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