Ejemplo 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)


		#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()
Ejemplo 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()
Ejemplo n.º 3
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()