Esempio n. 1
0
    def retreive(self):
        if self.is_available():

            #grab results
            results = self.parent_conn.recv()

            #update processTime. All processes must return a runtime through the pipe
            self.processTime, self.processTimeSet = self.rolling_average(
                self.processTimeSet, results[1])

            #Calculate real runtime. Diagnostic purposes only
            #In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
            actualRunTime = current_milli_time() - self.lastRetreival
            self.lastRetreival = current_milli_time()

            VN_logger.text(
                VN_logger.PERFORMANCE,
                "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} "
                .format(round(1000.0 / (self.runTime)),
                        round(1000.0 / actualRunTime),
                        round((1000.0 / results[1])),
                        round(1000.0 / self.captureTime),
                        len(multiprocessing.active_children())))
            return results

        return None
Esempio n. 2
0
    def set_velocity(self, velocity_x, velocity_y, velocity_z):
        #only let commands through at 10hz
        if (time.time() - self.last_set_velocity) > self.vel_update_rate:
            self.last_set_velocity = time.time()
            # create the SET_POSITION_TARGET_LOCAL_NED command
            msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
                0,  # time_boot_ms (not used)
                0,
                0,  # target system, target component
                mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # frame
                0x01C7,  # type_mask (ignore pos | ignore acc)
                0,
                0,
                0,  # x, y, z positions (not used)
                velocity_x,
                velocity_y,
                velocity_z,  # x, y, z velocity in m/s
                0,
                0,
                0,  # x, y, z acceleration (not used)
                0,
                0)  # yaw, yaw_rate (not used)

            # send command to vehicle
            self.vehicle.send_mavlink(msg)
            self.vehicle.flush()
            VN_logger.text(
                VN_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(
                    velocity_x, velocity_y, velocity_z))
Esempio n. 3
0
    def image_capture_background(self, imgcap_connection):
        # exit immediately if imgcap_connection is invalid
        if imgcap_connection is None:
            VN_logger.text(VN_logger.GENERAL, "image_capture failed because pipe is uninitialised")
            return

        # clear latest image
        latest_image = None

        while True:
            # constantly get the image from the webcam
            success_flag, image=self.camera.read()

            # if successful overwrite our latest image
            if success_flag:
                latest_image = image

            # check if the parent wants the image
            if imgcap_connection.poll():
                recv_obj = imgcap_connection.recv()
                # if -1 is received we exit
                if recv_obj == -1:
                    break

                # otherwise we return the latest image
                imgcap_connection.send(latest_image)

        # release camera when exiting
        self.camera.release()
Esempio n. 4
0
    def get_home(self, wait_for_arm=False):

        #wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
        if (wait_for_arm and self.last_home is None):
            VN_logger.text(
                VN_logger.GENERAL,
                'Waiting for intial home lock: Requires armed status....')
            while (self.vehicle.armed == False):
                time.sleep(0.3)
            VN_logger.text(VN_logger.GENERAL, 'Got valid intial home location')

        if (time.time() - self.last_home_call > self.home_update_rate):
            self.last_home_call = time.time()

            # download the vehicle waypoints
            mission_cmds = self.vehicle.commands
            mission_cmds.download()
            mission_cmds.wait_valid()
            # get the home lat and lon
            home_lat = mission_cmds[0].x
            home_lon = mission_cmds[0].y

            self.last_home = Location(home_lat, home_lon, 0)

        return self.last_home
Esempio n. 5
0
    def image_capture_background(self, imgcap_connection):
        # exit immediately if imgcap_connection is invalid
        if imgcap_connection is None:
            VN_logger.text(
                VN_logger.GENERAL,
                "image_capture failed because pipe is uninitialised")
            return

        # clear latest image
        latest_image = None

        while True:
            # constantly get the image from the webcam
            success_flag, image = self.camera.read()

            # if successful overwrite our latest image
            if success_flag:
                latest_image = image

            # check if the parent wants the image
            if imgcap_connection.poll():
                recv_obj = imgcap_connection.recv()
                # if -1 is received we exit
                if recv_obj == -1:
                    break

                # otherwise we return the latest image
                imgcap_connection.send(latest_image)

        # release camera when exiting
        self.camera.release()
