Exemplo n.º 1
0
	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]))
Exemplo n.º 2
0
    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()
Exemplo n.º 3
0
    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
Exemplo n.º 4
0
	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()
Exemplo n.º 5
0
	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()
Exemplo n.º 6
0
	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