def cameraCalibration(filename):
CALI_IMG_NUM = 50
# termination criteria for cv2.cornerSubPix
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(8,5,0) // (x, y, 0)
objp = np.zeros((9 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
drone = Tello()
drone.connect()
drone.streamon()
drone.get_frame_read()
# cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
# capturing img for calibration
print('Capturing...')
cnt = 0
while (True):
frame = drone.background_frame_read.frame
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
if ret == True:
print(f'chessboard_{cnt}')
cnt += 1
if cnt > CALI_IMG_NUM:
break
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners2)
# Draw and display the corners
frame = cv2.drawChessboardCorners(frame, (9, 6), corners2, ret)
cv2.imshow('chessboard', frame)
key = cv2.waitKey(200)
cv2.destroyAllWindows()
# calibration
print('calibrating...')
ret, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None)
# shape[::-1] : reversed slicing
# refine parameters
h, w = 720, 960 # telloCam shape
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoeffs,
(w, h), 0, (w, h))
f = cv2.FileStorage(filename, cv2.FILE_STORAGE_WRITE)
f.write("intrinsic", cameraMatrix)
f.write("distortion", distCoeffs)
f.write("newcameramtx", newcameramtx)
f.release()
print('calibration finished!!')
exit()
class DroneCamera():
def __init__(self):
self.drone = Tello() # Instantiate a Tello Object
self.drone.connect() # Connect to the drone
self.drone.streamoff() # In case stream never exited before
self.drone.streamon() # Turn on drone camera stream
time.sleep(5) # Give the stream time to start up
self.timer = 0 # Timing for printing statements
def __del__(self):
self.drone.streamoff()
def get_frame(self):
# Grab a frame and resize it
frame_read = self.drone.get_frame_read()
if frame_read.stopped:
return None
frame = cv2.resize(frame_read.frame, (360, 240))
# Print battery status to the log every 10 seconds
if (time.time() - self.timer > 10):
self.timer = time.time()
self.drone.get_battery()
# encode OpenCV raw frame to jpeg
ret, jpeg = cv2.imencode('.jpg', frame)
return jpeg.tobytes()
def run():
tello = Tello()
counter = 0
if not 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 tello.streamoff():
print("Could not stop video stream")
return
if not tello.streamon():
print("Could not start video stream")
return
frame_read = tello.get_frame_read()
while (True):
# Our operations on the frame come here
frame = frame_read.frame
time.sleep(10)
cv2.imwrite('frame-{}.png'.format(counter), frame_read.frame)
counter += 1
# Display the resulting frame
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
def main(args):
tello = Tello()
tello.connect()
tello.streamon()
# Create directory to save images if it doesn't exists
if args.save_img:
timestamp = str(time.time())
save_dir = Path(f"{args.save_dir}") / Path(timestamp)
save_dir.mkdir(parents=True, exist_ok=True)
fps_delay_ms = int((1 / args.fps) * 1000)
save_frame_count = 0
cv2.namedWindow("tello")
while True:
key = cv2.waitKey(fps_delay_ms)
if key & 0xFF == ord("q"):
# Exit if q pressed
cv2.destroyAllWindows()
break
img = tello.get_frame_read().frame
if img is not None:
# Show the image
cv2.imshow("tello", img)
# Save the images
if args.save_img:
cv2.imwrite(f"{str(save_dir)}/{save_frame_count:07d}.png", img)
save_frame_count += 1
class Config:
def __init__(self):
self.time_of_initialization = time.time()
self.props = self.read_properties()
self.use_drone = True
self.drone_flying = False
self.drone = None
self.calculate_additional_props()
if self.use_drone:
self.connect_to_drone()
else:
self.cap = cv2.VideoCapture(0)
def connect_to_drone(self):
self.drone = Tello()
self.drone.connect()
self.drone.streamon()
self.cap = self.drone.get_frame_read()
self.drone_takeoff()
self.drone.move_up(100)
def drone_respond_ready(self):
return time.time(
) - self.time_of_initialization > self.props["drone"]["respond_delay"]
def frame(self):
if self.use_drone:
frame = self.cap.frame
else:
ret, frame = self.cap.read()
return frame
def stop_streaming(self):
if self.use_drone:
self.drone.streamoff()
else:
self.cap.release()
def drone_emergency_stop(self):
self.drone.emergency()
self.drone_flying = False
def drone_takeoff(self):
self.drone.takeoff()
self.drone_flying = True
def read_properties(self):
with open('./config/properties.json') as json_file:
props = json.load(json_file)
return props
def calculate_additional_props(self):
self.props["img"]["height"] = 300
self.props["img"]["frame_center_x"] = self.props["img"]["width"] * 0.5
self.props["img"]["frame_center_y"] = self.props["img"]["height"] * 0.5
def connect(self):
ip_address = self.config['general']['ip_address']
tello = Tello(ip_address)
# tello = Tello('192.168.10.2'); y_ref = +30
# tello = Tello('192.168.10.4'); y_ref = -30
if not tello.connect():
print("Tello not connected")
exit(1)
if not tello.set_speed(int(self.config['pid']['velocity'])):
print("Not set speed to lowest possible")
exit(1)
# In case streaming is on. This happens when we quit this program without the escape key.
if not tello.streamoff():
print("Could not stop video stream")
exit(1)
if not tello.streamon():
print("Could not start video stream")
exit(1)
frame_read = tello.get_frame_read()
# ok = tello.connect()
#
# # In case streaming is on. This happens when we quit this program without the escape key.
# ok = tello.streamoff()
#
# ok = tello.streamon()
#
# ok = tello.set_speed(SPEED)
#
# frame_read = tello.get_frame_read()
if self.takeoff:
time.sleep(3)
ok = tello.takeoff()
time.sleep(3)
self.time_received_last_command = time.time()
self.tello = tello
self.frame_read = frame_read
class FrontEnd(object):
def __init__(self, window_width, window_height):
# Init pygame
pygame.init()
# Creat pygame window
self.screen = pygame.display.set_mode([window_width,
window_height]) # [960, 720]
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# create update timer
#pygame.time.set_timer(pygame.USEREVENT + 1, 50)
def run(self):
pygame.display.set_caption("Starting frame reading...")
frame_read = self.tello.get_frame_read()
pygame.display.set_caption("Receiving Tello video stream")
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
class missions(object):
def __init__(self):
pygame.init()
pygame.display.set_caption("Tello Feed")
self.screen = pygame.display.set_mode([960, 720])
self.tello = Tello()
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
eye_cascade = cv2.CascadeClassifier('haarcascade_eye.xml')
face_cascade = cv2.CascadeClassifier(
'haarcascade_frontalface_default.xml')
while True:
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
for (x, y, w, h) in faces:
cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
roi_gray = gray[y:y + h, x:x + w]
roi_color = frame[y:y + h, x:x + w]
eyes = eye_cascade.detectMultiScale(roi_gray)
for (ex, ey, ew, eh) in eyes:
cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh),
(0, 255, 0), 2)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / 60)
self.tello.end()
def __init__(self):
# Init Tello object that interacts with the Tello drone
tello = Tello(logging=False)
self.tello = tello
if not self.tello.connect():
raise Exception("Tello not connected")
if not self.tello.set_speed(10):
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():
raise Exception("Could not stop video stream")
if not self.tello.streamon():
raise Exception("Could not start video stream")
self.frame_read = tello.get_frame_read()
self.pilot = Pilot(tello, self)
self.key_ctrl = KeyboardController(tello)
def main():
print("Configuring tello Drone")
tello = Tello()
if not tello.connect():
print("Tello not connected")
return
if not tello.streamon():
print("Could not start video stream")
return
while (True):
frame = tello.get_frame_read()
cv2.imshow("DetectionResults", frame.frame)
raw_key = cv2.waitKey(1)
if raw_key == 27: #esc
break
cv2.destroyAllWindows()
tello.end()
def main():
tello = Tello()
tello.connect()
tello.streamon()
frame_read = tello.get_frame_read()
font = cv2.FONT_HERSHEY_COMPLEX
#try:
while True:
# Get frame
frame = frame_read.frame
_, buffer_img = cv2.imencode('.png', frame)
base64bytes = base64.b64encode(buffer_img)
base64string = base64bytes.decode('utf-8')
# URL of Lobe.AI model
url = "http://127.0.0.1:38100/predict/81cd92d8-d610-44d0-b331-f4ae718b4f7d"
payload = "{\"inputs\":{\"Image\":\"" + base64string + "\"}}"
response = requests.request("POST", url, data=payload)
response_dict = json.loads(response.text)
print(response_dict['outputs']['Prediction'][0])
cv2.putText(frame, response_dict['outputs']['Prediction'][0], (0, 90),
font, 4, (0, 0, 255), 2, cv2.LINE_AA)
# Show images
cv2.imshow('Webcam', frame)
# Exit when user press ESC key
k = cv2.waitKey(3) & 0xFF
if k == 27: # ESC Key
break
time.sleep(2)
# camera = frame_read
# print("before")
# camera = av.open(drone.get_video_stream())
# print("after")
# time.sleep(5)
try:
drone.takeoff()
except:
drone.land()
exit()
# time.sleep(10)
drone.streamon()
camera = drone.get_frame_read()
iterators = 0
imgCount = 0
IAmLost = 0
gates = 1
for n in range(gates):
close = False
tooManyWrong = 0
while (True):
# get_corners():
### Grabbing the video feed, "has frames" and "grabbed" check if
###there's a next frame, if there isn't, the feed will stop
### We'll have "img" and "image" for different purposes. "image" is the
### original video on top of which we draw, "img" is the one masked and
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.update()
elif event.type == QUIT:
should_stop = True
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == KEYUP:
self.keyup(event.key)
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw counter clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
dirname, basename = os.path.split(path)
filename, ext = os.path.splitext(basename)
fd, filename = tempfile.mkstemp(dir=dirname,
prefix=filename,
suffix=ext)
tempfile._name_sequence = orig
return filename
tello = Tello()
tello.connect()
keepRecording = True
tello.streamon()
frame_read = tello.get_frame_read()
def videoRecorder():
# create a VideoWriter object
height, width, _ = frame_read.frame.shape
video = cv2.VideoWriter(uniquify('video.avi'),
cv2.VideoWriter_fourcc(*'XVID'), 30,
(width, height))
while keepRecording:
video.write(frame_read.frame)
time.sleep(1 / 30)
video.release()
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.
"""
def getTakeOff(self):
return self.hasTakenOff
def setTakeoff(self, hasTakenOff):
self.hasTakenOff = hasTakenOff
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
###################################################################
##Drone Project Defined variables
self.hasTakenOff = False
self.specifiedTarget = known_face_names[0]
self.targetSeen = False
self.targetLeftSide = 320 # set to default
self.targetRightSide = 640
###################################################################
self.send_rc_control = False
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
detector = dlib.get_frontal_face_detector()
spinCounter = 0
while not should_stop:
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.update()
elif event.type == QUIT:
should_stop = True
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == KEYUP:
self.keyup(event.key)
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
#dets = detector(frame)
#for det in dets:
# cv2.rectangle(frame, (det.left(), det.top()), (det.right(), det.bottom()), color=(0,255,0), thickness=3)
# Resize frame of video to 1/4 size for faster face recognition processing
small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
# Convert the image from BGR color (which OpenCV uses) to RGB color (which face_recognition uses)
rgb_small_frame = small_frame[:, :, ::-1]
# Only process every other frame of video to save time
face_locations = face_recognition.face_locations(rgb_small_frame)
face_encodings = face_recognition.face_encodings(
rgb_small_frame, face_locations)
face_names = []
for face_encoding in face_encodings:
# See if the face is a match for the known face(s)
matches = face_recognition.compare_faces(
known_face_encodings, face_encoding)
name = "Unknown"
# If a match was found in known_face_encodings, just use the first one.
if True in matches:
first_match_index = matches.index(True)
name = known_face_names[
first_match_index] # This might not actually be correct, given it only uses the first one
face_names.append(name)
for (top, right, bottom,
left), name in zip(face_locations, face_names):
# Scale back up face locations since the frame we detected in was scaled to 1/4 size
top *= 4
right *= 4
bottom *= 4
left *= 4
# Draw a box around the face
cv2.rectangle(frame, (left, top), (right, bottom),
(0, 0, 255), 2)
# Draw a label with a name below the face
cv2.rectangle(frame, (left, bottom - 35), (right, bottom),
(0, 0, 255), cv2.FILLED)
font = cv2.FONT_HERSHEY_DUPLEX
cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0,
(255, 255, 255), 1)
# Updates when the target is seen
if (name == self.specifiedTarget):
self.targetSeen = True
self.targetLeftSide = left
self.targetRightSide = right
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
# Circles right until face detected
#while (targetSeen != True and self.hasTakenOff):
#self.tello.rotate_clockwise(self, 10)
#self.keydown(self, pygame.K_a)
if (self.targetSeen != True and self.hasTakenOff):
self.yaw_velocity = turnSpeed
spinCounter = spinCounter + 1
if (spinCounter > (5 * FPS)):
self.tello.land()
self.send_rc_control = False
self.tello.end()
#break ## ends loop and land drone
else:
if (self.targetLeftSide < 320):
#self.tello.rotate_clockwise(self, 10)
# self.keydown(self, pygame.K_a)
self.yaw_velocity = turnSpeed
# rotate left, target outside of the middle 1/3
elif (self.targetRightSide > 640):
#self.tello.rotate_counter_clockwise(self, 10)
# self.keydown(self, pygame.K_d)
self.yaw_velocity = -turnSpeed
else:
self.yaw_velocity = 0
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw counter clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.hasTakenOff = True
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations (yaw)
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# create update timer
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
def control_manual(self):
key = input('entraste en control manual')
time.sleep(2)
if key == '':
print
return
else:
if key == 'w':
print('entraste al comando')
self.tello.move_forward(30)
elif key == 's':
self.tello.move_down(30)
elif key == 'a':
self.tello.move_left(30)
elif key == ord('d'):
self.tello.move_right(30)
elif key == ord('e'):
self.tello.rotate_clockwise(30)
elif key == ord('q'):
self.tello.rotate_counter_clockwise(30)
elif key == ord('r'):
self.tello.move_up(30)
elif key == ord('f'):
self.tello.move_down(30)
print('termino control manual')
def run(self):
self.tello.connect()
self.tello.set_speed(self.speed)
# In case streaming is on. This happens when we quit this program without the escape key.
self.tello.streamoff()
self.tello.streamon()
#self.tello.takeoff()
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1:
self.update()
elif event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == pygame.KEYUP:
self.keyup(event.key)
if frame_read.stopped:
break
self.screen.fill([0, 0, 0])
frame = frame_read.frame
text = "Battery: {}%".format(self.tello.get_battery())
cv2.putText(frame, text, (5, 720 - 5), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
#x=threading.Thread(target=self.control_manual)
#x.start()
#x.join()
#time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
not self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
from djitellopy import Tello
import cv2
import numpy as np
tello = Tello()
tello.connect()
tello.streamon()
tello.takeoff()
rotated = False
while not rotated:
bg_frame = tello.get_frame_read()
if bg_frame:
img = cv2.resize(bg_frame.frame, (640, 480))
imgHsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower, higher = np.array([31, 111, 63]), np.array([179, 255, 255])
mask = cv2.inRange(imgHsv, lower, higher)
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
sending = False
for contour in contours:
arcLength = cv2.arcLength(contour, False)
print(arcLength)
if (arcLength > 200 and not sending):
approx = cv2.approxPolyDP(contour, 0.01 * arcLength, False)
cv2.drawContours(img, [approx], 0, (255, 0, 255), 2)
sending = True
tello.rotate_clockwise(360)
blended = cv2.bitwise_and(img, img, mask=mask)
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()
def process_tello_video_feed(handler_file, video_queue, stop_event, video_event, fly=False, tello_video_sim=False, display_tello_video=False):
"""
:param exit_event: Multiprocessing Event. When set, this event indicates that the process should stop.
:type exit_event:
:param video_queue: Thread Queue to send the video frame to
:type video_queue: threading.Queue
:param stop_event: Thread Event to indicate if this thread function should stop
:type stop_event: threading.Event
:param video_event: threading.Event to indicate when the main loop is ready for video
:type video_event: threading.Event
:param fly: Flag used to indicate whether the drone should fly. False is useful when you just want see the video stream.
:type fly: bool
:param max_speed_limit: Maximum speed that the drone will send as a command.
:type max_speed_limit: int
:return: None
:rtype:
"""
global tello, local_video_stream
last_show_video_queue_put_time = 0
handler_method = None
try:
if fly or ( not tello_video_sim and display_tello_video):
tello = Tello()
rtn = tello.connect()
LOGGER.debug(f"Connect Return: {rtn}")
if handler_file:
handler_file = handler_file.replace(".py", "")
handler_module = importlib.import_module(handler_file)
init_method = getattr(handler_module, 'init')
handler_method = getattr(handler_module, 'handler')
init_method(tello, fly_flag=fly)
frame_read = None
if tello and video_queue:
tello.streamon()
frame_read = tello.get_frame_read()
if fly:
tello.takeoff()
# send command to go no where
tello.send_rc_control(0, 0, 0, 0)
if tello_video_sim and local_video_stream is None:
local_video_stream = VideoStream(src=0).start()
time.sleep(2)
while not stop_event.isSet():
frame = _get_video_frame(frame_read, tello_video_sim)
if frame is None:
# LOGGER.debug("Failed to read video frame")
if handler_method:
handler_method(tello, frame, fly)
# else:
# # stop let keyboard commands take over
# if fly:
# tello.send_rc_control(0, 0, 0, 0)
continue
if handler_method:
handler_method(tello, frame, fly)
# else:
# # stop let keyboard commands take over
# if fly:
# tello.send_rc_control(0, 0, 0, 0)
# send frame to other processes
if video_queue and video_event.is_set():
try:
if time.time() - last_show_video_queue_put_time > show_video_per_second:
last_show_video_queue_put_time = time.time()
LOGGER.debug("Put video frame")
video_queue.put_nowait(frame)
except:
pass
except Exception as exc:
LOGGER.error(f"Exiting Tello Process with exception: {exc}")
traceback.print_exc()
finally:
# then the user has requested that we land and we should not process this thread
# any longer.
# to be safe... stop all movement
if fly:
tello.send_rc_control(0, 0, 0, 0)
stop_event.clear()
LOGGER.info("Leaving User Script Processing Thread.....")
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])
cv2.line(img, (int(frameWidth / 2) - deadZone, 0),
(int(frameWidth / 2) - deadZone, frameHeight), (255, 255, 0), 3)
cv2.line(img, (int(frameWidth / 2) + deadZone, 0),
(int(frameWidth / 2) + deadZone, frameHeight), (255, 255, 0), 3)
cv2.circle(img, (int(frameWidth / 2), int(frameHeight / 2)), 5,
(0, 0, 255), 5)
cv2.line(img, (0, int(frameHeight / 2) - deadZone),
(frameWidth, int(frameHeight / 2) - deadZone), (255, 255, 0), 3)
cv2.line(img, (0, int(frameHeight / 2) + deadZone),
(frameWidth, int(frameHeight / 2) + deadZone), (255, 255, 0), 3)
while True:
# GET THE IMAGE FROM TELLO
frame_read = me.get_frame_read()
myFrame = frame_read.frame
img = cv2.resize(myFrame, (width, height))
imgContour = img.copy()
imgHsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
h_min = cv2.getTrackbarPos("HUE Min", "HSV")
h_max = cv2.getTrackbarPos("HUE Max", "HSV")
s_min = cv2.getTrackbarPos("SAT Min", "HSV")
s_max = cv2.getTrackbarPos("SAT Max", "HSV")
v_min = cv2.getTrackbarPos("VALUE Min", "HSV")
v_max = cv2.getTrackbarPos("VALUE Max", "HSV")
lower = np.array([h_min, s_min, v_min])
upper = np.array([h_max, s_max, v_max])
mask = cv2.inRange(imgHsv, lower, upper)
tello = Tello()
if not tello.connect():
print("Tello not connected")
if not tello.set_speed(speed):
print("Not set speed to lowest possible")
# In case streaming is on. This happens when we quit this program without the escape key.
if not tello.streamoff():
print("Could not stop video stream")
if not tello.streamon():
print("Could not start video stream")
frame_cap = tello.get_frame_read()
###############################################################################
tello.takeoff()
time.sleep(3)
print(att_control(1000, False))
# print(att_control(2000, False))
# tello.go_xyz_speed(50, 50, 50, 50, 50, 50, 20)
# tello.rotate_counter_clockwise(90)
# time.sleep(rotate_sleep)
# tello.move_back(100)
# time.sleep(5)
# tello.move_right(100)
# time.sleep(4)
#
class TelloCV(object):
"""
TelloTracker builds keyboard controls on top of TelloPy as well
as generating images from the video stream and enabling opencv support
"""
def __init__(self):
self.prev_flight_data = None
self.record = False
self.tracking = False
self.keydown = False
self.date_fmt = '%Y-%m-%d_%H%M%S'
self.speed = 50
self.go_speed = 80
self.drone = Tello()
self.init_drone()
self.init_controls()
self.battery = self.drone.get_battery()
self.frame_read = self.drone.get_frame_read()
self.forward_time = 0
self.forward_flag = True
self.takeoff_time = 0
self.command_time = 0
self.command_flag = False
# trackingimport libh264decoder a color
# green_lower = (30, 50, 50)
# green_upper = (80, 255, 255)
# red_lower = (0, 50, 50)
# red_upper = (20, 255, 255)
blue_lower = np.array([0, 0, 0])
upper_blue = np.array([255, 255, 180])
bh_lower = (180, 30, 100)
bh_upper = (275, 50, 100)
self.track_cmd = ""
self.tracker = Tracker(960,
720,
blue_lower, upper_blue)
self.speed_list = [5, 10, 15, 20, 25]
self.frame_center = 480, 360
self.error = 60
def init_drone(self):
"""Connect, uneable streaming and subscribe to events"""
# self.drone.log.set_level(2)
self.drone.connect()
self.drone.streamon()
def on_press(self, keyname):
"""handler for keyboard listener"""
if self.keydown:
return
try:
self.keydown = True
keyname = str(keyname).strip('\'')
print('+' + keyname)
if keyname == 'Key.esc':
self.drone.quit()
exit(0)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(self.speed)
else:
key_handler(self.speed)
except AttributeError:
print('special key {0} pressed'.format(keyname))
def on_release(self, keyname):
"""Reset on key up from keyboard listener"""
self.keydown = False
keyname = str(keyname).strip('\'')
print('-' + keyname)
if keyname in self.controls:
key_handler = self.controls[keyname]
if isinstance(key_handler, str):
getattr(self.drone, key_handler)(0)
else:
key_handler(0)
def init_controls(self):
"""Define keys and add listener"""
self.controls = {
'w': lambda speed: self.drone.move_forward(speed),
's': lambda speed: self.drone.move_back(speed),
'a': lambda speed: self.drone.move_left(speed),
'd': lambda speed: self.drone.move_right(speed),
'Key.space': 'up',
'Key.shift': 'down',
'Key.shift_r': 'down',
'q': 'counter_clockwise',
'e': 'clockwise',
'i': lambda speed: self.drone.flip_forward(),
'k': lambda speed: self.drone.flip_back(),
'j': lambda speed: self.drone.flip_left(),
'l': lambda speed: self.drone.flip_right(),
# arrow keys for fast turns and altitude adjustments
'Key.left': lambda speed: self.drone.rotate_counter_clockwise(speed),
'Key.right': lambda speed: self.drone.rotate_clockwise(speed),
'Key.up': lambda speed: self.drone.move_up(speed),
'Key.down': lambda speed: self.drone.move_down(speed),
'Key.tab': lambda speed: self.drone.takeoff(),
# 'Key.tab': self.drone.takeoff(60),
'Key.backspace': lambda speed: self.drone.land(),
'p': lambda speed: self.palm_land(speed),
't': lambda speed: self.toggle_tracking(speed),
'r': lambda speed: self.toggle_recording(speed),
'z': lambda speed: self.toggle_zoom(speed),
'Key.enter': lambda speed: self.take_picture(speed)
}
self.key_listener = keyboard.Listener(on_press=self.on_press,
on_release=self.on_release)
self.key_listener.start()
# self.key_listener.join()
def process_frame(self):
"""convert frame to cv2 image and show"""
# print("TRACKING START")
frame = self.frame_read.frame
# self.drone.move_up(self.speed)
# image = self.write_hud(image)
# if self.record:
# self.record_vid(frame)
return frame
def move_up(self):
self.drone.move_up(self.speed)
def take_off(self):
self.drone.takeoff()
def go(self):
self.drone.move_forward(self.go_speed)
def move_left(self):
self.drone.move_left(270) # speed 테스트해서 조절하기
def go_window9(self):
self.drone.move_forward()
def rotate_right(self):
self.drone.rotate_clockwise()
def rotate_left(self):
self.drone.rotate_counter_clockwise()
def landing(self):
self.drone.land()
def write_hud(self, frame):
"""Draw drone info, tracking and record on frame"""
stats = self.prev_flight_data.split('|')
stats.append("Tracking:" + str(self.tracking))
if self.drone.zoom:
stats.append("VID")
else:
stats.append("PIC")
if self.record:
diff = int(time.time() - self.start_time)
mins, secs = divmod(diff, 60)
stats.append("REC {:02d}:{:02d}".format(mins, secs))
for idx, stat in enumerate(stats):
text = stat.lstrip()
cv2.putText(frame, text, (0, 30 + (idx * 30)),
cv2.FONT_HERSHEY_SIMPLEX,
1.0, (255, 0, 0), lineType=30)
return frame
def toggle_recording(self, speed):
"""Handle recording keypress, creates output stream and file"""
if speed == 0:
return
self.record = not self.record
if self.record:
datename = [os.getenv('HOME'), datetime.datetime.now().strftime(self.date_fmt)]
self.out_name = '{}/Pictures/tello-{}.mp4'.format(*datename)
print("Outputting video to:", self.out_name)
self.out_file = av.open(self.out_name, 'w')
self.start_time = time.time()
self.out_stream = self.out_file.add_stream(
'mpeg4', self.vid_stream.rate)
self.out_stream.pix_fmt = 'yuv420p'
self.out_stream.width = self.vid_stream.width
self.out_stream.height = self.vid_stream.height
if not self.record:
print("Video saved to ", self.out_name)
self.out_file.close()
self.out_stream = None
def record_vid(self, frame):
"""
convert frames to packets and write to file
"""
new_frame = av.VideoFrame(
width=frame.width, height=frame.height, format=frame.format.name)
for i in range(len(frame.planes)):
new_frame.planes[i].update(frame.planes[i])
pkt = None
try:
pkt = self.out_stream.encode(new_frame)
except IOError as err:
print("encoding failed: {0}".format(err))
if pkt is not None:
try:
self.out_file.mux(pkt)
except IOError:
print('mux failed: ' + str(pkt))
def take_picture(self, speed):
"""Tell drone to take picture, image sent to file handler"""
if speed == 0:
return
self.drone.take_picture()
def palm_land(self, speed):
"""Tell drone to land"""
if speed == 0:
return
self.drone.palm_land()
def toggle_tracking(self, speed):
""" Handle tracking keypress"""
if speed == 0: # handle key up event
return
self.tracking = not self.tracking
print("tracking:", self.tracking)
return
def toggle_zoom(self, speed):
"""
In "video" mode the self.drone sends 1280x720 frames.
In "photo" mode it sends 2592x1936 (952x720) frames.
The video will always be centered in the window.
In photo mode, if we keep the window at 1280x720 that gives us ~160px on
each side for status information, which is ample.
Video mode is harder because then we need to abandon the 16:9 display size
if we want to put the HUD next to the video.
"""
if speed == 0:
return
self.drone.set_video_mode(not self.drone.zoom)
def flight_data_handler(self, event, sender, data):
"""Listener to flight data from the drone."""
text = str(data)
if self.prev_flight_data != text:
self.prev_flight_data = text
def handle_flight_received(self, event, sender, data):
"""Create a file in ~/Pictures/ to receive image from the drone"""
path = '%s/Pictures/tello-%s.jpeg' % (
os.getenv('HOME'),
datetime.datetime.now().strftime(self.date_fmt))
with open(path, 'wb') as out_file:
out_file.write(data)
print('Saved photo to %s' % path)
def enable_mission_pads(self):
self.drone.enable_mission_pads()
def disable_mission_pads(self):
self.drone.disable_mission_pads()
def go_xyz_speed_mid(self, x, y, z, speed, mid):
self.drone.go_xyz_speed_mid(x, y, z, speed, mid)
# if function return True, set drone center to object's center
def track_mid(self, x, y):
midx, midy = 480, 360
distance_x = abs(midx - x)
distance_y = abs(midy - y)
print(x, y, distance_x, distance_y)
move_done = True
if y > midy + self.error + 15:
self.drone.move_down(20)
move_done = False
elif y < midy - self.error + 5:
self.drone.move_up(20)
move_done = False
elif x < midx - self.error:
self.drone.move_left(20)
move_done = False
elif x > midx + self.error:
self.drone.move_right(20)
move_done = False
return move_done
def track_x(self, x, left_count, right_count):
midx = 480
move_done = True
if x < midx - 100:
self.drone.move_left(20)
left_count += 1
move_done = False
elif x > midx + 100:
self.drone.move_right(20)
right_count += 1
move_done = False
return move_done
def go_slow(self):
self.drone.move_forward(30)
def go_fast(self):
self.drone.move_forward(200)
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.imgCount = 0
self.send_rc_control = False
self.c = 0
self.g = 0
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
self.set_var = 0
def run(self):
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# In case streaming is on. This happens when we quit this program without the escape key.
if not self.tello.streamoff():
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
frame_read = self.tello.get_frame_read()
should_stop = False
while not should_stop:
for event in pygame.event.get():
#if event.type == USEREVENT + 1:
#pass
#self.update()
if event.type == QUIT:
should_stop = True
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == KEYUP:
self.keyup(event.key)
if frame_read.stopped:
frame_read.stop()
break
self.screen.fill([0, 0, 0])
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
#frame = cv2.cvtColor(frame_read.frame)
img_c = 0
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, self.imgCount), frame)
cv2.imwrite("{}/tellocap{}.jpg".format(ddir1, img_c), frame)
self.imgCount = self.imgCount + 1
x_axis = 0
y_axis = 0
z_axis = 0
self.g = 0
command_arr = []
print("yooooo start")
try:
with open(
r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv"
) as f:
rows = csv.reader(f)
for command in rows:
command_arr.append(command)
print(command_arr)
except:
print("1st error")
try:
if (int(command_arr[2][0]) == 2):
print("land")
self.tello.land()
self.set_var = 0
self.g = 1
elif (int(command_arr[1][0]) == 1):
print("takeoff")
self.tello.takeoff()
self.set_var = 1
self.g = 1
with open(
r"C:\Users\hp\Desktop\Tello\ubuntushare\\commands.csv",
'w') as fl:
writer = csv.writer(fl, delimiter=',')
fl.truncate(0)
fl.close()
except:
print("error speech")
if (self.g == 0):
if (self.set_var == 1):
try:
with open(r"C:\Users\hp\Desktop\\ginnovators.csv",
'r') as csvfile:
# creating a csv reader object
csvreader = csv.reader(csvfile)
# extracting field names through first row
for row in csvreader:
values = row
print(values)
x_axis = int(values[0])
y_axis = int(values[1])
z_axis = int(values[2])
except:
print("csv error")
try:
if (-50 < x_axis < 50):
if (-38 < y_axis < 38):
if (-7 < z_axis < 18):
self.tello.send_rc_control(0, 0, 0, 0)
else:
if (z_axis > 18):
self.for_back_velocity = -30
self.tello.send_rc_control(
0, self.for_back_velocity, 0, 0)
elif (z_axis < -7):
self.for_back_velocity = 30
self.tello.send_rc_control(
0, self.for_back_velocity, 0, 0)
else:
if (y_axis > 38):
self.up_down_velocity = 30
self.tello.send_rc_control(
0, 0, self.up_down_velocity, 0)
elif (y_axis < -38):
self.up_down_velocity = -30
self.tello.send_rc_control(
0, 0, self.up_down_velocity, 0)
else:
if (x_axis > 50):
self.yaw_velocity = -20
self.tello.send_rc_control(
0, 0, 0, self.yaw_velocity)
elif (x_axis < -50):
self.yaw_velocity = 20
self.tello.send_rc_control(
0, 0, 0, self.yaw_velocity)
except:
print("error")
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / 25)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw counter clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.set_var = 1
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class FrontEnd(object):
def __init__(self):
# 드론과 상호작용하는 Tello 객체
self.tello = Tello()
# 드론의 속도 (-100~100)
#수직, 수평 속도
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.send_rc_control = False
# 실행 함수
def run(self):
#드론이 연결이 되지 않으면 함수 종료
if not self.tello.connect():
print("Tello not connected")
return
#drone의 제한속도가 적절하지 않은 경우
if not self.tello.set_speed(self.speed):
print("Not set speed to lowest possible")
return
# 프로그램을 비정상적인 방법으로 종료를 시도하여 비디오 화면이 꺼지지 않은 경우 종료.
if not self.tello.streamoff():
print("Could not stop video stream")
return
# 비디오가 켜지지않는 경우 종료.
if not self.tello.streamon():
print("Could not start video stream")
return
#프레임 단위로 인식
frame_read = self.tello.get_frame_read()
should_stop = False
imgCount = 0
OVERRIDE = False
oSpeed = args.override_speed
tDistance = args.distance
self.tello.get_battery()
# X축 안전 범위
szX = args.saftey_x
# Y축 안전 범위
szY = args.saftey_y
#디버깅 모드
if args.debug:
print("DEBUG MODE ENABLED!")
#비행을 멈취야할 상황이 주어지지 않은 경우
while not should_stop:
self.update()
#프레임 입력이 멈췄을 경우 while문 탈출
if frame_read.stopped:
frame_read.stop()
break
theTime = str(datetime.datetime.now()).replace(':', '-').replace(
'.', '_')
frame = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
frameRet = frame_read.frame
vid = self.tello.get_video_capture()
#저장할 경우
if args.save_session:
cv2.imwrite("{}/tellocap{}.jpg".format(ddir, imgCount),
frameRet)
frame = np.rot90(frame)
imgCount += 1
time.sleep(1 / FPS)
# 키보드 입력을 기다림
k = cv2.waitKey(20)
# 0을 눌러서 거리를 0으로 설정
if k == ord('0'):
if not OVERRIDE:
print("Distance = 0")
tDistance = 0
# 1을 눌러서 거리를 1으로 설정
if k == ord('1'):
if OVERRIDE:
oSpeed = 1
else:
print("Distance = 1")
tDistance = 1
# 2을 눌러서 거리를 2으로 설정
if k == ord('2'):
if OVERRIDE:
oSpeed = 2
else:
print("Distance = 2")
tDistance = 2
# 3을 눌러서 거리를 3으로 설정
if k == ord('3'):
if OVERRIDE:
oSpeed = 3
else:
print("Distance = 3")
tDistance = 3
# 4을 눌러서 거리를 4으로 설정
if k == ord('4'):
if not OVERRIDE:
print("Distance = 4")
tDistance = 4
# 5을 눌러서 거리를 5으로 설정
if k == ord('5'):
if not OVERRIDE:
print("Distance = 5")
tDistance = 5
# 6을 눌러서 거리를 6으로 설정
if k == ord('6'):
if not OVERRIDE:
print("Distance = 6")
tDistance = 6
# T를 눌러서 이륙
if k == ord('t'):
if not args.debug:
print("Taking Off")
self.tello.takeoff()
self.tello.get_battery()
self.send_rc_control = True
# L을 눌러서 착륙
if k == ord('l'):
if not args.debug:
print("Landing")
self.tello.land()
self.send_rc_control = False
# Backspace를 눌러서 명령을 덮어씀
if k == 8:
if not OVERRIDE:
OVERRIDE = True
print("OVERRIDE ENABLED")
else:
OVERRIDE = False
print("OVERRIDE DISABLED")
if OVERRIDE:
# S & W 눌러서 앞 & 뒤로 비행
if k == ord('w'):
self.for_back_velocity = int(S * oSpeed)
elif k == ord('s'):
self.for_back_velocity = -int(S * oSpeed)
else:
self.for_back_velocity = 0
# a & d 를 눌러서 왼쪽 & 오른쪽으로 회전
if k == ord('d'):
self.yaw_velocity = int(S * oSpeed)
elif k == ord('a'):
self.yaw_velocity = -int(S * oSpeed)
else:
self.yaw_velocity = 0
# Q & E 를 눌러서 위 & 아래로 비행
if k == ord('e'):
self.up_down_velocity = int(S * oSpeed)
elif k == ord('q'):
self.up_down_velocity = -int(S * oSpeed)
else:
self.up_down_velocity = 0
# c & z 를 눌러서 왼쪽 & 오른쪽으로 비행
if k == ord('c'):
self.left_right_velocity = int(S * oSpeed)
elif k == ord('z'):
self.left_right_velocity = -int(S * oSpeed)
else:
self.left_right_velocity = 0
# 프로그램 종료
if k == 27:
should_stop = True
break
gray = cv2.cvtColor(frameRet, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray,
scaleFactor=1.5,
minNeighbors=2)
# 대상 크기
tSize = faceSizes[tDistance]
# 중심 차원들
cWidth = int(dimensions[0] / 2)
cHeight = int(dimensions[1] / 2)
noFaces = len(faces) == 0
# 컨트롤을 얻고, 얼굴 좌표 등을 얻으면
if self.send_rc_control and not OVERRIDE:
for (x, y, w, h) in faces:
#
roi_gray = gray[y:y + h,
x:x + w] #(ycord_start, ycord_end)
roi_color = frameRet[y:y + h, x:x + w]
# 얼굴 상자 특성 설정
fbCol = (255, 0, 0) #BGR 0-255
fbStroke = 2
# 끝 좌표들은 x와 y를 제한하는 박스의 끝에 존재
end_cord_x = x + w
end_cord_y = y + h
end_size = w * 2
# 목표 좌표들
targ_cord_x = int((end_cord_x + x) / 2)
targ_cord_y = int((end_cord_y + y) / 2) + UDOffset
# 얼굴에서 화면 중심까지의 벡터를 계산
vTrue = np.array((cWidth, cHeight, tSize))
vTarget = np.array((targ_cord_x, targ_cord_y, end_size))
vDistance = vTrue - vTarget
#
if not args.debug:
# 회전
if vDistance[0] < -szX:
self.yaw_velocity = S
# self.left_right_velocity = S2
elif vDistance[0] > szX:
self.yaw_velocity = -S
# self.left_right_velocity = -S2
else:
self.yaw_velocity = 0
# 위 & 아래 (상승/하강)
if vDistance[1] > szY:
self.up_down_velocity = S
elif vDistance[1] < -szY:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
F = 0
if abs(vDistance[2]) > acc[tDistance]:
F = S
# 앞, 뒤
if vDistance[2] > 0:
self.for_back_velocity = S + F
elif vDistance[2] < 0:
self.for_back_velocity = -S - F
else:
self.for_back_velocity = 0
# 얼굴 테두리 박스를 그림
cv2.rectangle(frameRet, (x, y), (end_cord_x, end_cord_y),
fbCol, fbStroke)
# 목표를 원으로 그림
cv2.circle(frameRet, (targ_cord_x, targ_cord_y), 10,
(0, 255, 0), 2)
# 안전 구역을 그림
cv2.rectangle(frameRet,
(targ_cord_x - szX, targ_cord_y - szY),
(targ_cord_x + szX, targ_cord_y + szY),
(0, 255, 0), fbStroke)
# 드론의 얼굴 상자로부터의 상대적 벡터 위치를 구함.
cv2.putText(frameRet, str(vDistance), (0, 64),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255),
2)
# 인식되는 얼굴이 없으면 아무것도 안함.
if noFaces:
self.yaw_velocity = 0
self.up_down_velocity = 0
self.for_back_velocity = 0
print("NO TARGET")
# 화면의 중심을 그림. 드론이 목표 좌표와 맞추려는 대상이 됨.
cv2.circle(frameRet, (cWidth, cHeight), 10, (0, 0, 255), 2)
dCol = lerp(np.array((0, 0, 255)), np.array((255, 255, 255)),
tDistance + 1 / 7)
if OVERRIDE:
show = "OVERRIDE: {}".format(oSpeed)
dCol = (255, 255, 255)
else:
show = "AI: {}".format(str(tDistance))
# 선택된 거리를 그림
cv2.putText(frameRet, show, (32, 664), cv2.FONT_HERSHEY_SIMPLEX, 1,
dCol, 2)
# 결과 프레임을 보여줌.
cv2.imshow(f'Tello Tracking...', frameRet)
# 종료시에 배터리를 출력
self.tello.get_battery()
# 전부 완료되면 캡쳐를 해제함.
cv2.destroyAllWindows()
# 종료 전에 항상 호출. 자원들을 해제함.
self.tello.end()
def battery(self):
return self.tello.get_battery()[:2]
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
from djitellopy import Tello
import cv2
width = 320
height = 240
drone = Tello()
drone.connect()
print(drone.get_battery())
drone.streamoff()
drone.streamon()
while True:
frame_read = drone.get_frame_read()
myFrame = frame_read.frame
img = cv2.resize(myFrame, (width, height))
cv2.imshow("MyResult", img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
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)
cv2.putText(img,
f"({(points[-1][0]-500)/ 100}, {(points[-1][1]-500)/ 100}m)",
(points[-1][0] + 10, points[-1][1] + 30),
cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1)
#camera features
drone.streamoff()
drone.streamon()
while True:
val = getKeyInput()
#drone movements
drone.send_rc_control(val[0], val[1], val[2], val[3])
img = np.zeros((1000, 1000, 3), np.uint8)
if (points[-1][0] != val[4] or points[-1][1] != val[5]):
points.append((val[4], val[5]))
#drawing the poings with previous data passed
drawPoints(img, points)
#getting and resizing the drone fottage
droneFootage = drone.get_frame_read().frame
droneFootage = cv2.resize(droneFootage, (500, 500))
#showing the mapping of the drone and the drone footage
cv2.imshow("Drone Footage", droneFootage)
cv2.imshow("Out", img)
cv2.waitKey(1)
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)