Esempio n. 6
0
 def report_landing_target(self, angle_x, angle_y, distance):
     #only let commands through at 33hz
     if(time.time() - self.last_report_landing_target) > self.landing_update_rate:
         self.last_report_landing_target_ = time.time()
         # create the LANDING TARGET message
         msg = self.vehicle.message_factory.landing_target_encode(
                                                      0,       # landing target number (not used)
                                                      8, # frame
                                                      angle_x,   # Angular offset x axis
                                                      angle_y,   # Angular offset y axis
                                                      distance)  #Distance to target
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()
         VN_logger.text(VN_logger.AIRCRAFT, 'Sent AngX: {0}, AngY: {1}, Dist: {2}'.format(angle_x,angle_y,distance))
Esempio n. 7
0
	def retreive(self):
		if self.is_available():

			#grab results
			results = self.parent_conn.recv()

			#update processTime. All processes must return a runtime through the pipe
			self.processTime, self.processTimeSet = self.rolling_average(self.processTimeSet, results[0]) 

			#Calculate real runtime. Diagnostic purposes only
			#In an ideal system the dispatch rate would equal the retreival rate. That is not the case here
			actualRunTime = current_milli_time() - self.lastRetreival
			self.lastRetreival = current_milli_time()

			VN_logger.text(VN_logger.PERFORMANCE, "DispatchHz: {0} runHz: {1} ProcessHz: {2} CaptureHz: {3} Processes: {4} ".format(round(1000.0/(self.runTime)), round(1000.0/actualRunTime), round((1000.0/results[0])),round(1000.0/self.captureTime), len(multiprocessing.active_children())))
			return results

		return None
Esempio n. 8
0
 def set_velocity(self, velocity_x, velocity_y, velocity_z):
     #only let commands through at 10hz
     if(time.time() - self.last_set_velocity) > self.vel_update_rate:
         self.last_set_velocity = time.time()
         # create the SET_POSITION_TARGET_LOCAL_NED command
         msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
                                                      0,       # time_boot_ms (not used)
                                                      0, 0,    # target system, target component
                                                      mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
                                                      0x01C7,  # type_mask (ignore pos | ignore acc)
                                                      0, 0, 0, # x, y, z positions (not used)
                                                      velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
                                                      0, 0, 0, # x, y, z acceleration (not used)
                                                      0, 0)    # yaw, yaw_rate (not used)
         
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()
         VN_logger.text(VN_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(velocity_x,velocity_y,velocity_z))
Esempio n. 9
0
 def report_landing_target(self, angle_x, angle_y, distance):
     #only let commands through at 33hz
     if (time.time() -
             self.last_report_landing_target) > self.landing_update_rate:
         self.last_report_landing_target_ = time.time()
         # create the LANDING TARGET message
         msg = self.vehicle.message_factory.landing_target_encode(
             0,  # landing target number (not used)
             8,  # frame
             angle_x,  # Angular offset x axis
             angle_y,  # Angular offset y axis
             distance)  #Distance to target
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()
         VN_logger.text(
             VN_logger.AIRCRAFT,
             'Sent AngX: {0}, AngY: {1}, Dist: {2}'.format(
                 angle_x, angle_y, distance))
Esempio n. 10
0
    def get_home(self, wait_for_arm = False):

        #wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
        if(wait_for_arm and self.last_home is None):
            VN_logger.text(VN_logger.GENERAL, 'Waiting for intial home lock: Requires armed status....')
            while(self.vehicle.armed == False):
                time.sleep(0.3)
            VN_logger.text(VN_logger.GENERAL, 'Got valid intial home location')


        if(time.time() - self.last_home_call > self.home_update_rate):
            self.last_home_call = time.time()

            # download the vehicle waypoints
            mission_cmds = self.vehicle.commands
            mission_cmds.download()
            mission_cmds.wait_valid()
            # get the home lat and lon
            home_lat = mission_cmds[0].x
            home_lon = mission_cmds[0].y

            self.last_home = Location(home_lat,home_lon,0)

        return self.last_home
