def construct_truck_graph(env, params):
truck_dist_matrix = env["truck_dist_matrix"]
truck_nodes = env["truck_nodes"]
g_truck = Graph()
for i in range(truck_dist_matrix.shape[0]):
edge_list = {
tuple(truck_nodes[ni]):
np.round(dist / params["truck_settings"]['vel'], 2)
for ni, dist in enumerate(truck_dist_matrix[i])
if dist != np.inf and ni != i
}
g_truck.add_vertex(tuple(truck_nodes[i]), edge_list)
return g_truck
def construct_drone_graph(env, params, one_package=True):
drone_checkpoints, truck_nodes = env["drone_checkpoints"], env[
"truck_nodes"]
g_drone = Graph()
battery_time = params["drone_settings"]["full_battery_time"]
drone_vel = params["drone_settings"]["vel"]
rrt_list = []
if one_package:
nodes = truck_nodes
else:
nodes = np.concatenate([truck_nodes, drone_checkpoints])
for pi, p in enumerate(drone_checkpoints):
print(f'Calculating RRT: {pi+1}/{len(drone_checkpoints)}')
dists = np.round(np.linalg.norm(nodes - p, axis=1), 2)
times = dists / drone_vel
reachable_nodes_idx = (0 < times) & (times < battery_time)
reachable_nodes = nodes[reachable_nodes_idx]
edge_list = {}
for node in reachable_nodes:
drone_path = plan_rrt_star(env["search_space"],
env["obstacles"],
tuple(node),
tuple(p),
show=False)
if drone_path is None:
continue
# Add RRT path to list
rrt_list.append(drone_path)
dist = calc_path_length(drone_path)
time_to_travel = np.round(dist / drone_vel, 2)
if time_to_travel < battery_time:
edge_list[tuple(node)] = time_to_travel
if len(edge_list):
g_drone.add_vertex(tuple(np.array(p, dtype='int')), edge_list)
# drone_flight_range = params["env_struct"]["drone_flight_range"]
# for pi, p in enumerate(drone_checkpoints):
# print(f'Calculating RRT: {pi+1}/{len(drone_checkpoints)}')
# if params["env_struct"]["rrt_flight_range"]:
# edge_list = {}
# dists = np.round(np.linalg.norm(truck_nodes - p, axis=1), 2)
# reachable_truck_nodes_idx = dists < drone_flight_range
# reachable_truck_nodes = truck_nodes[reachable_truck_nodes_idx]
# for node in reachable_truck_nodes:
# drone_path = plan_rrt_star(env["search_space"], env["obstacles"],
# tuple(node), tuple(p), show=False)
# if drone_path is None:
# continue
# dist = calc_path_length(drone_path)
# if dist < drone_flight_range:
# time_to_travel = dist / params["drone_vel"]
# edge_list[tuple(node)] = time_to_travel
# else:
# dists = np.round(np.linalg.norm(truck_nodes - p, axis=1), 2)
# reachable_truck_nodes_idx = dists < drone_flight_range
# reachable_truck_nodes = truck_nodes[reachable_truck_nodes_idx]
# reachable_truck_nodes_dists = dists[reachable_truck_nodes_idx]
# edge_list = {tuple(n): np.round(reachable_truck_nodes_dists[ni] / params["drone_vel"], 2) for ni, n in enumerate(reachable_truck_nodes)}
#
# if len(edge_list):
# g_drone.add_vertex(tuple(np.array(p, dtype='int')), edge_list)
return g_drone, rrt_list
