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
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- Tab: Takeoff
- Shift: Land
- Space: Emergency Shutdown
- WASD: Forward, backward, left and right
- Q and E: Counter clockwise and clockwise rotations
- R and F: Up and down
- T: Start/Stop tracking
- C: Select central pixel value as new color for tracking
- #: Switch controllable parameter
- + and -: Raise or Lower controllable parameter
"""
def __init__(self):
# Init pygame
pygame.init()
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# general config
self.internalSpeed = 100
self.FPS = 20
self.hud_size = (800, 600)
# config of controllable parameters
self.controll_params = {
'Speed': 100,
'Color': 0,
}
self.controll_params_d = {
'Speed': 10,
'Color': 1,
}
self.controll_params_m = {
'Speed': 100,
'Color': 2,
}
# tracker config
self.color_lower = {
'blue': (100, 200, 50),
'red': (0, 200, 100),
'yellow': (20, 200, 130),
}
self.color_upper = {
'blue': (140, 255, 255),
'red': (20, 255, 255),
'yellow': (40, 255, 255),
}
self.current_color = np.array(self.color_lower['blue']) + np.array(
self.color_upper['blue'])
for i in range(0, 3):
self.current_color[i] = self.current_color[i] / 2
self.crange = (10, 50, 50)
# other params (no need to config)
self.current_parameter = 0
self.param_keys = list(self.controll_params.keys())
self.color_keys = list(self.color_lower.keys())
self.central_color = (0, 0, 0)
self.midx = int(self.hud_size[0] / 2)
self.midy = int(self.hud_size[1] / 2)
self.xoffset = 0
self.yoffset = 0
self.target_radius = 120
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.send_rc_control = False
self.isTracking = False
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode(self.hud_size)
# create update timer
pygame.time.set_timer(USEREVENT + 1, 50)
#
# 888b d888 d8b 888
# 8888b d8888 Y8P 888
# 88888b.d88888 888
# 888Y88888P888 8888b. 888 88888b. 888 .d88b. .d88b. 88888b.
# 888 Y888P 888 "88b 888 888 "88b 888 d88""88b d88""88b 888 "88b
# 888 Y8P 888 .d888888 888 888 888 888 888 888 888 888 888 888
# 888 " 888 888 888 888 888 888 888 Y88..88P Y88..88P 888 d88P
# 888 888 "Y888888 888 888 888 88888888 "Y88P" "Y88P" 88888P"
# 888
# 888
# 888
#
def run(self):
"""
Main loop.
Contains reading the incoming frames, the call for tracking and basic keyboard stuff.
"""
if not self.tello.connect():
print("Tello not connected")
return
if not self.tello.set_speed(self.internalSpeed):
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()
self.should_stop = False
while not self.should_stop:
# read frame
img = cv2.cvtColor(frame_read.frame, cv2.COLOR_BGR2RGB)
img = cv2.resize(img, self.hud_size, interpolation=cv2.INTER_AREA)
# get output from tracking
if self.isTracking:
self.track(img)
# produce hud
self.frame = self.write_hud(img.copy())
self.frame = np.fliplr(self.frame)
self.frame = np.rot90(self.frame)
self.frame = pygame.surfarray.make_surface(self.frame)
self.screen.fill([0, 0, 0])
self.screen.blit(self.frame, (0, 0))
pygame.display.update()
# handle input from dronet or user
for event in pygame.event.get():
if event.type == USEREVENT + 1:
self.send_input()
elif event.type == QUIT:
self.should_stop = True
elif event.type == KEYDOWN:
if (event.key == K_ESCAPE) or (event.key == K_BACKSPACE):
self.should_stop = True
else:
self.keydown(event.key)
elif event.type == KEYUP:
self.keyup(event.key)
# shutdown stream
if frame_read.stopped:
frame_read.stop()
break
# wait a little
time.sleep(1 / self.FPS)
# always call before finishing to deallocate resources
self.tello.end()
def track(self, frame):
"""
HSV color space tracking.
"""
# resize the frame, blur it, and convert it to the HSV
# color space
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_RGB2HSV)
self.central_color = hsv[self.midy, self.midx, :]
# construct a mask for the color then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, self.current_color - self.crange,
self.current_color + self.crange)
mask = cv2.erode(mask, None, iterations=3)
mask = cv2.dilate(mask, None, iterations=3)
# find contours in the mask and initialize the current
# (x, y) center of the ball
image, cnts, hierarchy = cv2.findContours(mask.copy(),
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
center = None
radius = 0
velocity = 0
# only proceed if at least one contour was found
if len(cnts) > 0:
# update color from mean color
mask_upstate = cv2.bitwise_and(hsv, hsv, mask=mask)
mean = cv2.mean(mask_upstate)
multiplier = float(mask.size) / (cv2.countNonZero(mask) + 0.001)
mean = np.array([multiplier * x for x in mean])
self.update_color(mean)
print(self.current_color)
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 40:
# draw the circle and centroid on the frame,
# then update the list of tracked points
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 0),
2)
self.xoffset = int(center[0] - self.midx)
self.yoffset = int(self.midy - center[1])
velocity = clamp(self.target_radius - radius, -40,
60) / 100 * self.controll_params['Speed']
else:
self.xoffset = 0
self.yoffset = 0
velocity = 0
else:
self.xoffset = 0
self.yoffset = 0
velocity = 0
xfact = self.xoffset / self.hud_size[0] * self.controll_params[
'Speed'] * 2
yfact = self.yoffset / self.hud_size[0] * self.controll_params[
'Speed'] * 2
self.for_back_velocity = int(velocity)
self.yaw_velocity = int(xfact)
self.up_down_velocity = int(yfact)
#
# 8888888 888 888b d888 888 888 888
# 888 888 8888b d8888 888 888 888
# 888 888 88888b.d88888 888 888 888
# 888 88888b. 88888b. 888 888 888888 888Y88888P888 .d88b. 888888 88888b. .d88b. .d88888 .d8888b
# 888 888 "88b 888 "88b 888 888 888 888 Y888P 888 d8P Y8b 888 888 "88b d88""88b d88" 888 88K
# 888 888 888 888 888 888 888 888 888 Y8P 888 88888888 888 888 888 888 888 888 888 "Y8888b.
# 888 888 888 888 d88P Y88b 888 Y88b. 888 " 888 Y8b. Y88b. 888 888 Y88..88P Y88b 888 X88
# 8888888 888 888 88888P" "Y88888 "Y888 888 888 "Y8888 "Y888 888 888 "Y88P" "Y88888 88888P'
# 888
# 888
# 888
#
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_w: # set forward velocity
self.isTracking = False
self.for_back_velocity = self.controll_params['Speed']
elif key == pygame.K_s: # set backward velocity
self.isTracking = False
self.for_back_velocity = -self.controll_params['Speed']
elif key == pygame.K_a: # set left velocity
self.isTracking = False
self.left_right_velocity = -self.controll_params['Speed']
elif key == pygame.K_d: # set right velocity
self.isTracking = False
self.left_right_velocity = self.controll_params['Speed']
elif key == pygame.K_r: # set up velocity
self.isTracking = False
self.up_down_velocity = self.controll_params['Speed']
elif key == pygame.K_f: # set down velocity
self.isTracking = False
self.up_down_velocity = -self.controll_params['Speed']
elif key == pygame.K_e: # set yaw clockwise velocity
self.isTracking = False
self.yaw_velocity = self.controll_params['Speed']
elif key == pygame.K_q: # set yaw counter clockwise velocity
self.isTracking = False
self.yaw_velocity = -self.controll_params['Speed']
elif key == pygame.K_TAB: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_LSHIFT: # land
self.isTracking = False
self.tello.land()
self.send_rc_control = False
elif key == pygame.K_SPACE: # emergency shutdown
self.isTracking = False
self.tello.emergency()
self.send_rc_control = False
self.should_stop = True
elif key == pygame.K_BACKSPACE: # emergency shutdown
self.isTracking = False
self.send_rc_control = False
self.should_stop = True
elif key == pygame.K_t: # arm tracking
self.isTracking = not self.isTracking
self.for_back_velocity = 0
self.yaw_velocity = 0
self.up_down_velocity = 0
elif key == pygame.K_c: # get new color
self.set_color(self.central_color)
self.for_back_velocity = 0
self.yaw_velocity = 0
self.up_down_velocity = 0
elif key == pygame.K_HASH: # switch parameters
if self.current_parameter == 0:
self.current_parameter = 1
else:
self.current_parameter = 0
elif key == pygame.K_PLUS: # raise current parameter
what = self.param_keys[self.current_parameter]
if self.controll_params[what] < self.controll_params_m[what] - 0.01:
self.controll_params[what] = self.controll_params[
what] + self.controll_params_d[what]
if (what == 'Color'):
self.reset_color()
elif key == pygame.K_MINUS: # lower current parameter
what = self.param_keys[self.current_parameter]
if self.controll_params[what] > 0.01:
self.controll_params[what] = self.controll_params[
what] - self.controll_params_d[what]
if (what == 'Color'):
self.reset_color()
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_w or key == pygame.K_s: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_r or key == pygame.K_f: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_q or key == pygame.K_e: # set zero yaw velocity
self.yaw_velocity = 0
def send_input(self):
""" Update routine. Send velocities to Tello."""
#print("V: " + str(self.for_back_velocity) + "; Y: " + str(self.yaw_velocity))
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)
#
# 888 888 888 888b d888 888 888 888
# 888 888 888 8888b d8888 888 888 888
# 888 888 888 88888b.d88888 888 888 888
# 8888888888 .d88b. 888 88888b. .d88b. 888d888 888Y88888P888 .d88b. 888888 88888b. .d88b. .d88888 .d8888b
# 888 888 d8P Y8b 888 888 "88b d8P Y8b 888P" 888 Y888P 888 d8P Y8b 888 888 "88b d88""88b d88" 888 88K
# 888 888 88888888 888 888 888 88888888 888 888 Y8P 888 88888888 888 888 888 888 888 888 888 "Y8888b.
# 888 888 Y8b. 888 888 d88P Y8b. 888 888 " 888 Y8b. Y88b. 888 888 Y88..88P Y88b 888 X88
# 888 888 "Y8888 888 88888P" "Y8888 888 888 888 "Y8888 "Y888 888 888 "Y88P" "Y88888 88888P'
# 888
# 888
# 888
#
def update_color(self, val):
"""
Adjusts the currently tracked color to input.
"""
if (cv2.mean(val) != 0):
for i in range(0, 2):
self.current_color[i] = clamp(
val[i], self.color_lower[self.color_keys[
self.controll_params['Color']]][i], self.color_upper[
self.color_keys[self.controll_params['Color']]][i])
def set_color(self, val):
self.current_color = np.array(val)
print(val)
def reset_color(self):
self.current_color = np.array(
self.color_lower[self.color_keys[self.controll_params['Color']]]
) + np.array(
self.color_upper[self.color_keys[self.controll_params['Color']]])
for i in range(0, 3):
self.current_color[i] = self.current_color[i] / 2
def write_hud(self, frame):
"""Draw drone info and record on frame"""
stats = ["TelloTracker"]
if self.isTracking:
stats.append("Tracking active.")
stats.append("Speed: {:03d}".format(self.controll_params['Speed']))
stats.append("Color: " +
self.color_keys[self.controll_params['Color']])
self.draw_arrows(frame)
else:
stats.append("Tracking disabled.")
img = cv2.circle(frame, (self.midx, self.midy), 10, (0, 0, 255), 1)
stats.append(self.param_keys[self.current_parameter] +
": {:4.1f}".format(self.controll_params[self.param_keys[
self.current_parameter]]))
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 draw_arrows(self, frame):
"""Show the direction vector output in the cv2 window"""
#cv2.putText(frame,"Color:", (0, 35), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, thickness=2)
cv2.arrowedLine(frame, (self.midx, self.midy),
(self.midx + self.xoffset, self.midy - self.yoffset),
(255, 0, 0), 5)
return frame
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):
kp_x = .25;
ki_x = 0;
kd_x = .05;
kp_y = .5;
ki_y = 0;
kd_y = .1;
kp_z = .5;
ki_z = 0;
kd_z = .1;
bias_x = 0;
bias_y = 0;
bias_z = 0;
prev_error_x = 0;
prev_error_y = 0;
prev_error_z = 0;
integral_x = 0;
integral_y = 0;
integral_z = 0;
desired_x = 0;
desired_y = 0;
desired_z = 75;
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)
img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = img.shape
avg1 = np.float32(gray)
avg2 = np.float32(gray)
res = cv2.aruco.detectMarkers(gray, aruco_dict, parameters = arucoParams)
imgWithAruco = gray # assign imRemapped_color to imgWithAruco directly
#if len(res[0]) > 0:
#print (res[0])
focal_length = size[1]
center = (size[1]/2, size[0]/2)
camera_matrix = np.array(
[[focal_length, 0, center[0]],
[0, focal_length, center[1]],
[0, 0, 1]], dtype = "double"
)
if res[1] != None: # if aruco marker detected
im_src = imgWithAruco
im_dst = imgWithAruco
pts_dst = np.array([[res[0][0][0][0][0], res[0][0][0][0][1]], [res[0][0][0][1][0], res[0][0][0][1][1]], [res[0][0][0][2][0], res[0][0][0][2][1]], [res[0][0][0][3][0], res[0][0][0][3][1]]])
pts_src = pts_dst
h, status = cv2.findHomography(pts_src, pts_dst)
imgWithAruco = cv2.warpPerspective(im_src, h, (im_dst.shape[1], im_dst.shape[0]))
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(res[0], markerLength, camera_matrix, dist_coeffs)
print(tvec[0][0][0],tvec[0][0][1],tvec[0][0][2])
img = cv2.aruco.drawAxis(imgWithAruco, camera_matrix, dist_coeffs, rvec, tvec, 10)
cameraPose = cameraPoseFromHomography(h)
if autopilot:
#PID controller
x = tvec[0][0][0];
y = tvec[0][0][1];
z = tvec[0][0][2];
iteration_time = 1/FPS;
error_x = desired_x - x;
integral_x = integral_x + (error_x * iteration_time );
derivative_x = (error_x - prev_error_x) / iteration_time;
output_x = kp_x*error_x + ki_x*integral_x + kd_x*derivative_x + bias_x;
prev_error_x = error_x;
error_y = desired_y - y;
integral_y = integral_y + (error_y * iteration_time );
derivative_y = (error_y - prev_error_y) / iteration_time;
output_y = kp_y*error_y + ki_y*integral_y + kd_y*derivative_y + bias_y;
prev_error_y = error_y;
error_z = desired_z - z;
integral_z = integral_z + (error_z * iteration_time );
derivative_z = (error_z - prev_error_z) / iteration_time;
output_z = kp_z*error_z + ki_z*integral_z + kd_z*derivative_z + bias_z;
prev_error_z = error_z;
#speed can only be 10-100, if outside of range, set to 10 or 100
if output_x>100:
output_x = 100;
elif output_x<-100:
output_x = -100;
"""
elif output_x<10 and output_x>0:
output_x = 10;
elif output_x>-10 and output_x<0:
output_x = -10;
"""
if output_y>100:
output_y = 100;
elif output_y<-100:
output_y = -100;
elif output_y<10 and output_y>0:
output_y = 10;
elif output_y>-10 and output_y<0:
output_y = -10;
if output_z>100:
output_z = 100;
elif output_z<-100:
output_z = -100;
elif output_z<10 and output_z>0:
output_z = 10;
elif output_z>-10 and output_z<0:
output_z = -10;
self.left_right_velocity = int(-output_x);
if output_x>10 or output_x<-10:
self.yaw_velocity = int(-output_x);
self.up_down_velocity = int(output_y);
self.for_back_velocity = int(-output_z);
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
for (x,y,w,h) in faces:
img = cv2.rectangle(img,(x,y),(x+w,y+h),(255,0,0),2)
roi_gray = gray[y:y+h, x:x+w]
roi_color = img[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)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
frame = np.rot90(img)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
#cv2.imshow('img', img)
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
cv2.destroyAllWindows()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_SPACE:
autopilot = ~autopilot;
if ~autopilot:
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_r: #release R to make the drone flip to the right (flex)
self.tello.flip_right
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if ~autopilot:
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_e: #release E to turn off all motors (for our automated landing)
self.tello.emergency()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity, self.for_back_velocity, self.up_down_velocity,
self.yaw_velocity)
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode(
[960, 720]) # ((width, height) of window))
# 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
if not self.tello.streamoff(
): # In case streaming is on. This happens when we quit this program without the escape key.
print("Could not stop video stream")
return
if not self.tello.streamon():
print("Could not start video stream")
return
kp_xyz = [.15, .5, .4] #proportional constants
ki_xyz = [0, 0, 0] #integral constants
kd_xyz = [.05, .1, .1] #derivative constants
desired_xyz = [0, 0, 30] #desired xyz distance from aruco
land_xyz = [
100, 100, 100
] #range needed within desired to cut motors + attempt landing
bias_xyz = [0, 0, 0]
integral_xyz = [0, 0, 0]
prev_error_xyz = [0, 0, 0]
error_xyz = [0, 0, 0]
plt.ion() #interactive mode on
t = 0
#time (used for plotting)
plt.pause(0.001)
t = t + 0.001
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])
img = frame_read.frame
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = img.shape
avg1 = np.float32(gray)
avg2 = np.float32(gray)
res = cv2.aruco.detectMarkers(gray,
aruco_dict,
parameters=arucoParams)
imgWithAruco = img # assign imRemapped_color to imgWithAruco directly
#if len(res[0]) > 0:
#print (res[0])
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]], [0, 0, 1]],
dtype="double")
if res[1] != None: # if aruco marker detected
im_src = imgWithAruco
im_dst = imgWithAruco
pts_dst = np.array([[res[0][0][0][0][0], res[0][0][0][0][1]],
[res[0][0][0][1][0], res[0][0][0][1][1]],
[res[0][0][0][2][0], res[0][0][0][2][1]],
[res[0][0][0][3][0], res[0][0][0][3][1]]])
pts_src = pts_dst
h, status = cv2.findHomography(pts_src, pts_dst)
imgWithAruco = cv2.warpPerspective(
im_src, h, (im_dst.shape[1], im_dst.shape[0]))
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
res[0], markerLength, camera_matrix, dist_coeffs)
img = cv2.aruco.drawAxis(imgWithAruco, camera_matrix,
dist_coeffs, rvec, tvec, 10)
cameraPose = cameraPoseFromHomography(h)
#PID controller
xyz = []
xyz.append(tvec[0][0][0])
xyz.append(tvec[0][0][1])
xyz.append(tvec[0][0][2])
plt.subplot(311)
plt.plot(t, xyz[0], 'rs')
plt.subplot(312)
plt.plot(t, xyz[1], 'gs')
plt.subplot(313)
plt.plot(t, xyz[2], 'bs')
iteration_time = 1 / FPS
error_xyz = []
derivative_xyz = []
output_xyz = []
for i in range(3):
error_xyz.append(desired_xyz[i] - xyz[i])
integral_xyz[i] = integral_xyz[i] + (error_xyz[i] *
iteration_time)
derivative_xyz.append(error_xyz[i] - prev_error_xyz[i])
output_xyz.append(kp_xyz[i] * error_xyz[i] +
ki_xyz[i] * integral_xyz[i] +
kd_xyz[i] * derivative_xyz[i] +
bias_xyz[i])
prev_error_xyz[i] = error_xyz[i]
#speed can only be 10-100, if outside of range, set to 10 or 100
if output_xyz[i] > 100:
output_xyz[i] = 100
elif output_xyz[i] < -100:
output_xyz[i] = -100
elif output_xyz[i] < 10 and output_xyz[i] > 0:
output_xyz[i] = 10
elif output_xyz[i] > -10 and output_xyz[i] < 0:
output_xyz[i] = -10
self.left_right_velocity = int(-output_xyz[0])
self.yaw_velocity = int(-output_xyz[0])
self.up_down_velocity = int(output_xyz[1])
self.for_back_velocity = int(-output_xyz[2])
if xyz[0] < desired_xyz[0] + land_xyz[0] and xyz[
0] > desired_xyz[0] - land_xyz[0] and xyz[
1] < desired_xyz[1] + land_xyz[1] and xyz[
1] > xyz[1] - land_xyz[1] and xyz[
2] < desired_xyz[2] + land_xyz[2] and xyz[
2] > desired_xyz[2] - land_xyz[2]:
self.tello.land()
self.send_rc_control = False
print("x: ", xyz[0], "y:", xyz[1], "z:", xyz[2], sep="\t")
#consider using gain scheduling (different constants at different distances)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
frame = np.rot90(img)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
#xyz graphing
plt.show()
plt.pause(1 / FPS)
t = t + 1 / FPS
#deallocate resources.
self.tello.end()
cv2.destroyAllWindows()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if 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 = True
elif key == pygame.K_e: #release E to turn off all motors (for our automated landing)
self.tello.emergency()
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
pygame.init()
# Creat pygame window
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Init Tello object that interacts with the Tello drone
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.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
hover = 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)
img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = img.shape
avg1 = np.float32(gray)
avg2 = np.float32(gray)
res = cv2.aruco.detectMarkers(gray,
aruco_dict,
parameters=arucoParams)
imgWithAruco = gray # assign imRemapped_color to imgWithAruco directly
#if len(res[0]) > 0:
#print (res[0])
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]], [0, 0, 1]],
dtype="double")
if res[1] != None: # if aruco marker detected
im_src = imgWithAruco
im_dst = imgWithAruco
pts_dst = np.array([[res[0][0][0][0][0], res[0][0][0][0][1]],
[res[0][0][0][1][0], res[0][0][0][1][1]],
[res[0][0][0][2][0], res[0][0][0][2][1]],
[res[0][0][0][3][0], res[0][0][0][3][1]]])
pts_src = pts_dst
h, status = cv2.findHomography(pts_src, pts_dst)
imgWithAruco = cv2.warpPerspective(
im_src, h, (im_dst.shape[1], im_dst.shape[0]))
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
res[0], markerLength, camera_matrix, dist_coeffs)
print(tvec[0][0][0], tvec[0][0][1], tvec[0][0][2])
img = cv2.aruco.drawAxis(imgWithAruco, camera_matrix,
dist_coeffs, rvec, tvec, 10)
cameraPose = cameraPoseFromHomography(h)
if tvec[0][0][2] > ZHI:
self.for_back_velocity = -S
elif tvec[0][0][2] < ZLO:
self.for_back_velocity = S
else:
self.for_back_velocity = 0
if tvec[0][0][1] > YHI:
self.up_down_velocity = S
elif tvec[0][0][1] < YLO:
self.up_down_velocity = -S
else:
self.up_down_velocity = 0
if tvec[0][0][0] > XHI:
self.left_right_velocity = -S
elif tvec[0][0][0] < XLO:
self.left_right_velocity = S
else:
self.left_right_velocity = 0
hover = 0
hover = hover + 1
if hover == 1:
self.for_back_velocity = -self.for_back_velocity
self.left_right_velocity = -self.left_right_velocity
self.up_down_velocity = -self.up_down_velocity
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
for (x, y, w, h) in faces:
img = cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 0),
2)
roi_gray = gray[y:y + h, x:x + w]
roi_color = img[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)
img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
frame = np.rot90(img)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
#cv2.imshow('img', img)
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
cv2.destroyAllWindows()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if 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_e: #release E to turn off all motors (for our automated landing)
self.tello.emergency()
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)
drone.send_rc_control(0, 0, 0, 0)
while True:
#get the drone fottage
frameRead = drone.get_frame_read()
myFrame = frameRead.frame
#resize the image to make it look somewhat good (still not good)
img = cv2.resize(myFrame, (WIDTH, HEIGHT))
#start counter checker
if startCounter == 0:
drone.takeoff()
startCounter = 1
#showing the drone footage
cv2.imshow("Drone Footage", img)
#enter a command for the drone to do this will block the footage since it is an input (VERY BAD)
cmd = input("<CMD>: ")
#very hacky way of doing it not recomended for use
eval(f"drone.{translateCommands(cmd)}")
#press q to quit the program, but first input command, since it stalls on that
if cv2.waitKey(1) & 0xFF == ord('q'):
me.land()
break
#press enter to ACTIVATE EMERGENCY LANDING
land = input("ACTIVATE EMERGENCY LANDING: ")
drone.emergency()
class Display(object):
""" Tello display
Press escape key to quit.
The controls implemented:
- T: Takeoff
- L: Land
- W, S, A, D: Forward, backward, left and right (x,y)
- Arrow Keys: Up and down. (z)
- Q and E: Counter clockwise and clockwise rotations (yaw)
"""
def __init__(self):
# Init pygame
pygame.init()
# Create 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.y_velocity = 0
self.x_velocity = 0
self.z_velocity = 0
self.yaw_velocity = 0
self.speed = 50
self.S = 10
self.send_rc_control = False
# Ticker implementation
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
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()
frame_read = self.tello.get_frame_read()
stop = False
while not stop:
for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1:
self.update()
elif event.type == pygame.QUIT:
stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
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
img_input = frame.copy()
img_input = model.prepare_input(img_input)
keypoints = model.predict_singlepose(img_input)
BATTERY = "Battery: {}%; Speed: {}".format(
self.tello.get_battery(), self.S)
cv2.putText(frame, BATTERY, (5, 420 - 5), cv2.FONT_HERSHEY_SIMPLEX,
1, (0, 0, 255), 2)
model.draw_pose(frame, keypoints)
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()
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update values based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_a: # set left velocity
self.x_velocity = -self.S
elif key == pygame.K_d: # set right velocity
self.x_velocity = self.S
elif key == pygame.K_w: # set forward velocity
self.y_velocity = self.S
elif key == pygame.K_s: # set backward velocity
self.y_velocity = -self.S
elif key == pygame.K_UP: # set up velocity
self.z_velocity = self.S
elif key == pygame.K_DOWN: # set down velocity
self.z_velocity = -self.S
elif key == pygame.K_LEFT: # set yaw counter clockwise velocity
self.yaw_velocity = -self.S
elif key == pygame.K_RIGHT: # set yaw clockwise velocity
self.yaw_velocity = self.S
elif key == pygame.K_1: # set velocity
self.S = 10
elif key == pygame.K_2: # set velocity
self.S = 20
elif key == pygame.K_3: # set velocity
self.S = 30
elif key == pygame.K_4: # set velocity
self.S = 40
elif key == pygame.K_5: # set velocity
self.S = 50
elif key == pygame.K_6: # set velocity
self.S = 60
elif key == pygame.K_7: # set velocity
self.S = 70
elif key == pygame.K_8: # set velocity
self.S = 80
elif key == pygame.K_9: # set velocity
self.S = 90
elif key == pygame.K_0: # set velocity
self.S = 100
elif key == pygame.K_BACKSPACE:
self.tello.emergency()
def keyup(self, key):
""" Update values for key release
Arguments:
key: pygame key
"""
if key == pygame.K_a or key == pygame.K_d: # set zero left/right velocity
self.x_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero forward/backward velocity
self.y_velocity = 0
elif key == pygame.K_UP or key == pygame.K_DOWN: # set zero up/down velocity
self.z_velocity = 0
elif key == pygame.K_RIGHT or key == pygame.K_LEFT: # 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
elif key == pygame.K_DELETE:
self.tello.send_rc_control(0, 0, 0, 100)
time.sleep(0.5)
self.tello.land()
def update(self):
""" Update and send all the events to the tello"""
if self.send_rc_control:
self.tello.send_rc_control(self.x_velocity, self.y_velocity,
self.z_velocity, self.yaw_velocity)
print("Landing...")
mdrone.for_back_velocity = 0
mdrone.yaw_velocity = 0
mdrone.land()
mdrone.send_rc_control = False
# WAIT FOR THE 'Q' BUTTON TO STOP
if cv2.waitKey(1) or k == ord('q'):
mdrone.land()
cv2.destroyAllWindows()
mdrone.end()
break
time.sleep(2)
elif cmd == ord('l'):
mdrone.emergency()
mdrone.land()
mdrone.end()
cv2.destroyAllWindows()
break
else:
continue
yaw_velocity = 0
else:
left_right_velocity = 0
for_back_velocity = 0
up_down_velocity = 0
yaw_velocity = 0
# Send the velocities to drone
if tello.send_rc_control:
tello.send_rc_control(left_right_velocity, for_back_velocity,
up_down_velocity, yaw_velocity)
counter = counter + 1
# Display the image
cv2.imshow('Drone X', video_user)
# comming soon (emergency bottom)
if cv2.waitKey(20) & 0xFF == 'e':
tello.emergency()
# coming soon (quit video)
if cv2.waitKey(20) & 0xFF == 27:
break
# Release
# And then to destroy
tello.stop_video_capture()
video_user.release()
cv2.destroyAllWindows()
class FrontEnd(object):
""" Maintains the Tello display and moves it through the keyboard keys.
Press escape key to quit.
The controls are:
- T: Takeoff
- L: Land
- Arrow keys: Forward, backward, left and right.
- A and D: Counter clockwise and clockwise rotations
- W and S: Up and down.
"""
def __init__(self):
# Init pygame
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 = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
time.sleep(1 / FPS)
# Call it always before finishing. I deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw counter clockwise velocity
self.yaw_velocity = S
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
self.tello.land()
self.send_rc_control = False
elif key == pygame.K_e: #release E to turn off all motors (for our automated landing)
self.tello.emergency()
self.send_rc_control = False
elif key == pygame.K_r: #release R to make the drone flip to the right (flex)
self.tello.flip_right
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)