Esempio n. 11
0
    def get_camera(self, index):
        VN_logger.text(VN_logger.GENERAL, 'Starting Camera....')

        # setup video capture
        self.camera = cv2.VideoCapture(index)
        self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, self.img_width)
        self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, self.img_height)

        # check we can connect to camera
        if not self.camera.isOpened():
            VN_logger.text(VN_logger.GENERAL,
                           "failed to open camera, exiting!")
            sys.exit(0)

        VN_logger.text(VN_logger.GENERAL, 'Camera Open!')

        return self.camera
Esempio n. 12
0
    def get_camera(self,index):
        VN_logger.text(VN_logger.GENERAL, 'Starting Camera....')


        # setup video capture
        self.camera = cv2.VideoCapture(index)
        self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,self.img_width)
        self.camera.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,self.img_height)

        # check we can connect to camera
        if not self.camera.isOpened():
            VN_logger.text(VN_logger.GENERAL,"failed to open camera, exiting!")
            sys.exit(0)

        VN_logger.text(VN_logger.GENERAL, 'Camera Open!')

        return self.camera
Esempio n. 13
0

	 	#terminate program
	 	VN_logger.text(VN_logger.GENERAL, 'Vehicle disconnected, Program Terminated')
	 	if(self.simulator == False):
	 		VN_video.stop_capture()




# if starting from mavproxy
if __name__ == "__builtin__":
	# start precision landing
	strat = PrecisionLand()

	# connect to droneapi
	VN_logger.text(VN_logger.GENERAL, 'Connecting to vehicle...')
	strat.connect()
	VN_logger.text(VN_logger.GENERAL, 'Vehicle connected!')

	# run strategy
	strat.run()








Esempio n. 14
0
    def run(self):

        VN_logger.text(VN_logger.GENERAL, "Running {0}".format(self.name()))

        # start a video capture
        if self.simulator:
            VN_logger.text(VN_logger.GENERAL, "Using simulator")
            sim.load_target(self.target_file, self.target_size)
            sim.set_target_location(veh_control.get_home(True))
            # sim.set_target_location(Location(0,0,0))

        else:
            VN_video.start_capture(self.camera_index)

        # create an image processor
        # detector = CircleDetector()
        detector = TopCode()

        # create a queue for images
        imageQueue = []

        # initilize autopilot variables
        location = Location(0, 0, 0)
        attitude = Attitude(0, 0, 0)

        while veh_control.is_connected():

            """
            #kill camera for testing
            if(cv2.waitKey(2) == 1113938):
                self.kill_camera =  not self.kill_camera
            """

            # 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.always_run) or (veh_control.get_mode() == "LAND") or (veh_control.get_mode() == "RTL"):

                # update how often we dispatch a command
                VN_dispatcher.calculate_dispatch_schedule()

                # update simulator
                if self.simulator:
                    # get info from autopilot
                    location = veh_control.get_location()
                    attitude = veh_control.get_attitude()
                    sim.refresh_simulator(location, attitude)

                # grab an image
                frame = None
                capStart = current_milli_time()
                if self.simulator:
                    frame = sim.get_frame()
                else:
                    frame = VN_video.get_image()
                capStop = current_milli_time()

                """
                if(self.kill_camera):
                    frame[:] = (0,255,0)
                """

                # update capture time
                VN_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 VN_dispatcher.is_ready():
                    # queue the image for later use: displaying image, overlays, recording
                    imageQueue.append((frame, self.frames_captured))

                    # the function must be run directly from the class
                    VN_dispatcher.dispatch(target=detector.analyze_frame_async, args=(frame, self.frames_captured))

                    self.frames_captured += 1

                # retreive results
                if VN_dispatcher.is_available():

                    # results of image processor
                    results = VN_dispatcher.retreive()

                    # get image that was passed with the image processor
                    for f in imageQueue:
                        img, frame_id = f
                        if results[0] == frame_id:
                            imageQueue.remove(f)
                            break
                    VN_logger.text(VN_logger.GENERAL, "Frame {0}".format(frame_id))

                    # overlay gui
                    # rend_Image = gui.add_target_highlights(img, results[3])
                    rend_Image = gui.add_ring_highlights(img, results[4])

                    # show/record images
                    VN_logger.image(VN_logger.RAW, img)
                    VN_logger.image(VN_logger.GUI, rend_Image)

                    # display/log data
                    VN_logger.text(
                        VN_logger.ALGORITHM,
                        "RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}".format(
                            results[1], results[2], results[3], results[4]
                        ),
                    )
                    VN_logger.text(VN_logger.DEBUG, "Rings detected: {0}".format(len(results[4])))

                    # send results if we found the landing pad
                    if results[2] is not None:
                        # shift origin to center of the image
                        x_pixel = results[2][0] - (self.camera_width / 2.0)
                        y_pixel = results[2][1] - (
                            self.camera_height / 2.0
                        )  # y-axis is inverted??? Works with arducopter

                        # convert target location to angular radians
                        x_angle = x_pixel * (self.camera_hfov / self.camera_width) * (math.pi / 180.0)
                        y_angle = y_pixel * (self.camera_vfov / self.camera_height) * (math.pi / 180.0)

                        # send commands to autopilot
                        veh_control.report_landing_target(x_angle, y_angle, results[3], 0, 0)

            else:
                VN_logger.text(VN_logger.GENERAL, "Not in landing mode")

        # terminate program
        VN_logger.text(VN_logger.GENERAL, "Vehicle disconnected, Program Terminated")
        if self.simulator == False:
            VN_video.stop_capture()
