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
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))
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()
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
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()
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))
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
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))
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))
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
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
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
#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()
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()
) # 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()
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()
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()