def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
'''
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:'''
sc_video.start_capture(self.camera_index)
#camera = balloon_video.get_camera()
video_writer = balloon_video.open_video_writer()
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
print location
print attitude
# Take each frame
#_, frame = camera.read()
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
# grab an image
capStart = current_milli_time()
frame = sc_video.get_image()
capStop = current_milli_time()
#frame = sc_video.undisort_image(frame)
#cv2.imshow('frame',frame)
# write the frame
video_writer.write(frame)
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
#######
sc_dispatcher.dispatch(target=balloon_finder.analyse_frame, args=(frame,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
#rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
#sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'found: {0} x: {1} y: {2} radius: {3}'.format(results[0],results[1],results[2],results[3]))
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if (self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if (self.allow_reset):
sc_logger.text(sc_logger.GENERAL,
'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
'''
#get info from autopilot
location = Location(0.000009,0,location.alt)
attitude = Attitude(0,0,0)
'''
#update simulator
if (self.simulator):
sim.refresh_simulator(location, attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
'''
if(self.kill_camera):
frame[:] = (0,255,0)
'''
#update capture time
sc_dispatcher.update_capture_time(capStop - capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location, attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame,
args=(
frame,
attitude,
))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL,
'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(
sc_logger.ALGORITHM,
'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'
.format(results[0], results[1], results[2],
results[3]))
sc_logger.text(sc_logger.AIRCRAFT, attitude)
sc_logger.text(sc_logger.AIRCRAFT, location)
#send commands to autopilot
self.control(results, attitude, location)
else:
if (self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL,
'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL,
'Vehicle disconnected, Program Terminated')
if (self.simulator == False):
sc_video.stop_capture()
def analyze_frame(self, child_conn, img, craftAttitude):
#start timer
start = current_milli_time()
#blur image and grayscale
#img = cv2.medianBlur(img,5)
#grayscale image
cimg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#canny edge detector
edges = cv2.Canny(cimg, 100, 200, 3)
if edges is not None:
#locate contours
contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
#turn contours into ellipses
circles = np.empty((len(contours)), object)
circlesCnt = 0
for i in range(0, len(contours)):
contour = contours[i]
#make sure contour contains enough point for an ellipse
if (len(contour) > 4):
#detect an ellipse
ellipse = cv2.fitEllipse(contour)
#only take ellipses which are round
if self.checkEccentricity(ellipse, self.eccentricity):
circles[circlesCnt] = ellipse
circlesCnt += 1
#if circles were found then we look for nested circles
if circlesCnt > 0:
#get rid of null elements
circles = np.resize(circles, circlesCnt)
#look for nested ellipses
nestedCircles = self.detectNested(circles)
#if at least min_circles circles are nested look for target
#Times min_circles by two because we haven't removed repeat/stacked circles yet
if len(nestedCircles) > (self.min_circles * 2):
#look for circles with a common center
self.finalTarget, center = self.findCommonCenter(
nestedCircles)
#we found the target position on xy-plane
if self.finalTarget is not None:
#decode the target rings for a list of ring ratios
ratios = self.tagAspectRatio(self.finalTarget)
#try to calculate distance to target
if ratios is not None:
distance = self.calcDistToTarget(
self.finalTarget, ratios)
stop = current_milli_time()
child_conn.send((stop - start, center, distance,
self.finalTarget))
return
#unable to calculate distance due to invalid data
else:
stop = current_milli_time()
child_conn.send(
(stop - start, center, -1, self.finalTarget))
return
#unable to locate target
stop = current_milli_time()
child_conn.send((stop - start, None, -1, None))
return
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
sim.set_target_location(veh_control.get_home())
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if(self.allow_reset):
sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
'''
#get info from autopilot
location = Location(0.000009,0,location.alt)
attitude = Attitude(0,0,0)
'''
#update simulator
if(self.simulator):
sim.refresh_simulator(location,attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
'''
if(self.kill_camera):
frame[:] = (0,255,0)
'''
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
'''
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
'''
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#send commands to autopilot
self.control(results,attitude,location)
else:
if(self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated')
if(self.simulator == False):
sc_video.stop_capture()
def run(self):
sc_logger.text(sc_logger.GENERAL, 'running {0}'.format(self.name()))
#start a video capture
if(self.simulator):
sc_logger.text(sc_logger.GENERAL, 'Using simulator')
#-35.362664, 149.166803 -35.362902 149.166249
sim.set_target_location(Location(-35.363134 ,149.165429,0))
#sim.set_target_location(Location(0,0,0))
else:
sc_video.start_capture(self.camera_index)
#camera = balloon_video.get_camera()
video_writer = balloon_video.open_video_writer()
#create an image processor
detector = CircleDetector()
#create a queue for images
imageQueue = Queue.Queue()
#create a queue for vehicle info
vehicleQueue = Queue.Queue()
while veh_control.is_connected():
'''
#kill camera for testing
if(cv2.waitKey(2) == 1113938):
self.kill_camera = not self.kill_camera
'''
#Reintialize the landing program when entering a landing mode
if veh_control.controlling_vehicle():
if not self.in_control:
if(self.allow_reset):
sc_logger.text(sc_logger.GENERAL, 'Program initialized to start state')
self.initialize_landing()
self.in_control = True
else:
self.in_control = False
# seach
if(self.flag==False):
# has issues
#search in area
if(self.point==1):
point1 = Location(self.point1_lat, self.point1_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point1_lon):
self.point=2
if(self.point==2):
point2 = Location(self.point2_lat, self.point2_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point2)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point1_lat):
self.point=3
if(self.point==3):
point3 = Location(self.point3_lat, self.point3_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point3)
self.vehicle.flush()
if(veh_control.get_location().lon>=self.point3_lon):
self.point=4
if(self.point==4):
point4 = Location(self.point4_lat, self.point4_lon, 5, is_relative=True)
#point1 = Location(30.264233, 120.118813, 3, is_relative=True)
self.vehicle.commands.goto(point4)
self.vehicle.flush()
if(veh_control.get_location().lat>=self.point4_lat):
self.point=1
'''
print "Going to first point..."
#30.264233, 120.118813 -35.362664, 149.166803
point1 = Location(30.264233, 120.118813, 4, is_relative=True)
self.vehicle.commands.goto(point1)
self.vehicle.flush()
'''
#we are in the landing zone or in a landing mode and we are still running the landing program
#just because the program is running does not mean it controls the vehicle
#i.e. in the landing area but not in a landing mode
#FIXME add inside_landing_area() back to conditional
if (self.in_control or self.always_run) and self.pl_enabled:
#update how often we dispatch a command
sc_dispatcher.calculate_dispatch_schedule()
#get info from autopilot
location = veh_control.get_location()
attitude = veh_control.get_attitude()
#update simulator
if(self.simulator):
sim.refresh_simulator(location,attitude)
# grab an image
capStart = current_milli_time()
frame = self.get_frame()
capStop = current_milli_time()
# write the frame
video_writer.write(frame)
#update capture time
sc_dispatcher.update_capture_time(capStop-capStart)
#Process image
#We schedule the process as opposed to waiting for an available core
#This brings consistancy and prevents overwriting a dead process before
#information has been grabbed from the Pipe
if sc_dispatcher.is_ready():
#queue the image for later use: displaying image, overlays, recording
imageQueue.put(frame)
#queue vehicle info for later use: position processing
vehicleQueue.put((location,attitude))
#the function must be run directly from the class
sc_dispatcher.dispatch(target=detector.analyze_frame, args=(frame,attitude,))
#retreive results
if sc_dispatcher.is_available():
sc_logger.text(sc_logger.GENERAL, 'Frame {0}'.format(self.frame_count))
self.frame_count += 1
#results of image processor
results = sc_dispatcher.retreive()
# get image that was passed with the image processor
img = imageQueue.get()
#get vehicle position that was passed with the image processor
location, attitude = vehicleQueue.get()
#overlay gui
rend_Image = gui.add_target_highlights(img, results[3])
#show/record images
sc_logger.image(sc_logger.RAW, img)
sc_logger.image(sc_logger.GUI, rend_Image)
#display/log data
sc_logger.text(sc_logger.ALGORITHM,'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'.format(results[0],results[1],results[2],results[3]))
sc_logger.text(sc_logger.AIRCRAFT,attitude)
sc_logger.text(sc_logger.AIRCRAFT,location)
#send commands to autopilot
if(results[2]!=-1):
self.flag = True
if(self.flag==True):
self.control(results,attitude,location)
else:
if(self.pl_enabled == False):
sc_logger.text(sc_logger.GENERAL, 'Landing disabled')
else:
sc_logger.text(sc_logger.GENERAL, 'Not in landing mode or Landing Area')
#terminate program
sc_logger.text(sc_logger.GENERAL, 'Vehicle disconnected, Program Terminated')
if(self.simulator == False):
sc_video.stop_capture()
def analyze_frame(self, child_conn, img, craftAttitude): #start timer start = current_milli_time() #blur image and grayscale #img = cv2.medianBlur(img,5) #grayscale image cimg = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) #canny edge detector edges = cv2.Canny(cimg,100,200,3) if edges is not None: #locate contours contours, hierarchy = cv2.findContours(edges,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) #turn contours into ellipses circles = np.empty((len(contours)),object) circlesCnt = 0 for i in range(0,len(contours)): contour = contours[i] #make sure contour contains enough point for an ellipse if(len(contour) > 4): #detect an ellipse ellipse = cv2.fitEllipse(contour) #only take ellipses which are round if self.checkEccentricity(ellipse,self.eccentricity): circles[circlesCnt] = ellipse circlesCnt += 1 #if circles were found then we look for nested circles if circlesCnt > 0: #get rid of null elements circles = np.resize(circles,circlesCnt) #look for nested ellipses nestedCircles = self.detectNested(circles) #if at least min_circles circles are nested look for target #Times min_circles by two because we haven't removed repeat/stacked circles yet if len(nestedCircles) > (self.min_circles * 2): #look for circles with a common center self.finalTarget, center = self.findCommonCenter(nestedCircles) #we found the target position on xy-plane if self.finalTarget is not None: #decode the target rings for a list of ring ratios ratios = self.tagAspectRatio(self.finalTarget) #try to calculate distance to target if ratios is not None: distance = self.calcDistToTarget(self.finalTarget,ratios) stop = current_milli_time() child_conn.send((stop-start,center, distance, self.finalTarget)) return #unable to calculate distance due to invalid data else: stop = current_milli_time() child_conn.send(( stop-start, center, -1, self.finalTarget)) return #unable to locate target stop = current_milli_time() child_conn.send((stop-start,None,-1,None)) return
