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 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 is_uav_at_location(current_location, waypoint_location, threshold_dist=5):
dx, dy, dz = SUASSystem.haversine(waypoint_location, current_location)
euclidean_dist = (dx**2 + dy**2 + dz**2)**0.5
if euclidean_dist < threshold_dist:
return True
return False
def run_sda_process(logger_queue, waypoints, sda_status, sda_avoid_coords, UAV_status, vehicle_state_data, mission_information_data):
SUASSystem.suas_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")
starting_coords = vehicle_state_data[0].get_location()
while True:
if sda_status.value == "connected":
current_location = vehicle_state_data[0].get_location()
current_location.alt = current_location.get_alt() - SUASSystem.GCSSettings.MSL_ALT
current_waypoint_number = vehicle_state_data[0].get_current_waypoint_number()
if current_waypoint_number != 0:
current_uav_waypoint = waypoints[current_waypoint_number - 1]
current_uav_waypoint_location = SUASSystem.Location(current_uav_waypoint.x, current_uav_waypoint.y, current_uav_waypoint.z * 3.28084)
if is_waypoint_avoidable(current_uav_waypoint) and not is_uav_at_location(current_location, current_uav_waypoint_location):
sda_converter.set_waypoint(current_uav_waypoint_location)
sda_converter.reset_obstacles()
for stationary_obstacle in mission_information_data[0]["stationary_obstacles"]:
sda_converter.add_obstacle(stationary_obstacle)
sda_converter.set_uav_position(current_location)
if not UAV_status.value == "GUIDED":
sda_converter.avoid_obstacles()
if not sda_converter.has_uav_completed_guided_path():
UAV_status.value = "GUIDED"
try:
sda_avoid_coords[0] = sda_converter.get_uav_avoid_coordinates()
except:
sda_avoid_coords.append(sda_converter.get_uav_avoid_coordinates())
if sda_converter.has_uav_completed_guided_path():
print("changing to auto")
UAV_status.value = "AUTO"
sda_converter.current_path = numpy.array([])
sleep(0.5)
def get_interop_data(self):
try:
active_interop_mission = self.interop_client[0].get_active_mission(
)
obstacles = self.interop_client[0].get_obstacles()
active_interop_mission_json = SUASSystem.get_mission_json(
active_interop_mission, obstacles)
data = {
"status":
"connected",
"emergent_position": [
active_interop_mission_json["emergent_last_known_pos"]
["latitude"],
active_interop_mission_json["emergent_last_known_pos"]
["longitude"]
],
"airdrop_position": [
active_interop_mission_json["air_drop_pos"]["latitude"],
active_interop_mission_json["air_drop_pos"]["longitude"]
],
"off-axis_position": [
active_interop_mission_json["off_axis_target_pos"]
["latitude"],
active_interop_mission_json["off_axis_target_pos"]
["longitude"]
],
"search_grid_points":
[{
"latitude": search_grid_point["latitude"],
"longitude": search_grid_point["longitude"],
"order": search_grid_point["order"]
} for search_grid_point in
active_interop_mission_json["search_grid_points"]]
}
except:
data = {
"status": "disconnected",
"emergent_position": [0, 0],
"airdrop_position": [0, 0],
"off-axis_position": [0, 0],
"search_grid_points": []
}
return data
def __init__(self):
self.manager = multiprocessing.Manager()
self.sda_status = self.manager.Value('s', "disconnected")
self.img_proc_status = self.manager.Value('s', "disconnected")
self.sda_start_time = self.manager.Value(
'i', int(datetime.utcnow().strftime("%s")))
self.img_proc_start_time = self.manager.Value(
'i', int(datetime.utcnow().strftime("%s")))
self.targets_to_submit = self.manager.list()
self.autonomous_targets_to_submit = self.manager.list()
self.location_log = self.manager.list()
self.interop_client = self.manager.list()
self.interop_client.append(SUASSystem.InteropClientConverter())
self.interop_data = self.manager.list()
self.interop_data.append(self.get_interop_data())
self.gcs_process = multiprocessing.Process(
target=SUASSystem.gcs_process,
args=(self.sda_status, self.img_proc_status, self.interop_client,
self.targets_to_submit, self.location_log,
self.autonomous_targets_to_submit))
self.gcs_process.start()
def gcs_process(sda_status, img_proc_status, interop_client_array,
targets_to_submit, location_log, autonomous_targets_to_submit):
# Setup logging information
logger_queue = multiprocessing.Queue(-1)
logger_listener_process = multiprocessing.Process(
target=listener_process,
args=(logger_queue, logger_listener_configurer))
logger_listener_process.start()
logger_worker_configurer(logger_queue)
manager = multiprocessing.Manager()
vehicle_state_data = manager.list()
mission_information_data = manager.list()
vehicle = connect_to_vehicle()
# SDA
#waypoints = download_waypoints(vehicle)
#sda_avoid_coords = manager.list()
#UAV_status = manager.Value('s', "AUTO")
if len(interop_client_array) != 0:
mission_information_data.append(
get_mission_json(interop_client_array[0].get_active_mission(),
interop_client_array[0].get_obstacles()))
else:
print(
"[Error] : The GCS process is unable to load mission data from the Interoperability server"
)
mission_information_data.append({})
vehicle_state_data.append(
SUASSystem.get_vehicle_state(vehicle, GCSSettings.MSL_ALT))
competition_viewer_process = initialize_competition_viewer_process(
vehicle_state_data, mission_information_data)
sd_card_process = load_sd_card(location_log, interop_client_array)
img_proc_process = initialize_image_processing_process(
logger_queue, location_log, targets_to_submit, interop_client_array)
autonomous_img_proc_process = initialize_autonomous_image_processing_process(
logger_queue, interop_client_array, img_proc_status,
autonomous_targets_to_submit)
# SDA
#sda_process = initialize_sda_process(logger_queue, sda_status, UAV_status, waypoints, sda_avoid_coords, vehicle_state_data, mission_information_data)
log(gcs_logger_name, "Completed instantiation of all child processes")
while True:
current_location = get_location(vehicle)
current_location_json = {
"latitude": current_location.get_lat(),
"longitude": current_location.get_lon(),
"altitude": current_location.get_alt(),
"epoch_time": time.time()
}
location_log.append(current_location_json)
vehicle_state_data[0] = SUASSystem.get_vehicle_state(
vehicle, GCSSettings.MSL_ALT)
if len(interop_client_array) != 0:
interop_client_array[0].post_telemetry(
current_location, vehicle_state_data[0].get_direction())
mission_information_data[0] = get_mission_json(
interop_client_array[0].get_active_mission(),
interop_client_array[0].get_obstacles())
# NOTE: The following commented code enables autonomous SDA. It has been left in this codebase to make it easier for future teams to
# understand the code. Please do not remove from codebase 2017/2018
#if (vehicle.location.global_relative_frame.alt * 3.28084) > GCSSettings.SDA_MIN_ALT and sda_status.value.lower() == "connected":
# if (UAV_status.value == "GUIDED"):
# sda_avoid_feet_height = Location(sda_avoid_coords[0].get_lat(), sda_avoid_coords[0].get_lon(), sda_avoid_coords[0].get_alt()*3.28084)
# log("root", "Avoiding obstacles...")
# vehicle.mode = dronekit.VehicleMode("GUIDED")
# vehicle.simple_goto(sda_avoid_coords[0].as_global_relative_frame())
# if UAV_status.value == 'AUTO' and vehicle.mode.name != "AUTO":
# vehicle.mode = dronekit.VehicleMode("AUTO")
sleep(0.25)
def gcs_process(sda_status, img_proc_status, interop_client_array,
targets_to_submit, location_log, autonomous_targets_to_submit):
# Setup logging information
logger_queue = multiprocessing.Queue(-1)
logger_listener_process = multiprocessing.Process(
target=listener_process,
args=(logger_queue, logger_listener_configurer))
logger_listener_process.start()
logger_worker_configurer(logger_queue)
manager = multiprocessing.Manager()
vehicle_state_data = manager.list()
mission_information_data = manager.list()
vehicle = connect_to_vehicle()
# SDA
#waypoints = download_waypoints(vehicle)
#sda_avoid_coords = manager.list()
#UAV_status = manager.Value('s', "AUTO")
if len(interop_client_array) != 0:
mission_information_data.append(
get_mission_json(interop_client_array[0].get_active_mission(),
interop_client_array[0].get_obstacles()))
else:
print(
"[Error] : The GCS process is unable to load mission data from the Interoperability server"
)
mission_information_data.append({})
vehicle_state_data.append(
SUASSystem.get_vehicle_state(vehicle, GCSSettings.MSL_ALT))
competition_viewer_process = initialize_competition_viewer_process(
vehicle_state_data, mission_information_data)
sd_card_process = load_sd_card(location_log, interop_client_array)
img_proc_process = initialize_image_processing_process(
logger_queue, location_log, targets_to_submit, interop_client_array)
autonomous_img_proc_process = initialize_autonomous_image_processing_process(
logger_queue, interop_client_array, img_proc_status,
autonomous_targets_to_submit)
# SDA
#sda_process = initialize_sda_process(logger_queue, sda_status, UAV_status, waypoints, sda_avoid_coords, vehicle_state_data, mission_information_data)
log(gcs_logger_name, "Completed instantiation of all child processes")
loop_init = time.time()
active_mission_json_data = interop_client_array[0].get_active_mission()
while True:
get_init = time.time()
current_location = get_location(vehicle)
get_end = time.time()
construct_init = time.time()
current_location_json = {
"latitude": current_location.get_lat(),
"longitude": current_location.get_lon(),
"altitude": current_location.get_alt(),
"epoch_time": time.time()
}
construct_end = time.time()
location_init = time.time()
location_log.append(current_location_json)
location_end = time.time()
vehicle_init = time.time()
vehicle_state_data[0] = SUASSystem.get_vehicle_state(
vehicle, GCSSettings.MSL_ALT)
vehicle_end = time.time()
if len(interop_client_array) != 0:
tel_init_1 = time.time()
interop_client_array[0].post_telemetry(
current_location, vehicle_state_data[0].get_direction())
tel_end_1 = time.time()
tel_init_2 = time.time()
mission_information_data[0] = get_mission_json(
active_mission_json_data,
interop_client_array[0].get_obstacles())
tel_end_2 = time.time()
get_diff = get_end - get_init
construct_diff = construct_end - construct_init
location_diff = location_end - location_init
vehicle_diff = vehicle_end - vehicle_init
tel_diff_1 = tel_end_1 - tel_init_1
tel_diff_2 = tel_end_2 - tel_init_2
loop_end = time.time()
loop_diff = loop_end - loop_init
if loop_diff > 1:
print("-----------------------------------")
print(
"Error -- Interop hasn't been updated for more than one second!"
)
print("get_location: " + str(get_diff))
print("current_location_json: " + str(construct_diff))
print("location_log.append: " + str(location_diff))
print("get_vehicle_state: " + str(vehicle_diff))
print("post_telemetry: " + str(tel_diff_1))
print("get_mission_json: " + str(tel_diff_2))
print("Total Loop time: " + str(loop_diff))
print("-----------------------------------")
loop_init = loop_end
import SimpleWebSocketServer
import SUASSystem
def run_competition_viewer_process(vehicle_state_data, mission_information_data):
competition_viewer_server = SimpleWebSocketServer.SimpleWebSocketServer('', 8000,
SUASSystem.CompetitionViewerSocket,
vehicle_state_data,
mission_information_data
)
competition_viewer_server.serveforever()
if __name__ == '__main__':
demo_vehicle_state_data = [SUASSystem.VehicleState(10, 10, 10, 250, 5, [2,2,2], False, 2)]
demo_mission_information_data = [{'stationary_obstacles': [{'latitude': 38.8711157, 'cylinder_height': 850.0, 'cylinder_radius': 25.0, 'longitude': -77.3221448}], 'fly_zones': [{'altitude_msl_max': 850.0, 'boundary_pts': [{'latitude': 38.8700862, 'order': 1, 'longitude': -77.3225069}, {'latitude': 38.870579, 'order': 2, 'longitude': -77.3233008}, {'latitude': 38.8719573, 'order': 3, 'longitude': -77.3210049}, {'latitude': 38.8712389, 'order': 4, 'longitude': -77.3210049}], 'altitude_msl_min': 450.0}], 'off_axis_target_pos': {'latitude': 0.0, 'longitude': 0.0}, 'moving_obstacles': [{'latitude': 0.0, 'altitude_msl': 0.0, 'cylinder_radius': 1.0, 'longitude': 0.0}], 'mission_waypoints': [{'latitude': 38.8706793, 'altitude_msl': 600.0, 'order': 1, 'longitude': -77.3227322}, {'latitude': 38.8713559, 'altitude_msl': 700.0, 'order': 2, 'longitude': -77.321949}, {'latitude': 38.8704955, 'altitude_msl': 600.0, 'order': 3, 'longitude': -77.3223996}], 'emergent_last_known_pos': {'latitude': 0.0, 'longitude': 0.0}, 'search_grid_points': [{'latitude': 38.8700862, 'altitude_msl': 450.0, 'order': 1, 'longitude': -77.3225069}, {'latitude': 38.870579, 'altitude_msl': 450.0, 'order': 2, 'longitude': -77.3233008}, {'latitude': 38.8719573, 'altitude_msl': 450.0, 'order': 3, 'longitude': -77.3210049}, {'latitude': 38.8712389, 'altitude_msl': 450.0, 'order': 4, 'longitude': -77.3210049}], 'home_pos': {'latitude': 38.8711157, 'longitude': -77.3214769}, 'air_drop_pos': {'latitude': 38.8715584, 'longitude': -77.3214769}}]
run_competition_viewer_process(demo_vehicle_state_data, demo_mission_information_data)
def gcs_process(sda_status, img_proc_status, interop_client_array, targets_to_submit, location_log, autonomous_targets_to_submit):
"""
This method executes all interop functions and necessary vehicle logging and connection functionality.
"""
# Setup logging information
print("got to GCS process")
logger_queue = multiprocessing.Queue(-1)
logger_listener_process = multiprocessing.Process(target=listener_process, args=(
logger_queue,
logger_listener_configurer
))
logger_listener_process.start()
logger_worker_configurer(logger_queue)
manager = multiprocessing.Manager()
vehicle_state_data = manager.list()
mission_information_data = manager.list()
vehicle = connect_to_vehicle()
rover = connect_to_rover()
# SDA
#waypoints = download_waypoints(vehicle)
#sda_avoid_coords = manager.list()
#UAV_status = manager.Value('s', "AUTO")
print("got to if statement before get missions")
if len(interop_client_array) != 0:
try:
mission = interop_client_array[0].get_active_mission()
stationaryobstacles = interop_client_array[0].get_obstacles()
mission_information_data.append( get_mission_json( mission,stationaryobstacles) )
print("after mission_information_data append statement")
#get_mission_json(interop_client_array[0].get_active_mission(), interop_client_array[0].get_obstacles())
except Exception as e: print(e)
#line below is the original:
else:
print("[Error] : The GCS process is unable to load mission data from the Interoperability server")
mission_information_data.append({})
print ("after mission information data append 2 statement")
print("got to after the get missions if statement")
vehicle_state_data.append(SUASSystem.get_vehicle_state(vehicle, GCSSettings.MSL_ALT))
competition_viewer_process = initialize_competition_viewer_process(vehicle_state_data, mission_information_data)
sd_card_process = load_sd_card(location_log, interop_client_array)
img_proc_process = initialize_image_processing_process(logger_queue, location_log, targets_to_submit, interop_client_array)
autonomous_img_proc_process = initialize_autonomous_image_processing_process(logger_queue, interop_client_array, img_proc_status, autonomous_targets_to_submit)
# SDA
#sda_process = initialize_sda_process(logger_queue, sda_status, UAV_status, waypoints, sda_avoid_coords, vehicle_state_data, mission_information_data)
log(gcs_logger_name, "Completed instantiation of all child processes")
while True:
current_location = get_location(vehicle)
current_location_json = {
"latitude": current_location.get_lat(),
"longitude": current_location.get_lon(),
"altitude": current_location.get_alt(),
"epoch_time": time.time()
}
location_log.append(current_location_json)
vehicle_state_data[0] = SUASSystem.get_vehicle_state(vehicle, GCSSettings.MSL_ALT)
print("got to if statement before post telem")
if len(interop_client_array) != 0:
print("got to post telem statement")
interop_client_array[0].post_telemetry(current_location, vehicle_state_data[0].get_direction())
print("after post telem statement")
try:
#print(interop_client_array[0])
#mission_information_data = mission_information_data.append(get_mission_json(interop_client_array[0].get_active_mission(), interop_client_array[0].get_obstacles()))
#mission_information_data = mission_information_data.append(mission, stationaryobstacles)
mission = interop_client_array[0].get_active_mission()
print("after get active mission")
stationaryobstacles = interop_client_array[0].get_obstacles()
print("before get active mission append statement")
mission_information_data.append( get_mission_json( mission,stationaryobstacles) )
#mission_information_data[0] = get_mission_json(interop_client_array[0].get_active_mission(), interop_client_array[0].get_obstacles())
except Exception as e: print(e)
print("after mission information data line")
# NOTE: The following commented code enables autonomous SDA. It has been left in this codebase to make it easier for future teams to
# understand the code. Please do not remove from codebase 2017/2018
#if (vehicle.location.global_relative_frame.alt * 3.28084) > GCSSettings.SDA_MIN_ALT and sda_status.value.lower() == "connected":
# if (UAV_status.value == "GUIDED"):
# sda_avoid_feet_height = Location(sda_avoid_coords[0].get_lat(), sda_avoid_coords[0].get_lon(), sda_avoid_coords[0].get_alt()*3.28084)
# log("root", "Avoiding obstacles...")
# vehicle.mode = dronekit.VehicleMode("GUIDED")
# vehicle.simple_goto(sda_avoid_coords[0].as_global_relative_frame())
# if UAV_status.value == 'AUTO' and vehicle.mode.name != "AUTO":
# vehicle.mode = dronekit.VehicleMode("AUTO")
sleep(0.25)