Esempio n. 15
0
                        )  # y-axis is inverted??? Works with arducopter

                        # convert target location to angular radians
                        x_angle = x_pixel * (self.camera_hfov / self.camera_width) * (math.pi / 180.0)
                        y_angle = y_pixel * (self.camera_vfov / self.camera_height) * (math.pi / 180.0)

                        # send commands to autopilot
                        veh_control.report_landing_target(x_angle, y_angle, results[3], 0, 0)

            else:
                VN_logger.text(VN_logger.GENERAL, "Not in landing mode")

        # terminate program
        VN_logger.text(VN_logger.GENERAL, "Vehicle disconnected, Program Terminated")
        if self.simulator == False:
            VN_video.stop_capture()


# if starting from mavproxy
if __name__ == "__builtin__":
    # start precision landing
    strat = PrecisionLand()

    # connect to droneapi
    VN_logger.text(VN_logger.GENERAL, "Connecting to vehicle...")
    strat.connect()
    VN_logger.text(VN_logger.GENERAL, "Vehicle connected!")

    # run strategy
    strat.run()
Esempio n. 16
0
                        y_angle = y_pixel * (self.camera_vfov /
                                             self.camera_height) * (math.pi /
                                                                    180.0)

                        #send commands to autopilot
                        veh_control.report_landing_target(
                            x_angle, y_angle, results[3], 0, 0)

            else:
                VN_logger.text(VN_logger.GENERAL, 'Not in landing mode')

        #terminate program
        VN_logger.text(VN_logger.GENERAL,
                       'Vehicle disconnected, Program Terminated')
        if (self.simulator == False):
            VN_video.stop_capture()


# if starting from mavproxy
if __name__ == "__builtin__":
    # start precision landing
    strat = PrecisionLand()

    # connect to droneapi
    VN_logger.text(VN_logger.GENERAL, 'Connecting to vehicle...')
    strat.connect()
    VN_logger.text(VN_logger.GENERAL, 'Vehicle connected!')

    # run strategy
    strat.run()
