From 244fcb7d4df59b022175258fc649a6120e5c1248 Mon Sep 17 00:00:00 2001 From: MaximeBouton Date: Tue, 12 Nov 2019 17:43:19 -0800 Subject: [PATCH 1/2] update and clean up --- README.md | 9 + src/AutoUrban.jl | 53 +++- src/{simulation => }/actions.jl | 0 src/drivermodels/MOBILDriver.jl | 2 +- src/drivermodels/env_drivers_backup.jl | 121 --------- src/drivermodels/main.jl | 18 -- .../render_drivers.jl | 0 src/render/main.jl | 1 - src/roadway/IntersectionEvent.jl | 231 ------------------ src/roadway/convert2xodr.jl | 14 +- src/roadway/main.jl | 23 -- src/simulation/main.jl | 15 -- 12 files changed, 66 insertions(+), 421 deletions(-) rename src/{simulation => }/actions.jl (100%) delete mode 100644 src/drivermodels/env_drivers_backup.jl delete mode 100644 src/drivermodels/main.jl rename src/{render => drivermodels}/render_drivers.jl (100%) delete mode 100644 src/render/main.jl delete mode 100644 src/roadway/IntersectionEvent.jl delete mode 100644 src/roadway/main.jl delete mode 100644 src/simulation/main.jl diff --git a/README.md b/README.md index 6f098bb..aa877af 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,15 @@ A Julia package expanding the [AutomotiveDrivingModels.jl](https://github.com/si ## Installation +Preferred way, by adding the SISL registry: + +``` +] registry add https://github.com/sisl/Registry +] add AutoUrban +``` + +Other way, by manually adding all the dependencies: + ```julia using Pkg Pkg.add(PackageSpec(url="https://github.com/sisl/Vec.jl")) diff --git a/src/AutoUrban.jl b/src/AutoUrban.jl index b021ae1..6c6e8a3 100644 --- a/src/AutoUrban.jl +++ b/src/AutoUrban.jl @@ -9,9 +9,54 @@ using Printf using LinearAlgebra using Distributions -include("simulation/main.jl") -include("drivermodels/main.jl") -include("render/main.jl") -include("roadway/main.jl") +export + LatLonAccelDirection, + AccelSteeringDirection, + NextState + +include("actions.jl") + +export + initialize_XML, + get_lane_length, + handle_junctions, + convert_roadway!, + Connection, + Junction, + connect_two_lane_general!, + connect_two_seg!, + connect_two_seg_general!, + add_connection!, + add_junction!, + gen_connected_lanes, + gen_intersection, + gen_loop_roadway, + add_line!, + in_lanes, + get_max_curvature + +include("roadway/roadway_generation.jl") +include("roadway/roadway_perception.jl") +include("roadway/roadway_intersection.jl") +include("roadway/convert2xodr.jl") + +export + AccSteerDriver, + IDMDriver, + UrbanDriver, + MultiPtsDriver, + MultiPtsTurningDriver, + MOBILDriver, + excute_action! + +include("drivermodels/acc_steer_drivers.jl") +include("drivermodels/IDMDriver.jl") +include("drivermodels/MOBILDriver.jl") +include("drivermodels/urban_drivers.jl") +include("drivermodels/multiPts_driver.jl") +include("drivermodels/multiPts_functions_GD.jl") +include("drivermodels/waypoints_setting_functions.jl") +include("drivermodels/multiPtsTurning_driver.jl") +include("drivermodels/render_drivers.jl") end diff --git a/src/simulation/actions.jl b/src/actions.jl similarity index 100% rename from src/simulation/actions.jl rename to src/actions.jl diff --git a/src/drivermodels/MOBILDriver.jl b/src/drivermodels/MOBILDriver.jl index cc3bbe0..d89a25a 100644 --- a/src/drivermodels/MOBILDriver.jl +++ b/src/drivermodels/MOBILDriver.jl @@ -1,6 +1,6 @@ import AutomotiveDrivingModels.observe! -mutable struct MOBILDriver <: LaneChangeModel +mutable struct MOBILDriver <: LaneChangeModel{LaneChangeChoice} dir::Int #rec::SceneRecord diff --git a/src/drivermodels/env_drivers_backup.jl b/src/drivermodels/env_drivers_backup.jl deleted file mode 100644 index 589f896..0000000 --- a/src/drivermodels/env_drivers_backup.jl +++ /dev/null @@ -1,121 +0,0 @@ -export EnvDriver - -type EnvDriver <: DriverModel{LatLonAccelDirection} - rec::SceneRecord - mlon::LaneFollowingDriver - mlat::LateralDriverModel - mlane::LaneChangeModel - yield::Bool - yield_count::Int - lane_desire::Int - coorperative::Symbol - longAcc::Float64 - latAcc::Float64 - direction::Int - a_cp_max::Float64 - allwaystops::Array{AllWayStop} - allwaystop_ind::Int - allwaystop_waitlist::Array{Int} - allwaystop_stopcount::Int - allwaystop_waittime::Int - allwaystop_horizon::Float64 - - function EnvDriver( - timestep::Float64; - rec::SceneRecord = SceneRecord(1, timestep), - mlon::LaneFollowingDriver=IDMDriver(), - mlat::LateralDriverModel=ProportionalLaneTracker(), - mlane::LaneChangeModel=MOBIL(timestep,mlon=IDMDriver()), - longAcc::Float64 = 0.0, - latAcc::Float64 = 0.0, - direction::Int = 1, - a_cp_max::Float64 = (rand()*0.4+0.5)*9.8, - allwaystop_waittime::Int = rand(10:40), - allwaystop_horizon::Float64 = rand()*30.0+10.0 - ) - - retval = new() - retval.rec = rec - retval.mlon = mlon - retval.mlat = mlat - retval.mlane = mlane - retval.rec = rec - retval.longAcc = longAcc - retval.latAcc = latAcc - retval.direction = direction - retval.a_cp_max = a_cp_max - retval.allwaystop_waittime = allwaystop_waittime - retval.allwaystop_horizon = allwaystop_horizon - retval.allwaystops = AllWayStop[] - retval.allwaystop_ind = 0 - retval.allwaystop_waitlist = Int[] - retval.allwaystop_stopcount = 0 - retval - end -end -AutomotiveDrivingModels.get_name(::EnvDriver) = "EnvDriver" - -function set_desired_speed!(model::EnvDriver, v_des::Float64) - AutomotiveDrivingModels.set_desired_speed!(model.mlon, v_des) - AutomotiveDrivingModels.set_desired_speed!(model.mlane, v_des) - model -end - -function track_longitudinal!(driver::LaneFollowingDriver, scene::Scene, roadway::Roadway, vehicle_index::Int, fore::NeighborLongitudinalResult) - v_ego = scene[vehicle_index].state.v - if fore.ind != 0 - headway, v_oth = fore.Δs, scene[fore.ind].state.v - else - headway, v_oth = NaN, NaN - end - return track_longitudinal!(driver, v_ego, v_oth, headway) -end - -function AutomotiveDrivingModels.observe!(driver::EnvDriver, scene::Scene, roadway::Roadway, egoid::Int) - - AutomotiveDrivingModels.update!(driver.rec, scene) - observe!(driver.mlane, scene, roadway, egoid) - lane_change_action = rand(driver.mlane) - - vehicle_index = findfirst(scene, egoid) - lane_change_action = LaneChangeChoice(DIR_MIDDLE) - currentLane = scene[vehicle_index].state.posF.roadind.tag.lane - - laneoffset = get_lane_offset(lane_change_action, driver.rec, roadway, vehicle_index) - lateral_speed = convert(Float64, get(VELFT, driver.rec, roadway, vehicle_index)) - - - if lane_change_action.dir == DIR_MIDDLE - fore = get_neighbor_fore_along_lane(scene, vehicle_index, roadway, VehicleTargetPointFront(), VehicleTargetPointRear(), VehicleTargetPointFront()) - elseif lane_change_action.dir == DIR_LEFT - fore = get_neighbor_fore_along_left_lane(scene, vehicle_index, roadway, VehicleTargetPointFront(), VehicleTargetPointRear(), VehicleTargetPointFront()) - else - @assert(lane_change_action.dir == DIR_RIGHT) - fore = get_neighbor_fore_along_right_lane(scene, vehicle_index, roadway, VehicleTargetPointFront(), VehicleTargetPointRear(), VehicleTargetPointFront()) - end - - - acc_allwaystop = reactAllWayStop!(driver, scene, roadway, egoid) - - AutomotiveDrivingModels.track_lateral!(driver.mlat, laneoffset, lateral_speed) - #AutomotiveDrivingModels.track_longitudinal!(driver.mlon, scene, roadway, vehicle_index, fore.ind) - roadind = scene[vehicle_index].state.posF.roadind - max_k,distance = get_max_curvature(roadind, roadway, 25.0,direction = driver.direction) - #println(max_k,distance) - v_max = sqrt(driver.a_cp_max/abs(max_k)) - v_des_old = driver.mlon.v_des - set_desired_speed!(driver.mlon, min(v_des_old,v_max)) - track_longitudinal!(driver.mlon, scene, roadway, vehicle_index, fore) - set_desired_speed!(driver.mlon, v_des_old) - - driver.latAcc = rand(driver.mlat) - driver.longAcc = min(acc_allwaystop,rand(driver.mlon).a) - driver -end - -function Base.rand(driver::EnvDriver) - LatLonAccelDirection(driver.latAcc, driver.longAcc, driver.direction) -end - -Distributions.pdf(driver::EnvDriver, a::LatLonAccelDirection) = pdf(driver.mlat, a.a_lat) * pdf(driver.mlon, a.a_lon) -Distributions.logpdf(driver::EnvDriver, a::LatLonAccelDirection) = logpdf(driver.mlat, a.a_lat) * logpdf(driver.mlon, a.a_lon) diff --git a/src/drivermodels/main.jl b/src/drivermodels/main.jl deleted file mode 100644 index 48e43df..0000000 --- a/src/drivermodels/main.jl +++ /dev/null @@ -1,18 +0,0 @@ -export - AccSteerDriver, - IDMDriver, - UrbanDriver, - MultiPtsDriver, - MultiPtsTurningDriver, - MOBILDriver, - excute_action! - - -include("acc_steer_drivers.jl") -include("IDMDriver.jl") -include("MOBILDriver.jl") -include("urban_drivers.jl") -include("multiPts_driver.jl") -include("multiPts_functions_GD.jl") -include("waypoints_setting_functions.jl") -include("multiPtsTurning_driver.jl") diff --git a/src/render/render_drivers.jl b/src/drivermodels/render_drivers.jl similarity index 100% rename from src/render/render_drivers.jl rename to src/drivermodels/render_drivers.jl diff --git a/src/render/main.jl b/src/render/main.jl deleted file mode 100644 index fa7387c..0000000 --- a/src/render/main.jl +++ /dev/null @@ -1 +0,0 @@ -include("render_drivers.jl") \ No newline at end of file diff --git a/src/roadway/IntersectionEvent.jl b/src/roadway/IntersectionEvent.jl deleted file mode 100644 index 1b81daf..0000000 --- a/src/roadway/IntersectionEvent.jl +++ /dev/null @@ -1,231 +0,0 @@ -abstract IntersectionEvent -#####under developing -type AllWayStop <: IntersectionEvent - connections::Array{Tuple{Int,Int}} - entrances::Array{Int} - exits::Array{Int} - - function AllWayStop() - new(Tuple{Int,Int}[],Int[],Int[]) - end - - function AllWayStop(connections::Array{Tuple{Int,Int}}) - entrances = [] - exits = [] - for connection in connections - if isempty(find(entrances .== connection[1])) - push!(entrances,connection[1]) - end - if isempty(find(exits .== connection[2])) - push!(exits,connection[2]) - end - end - new(connections,entrances,exits) - end -end - -function updateAllWayStops!(driver::DriverModel,allwaystops::Array{AllWayStop}) - driver.allwaystops = allwaystops - driver -end - -function get_distance_to_stop(veh::Vehicle,allwaystop::AllWayStop,roadway::Roadway) - vehSegid = veh.state.posF.roadind.tag.segment - vehLaneid = veh.state.posF.roadind.tag.lane - vehTag = veh.state.posF.roadind.tag - distance_to_stop = -1.0 - if !isempty(find(allwaystop.entrances .== vehSegid)) - lane = roadway[vehTag] - cindS = curveindex_end(lane.curve) - endPoint = lane.curve[cindS] - distance_to_stop = endPoint.s - veh.state.posF.s - end - return distance_to_stop -end - -function stop_before(v::Float64,distance_to_stop::Float64,horizon::Float64=MAX_HORIZON,model::IDMDriver = IDMDriver()) - s_min = CAR_LENGTH/2 - - s_gap = distance_to_stop - acc = Inf - if s_gap < horizon - if s_gap > 0.0 - Δv = 0.0 - v - s_des = s_min + v*model.T - v*Δv / (2*sqrt(model.a_max*model.d_cmf)) - v_ratio = model.v_des > 0.0 ? (v/model.v_des) : 1.0 - acc = model.a_max * (1.0 - v_ratio^model.δ - (s_des/s_gap)^2) - elseif s_gap > -CAR_LENGTH - acc = -model.d_max - else - Δv = model.v_des - v - acc = Δv*model.k_spd - end - - if isnan(acc) - - warn("IDM acceleration was NaN!") - if s_gap > 0.0 - Δv = veh_target.state.v - v - s_des = model.s_min + v*model.T - v*Δv / (2*sqrt(model.a_max*model.d_cmf)) - println("\tΔv: ", Δv) - println("\ts_des: ", s_des) - println("\tv_des: ", model.v_des) - println("\tδ: ", model.δ) - println("\ts_gap: ", s_gap) - elseif s_gap > -veh_ego.def.length - println("\td_max: ", model.d_max) - end - - acc = 0.0 - end - else - # no lead vehicle, just drive to match desired speed - Δv = model.v_des - v - acc = Δv*model.k_spd # predicted accel to match target speed - end - acc = clamp(acc, -model.d_max, model.a_max) - return acc -end - - -function update_waitlist!(driver, new::Bool,ego_distance::Float64,allwaystop::AllWayStop, scene::Scene, roadway::Roadway, egoid::Int) - vehicle_index = findfirst(scene, egoid) - ego_veh = scene[vehicle_index] - if new #add veh - empty!(driver.allwaystop_waitlist) - for (i,veh) in enumerate(scene) - dist = get_distance_to_stop(veh,allwaystop,roadway) - if dist > 0.0 && dist < ego_distance - push!(driver.allwaystop_waitlist,i) - end - end - else #delete veh - new_waitlist = [] - for veh_ind in driver.allwaystop_waitlist - veh = scene.entities[veh_ind] - vehSegid = veh.state.posF.roadind.tag.segment - if isempty(find(allwaystop.exits .== vehSegid)) #this veh is still in waiting zone - push!(new_waitlist,veh_ind) - end - end - driver.allwaystop_waitlist = new_waitlist - end -end - -function reactAllWayStop!(driver, scene::Scene, roadway::Roadway, egoid::Int) - vehicle_index = findfirst(scene, egoid) - ego_veh = scene[vehicle_index] - acc = Inf - for i = 1:length(driver.allwaystops) - distance_to_stop = get_distance_to_stop(ego_veh,driver.allwaystops[i],roadway) - if distance_to_stop > 0.0 && distance_to_stop < driver.allwaystop_horizon - allwaystop = driver.allwaystops[i] - if distance_to_stop < CAR_LENGTH #in the waiting zone - if i == driver.allwaystop_ind - update_waitlist!(driver, false,distance_to_stop,allwaystop, scene, roadway, egoid) - if isempty(driver.allwaystop_waitlist) && driver.allwaystop_stopcount > driver.allwaystop_waittime #time to go! - return acc - else #wait - if abs(ego_veh.state.v) < 0.1 - driver.allwaystop_stopcount += 1 - end - acc = stop_before(ego_veh.state.v,distance_to_stop,driver.allwaystop_horizon,driver.mlon) - return acc - end - else # encounter new waiting zone - driver.allwaystop_stopcount = 0 - acc = stop_before(ego_veh.state.v,distance_to_stop,driver.allwaystop_horizon,driver.mlon) - driver.allwaystop_ind = i - update_waitlist!(driver, true,distance_to_stop,allwaystop, scene, roadway, egoid) - return acc - end - else #outside the waiting zone - acc = stop_before(ego_veh.state.v,distance_to_stop,driver.allwaystop_horizon,driver.mlon) - break - end - end - end - #no allwaystop is encounterd - driver.allwaystop_ind = 0 - empty!(driver.allwaystop_waitlist) - return acc -end - -########################################################## -type TrafficLight <: IntersectionEvent - color::Int #0 green 1 red 2 yellow - pos::VecSE2 -end - -function updateTrafficLights!(driver::DriverModel,trafficlights::Array{TrafficLight}) - driver.trafficlights = trafficlights - driver -end - -function get_yellowlight_decisison(distance) - decision = 0 - if distance > MAX_HORIZON - decision = 0 - else - prob = distance*0.7/MAX_HORIZON - #println("distance is : ",distance, " prob is : ",prob) - if rand() < prob - decision = -1 - end - end - return decision -end - -function reac_yellowlight(driver,scene,roadway,foreResult) - acc = Inf - if driver.yellowlight_decision == -1 - acc = reac_redlight(driver,scene,roadway,foreResult) - end - return acc -end - -function reac_redlight(driver,scene,roadway,foreResult) - s_min_old = driver.mlon.s_min - driver.mlon.s_min = 2.0 - AutomotiveDrivingModelsOld.track_longitudinal!(driver.mlon, scene, roadway, 1, foreResult.ind) - driver.mlon.s_min = s_min_old - acc = rand(driver.mlon) - return acc -end - -function reactTrafficLights(driver, scene::Scene, roadway::Roadway, egoid::Int) - vehicle_index = findfirst(scene, egoid) - ego_veh = scene[vehicle_index] - scene= Scene(length(driver.trafficlights)+1) - push!(scene,Vehicle(ego_veh.state, - VehicleDef(1, AgentClass.CAR, CAR_LENGTH, CAR_WIDTH))) - for i=1:length(driver.trafficlights) - push!(scene,Vehicle(VehicleState(driver.trafficlights[i].pos, roadway, 0.0), - VehicleDef(AgentClass.CAR, 1.0, 3.0),i)) - end - foreResult = get_neighbor_fore_along_lane(scene, 1, roadway) - light_ind = 0 - distance = 0.0 - acc = Inf - if foreResult.ind != 0 && foreResult.Δs < MAX_HORIZON - light_ind = foreResult.ind - 1 - distance = foreResult.Δs - if driver.trafficlights[light_ind].color == 2 - #println("light_id : ",light_ind," driver.yellowlight_id: ",driver.yellowlight_id) - if light_ind == driver.yellowlight_id - #println("continue previous decision ", driver.yellowlight_decision) - else - driver.yellowlight_id = light_ind - driver.yellowlight_decision = get_yellowlight_decisison(distance) - end - acc = reac_yellowlight(driver,scene,roadway,foreResult) - else - driver.yellowlight_decision = 0 - driver.yellowlight_id = 0 - end - if driver.trafficlights[light_ind].color == 1 - acc = reac_redlight(driver,scene,roadway,foreResult) - end - end - return acc -end \ No newline at end of file diff --git a/src/roadway/convert2xodr.jl b/src/roadway/convert2xodr.jl index e6733fc..51a5d0a 100644 --- a/src/roadway/convert2xodr.jl +++ b/src/roadway/convert2xodr.jl @@ -165,27 +165,27 @@ function handle_junction!(r,id,junction,roadway) laneLink["from"] = convert_JuliaLaneId2VTDLaneId(LaneTag(connection.source,connection.laneConnections[i][1]),roadway) laneLink["to"] = convert_JuliaLaneId2VTDLaneId(LaneTag(connection.path,i),roadway) end - for road in findall(r,"road") + for road in findall("road", r) if parse(Int,road["id"]) == connection.source - for successor in findall(road,"link/successor") + for successor in findall("link/successor", road) successor["elementType"] = "junction" successor["elementId"] = junctionId delete!(successor, "contactPoint") end - for successor in findall(road,"lanes/laneSection/right/lane/link/successor") + for successor in findall("lanes/laneSection/right/lane/link/successor", road) unlink!(successor) end end if parse(Int,road["id"]) == connection.path road["junction"] = junctionId - for roadi in findall(r,"road") + for roadi in findall("road", r) if parse(Int,roadi["id"]) == roadway.segments[connection.path].lanes[1].exits[1].target.tag.segment - for predecessor in findall(roadi,"link/predecessor") + for predecessor in findall("link/predecessor", roadi) predecessor["elementType"] = "junction" predecessor["elementId"] = junctionId delete!(predecessor, "contactPoint") end - for predecessor in findall(roadi,"lanes/laneSection/right/lane/link/predecessor") + for predecessor in findall("lanes/laneSection/right/lane/link/predecessor", roadi) unlink!(predecessor) end end @@ -206,4 +206,4 @@ function convert_roadway!(r,roadway) convert_seg!(seg,r,roadway) end #resolveRoadJunction!(r,roadway) -end \ No newline at end of file +end diff --git a/src/roadway/main.jl b/src/roadway/main.jl deleted file mode 100644 index 70529fa..0000000 --- a/src/roadway/main.jl +++ /dev/null @@ -1,23 +0,0 @@ -export - initialize_XML, - get_lane_length, - handle_junctions, - convert_roadway!, - Connection, - Junction, - connect_two_lane_general!, - connect_two_seg!, - connect_two_seg_general!, - add_connection!, - add_junction!, - gen_connected_lanes, - gen_intersection, - gen_loop_roadway, - add_line!, - in_lanes, - get_max_curvature - -include("roadway_generation.jl") -include("roadway_perception.jl") -include("roadway_intersection.jl") -include("convert2xodr.jl") diff --git a/src/simulation/main.jl b/src/simulation/main.jl deleted file mode 100644 index 07c9852..0000000 --- a/src/simulation/main.jl +++ /dev/null @@ -1,15 +0,0 @@ -export - LatLonAccelDirection, - AccelSteeringDirection, - NextState, - convert2vehicle - -include("actions.jl") - -function convert2vehicle(veh::Records.Entity{AutomotiveDrivingModels.VehicleState,AutomotiveDrivingModels.BicycleModel,Int64}) - return Entity(veh.state,veh.def.def,veh.id) -end - -function convert2vehicle(veh::Records.Entity{AutomotiveDrivingModels.VehicleState,AutomotiveDrivingModels.VehicleDef,Int64}) - return veh -end \ No newline at end of file From d9a994d5ab8131d098a0b02fa4952801250ad3de Mon Sep 17 00:00:00 2001 From: MaximeBouton Date: Tue, 12 Nov 2019 17:43:44 -0800 Subject: [PATCH 2/2] bump version --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 107ed39..31e7caa 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "AutoUrban" uuid = "1385418a-9289-5a49-ab93-843b508de4cb" repo = "https://github.com/sisl/AutoUrban.jl" -version = "0.2.2" +version = "0.2.3" [deps] AutoViz = "82aa6e0c-a491-5edf-8d4b-c16b98e4ea17"