def qr_tracking(droneVision:DroneVisionGUI, bebop:Bebop):
cv2.namedWindow('qr')
while cv2.getWindowProperty('qr', 0) >= 0:
img = droneVision.get_latest_valid_picture()
x,y,w,h = None,None,None,None
try:
rect = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][2]
poly = zbar.decode(img, symbols=[zbar.ZBarSymbol.QRCODE])[0][3]
x,y,w,h = rect
p1,p2,p3,p4 = poly
except IndexError:
pass
if x is not None:
# cv2.rectangle(img,(x,y),(x+w,y+h),(0,0,255))
pts = np.array([[p1[0],p1[1]], [p2[0],p2[1]], [p3[0],p3[1]], [p4[0],p4[1]]], np.int32)
pts.reshape(-1,1,2)
cv2.polylines(img, [pts], True, (255, 0, 255), 5)
area_t1 = abs((p1[0]*(p2[1]-p4[1])+p2[0]*(p4[1]-p1[1])+p4[0]*(p1[1]-p2[1]))/2.0)
area_t2 = abs((p3[0] * (p2[1] - p4[1]) + p2[0] * (p4[1] - p3[1]) + p4[0] * (p3[1] - p2[1])) / 2.0)
area = area_t2+area_t1
backup_threshold = 20000
fwd_threshold = 10000
if w is not None and area > backup_threshold:
print("GOING BACK")
bebop.fly_direct(roll=0, pitch=-20, yaw=0, vertical_movement=0, duration=0.07)
elif w is not None and area < fwd_threshold:
print("GOING FORWARD")
bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.07)
if x is not None and x + (w / 2.0) > 550:
print("GOING RIGHT")
bebop.fly_direct(roll=0, pitch=0, yaw=100, vertical_movement=0, duration=0.1)
elif x is not None and x + (w / 2.0) < 300:
print("GOING LEFT")
bebop.fly_direct(roll=0, pitch=0, yaw=-100, vertical_movement=0, duration=0.1)
if x is not None and y + (h / 2.0) > 380:
print("GOING DOWN")
bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=-40, duration=0.1)
elif x is not None and y + (h / 2.0) < 100:
print("GOING UP")
bebop.fly_direct(roll=0, pitch=0, yaw=0, vertical_movement=40, duration=0.1)
cv2.imshow('qr', img)
cv2.waitKey(10)
bebop.safe_land(10)
cv2.destroyAllWindows()
def face_tracking(droneVision:DroneVisionGUI,bebop:Bebop):
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
cv2.namedWindow("face_tracking")
frame = droneVision.get_latest_valid_picture()
while cv2.getWindowProperty('face_tracking', 0) >= 0:
frame = droneVision.get_latest_valid_picture()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
(x,y,w,h) = None, None, None, None
if len(faces) > 0:
(x,y,w,h) = faces[0]
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 255), 1)
print("Face Size: "+ str(w*h))
backup_threshold = 3600
fwd_threshold = 2000
if w is not None and w*h > backup_threshold:
print("GOING BACK")
bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.1)
elif w is not None and w*h < fwd_threshold:
print("GOING FORWARD")
bebop.fly_direct(roll=0, pitch=20, yaw=0, vertical_movement=0, duration=0.1)
if x is not None and x+(w/2.0) > 650:
print("GOING RIGHT")
bebop.fly_direct(roll=0,pitch=0,yaw=70,vertical_movement=0,duration=0.1)
elif x is not None and x+(w/2.0) < 200:
print("GOING LEFT")
bebop.fly_direct(roll=0, pitch=0, yaw=-70, vertical_movement=0, duration=0.1)
cv2.imshow("face_tracking",frame)
cv2.waitKey(10)
bebop.safe_land(10)
cv2.destroyAllWindows()
# disconnect nicely so we don't need a reboot
print("disconnecting")
bebop.disconnect()
while True:
bebop = Bebop()
# connect to the bebop
success = bebop.connect(5)
if (success):
# start up the video
bebopVision = DroneVisionGUI(
bebop,
is_bebop=True,
user_code_to_run=demo_user_code_after_vision_opened,
user_args=(bebop, ))
userVision = UserVision(bebopVision)
bebopVision.set_user_callback_function(
userVision.save_pictures,
user_callback_args=None) # calls save picture continuously
frame = bebopVision.get_latest_valid_picture()
bebopVision.open_video()
else:
print("Error connecting to bebop. Retry")
def color_tracking(drone_vision:DroneVisionGUI, bebop:Bebop):
def show_hist(hist):
"""Takes in the histogram, and displays it in the hist window."""
bin_count = hist.shape[0]
bin_w = 24
img = np.zeros((256, bin_count * bin_w, 3), np.uint8)
for i in range(bin_count):
h = int(hist[i])
cv2.rectangle(img, (i * bin_w + 2, 255), ((i + 1) * bin_w - 2, 255 - h),
(int(180.0 * i / bin_count), 255, 255),
-1)
img = cv2.cvtColor(img, cv2.COLOR_HSV2BGR)
cv2.imshow('hist', img)
showBackProj = False
showHistMask = False
frame = drone_vision.get_latest_valid_picture()
if frame is not None:
(hgt, wid, dep) = frame.shape
cv2.namedWindow('camshift')
cv2.namedWindow('hist')
cv2.moveWindow('hist', 700, 100) # Move to reduce overlap
# Initialize the track window to be the whole frame
track_window = (0, 0, wid, hgt)
#
# Initialize the histogram from the stored image
# Here I am faking a stored image with just a couple of blue colors in an array
# you would want to read the image in from the file instead
histImage = np.array([[[110, 70, 50]],
[[111, 128, 128]],
[[115, 100, 100]],
[[117, 64, 50]],
[[117, 200, 200]],
[[118, 76, 100]],
[[120, 101, 210]],
[[121, 85, 70]],
[[125, 129, 199]],
[[128, 81, 78]],
[[130, 183, 111]]], np.uint8)
histImage = cv2.imread('orange.jpg')
histImage = cv2.cvtColor(histImage,cv2.COLOR_BGR2HSV)
maskedHistIm = cv2.inRange(histImage, np.array((0., 60., 32.)), np.array((180., 255., 255.)))
cv2.imshow("masked",maskedHistIm)
cv2.imshow("histim",histImage)
hist = cv2.calcHist([histImage], [0], maskedHistIm, [16], [0, 180])
cv2.normalize(hist, hist, 0, 255, cv2.NORM_MINMAX)
hist = hist.reshape(-1)
show_hist(hist)
# start processing frames
while cv2.getWindowProperty('camshift', 0) >= 0:
frame = drone_vision.get_latest_valid_picture()
vis = frame.copy()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # convert to HSV
mask = cv2.inRange(hsv, np.array((0., 60., 32.)),
np.array((180., 255., 255.))) # eliminate low and high saturation and value values
# The next line shows which pixels are being used to make the histogram.
# it sets to black all the ones that are masked away for being too over or under-saturated
if showHistMask:
vis[mask == 0] = 0
prob = cv2.calcBackProject([hsv], [0], hist, [0, 180], 1)
prob &= mask
term_crit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1)
track_box, track_window = cv2.CamShift(prob, track_window, term_crit)
print(track_box[1][0]*track_box[1][1])
if showBackProj:
vis[:] = prob[..., np.newaxis]
try:
cv2.ellipse(vis, track_box, (0, 0, 255), 2)
area = track_box[1][0]*track_box[1][1]
if area > 7000:
print("GOING BACK")
bebop.fly_direct(roll=0,pitch=-20,yaw=0,vertical_movement=0,duration=0.5)
#bebop.smart_sleep(1)
elif area < 4000:
print("GOING FORWARD")
bebop.fly_direct(roll=0,pitch=20,yaw=0,vertical_movement=0,duration=0.5)
#bebop.smart_sleep(1)
except:
pass
# print("Track box:", track_box)
cv2.imshow('camshift', vis)
ch = chr(0xFF & cv2.waitKey(5))
if ch == 'q':
break
elif ch == 'b':
showBackProj = not showBackProj
elif ch == 'v':
showHistMask = not showHistMask
bebop.safe_land(10)
cv2.destroyAllWindows()
class DroneColorSegTest:
def __init__(self, testFlying, mamboAddr, use_wifi):
self.bb = [0, 0, 0, 0]
self.bb_threshold = 4000
self.bb_trigger = False
self.testFlying = testFlying
self.mamboAddr = mamboAddr
self.use_wifi = use_wifi
self.mambo = Mambo(self.mamboAddr, self.use_wifi)
self.mamboVision = DroneVisionGUI(
self.mambo,
is_bebop=False,
buffer_size=200,
user_code_to_run=self.mambo_fly_function,
user_args=None)
def color_segmentation(self, args):
img = self.mamboVision.get_latest_valid_picture()
if img is not None:
[((x1, y1), (x2, y2)), ln_color] = cd_color_segmentation(img)
self.bb = [x1, y1, x2, y2]
bb_size = self.get_bb_size()
print('bb_size:', bb_size)
# cv2.imwrite('test_file.png', img) # uncomment to save latest pic
if bb_size >= self.bb_threshold:
print('orange detected')
self.bb_trigger = True
# else:
# self.bb_trigger = False
else:
print('image is None')
def get_bb_size(self):
''' returns area of self.bb (bounding box) '''
return (self.bb[2] - self.bb[0]) * (self.bb[3] - self.bb[1])
def mambo_fly_function(self, mamboVision, args):
"""
self.mambo takes off and yaws slowly in a circle until the vision processing
detects a large patch of orange. It then performs a flip and lands.
"""
if (self.testFlying):
print("taking off!")
self.mambo.safe_takeoff(5)
if (self.mambo.sensors.flying_state != "emergency"):
print("flying state is %s" % self.mambo.sensors.flying_state)
print("Flying direct: going up")
self.mambo.fly_direct(roll=0,
pitch=0,
yaw=0,
vertical_movement=15,
duration=2)
print("flying state is %s" % self.mambo.sensors.flying_state)
print("yawing slowly")
for deg in range(36):
self.mambo.turn_degrees(10)
if self.bb_trigger:
break
self.mambo.smart_sleep(1)
print("flying state is %s" % self.mambo.sensors.flying_state)
print("flip left")
success = self.mambo.flip(direction="left")
print("self.mambo flip result %s" % success)
self.mambo.smart_sleep(2)
print("landing")
print("flying state is %s" % self.mambo.sensors.flying_state)
self.mambo.safe_land(5)
else:
print("Sleeeping for 15 seconds - move the self.mambo around")
self.mambo.smart_sleep(15)
# done doing vision demo
print("Ending the sleep and vision")
self.mamboVision.close_video()
self.mambo.smart_sleep(5)
print("disconnecting")
self.mambo.disconnect()
def run_test(self):
print("trying to connect to self.mambo now")
success = self.mambo.connect(num_retries=3)
print("connected: %s" % success)
if (success):
# get the state information
print("sleeping")
self.mambo.smart_sleep(1)
self.mambo.ask_for_state_update()
self.mambo.smart_sleep(1)
print("Preparing to open vision")
self.mamboVision.set_user_callback_function(
self.color_segmentation, user_callback_args=None)
self.mamboVision.open_video()