Esempio n. 17
0
    def run(self):

        VN_logger.text(VN_logger.GENERAL, 'Running {0}'.format(self.name()))

        #start a video capture
        if (self.simulator):
            VN_logger.text(VN_logger.GENERAL, 'Using simulator')
            sim.load_target(self.target_file, self.target_size)
            sim.set_target_location(veh_control.get_home(True))
            #sim.set_target_location(Location(0,0,0))

        else:
            VN_video.start_capture(self.camera_index)

        #create an image processor
        #detector = CircleDetector()
        detector = TopCode()

        #create a queue for images
        imageQueue = []

        #initilize autopilot variables
        location = Location(0, 0, 0)
        attitude = Attitude(0, 0, 0)

        while veh_control.is_connected():
            '''
            #kill camera for testing
            if(cv2.waitKey(2) == 1113938):
                self.kill_camera =  not self.kill_camera
            '''

            #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.always_run) or (veh_control.get_mode()
                                     == "LAND") or (veh_control.get_mode()
                                                    == "RTL"):

                #update how often we dispatch a command
                VN_dispatcher.calculate_dispatch_schedule()

                #update simulator
                if (self.simulator):
                    #get info from autopilot
                    location = veh_control.get_location()
                    attitude = veh_control.get_attitude()
                    sim.refresh_simulator(location, attitude)

                # grab an image
                frame = None
                capStart = current_milli_time()
                if (self.simulator):
                    frame = sim.get_frame()
                else:
                    frame = VN_video.get_image()
                capStop = current_milli_time()
                '''
                if(self.kill_camera):
                    frame[:] = (0,255,0)
                '''

                #update capture time
                VN_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 VN_dispatcher.is_ready():
                    #queue the image for later use: displaying image, overlays, recording
                    imageQueue.append((frame, self.frames_captured))

                    #the function must be run directly from the class
                    VN_dispatcher.dispatch(target=detector.analyze_frame_async,
                                           args=(frame, self.frames_captured))

                    self.frames_captured += 1

                #retreive results
                if VN_dispatcher.is_available():

                    #results of image processor
                    results = VN_dispatcher.retreive()

                    # get image that was passed with the image processor
                    for f in imageQueue:
                        img, frame_id = f
                        if results[0] == frame_id:
                            imageQueue.remove(f)
                            break
                    VN_logger.text(VN_logger.GENERAL,
                                   'Frame {0}'.format(frame_id))

                    #overlay gui
                    #rend_Image = gui.add_target_highlights(img, results[3])
                    rend_Image = gui.add_ring_highlights(img, results[4])

                    #show/record images
                    VN_logger.image(VN_logger.RAW, img)
                    VN_logger.image(VN_logger.GUI, rend_Image)

                    #display/log data
                    VN_logger.text(
                        VN_logger.ALGORITHM,
                        'RunTime: {0} Center: {1} Distance: {2} Raw Target: {3}'
                        .format(results[1], results[2], results[3],
                                results[4]))
                    VN_logger.text(
                        VN_logger.DEBUG,
                        'Rings detected: {0}'.format(len(results[4])))

                    #send results if we found the landing pad
                    if (results[2] is not None):
                        #shift origin to center of the image
                        x_pixel = results[2][0] - (self.camera_width / 2.0)
                        y_pixel = results[2][1] - (
                            self.camera_height / 2.0
                        )  #y-axis is inverted??? Works with arducopter

                        #convert target location to angular radians
                        x_angle = x_pixel * (self.camera_hfov /
                                             self.camera_width) * (math.pi /
                                                                   180.0)
                        y_angle = y_pixel * (self.camera_vfov /
                                             self.camera_height) * (math.pi /
                                                                    180.0)

                        #send commands to autopilot
                        veh_control.report_landing_target(
                            x_angle, y_angle, results[3], 0, 0)

            else:
                VN_logger.text(VN_logger.GENERAL, 'Not in landing mode')

        #terminate program
        VN_logger.text(VN_logger.GENERAL,
                       'Vehicle disconnected, Program Terminated')
        if (self.simulator == False):
            VN_video.stop_capture()