diff --git a/scripts/benchmark_mult_agents.jl b/scripts/benchmark_mult_agents.jl index 9d97496..20c758d 100644 --- a/scripts/benchmark_mult_agents.jl +++ b/scripts/benchmark_mult_agents.jl @@ -79,7 +79,7 @@ function main() # Off transit preprocessing otg = OffTransitGraph(depots = depots, sites = sites) - trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, + aug_trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, length(depots), length(sites), vcat(depots, sites), drone_params) @@ -91,7 +91,7 @@ function main() # Set up the shadow env to do task allocation with env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, - stop_idx_to_trips = stop_idx_to_trips, trips_fws_dists = trips_fws_dists, + stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists, drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [], threshold_global_conflicts = 10) diff --git a/scripts/benchmark_mult_agents_light.jl b/scripts/benchmark_mult_agents_light.jl index bacfa27..08d8337 100644 --- a/scripts/benchmark_mult_agents_light.jl +++ b/scripts/benchmark_mult_agents_light.jl @@ -66,7 +66,7 @@ function main() # Off transit preprocessing otg = OffTransitGraph(depots = depots, sites = sites) - trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, + aug_trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, length(depots), length(sites), vcat(depots, sites), drone_params) @@ -78,7 +78,7 @@ function main() # Set up the shadow env to do task allocation with env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, - stop_idx_to_trips = stop_idx_to_trips, trips_fws_dists = trips_fws_dists, + stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists, drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [], threshold_global_conflicts = 5) diff --git a/scripts/benchmark_replanning.jl b/scripts/benchmark_replanning.jl index 72cc49e..e08aa82 100644 --- a/scripts/benchmark_replanning.jl +++ b/scripts/benchmark_replanning.jl @@ -72,7 +72,7 @@ function main() # Off transit preprocessing otg = OffTransitGraph(depots = depots, sites = sites) - trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, + aug_trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, length(depots), length(sites), vcat(depots, sites), drone_params) @@ -83,7 +83,7 @@ function main() # Set up the shadow env to do task allocation with env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, - stop_idx_to_trips = stop_idx_to_trips, trips_fws_dists = trips_fws_dists, + stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists, drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [], threshold_global_conflicts = 10) diff --git a/scripts/preprocess_travel_time_estimates.jl b/scripts/preprocess_travel_time_estimates.jl index 92fd5f0..463fdf6 100644 --- a/scripts/preprocess_travel_time_estimates.jl +++ b/scripts/preprocess_travel_time_estimates.jl @@ -36,7 +36,7 @@ function main() drone_params = parse_drone_params(drone_params_file) tg = load_transit_graph_latlong(stop_coords_file, trips_file, TRANSIT_CAP_RANGE, rng) - tg, stop_idx_to_trips, trips_fws_dists, stops_nn_tree, nn_idx_to_stop = + tg, stop_idx_to_trips, aug_trips_fws_dists, stops_nn_tree, nn_idx_to_stop = transit_graph_preprocessing(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, drone_params) @@ -56,7 +56,7 @@ function main() env = MAPFTransitEnv(off_transit_graph = dummy_otg, transit_graph = tg, state_graph = state_graph, agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, stops_nn_tree = stops_nn_tree, nn_idx_to_stop = nn_idx_to_stop, stop_idx_to_trips = stop_idx_to_trips, - trips_fws_dists = trips_fws_dists, depot_to_sites_dists = Matrix{Float64}(undef, 0, 0), + aug_trips_fws_dists = aug_trips_fws_dists, depot_to_sites_dists = Matrix{Float64}(undef, 0, 0), drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [], threshold_global_conflicts = 0) diff --git a/scripts/test_replanning_heuristics.jl b/scripts/test_replanning_heuristics.jl index 6db0966..958312f 100644 --- a/scripts/test_replanning_heuristics.jl +++ b/scripts/test_replanning_heuristics.jl @@ -43,7 +43,7 @@ lon_dist = Uniform(bb_params.lon_start, bb_params.lon_end) # Transit Graph Preprocessing tg = load_transit_graph_latlong(stop_coords_file, trips_file, TRANSIT_CAP_RANGE, rng) -tg, stop_idx_to_trips, trips_fws_dists, stops_nn_tree, nn_idx_to_stop = +tg, stop_idx_to_trips, aug_trips_fws_dists, stops_nn_tree, nn_idx_to_stop = transit_graph_preprocessing(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, drone_params) sites = [LatLonCoords((lat = rand(rng, lat_dist), lon = rand(rng, lon_dist))) for i = 1:N_SITES] @@ -52,7 +52,7 @@ depot_sites = vcat(depots, sites) # Load OTG stuff otg = OffTransitGraph(depots = depots, sites = sites) depot_to_sites_dists = generate_depot_to_sites_dists(otg, tg, stops_nn_tree, nn_idx_to_stop, stop_idx_to_trips, - trips_fws_dists, MultiAgentAllocationTransit.distance_lat_lon_euclidean) + aug_trips_fws_dists, MultiAgentAllocationTransit.distance_lat_lon_euclidean) state_graph, depot_sites_to_vtx, trip_to_vtx_range = setup_state_graph(tg, otg) @@ -60,7 +60,7 @@ state_graph, depot_sites_to_vtx, trip_to_vtx_range = setup_state_graph(tg, otg) env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = AgentState[], depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, stops_nn_tree = stops_nn_tree, nn_idx_to_stop = nn_idx_to_stop, stop_idx_to_trips = stop_idx_to_trips, - trips_fws_dists = trips_fws_dists, depot_to_sites_dists = depot_to_sites_dists, + aug_trips_fws_dists = aug_trips_fws_dists, depot_to_sites_dists = depot_to_sites_dists, drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = []) diff --git a/scripts/test_sf_mult_agents.jl b/scripts/test_sf_mult_agents.jl index 533f577..e1cd710 100644 --- a/scripts/test_sf_mult_agents.jl +++ b/scripts/test_sf_mult_agents.jl @@ -47,7 +47,7 @@ tg = load_transit_graph_latlong(stop_coords_file, trips_file, TRANSIT_CAP_RANGE, tg, stop_idx_to_trips = transit_graph_preprocessing(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, drone_params) -trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, +aug_trips_fws_dists = augmented_trip_meta_graph_fws_dists(tg, MultiAgentAllocationTransit.distance_lat_lon_euclidean, length(depots), length(sites), vcat(depots, sites), drone_params) @@ -59,7 +59,7 @@ agent_states = [AgentState(task=agt_task) for agt_task in agent_tasks] env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = agent_states, depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, - stop_idx_to_trips = stop_idx_to_trips, trips_fws_dists = trips_fws_dists, + stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists, plan_ref_times = zeros(length(agent_tasks)), drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [0 for _ = 1:length(agent_tasks)]) @@ -80,7 +80,7 @@ agent_states = [AgentState(task=agt_task) for agt_task in agent_tasks] env = MAPFTransitEnv(off_transit_graph = otg, transit_graph = tg, state_graph = state_graph, agent_states = agent_states, depot_sites_to_vtx = depot_sites_to_vtx, trip_to_vtx_range = trip_to_vtx_range, - stop_idx_to_trips = stop_idx_to_trips, trips_fws_dists = trips_fws_dists, + stop_idx_to_trips = stop_idx_to_trips, aug_trips_fws_dists = aug_trips_fws_dists, plan_ref_times = zeros(length(agent_tasks)), valid_transit_options = zeros(length(agent_tasks)), drone_params = drone_params, dist_fn = MultiAgentAllocationTransit.distance_lat_lon_euclidean, curr_site_points = [0 for _ = 1:length(agent_tasks)]) diff --git a/src/gtfs_parser.jl b/src/gtfs_parser.jl index 1310ecd..38ca21a 100644 --- a/src/gtfs_parser.jl +++ b/src/gtfs_parser.jl @@ -1,3 +1,10 @@ +""" + generate_stop_file(stop_txt::String, params::CityParams, out_file::String) + +Convert the stops.txt GTFS file for a transit network to a dict that maps +integer stop ID to lat-long coordinates, and saves it to an output JSON file. +The CityParams is used to filter out trips outside the bounding box. +""" function generate_stop_file(stop_txt::String, params::CityParams, out_file::String) stop_file = CSV.File(stop_txt) @@ -38,11 +45,14 @@ function generate_stop_file(stop_txt::String, params::CityParams, out_file::Stri end -## Loop through route IDs -## Choose any one trip randomly with that route ID - note TRIP ID -## Go to stop_times, look up stop sequence and STOP IDs for the trip, note TIMES -## Ignore trips with any stops outside bounding box -## Save trips with zero-relative time in another file +""" + generate_trip_file(route_txt::String, trip_txt::String, + stop_time_txt::String, stop_coords::Dict{Int64,LatLonCoords}, + out_file::String, rng::RNG) where {RNG <: AbstractRNG} + +Parse the GTFS trips.txt file. Takes the routes.txt file, chooses any trip from that +route type, and generates the corresponding sequence of stop IDs from the stop_coords Dict. +""" function generate_trip_file(route_txt::String, trip_txt::String, stop_time_txt::String, stop_coords::Dict{Int64,LatLonCoords}, out_file::String, rng::RNG) where {RNG <: AbstractRNG} diff --git a/src/mapf_transit.jl b/src/mapf_transit.jl index a9b6d6b..6722591 100644 --- a/src/mapf_transit.jl +++ b/src/mapf_transit.jl @@ -276,13 +276,13 @@ function distance_heuristic(env::MAPFTransitEnv, s::MAPFTransitVertexState) mindist = Inf for tid in env.stop_idx_to_trips[stop_id] - trip_to_ds = env.trips_fws_dists[n_depots + n_sites + tid, depot_site_idx] + trip_to_ds = env.aug_trips_fws_dists[n_depots + n_sites + tid, depot_site_idx] mindist = (trip_to_ds < mindist) ? trip_to_ds : mindist end return mindist else - return env.trips_fws_dists[s.idx, depot_site_idx] + return env.aug_trips_fws_dists[s.idx, depot_site_idx] end end diff --git a/src/preprocessing.jl b/src/preprocessing.jl index bf54234..787e683 100644 --- a/src/preprocessing.jl +++ b/src/preprocessing.jl @@ -1,6 +1,11 @@ -## Create augmented trip metagraph with trip vertices AND depot-sites -## Then run FWS on them to get min flight distance -## Index: 1....n_depots....n_depots+n_sites.....n_deps+n_sites+n_trips +""" + augmented_trip_meta_graph_fws_dists(tg::TG, dist_fn::Function, + n_depots::Int64, n_sites::Int64, + depot_sites::Vector{LOC}, drone_params::DroneParams) where {LOC, TG <: TransitGraph} + +Get distance matrix from Floyd-Warshall Shortest Paths alg on the augmented +trip metagraph. See Appendix II-B for details. +""" function augmented_trip_meta_graph_fws_dists(tg::TG, dist_fn::Function, n_depots::Int64, n_sites::Int64, depot_sites::Vector{LOC}, drone_params::DroneParams) where {LOC, TG <: TransitGraph} diff --git a/src/types.jl b/src/types.jl index acea854..c5a8ae4 100644 --- a/src/types.jl +++ b/src/types.jl @@ -169,28 +169,35 @@ Tracks the current state of the agent; current task; whether it has delivered it dist_flown::Float64 = 0.0 end +""" + MAPFTransitEnv{OTG <: OffTransitGraph, TG <: TransitGraph, MVTS <: MAPFTransitVertexState} <: MAPFEnvironment + +The overarching structure to track the MAPF-TN contextual information. +N.B. - There are some hacks here to track drone state with time for the replanning. The MAPF library +is typically not well-suited to receding-horizon execution. Email me if you need to figure that out. +""" @with_kw mutable struct MAPFTransitEnv{OTG <: OffTransitGraph, TG <: TransitGraph, MVTS <: MAPFTransitVertexState} <: MAPFEnvironment off_transit_graph::OTG transit_graph::TG - state_graph::SimpleVListGraph{MVTS} # Vertex IDs are d-1 etc, s-1 etc, r-1-1 etc. + state_graph::SimpleVListGraph{MVTS} # Vertex IDs are d-1 etc, s-1 etc, r-1-1 etc. agent_states::Vector{AgentState} - depot_sites_to_vtx::Dict{String,Int64} # Maps depot and site IDs to their vertex ID in graph - needed for start-goal IDXs - trip_to_vtx_range::Vector{Tuple{Int64,Int64}}# Maps the ID from NNTree to stop id - stop_idx_to_trips::Dict{Int64,Set{Int64}} # Maps the stop ID to trips passing through them - trips_fws_dists::Matrix{Float64} + depot_sites_to_vtx::Dict{String,Int64} # Maps depot and site IDs to their vertex ID in graph - needed for start-goal IDXs + trip_to_vtx_range::Vector{Tuple{Int64,Int64}} # Maps the ID from NNTree to stop id + stop_idx_to_trips::Dict{Int64,Set{Int64}} # Maps the stop ID to trips passing through them + aug_trips_fws_dists::Matrix{Float64} # The distance matrix from the augmented trip metagraph drone_params::DroneParams - dist_fn::Function - curr_site_points::Vector{Int64} - curr_goal_idx::Int64 = 0 - plan_ref_times::Vector{Float64} = Float64[] - # Diagnostics - valid_transit_options::Vector{Int64} = Int64[] - any_invalid_path::Bool = false - valid_path_dists::Vector{Float64} = Float64[] - num_global_conflicts::Int64 = 0 - threshold_global_conflicts::Int64 = 10 + dist_fn::Function # Implements the distance metric between coordinates (lat-long/grid point) + curr_site_points::Vector{Int64} # For each agent, which point on its solution vector is the intermediate goal, i.e. p + curr_goal_idx::Int64 = 0 # Tracks the current goal index for each low-level single-agent search + plan_ref_times::Vector{Float64} = Float64[] # Tracks the system time from which each agent is being planned for + # Solution Diagnostics + valid_transit_options::Vector{Int64} = Int64[] # The number of transit options used by each agent + any_invalid_path::Bool = false # Is either subpath of an agent an invalid one, i.e. no weight-constrained path found + valid_path_dists::Vector{Float64} = Float64[] # For the valid agent subpaths, what is the effective distance covered + num_global_conflicts::Int64 = 0 # The number of high-level ECBS conflicts + threshold_global_conflicts::Int64 = 10 # The threshold for high-level conflicts after which an error is thrown. TODO: Need to diagnose better end