def run_sda_process(logger_queue, waypoints, sda_status, sda_avoid_coords,
vehicle_state_data, mission_information_data):
SUASSystem.logging.logger_worker_configurer(logger_queue)
logger_name = multiprocessing.current_process().name
log(logger_name, "Instantiating SDA converter")
while True:
try:
sda_converter = SUASSystem.SDAConverter(
vehicle_state_data[0].get_location(),
mission_information_data[0]["fly_zones"])
break
except:
sleep(0.1)
log(logger_name, "SDA converter instantiated")
while True:
if "enabled" in str(sda_status.value).lower():
current_location = vehicle_state_data[0].get_location()
current_waypoint_number = vehicle_state_data[
0].get_current_waypoint_number()
if current_waypoint_number != 0:
current_uav_waypoint = waypoints[current_waypoint_number - 1]
sda_converter.set_waypoint(
SUASSystem.Location(current_uav_waypoint.x,
current_uav_waypoint.y,
current_uav_waypoint.z * 3.28084))
else:
sda_converter.set_waypoint(
SUASSystem.Location(waypoints[1].x, waypoints[1].y,
waypoints[1].z))
sda_converter.reset_obstacles()
for stationary_obstacle in mission_information_data[
"stationary_obstacles"]:
sda_converter.add_obstacle(
get_obstacle_location(stationary_obstacle, MSL_ALT),
stationary_obstacle)
for moving_obstacle in mission_information_data[
"moving_obstacles"]:
sda_converter.add_obstacle(
get_obstacle_location(moving_obstacle, MSL_ALT),
moving_obstacle)
sda_converter.set_uav_position(current_location)
sda_converter.avoid_obstacles()
if not sda_converter.has_uav_completed_guided_path():
try:
sda_avoid_coords[
0] = sda_converter.get_uav_avoid_coordinates()
except:
sda_avoid_coords.append(
sda_converter.get_uav_avoid_coordinates())
sleep(0.5)
def initialize_image_processing_process(img_proc_status, location_log,
targets_to_submit):
log(gcs_logger_name, "Instantiating Image Processing process")
img_proc_process = multiprocessing.Process(
target=SUASSystem.run_img_proc_process,
args=(logger_queue, img_proc_status, location_log, targets_to_submit))
#img_proc_process.start()
log(gcs_logger_name, "Image Processing process instantiated")
return img_proc_process
def connect_to_vehicle():
log(gcs_logger_name,
"Connecting to UAV on: %s" % GCSSettings.UAV_CONNECTION_STRING)
vehicle = dronekit.connect(GCSSettings.UAV_CONNECTION_STRING,
wait_ready=True)
vehicle.wait_ready('autopilot_version')
log(gcs_logger_name,
"Connected to UAV on: %s" % GCSSettings.UAV_CONNECTION_STRING)
SUASSystem.logging.log_vehicle_state(vehicle, gcs_logger_name)
return vehicle
def download_waypoints(vehicle):
log(
gcs_logger_name, "Downloading waypoints from UAV on: %s" %
GCSSettings.UAV_CONNECTION_STRING)
waypoints = vehicle.commands
waypoints.download()
waypoints.wait_ready()
log(
gcs_logger_name, "Waypoints successfully downloaded from UAV on: %s" %
GCSSettings.UAV_CONNECTION_STRING)
return waypoints
def gcs_process(sda_status, img_proc_status, interop_position_update_rate):
manager = multiprocessing.Manager()
vehicle_state_data = manager.list()
mission_information_data = manager.list()
targets_to_submit = manager.list()
sda_avoid_coords = manager.list()
location_log = manager.list()
vehicle = connect_to_vehicle()
waypoints = download_waypoints(vehicle)
competition_viewer_process = initialize_competition_viewer_process(
vehicle_state_data, mission_information_data)
img_proc_process = initialize_image_processing_process(
img_proc_status, location_log, targets_to_submit)
sda_process = initialize_sda_process(sda_status, waypoints,
sda_avoid_coords, vehicle_state_data,
mission_information_data)
log(gcs_logger_name, "Completed instantiation of all child processes")
interop_client = SUASSystem.InteropClientConverter(
GCSSettings.MSL_ALT, GCSSettings.INTEROP_URL,
GCSSettings.INTEROP_USERNAME, GCSSettings.INTEROP_PASSWORD)
guided_waypoint_location = None
vehicle_state_data.append(
SUASSystem.get_vehicle_state(vehicle, GCSSettings.MSL_ALT))
while True:
interop_position_update_rate.value += 1
vehicle_state_data[0] = SUASSystem.get_vehicle_state(
vehicle, GCSSettings.MSL_ALT)
current_location = SUASSystem.Location(
vehicle.location.global_relative_frame.lat,
vehicle.location.global_relative_frame.lon,
vehicle.location.global_relative_frame.alt)
print(current_location)
location_log.append(current_location)
"""if (vehicle.location.global_relative_frame.alt * 3.28084) > GCSSettings.SDA_MIN_ALT and (vehicle.mode.name == "GUIDED" or vehicle.mode.name == "AUTO"):
log("root", "Avoiding obstacles...")
vehicle.mode = VehicleMode("GUIDED")
guided_waypoint_location = sda_avoid_coords[0][0]
vehicle.simple_goto(guided_waypoint_location.as_global_relative_frame())
if waypoint_location:
if vehicle.mode.name == "GUIDED" and has_uav_reached_waypoint(current_location, guided_waypoint_location):
vehicle.mode = VehicleMode("AUTO")"""
interop_client.post_telemetry(current_location,
vehicle_state_data[0].get_direction())
sleep(0.1)
def initialize_competition_viewer_process(vehicle_state_data,
mission_information_data):
log(gcs_logger_name, "Instantiating Competition Viewer process")
competition_viewer_process = multiprocessing.Process(
target=SUASSystem.run_competition_viewer_process,
args=(
vehicle_state_data,
mission_information_data,
))
competition_viewer_process.start()
log(gcs_logger_name, "Competition Viewer process instantiated")
return competition_viewer_process
def initialize_sda_process(sda_status, waypoints, sda_avoid_coords,
vehicle_state_data, mission_information_data):
log(gcs_logger_name, "Instantiating SDA process")
sda_process = multiprocessing.Process(target=SUASSystem.run_sda_process,
args=(
logger_queue,
waypoints,
sda_status,
sda_avoid_coords,
vehicle_state_data,
mission_information_data,
))
#sda_process.start()
log(gcs_logger_name, "SDA process instantiated")
return sda_process
def run_img_proc_process(logger_queue, img_proc_status, location_log,
targets_to_submit):
SUASSystem.logging.logger_worker_configurer(logger_queue)
logger_name = multiprocessing.current_process().name
log(logger_name, "Instantiating letter categorizer")
eigenvectors = load_numpy_arr(
GCSSettings.BASE_LETTER_CATEGORIZER_PCA_PATH +
"/Data/Eigenvectors/eigenvectors 0.npy")
projections_path = GCSSettings.BASE_LETTER_CATEGORIZER_PCA_PATH + "/Data/Projections"
mean = load_numpy_arr(GCSSettings.BASE_LETTER_CATEGORIZER_PCA_PATH +
"/Data/Mean/mean_img 0.npy")
num_dim = 20
letter_categorizer = Categorizer(eigenvectors, mean, projections_path,
KMeansCompare, num_dim)
log(logger_name, "Letter categorizer instantiated successfully")
log(logger_name, "Instantiating orientation solver")
orientation_eigenvectors = load_numpy_arr(
GCSSettings.BASE_ORIENTATION_CLASSIFIER_PCA_PATH +
"/Data/Eigenvectors/eigenvectors 0.npy")
orientation_projections_path = GCSSettings.BASE_ORIENTATION_CLASSIFIER_PCA_PATH + "/Data/Projections"
orientation_mean = load_numpy_arr(
GCSSettings.BASE_ORIENTATION_CLASSIFIER_PCA_PATH +
"/Data/Mean/mean_img 0.npy")
orientation_num_dim = 50
orientation_solver = OrientationSolver(
orientation_eigenvectors, orientation_mean,
GCSSettings.BASE_ORIENTATION_CLASSIFIER_PCA_PATH, orientation_num_dim)
log(logger_name, "Orientation solver instantiated")
if os.path.exists(GCSSettings.GENERATED_DATA_LOCATION):
shutil.rmtree(GCSSettings.GENERATED_DATA_LOCATION)
os.mkdir(GCSSettings.GENERATED_DATA_LOCATION)
os.mkdir(
os.path.join(GCSSettings.GENERATED_DATA_LOCATION,
"object_file_format"))
sd_path = os.path.join("/Volumes", GCSSettings.SD_CARD_NAME, "DCIM")
gps_coords = []
while True:
if "enabled" in str(img_proc_status.value).lower():
if len(location_log) != 0:
try:
gps_coords.append(location_log[0])
gps_coords = gps_coords[1:]
except:
pass
log(logger_name, "Waiting for path to exist...")
# Wait for SD card to be loaded
if not os.path.exists(sd_path):
sleep(1)
continue
# Once SD card is loaded, begin processing
# For every image on the SD card
# 1. Load the image
# 2. Process the crops, determine the ones that are targets
# 3. Save the targets to GENERATED_DATA_LOCATION
# 4. Upload the targets to interop server
log(logger_name, "Beginning image processing...")
geo_stamps = GeoStamps(
[GeoStamp([38.38875, -77.27532],
datetime.now())]) #gps_coords)
crop_index = 0
all_target_crops = []
for pic_folder in os.listdir(sd_path):
pictures_dir_path = os.path.join(sd_path, pic_folder)
for pic_name in os.listdir(pictures_dir_path):
if ".SRW" in pic_name:
start_time = default_timer()
log(logger_name, "Loading image " + pic_name)
pic_path = os.path.join(pictures_dir_path, pic_name)
img = rawpy.imread(pic_path).postprocess()
rgb_image = Image.fromarray(numpy.roll(img, 1, axis=0))
image_timestamp = get_image_timestamp(pic_path)
target_crops = TargetCropper3.get_target_crops_from_img(
rgb_image, image_timestamp, geo_stamps, 6)
target_crops = TargetCrop.get_non_duplicate_crops(
all_target_crops, target_crops,
GCSSettings.MIN_DIST_BETWEEN_TARGETS_KM)
all_target_crops.extend(target_crops)
log(
logger_name,
"Finished processing " + pic_name + " in " +
str(default_timer() - start_time) + " seconds")
for target_crop in target_crops:
try:
log(
logger_name,
"Identifying target characteristics of target #"
+ str(crop_index))
runtime_target = RuntimeTarget(
target_crop, letter_categorizer,
orientation_solver)
target_json_output = runtime_target.get_competition_json_output(
)
log(
logger_name,
"Saving target characteristics of target #"
+ str(crop_index))
output_pic_name = os.path.join(
GENERATED_DATA_LOCATION,
"object_file_format",
str(crop_index) + ".png")
output_json_name = os.path.join(
GENERATED_DATA_LOCATION,
"object_file_format",
str(crop_index) + ".txt")
save_json_data(
output_json_name,
{"target_json_output": "Testing"
}) #target_json_output)
target_crop.get_crop_img().save(
output_pic_name)
crop_index += 1
except:
log(logger_name,
"ERROR: Could not process a target crop")
sleep(0.5)