diff --git a/.travis.yml b/.travis.yml index 16f8bf8..02200e1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,7 +5,8 @@ os: - linux # - osx julia: - - 0.6 + - 0.7 + - 1.0 - nightly matrix: allow_failures: @@ -14,6 +15,6 @@ notifications: email: false script: - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi - - julia -e 'Pkg.clone(pwd()); Pkg.clone("https://github.com/tawheeler/Vec.jl.git"); Pkg.clone("https://github.com/sisl/Records.jl.git"); Pkg.add("Colors"); Pkg.add("Cairo"); Pkg.add("Reel"); Pkg.build("AutomotiveDrivingModels"); Pkg.clone("https://github.com/sisl/AutoViz.jl.git"); Pkg.test("AutomotiveDrivingModels"; coverage=true)' + - julia --project --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/sisl/Records.jl")); Pkg.add(PackageSpec(url="https://github.com/sisl/Vec.jl")); Pkg.build("AutomotiveDrivingModels"); Pkg.test("AutomotiveDrivingModels"; coverage=true)' after_success: - - julia -e 'cd(Pkg.dir("AutomotiveDrivingModels")); Pkg.add("Coverage"); using Coverage; Coveralls.submit(Coveralls.process_folder())' + - julia --project -e 'using Pkg; cd(Pkg.dir("AutomotiveDrivingModels")); Pkg.add("Coverage"); using Coverage; Coveralls.submit(Coveralls.process_folder())' diff --git a/Project.toml b/Project.toml new file mode 100644 index 0000000..51f54ec --- /dev/null +++ b/Project.toml @@ -0,0 +1,18 @@ +name = "AutomotiveDrivingModels" +uuid = "99497e54-f3d6-53d3-a3a9-fa9315a7f1ba" +repo = "https://github.com/sisl/AutomotiveDrivingModels.jl.git" +version = "0.7.0" + +[deps] +Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" +Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a" +Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" +Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7" +Reexport = "189a3867-3050-52da-a836-e630ba90ab69" + +[extras] +Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" + +[targets] +test = ["Test"] diff --git a/REQUIRE b/REQUIRE deleted file mode 100644 index 2e18270..0000000 --- a/REQUIRE +++ /dev/null @@ -1,6 +0,0 @@ -julia 0.6 - -Reexport -DataArrays -Distributions -Parameters \ No newline at end of file diff --git a/deps/build.jl b/deps/build.jl index c915571..6403c9e 100644 --- a/deps/build.jl +++ b/deps/build.jl @@ -1,10 +1,8 @@ +using Pkg packages = keys(Pkg.installed()) if !in("Vec", packages) - Pkg.clone("https://github.com/sisl/Vec.jl.git") + Pkg.add(PackageSpec(url="https://github.com/sisl/Vec.jl.git")) end -warn("Checking out Vec.jl branch 0_6 for compatibility with julia 0.6") -Pkg.checkout("Vec", "0_6") if !in("Records", packages) - Pkg.clone("https://github.com/sisl/Records.jl.git") + Pkg.add(PackageSpec(url="https://github.com/sisl/Records.jl.git")) end - diff --git a/src/1d/actions.jl b/src/1d/actions.jl index 0605f45..fbffda0 100644 --- a/src/1d/actions.jl +++ b/src/1d/actions.jl @@ -8,7 +8,7 @@ end Base.show(io::IO, a::LaneFollowingAccel) = @printf(io, "LaneFollowingAccel(%6.3f)", a.a) Base.length(::Type{LaneFollowingAccel}) = 1 Base.convert(::Type{LaneFollowingAccel}, v::Vector{Float64}) = LaneFollowingAccel(v[1]) -function Base.copy!(v::Vector{Float64}, a::LaneFollowingAccel) +function Base.copyto!(v::Vector{Float64}, a::LaneFollowingAccel) v[1] = a.a v end diff --git a/src/2d/actions/accel_desang.jl b/src/2d/actions/accel_desang.jl index a7249f3..389964e 100644 --- a/src/2d/actions/accel_desang.jl +++ b/src/2d/actions/accel_desang.jl @@ -5,7 +5,7 @@ end Base.show(io::IO, a::AccelDesang) = @printf(io, "AccelDesang(%6.3f,%6.3f)", a.a, a.ϕdes) Base.length(::Type{AccelDesang}) = 2 Base.convert(::Type{AccelDesang}, v::Vector{Float64}) = AccelDesang(v[1], v[2]) -function Base.copy!(v::Vector{Float64}, a::AccelDesang) +function Base.copyto!(v::Vector{Float64}, a::AccelDesang) v[1] = a.a v[2] = a.ϕdes v diff --git a/src/2d/actions/accel_steering.jl b/src/2d/actions/accel_steering.jl index 51e34d8..3519ad3 100644 --- a/src/2d/actions/accel_steering.jl +++ b/src/2d/actions/accel_steering.jl @@ -5,7 +5,7 @@ end Base.show(io::IO, a::AccelSteeringAngle) = @printf(io, "AccelSteeringAngle(%6.3f,%6.3f)", a.a, a.δ) Base.length(::Type{AccelSteeringAngle}) = 2 Base.convert(::Type{AccelSteeringAngle}, v::Vector{Float64}) = AccelSteeringAngle(v[1], v[2]) -function Base.copy!(v::Vector{Float64}, a::AccelSteeringAngle) +function Base.copyto!(v::Vector{Float64}, a::AccelSteeringAngle) v[1] = a.a v[2] = a.δ v diff --git a/src/2d/actions/accel_turnrate.jl b/src/2d/actions/accel_turnrate.jl index bd622ab..28832f6 100644 --- a/src/2d/actions/accel_turnrate.jl +++ b/src/2d/actions/accel_turnrate.jl @@ -5,7 +5,7 @@ end Base.show(io::IO, a::AccelTurnrate) = @printf(io, "AccelTurnrate(%6.3f,%6.3f)", a.a, a.ω) Base.length(::Type{AccelTurnrate}) = 2 Base.convert(::Type{AccelTurnrate}, v::Vector{Float64}) = AccelTurnrate(v[1], v[2]) -function Base.copy!(v::Vector{Float64}, a::AccelTurnrate) +function Base.copyto!(v::Vector{Float64}, a::AccelTurnrate) v[1] = a.a v[2] = a.ω v diff --git a/src/2d/actions/lat_lon_accel.jl b/src/2d/actions/lat_lon_accel.jl index a589609..dbcf8c0 100644 --- a/src/2d/actions/lat_lon_accel.jl +++ b/src/2d/actions/lat_lon_accel.jl @@ -9,12 +9,12 @@ end Base.show(io::IO, a::LatLonAccel) = @printf(io, "LatLonAccel(%6.3f, %6.3f)", a.a_lat, a.a_lon) Base.length(::Type{LatLonAccel}) = 2 Base.convert(::Type{LatLonAccel}, v::Vector{Float64}) = LatLonAccel(v[1], v[2]) -function Base.copy!(v::Vector{Float64}, a::LatLonAccel) +function Base.copyto!(v::Vector{Float64}, a::LatLonAccel) v[1] = a.a_lat v[2] = a.a_lon v end -function propagate{D<:Union{VehicleDef, BicycleModel}}(veh::Entity{VehicleState, D, Int}, action::LatLonAccel, roadway::Roadway, ΔT::Float64) +function propagate(veh::Entity{VehicleState, D, Int}, action::LatLonAccel, roadway::Roadway, ΔT::Float64) where {D<:Union{VehicleDef, BicycleModel}} a_lat = action.a_lat a_lon = action.a_lon @@ -33,7 +33,7 @@ function propagate{D<:Union{VehicleDef, BicycleModel}}(veh::Entity{VehicleState, dt₂ = dt + a_lat*ΔT speed₂ = sqrt(dt₂*dt₂ + ds₂*ds₂) v₂ = sqrt(dt₂*dt₂ + ds₂*ds₂) # v is the magnitude of the velocity vector - ϕ₂ = atan2(dt₂, ds₂) + ϕ₂ = atan(dt₂, ds₂) roadind = move_along(veh.state.posF.roadind, roadway, Δs) footpoint = roadway[roadind] diff --git a/src/2d/behaviors/lane_change_models/lane_change_models.jl b/src/2d/behaviors/lane_change_models/lane_change_models.jl index 7666d24..cba7e7e 100644 --- a/src/2d/behaviors/lane_change_models/lane_change_models.jl +++ b/src/2d/behaviors/lane_change_models/lane_change_models.jl @@ -11,7 +11,7 @@ end Base.show(io::IO, a::LaneChangeChoice) = @printf(io, "LaneChangeChoice(%d)", a.dir) Base.length(::Type{LaneChangeChoice}) = 1 Base.convert(::Type{LaneChangeChoice}, v::Vector{Float64}) = LaneChangeChoice(convert(Int, v[1])) -function Base.copy!(v::Vector{Float64}, a::LaneChangeChoice) +function Base.copyto!(v::Vector{Float64}, a::LaneChangeChoice) v[1] = a.dir v end diff --git a/src/2d/evaluation/foldsets.jl b/src/2d/evaluation/foldsets.jl index ac23890..749a302 100644 --- a/src/2d/evaluation/foldsets.jl +++ b/src/2d/evaluation/foldsets.jl @@ -40,15 +40,18 @@ function _find_next_valid_fold_match(foldset::FoldSet, state::Int) end state + 1 # returns length(foldset.assignment) + 1 on fail end -Base.start(foldset::FoldSet) = _find_next_valid_fold_match(foldset, 0) -Base.done(foldset::FoldSet, state::Int) = state > length(foldset.assignment) -function Base.next(foldset::FoldSet, state::Int) + +function Base.iterate(foldset::FoldSet, state::Int = _find_next_valid_fold_match(foldset, 0)) + if state > length(foldset.assignment) + return nothing + end @assert(check_fold_match(foldset.assignment[state], foldset)) - state, _find_next_valid_fold_match(foldset, state) + return state, _find_next_valid_fold_match(foldset, state) end + function Base.length(foldset::FoldSet) len = 0 - state = start(foldset) + state = _find_next_valid_fold_match(foldset, 0) while !done(foldset, state) item, state = next(foldset, state) len += 1 diff --git a/src/2d/features/feature_extractors.jl b/src/2d/features/feature_extractors.jl index 9b9c901..8c19c7e 100644 --- a/src/2d/features/feature_extractors.jl +++ b/src/2d/features/feature_extractors.jl @@ -11,7 +11,7 @@ export abstract type AbstractFeatureExtractor end rec_length(::AbstractFeatureExtractor) = 1 # length of the SceneRecord for best results Base.length(::AbstractFeatureExtractor) = error("Not Impemeneted") -pull_features!{F<:AbstractFloat}(::AbstractFeatureExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) = error("Not Implemented") +pull_features!(::AbstractFeatureExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) where {F<:AbstractFloat} = error("Not Implemented") ############################################################### # FeatureExtractor @@ -24,7 +24,7 @@ mutable struct FeatureExtractor <: AbstractFeatureExtractor end rec_length(ext::FeatureExtractor) = ext.rec_length # length of the SceneRecord for best results Base.length(ext::FeatureExtractor) = length(ext.features) -function pull_features!{F<:AbstractFloat}(ext::FeatureExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) +function pull_features!(ext::FeatureExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) where {F<:AbstractFloat} # NOTE(tim): this is an interation over an abstract vector # as such, this will be slow @@ -42,10 +42,10 @@ struct SubsetExtractor{F<:AbstractFeatureExtractor, G<:AbstractFloat} <: Abstrac subset::Vector{Int} features::Vector{G} end -SubsetExtractor{G<:AbstractFloat}(extractor::AbstractFeatureExtractor, subset::Vector{Int}, ::Type{G}=Float64) = SubsetExtractor(extractor, subset, Array{G}(length(extractor))) +SubsetExtractor(extractor::AbstractFeatureExtractor, subset::Vector{Int}, ::Type{G}=Float64) where {G<:AbstractFloat} = SubsetExtractor(extractor, subset, Array{G}(length(extractor))) rec_length(ext::SubsetExtractor) = rec_length(ext.extractor) Base.length(ext::SubsetExtractor) = length(ext.subset) -function pull_features!{F<:AbstractFloat}(ext::SubsetExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) +function pull_features!(ext::SubsetExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) where {F<:AbstractFloat} pull_features!(ext.extractor, ext.features, rec, roadway, vehicle_index, pastframe) for (i,j) in enumerate(ext.subset) features[i] = ext.features[j] @@ -63,7 +63,7 @@ struct StandardizingExtractor{F<:AbstractFeatureExtractor} <: AbstractFeatureExt end rec_length(ext::StandardizingExtractor) = rec_length(ext.extractor) Base.length(ext::StandardizingExtractor) = length(ext.extractor) -function pull_features!{F<:AbstractFloat}(ext::StandardizingExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) +function pull_features!(ext::StandardizingExtractor, features::Vector{F}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0) where {F<:AbstractFloat} pull_features!(ext.extractor, features, rec, roadway, vehicle_index, pastframe) for i in 1 : length(features) features[i] = (features[i] - ext.μ[i]) / ext.σ[i] diff --git a/src/2d/features/features.jl b/src/2d/features/features.jl index d4f2b5e..c776757 100644 --- a/src/2d/features/features.jl +++ b/src/2d/features/features.jl @@ -1,23 +1,23 @@ -function Base.get{S<:VehicleState,D,I,R}(::Feature_PosFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_PosFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} FeatureValue(rec[pastframe][vehicle_index].state.posF.t) end -function Base.get{S<:VehicleState,D,I,R}(::Feature_PosFyaw, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_PosFyaw, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} FeatureValue(rec[pastframe][vehicle_index].state.posF.ϕ) end -function Base.get{S<:VehicleState,D,I,R}(::Feature_Speed, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_Speed, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} FeatureValue(rec[pastframe][vehicle_index].state.v) end -function Base.get{S<:VehicleState,D,I,R}(::Feature_VelFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_VelFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} veh = rec[pastframe][vehicle_index] FeatureValue(veh.state.v*cos(veh.state.posF.ϕ)) end -function Base.get{S<:VehicleState,D,I,R}(::Feature_VelFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_VelFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} veh = rec[pastframe][vehicle_index] FeatureValue(veh.state.v*sin(veh.state.posF.ϕ)) end generate_feature_functions("TurnRateG", :turnrateG, Float64, "rad/s") -function Base.get{S<:VehicleState,D,I,R}(::Feature_TurnRateG, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; frames_back::Int=1) +function Base.get(::Feature_TurnRateG, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; frames_back::Int=1) where {S<:VehicleState,D,I,R} id = rec[pastframe][vehicle_index].id @@ -28,7 +28,7 @@ function Base.get{S<:VehicleState,D,I,R}(::Feature_TurnRateG, rec::EntityQueueRe veh_index_curr = vehicle_index veh_index_prev = findfirst(rec[pastframe2], id) - if veh_index_prev != 0 + if veh_index_prev != nothing curr = rec[pastframe][veh_index_curr].state.posG.θ past = rec[pastframe2][veh_index_prev].state.posG.θ Δt = get_elapsed_time(rec, pastframe2, pastframe) @@ -39,21 +39,21 @@ function Base.get{S<:VehicleState,D,I,R}(::Feature_TurnRateG, rec::EntityQueueRe retval end generate_feature_functions("TurnRateF", :turnrateF, Float64, "rad/s") -function Base.get{S<:VehicleState,D,I,R}(::Feature_TurnRateF, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_TurnRateF, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} _get_feature_derivative_backwards(POSFYAW, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("AngularRateG", :angrateG, Float64, "rad/s²") -function Base.get{S<:VehicleState,D,I,R}(::Feature_AngularRateG, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_AngularRateG, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} _get_feature_derivative_backwards(TURNRATEG, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("AngularRateF", :angrateF, Float64, "rad/s²") -function Base.get{S<:VehicleState,D,I,R}(::Feature_AngularRateF, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_AngularRateF, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S<:VehicleState,D,I,R} _get_feature_derivative_backwards(TURNRATEF, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("DesiredAngle", :desang, Float64, "rad") -function Base.get{S<:VehicleState,D,I,R}(::Feature_DesiredAngle, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; +function Base.get(::Feature_DesiredAngle, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; kp_desired_angle::Float64 = 1.0, - ) + ) where {S<:VehicleState,D,I,R} retval = FeatureValue(0.0, FeatureState.INSUF_HIST) if pastframe_inbounds(rec, pastframe) && pastframe_inbounds(rec, pastframe-1) @@ -464,9 +464,9 @@ end ############################################# generate_feature_functions("Is_Colliding", :is_colliding, Bool, "-", lowerbound=0.0, upperbound=1.0) -function Base.get{S,D,I,R}(::Feature_Is_Colliding, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; +function Base.get(::Feature_Is_Colliding, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0; mem::CPAMemory=CPAMemory(), - ) + ) where {S,D,I,R} scene = rec[pastframe] is_colliding = convert(Float64, get_first_collision(scene, vehicle_index, mem).is_colliding) diff --git a/src/2d/features/lidar_sensors.jl b/src/2d/features/lidar_sensors.jl index 56f0289..57a4012 100644 --- a/src/2d/features/lidar_sensors.jl +++ b/src/2d/features/lidar_sensors.jl @@ -32,8 +32,8 @@ function LidarSensor(nbeams::Int; nbeams = 0 end - ranges = Array{Float64}(nbeams) - range_rates = Array{Float64}(nbeams) + ranges = Array{Float64}(undef, nbeams) + range_rates = Array{Float64}(undef, nbeams) LidarSensor(angles, ranges, range_rates, max_range, ConvexPolygon(4)) end nbeams(lidar::LidarSensor) = length(lidar.angles) @@ -295,7 +295,7 @@ function get_lane_portions(roadway::Roadway, x::Real, y::Real, lane_portion_max_ for lane in seg.lanes f = curvept -> normsquared(VecE2(curvept.pos - P)) ≤ Δ² i = findfirst(f, lane.curve) - if i != 0 + if i != nothing j = findlast(f, lane.curve) @assert(j != 0) push!(lane_portions, LanePortion(lane.tag, i, j)) diff --git a/src/2d/roadway/roadway_generation.jl b/src/2d/roadway/roadway_generation.jl index 42a3e60..2c0e236 100644 --- a/src/2d/roadway/roadway_generation.jl +++ b/src/2d/roadway/roadway_generation.jl @@ -7,11 +7,11 @@ export function gen_straight_curve(A::VecE2, B::VecE2, nsamples::Int) - θ = atan2(B-A) + θ = atan(B-A) δ = norm(B-A)/(nsamples-1) s = 0.0 - curve = Array{CurvePt}(nsamples) + curve = Array{CurvePt}(undef, nsamples) for i in 1 : nsamples t = (i-1)/(nsamples-1) P = lerp(A,B,t) @@ -31,7 +31,7 @@ function gen_straight_segment(seg_id::Int, nlanes::Int, length::Float64=1000.0; boundary_middle::LaneBoundary=LaneBoundary(:broken, :white), ) - seg = RoadSegment(seg_id, Array{Lane}(nlanes)) + seg = RoadSegment(seg_id, Array{Lane}(undef, nlanes)) y = -lane_widths[1]/2 for i in 1 : nlanes y += lane_widths[i]/2 @@ -66,13 +66,13 @@ function gen_bezier_curve(A::VecSE2, B::VecSE2, rA::Float64, rB::Float64, nsampl c = d + polar(-rB, B.θ) s = 0.0 - curve = Array{CurvePt}(nsamples) + curve = Array{CurvePt}(undef, nsamples) for i in 1 : nsamples t = (i-1)/(nsamples-1) P = lerp(a,b,c,d,t) P′ = 3*(1-t)^2*(b-a) + 6*(1-t)*t*(c-b) + 3*t^2*(d-c) P′′ = 6*(1-t)*(c-2b+a) + 6t*(d-2*c+b) - θ = atan2(P′) + θ = atan(P′) κ = (P′.x*P′′.y - P′.y*P′′.x)/(P′.x^2 + P′.y^2)^1.5 # signed curvature if i > 1 @@ -116,10 +116,10 @@ Generate a roadway that is a rectangular racetrack with rounded corners. radius = turn radius [m] ______________________ - / \ + / \\ | | | | - \______________________/ + \\______________________/ """ function gen_stadium_roadway(nlanes::Int; length::Float64=100.0, @@ -139,15 +139,15 @@ function gen_stadium_roadway(nlanes::Int; C = VecE2(0.0, width + radius) D = VecE2(0.0, radius) - seg1 = RoadSegment(1, Array{Lane}(nlanes)) - seg2 = RoadSegment(2, Array{Lane}(nlanes)) - seg3 = RoadSegment(3, Array{Lane}(nlanes)) - seg4 = RoadSegment(4, Array{Lane}(nlanes)) + seg1 = RoadSegment(1, Array{Lane}(undef, nlanes)) + seg2 = RoadSegment(2, Array{Lane}(undef, nlanes)) + seg3 = RoadSegment(3, Array{Lane}(undef, nlanes)) + seg4 = RoadSegment(4, Array{Lane}(undef, nlanes)) for i in 1 : nlanes - curvepts1 = Array{CurvePt}(ncurvepts_per_turn) - curvepts2 = Array{CurvePt}(ncurvepts_per_turn) - curvepts3 = Array{CurvePt}(ncurvepts_per_turn) - curvepts4 = Array{CurvePt}(ncurvepts_per_turn) + curvepts1 = Array{CurvePt}(undef, ncurvepts_per_turn) + curvepts2 = Array{CurvePt}(undef, ncurvepts_per_turn) + curvepts3 = Array{CurvePt}(undef, ncurvepts_per_turn) + curvepts4 = Array{CurvePt}(undef, ncurvepts_per_turn) r = radius + lane_width*(i-1) for j in 1:ncurvepts_per_turn diff --git a/src/2d/roadway/roadways.jl b/src/2d/roadway/roadways.jl index 85ad801..2d71245 100644 --- a/src/2d/roadway/roadways.jl +++ b/src/2d/roadway/roadways.jl @@ -42,8 +42,8 @@ function Base.write(io::IO, c::LaneConnection) @printf(io, "%s (%d %.6f) ", c.downstream ? "D" : "U", c.mylane.i, c.mylane.t) write(io, c.target) end -function Base.parse(::Type{LaneConnection}, line::String) - cleanedline = replace(line, r"(\(|\))", "") +function Base.parse(::Type{LaneConnection}, line::AbstractString) + cleanedline = replace(line, r"(\(|\))" => "") tokens = split(cleanedline, ' ') @assert(tokens[1] == "D" || tokens[1] == "U") @@ -94,10 +94,10 @@ mutable struct Lane retval.entrances = entrances if next != NULL_ROADINDEX - unshift!(retval.exits, LaneConnection(true, curveindex_end(retval.curve), next)) + pushfirst!(retval.exits, LaneConnection(true, curveindex_end(retval.curve), next)) end if prev != NULL_ROADINDEX - unshift!(retval.entrances, LaneConnection(false, CURVEINDEX_START, prev)) + pushfirst!(retval.entrances, LaneConnection(false, CURVEINDEX_START, prev)) end retval @@ -119,8 +119,8 @@ hash(l::Lane, h::UInt) = hash(l.curve, hash(l.width, hash(l.speed_limit, hash(l. has_next(lane::Lane) = !isempty(lane.exits) && lane.exits[1].mylane == curveindex_end(lane.curve) has_prev(lane::Lane) = !isempty(lane.entrances) && lane.entrances[1].mylane == CURVEINDEX_START -is_in_exits(lane::Lane, target::LaneTag) = findfirst(lc->lc.target.tag == target, lane.exits) != 0 -is_in_entrances(lane::Lane, target::LaneTag) = findfirst(lc->lc.target.tag == target, lane.entrances) != 0 +is_in_exits(lane::Lane, target::LaneTag) = findfirst(lc->lc.target.tag == target, lane.exits) != nothing +is_in_entrances(lane::Lane, target::LaneTag) = findfirst(lc->lc.target.tag == target, lane.entrances) != nothing function connect!(source::Lane, dest::Lane) @@ -129,8 +129,8 @@ function connect!(source::Lane, dest::Lane) cindS = curveindex_end(source.curve) cindD = CURVEINDEX_START - unshift!(source.exits, LaneConnection(true, cindS, RoadIndex(cindD, dest.tag))) - unshift!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, source.tag))) + pushfirst!(source.exits, LaneConnection(true, cindS, RoadIndex(cindD, dest.tag))) + pushfirst!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, source.tag))) (source, dest) end function connect!(source::Lane, cindS::CurveIndex, dest::Lane, cindD::CurveIndex) @@ -188,7 +188,7 @@ end function Base.read(io::IO, ::MIME"text/plain", ::Type{Roadway}) lines = readlines(io) line_index = 1 - if contains(lines[line_index], "ROADWAY") + if occursin("ROADWAY", lines[line_index]) line_index += 1 end @@ -199,11 +199,11 @@ function Base.read(io::IO, ::MIME"text/plain", ::Type{Roadway}) end nsegs = parse(Int, advance!()) - roadway = Roadway(Array{RoadSegment}(nsegs)) + roadway = Roadway(Array{RoadSegment}(undef, nsegs)) for i_seg in 1:nsegs segid = parse(Int, advance!()) nlanes = parse(Int, advance!()) - seg = RoadSegment(segid, Array{Lane}(nlanes)) + seg = RoadSegment(segid, Array{Lane}(undef, nlanes)) for i_lane in 1:nlanes @assert(i_lane == parse(Int, advance!())) tag = LaneTag(segid, i_lane) @@ -227,10 +227,10 @@ function Base.read(io::IO, ::MIME"text/plain", ::Type{Roadway}) end npts = parse(Int, advance!()) - curve = Array{CurvePt}(npts) + curve = Array{CurvePt}(undef, npts) for i_pt in 1:npts line = advance!() - cleanedline = replace(line, r"(\(|\))", "") + cleanedline = replace(line, r"(\(|\))" => "") tokens = split(cleanedline, ' ') x = parse(Float64, tokens[1]) y = parse(Float64, tokens[2]) @@ -322,7 +322,7 @@ function has_lanetag(roadway::Roadway, tag::LaneTag) 1 ≤ tag.lane ≤ length(seg.lanes) end -immutable RoadProjection +struct RoadProjection curveproj::CurveProjection tag::LaneTag end @@ -592,7 +592,7 @@ function _fit_curve( @assert(!any(s->isnan(s), y_arr)) @assert(!any(s->isnan(s), θ_arr)) - curve = Array{CurvePt}(n) + curve = Array{CurvePt}(undef, n) for i in 1 : n pos = VecSE2(x_arr[i], y_arr[i], θ_arr[i]) curve[i] = CurvePt(pos, s_arr[i], κ_arr[i], κd_arr[i]) @@ -610,13 +610,13 @@ Return a Roadway generated from a DXF file """ function read_dxf(io::IO, ::Type{Roadway}; dist_threshold_lane_connect::Float64 = 0.25, # [m] - desired_distance_between_curve_samples::Float64 = 1.0; # [m] + desired_distance_between_curve_samples::Float64 = 1.0 # [m] ) lines = readlines(io) i = findfirst(lines, "ENTITIES\n") - i != 0 || error("ENTITIES section not found") + i != nothing || error("ENTITIES section not found") ################################################### # Pull pts for each lane @@ -638,7 +638,7 @@ function read_dxf(io::IO, ::Type{Roadway}; N = parse(Int, lines[i+1]) N > 0 || error("Empty line segment!") - pts = Array{VecE2}(N) + pts = Array{VecE2}(undef, N) i = findnext(lines, " 10\n", i) i != 0 || error("First point not found in AcDbPolyline!") @@ -745,14 +745,14 @@ function read_dxf(io::IO, ::Type{Roadway}; # then project each lane's midpoint to the perpendicular at the midpoint @assert(!isempty(lanetags)) - proj_positions = Array{Float64}(length(lanetags)) + proj_positions = Array{Float64}(undef, length(lanetags)) first_lane_pts = lane_pts_dict[lanetags[1]] n = length(first_lane_pts) lo = first_lane_pts[div(n,2)] hi = first_lane_pts[div(n,2)+1] midpt_orig = (lo + hi)/2 - dir = polar(1.0, atan2(hi - lo) + π/2) # direction perpendicular (left) of lane + dir = polar(1.0, atan(hi - lo) + π/2) # direction perpendicular (left) of lane for (i,tag) in enumerate(lanetags) pts = lane_pts_dict[tag] @@ -768,7 +768,7 @@ function read_dxf(io::IO, ::Type{Roadway}; boundary_right = i == 1 ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white) pts = lane_pts_dict[tag] - pt_matrix = Array{Float64}(2, length(pts)) + pt_matrix = Array{Float64}(undef, 2, length(pts)) for (k,P) in enumerate(pts) pt_matrix[1,k] = P.x pt_matrix[2,k] = P.y @@ -809,7 +809,7 @@ function read_dxf(io::IO, ::Type{Roadway}; end else # otherwise connect as before - unshift!(lane.exits, LaneConnection(true, cindS, RoadIndex(cindD, dest.tag))) + pushfirst!(lane.exits, LaneConnection(true, cindS, RoadIndex(cindD, dest.tag))) push!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, lane.tag))) end end @@ -831,7 +831,7 @@ function read_dxf(io::IO, ::Type{Roadway}; else # a standard connection push!(prev.exits, LaneConnection(true, cindS, RoadIndex(cindD, lane.tag))) - unshift!(lane.entrances, LaneConnection(false, cindD, RoadIndex(cindS, prev.tag))) + pushfirst!(lane.entrances, LaneConnection(false, cindD, RoadIndex(cindS, prev.tag))) end end diff --git a/src/2d/roadway/splines.jl b/src/2d/roadway/splines.jl index 1f8b1d3..b3329be 100644 --- a/src/2d/roadway/splines.jl +++ b/src/2d/roadway/splines.jl @@ -50,7 +50,7 @@ function _fit_open(pts::AbstractVector{Float64} ) M[n+1,n+1] = 2 M[1,1] = 2 - Y = Array{Float64}(n+1) + Y = Array{Float64}(undef, n+1) for i = 1 : n+1 ind_hi = min(i+1,n) ind_lo = max(1,i-1) @@ -59,7 +59,7 @@ function _fit_open(pts::AbstractVector{Float64} ) D = M\Y - spline_coeffs = Array{Float64}(4, n) # col is + spline_coeffs = Array{Float64}(undef, 4, n) # col is spline_coeffs[1,:] = pts[1:n] spline_coeffs[2,:] = D[1:n] spline_coeffs[3,:] = 3*(pts[2:n+1] - pts[1:n]) -2*D[1:n]-D[2:n+1] @@ -89,7 +89,7 @@ function _fit_closed(pts::AbstractVector{Float64} ) M[1,n+1] = 1 M[n+1,1] = 1 - Y = Array{Float64}(n+1) + Y = Array{Float64}(undef, n+1) Y[1] = 3*(pts[2] - pts[n+1]) for i = 2 : n Y[i] = 3*(pts[i+1] - pts[i-1]) @@ -98,7 +98,7 @@ function _fit_closed(pts::AbstractVector{Float64} ) D = M\Y - spline_coeffs = Array{Float64}(4, n+1) # col is + spline_coeffs = Array{Float64}(undef, 4, n+1) # col is spline_coeffs[1,:] = pts spline_coeffs[2,:] = D spline_coeffs[3,1:n] = 3*(pts[2:n+1] - pts[1:n]) -2*D[1:n]-D[2:n+1] @@ -115,7 +115,7 @@ function _fit_open(pts::Matrix{Float64}) # 2×n {x,y} d,n = size(pts) n -= 1 - Y = Array{Float64}(n+1) + Y = Array{Float64}(undef, n+1) M = sparse(Int[], Int[], Float64[], n+1,n+1) for i in 1 : n @@ -126,7 +126,7 @@ function _fit_open(pts::Matrix{Float64}) # 2×n {x,y} M[n+1,n+1] = 2.0 M[1,1] = 2.0 - retval = Array{Matrix{Float64}}(d) + retval = Array{Matrix{Float64}}(undef, d) for k in 1 : d for i in 1 : n+1 @@ -137,7 +137,7 @@ function _fit_open(pts::Matrix{Float64}) # 2×n {x,y} D = M \ Y - spline_coeffs = Array{Float64}(4, n) # col is for a + b⋅t + c⋅t² + d⋅t³ + spline_coeffs = Array{Float64}(undef, 4, n) # col is for a + b⋅t + c⋅t² + d⋅t³ spline_coeffs[1,:] = pts[k,1:n] # x₀ spline_coeffs[2,:] = D[1:n] # x'₀ spline_coeffs[3,:] = 3*(pts[k,2:n+1]' - pts[k,1:n]') -2*D[1:n] - D[2:n+1] # -3x₀ + 3x₁ - 2x'₀ - x'₁ @@ -149,7 +149,7 @@ function _fit_open(pts::Matrix{Float64}) # 2×n {x,y} end function _fit_closed(pts::AbstractMatrix{Float64}) d = size(pts,1) - retval = Array{Matrix{Float64}}(d) + retval = Array{Matrix{Float64}}(undef, d) for i = 1 : d retval[i] = _fit_closed(vec(pts[i,:])) end @@ -184,7 +184,7 @@ function sample_spline(spline_coeffs::AbstractVector{Float64}, t_arr::AbstractVe c = spline_coeffs[3] d = spline_coeffs[4] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = a + t*(b + t*(c + t*d)) end @@ -195,7 +195,7 @@ function sample_spline(spline_coeffs::AbstractMatrix{Float64}, t_arr::AbstractVe # for t ∈ [1,2] we use spline_coeffs[:,2] # etc. @assert(size(spline_coeffs, 1) == 4) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2)) retval[i] = sample_spline(spline_coeffs[:,col_ind], t-col_ind+1) @@ -222,7 +222,7 @@ function sample_spline_derivative(spline_coeffs::AbstractVector{Float64}, t_arr: c = spline_coeffs[3] d = spline_coeffs[4] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = b + t*(2c + t*3d) end @@ -233,7 +233,7 @@ function sample_spline_derivative(spline_coeffs::AbstractMatrix{Float64}, t_arr: # for t ∈ [1,2] we use spline_coeffs[:,2] # etc. @assert(size(spline_coeffs, 1) == 4) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2)) retval[i] = sample_spline_derivative(spline_coeffs[:,col_ind], t-col_ind+1) @@ -260,7 +260,7 @@ function sample_spline_derivative2(spline_coeffs::AbstractVector{Float64}, t_arr c = spline_coeffs[3] d = spline_coeffs[4] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = 2c + t*6d end @@ -271,7 +271,7 @@ function sample_spline_derivative2(spline_coeffs::AbstractMatrix{Float64}, t_arr # for t ∈ [1,2] we use spline_coeffs[:,2] # etc. @assert(size(spline_coeffs, 1) == 4) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2)) retval[i] = sample_spline_derivative2(spline_coeffs[:,col_ind], t-col_ind+1) @@ -306,7 +306,7 @@ function sample_spline_speed(spline_coeffs_x::AbstractVector{Float64}, spline_co cy = spline_coeffs_y[3] dy = spline_coeffs_y[4] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) dxdt = bx + t*(2cx + t*3dx) dydt = by + t*(2cy + t*3dy) @@ -323,7 +323,7 @@ function sample_spline_speed(spline_coeffs_x::AbstractMatrix{Float64}, spline_co @assert(size(spline_coeffs_x, 1) == 4) @assert(size(spline_coeffs_y, 1) == 4) @assert(n == size(spline_coeffs_y, 2)) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, n) retval[i] = sample_spline_speed(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1) @@ -350,9 +350,9 @@ function sample_spline_theta(spline_coeffs_x::AbstractVector{Float64}, spline_co y1 = sample_spline(spline_coeffs_y, t_lo) y2 = sample_spline(spline_coeffs_y, t_hi) - # println("(t, lo, hi) $t $t_lo $t_hi, ($(atan2(y2-y1, x2-x1)))") + # println("(t, lo, hi) $t $t_lo $t_hi, ($(atan(y2-y1, x2-x1)))") - atan2(y2-y1, x2-x1) + atan(y2-y1, x2-x1) end function sample_spline_theta(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t::Float64) # for t ∈ (-∞,1] we use spline_coeffs[:,1] @@ -368,7 +368,7 @@ end function sample_spline_theta(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t_arr::AbstractVector{Float64}) # here t is generally expected to be t ∈ [0,1] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = sample_spline_theta(spline_coeffs_x, spline_coeffs_y, t) end @@ -383,7 +383,7 @@ function sample_spline_theta(spline_coeffs_x::AbstractMatrix{Float64}, spline_co @assert(size(spline_coeffs_x, 1) == 4) @assert(size(spline_coeffs_y, 1) == 4) @assert(n == size(spline_coeffs_y, 2)) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, n) retval[i] = sample_spline_theta(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1) @@ -415,7 +415,7 @@ end function sample_spline_curvature(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t_arr::AbstractVector{Float64}) # here t is generally expected to be t ∈ [0,1] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = sample_spline_curvature(spline_coeffs_x, spline_coeffs_y, t) end @@ -430,7 +430,7 @@ function sample_spline_curvature(spline_coeffs_x::AbstractMatrix{Float64}, splin @assert(size(spline_coeffs_x, 1) == 4) @assert(size(spline_coeffs_y, 1) == 4) @assert(n == size(spline_coeffs_y, 2)) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, n) retval[i] = sample_spline_curvature(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1) @@ -474,7 +474,7 @@ function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractVector{F # here t is generally expected to be t ∈ [0,1] - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) retval[i] = sample_spline_derivative_of_curvature(spline_coeffs_x, spline_coeffs_y, t, stepsize=stepsize) end @@ -492,7 +492,7 @@ function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractMatrix{F @assert(size(spline_coeffs_x, 1) == 4) @assert(size(spline_coeffs_y, 1) == 4) @assert(n == size(spline_coeffs_y, 2)) - retval = Array{Float64}(length(t_arr)) + retval = Array{Float64}(undef, length(t_arr)) for (i,t) in enumerate(t_arr) col_ind = clamp(ceil(Int, t), 1, n) retval[i] = sample_spline_derivative_of_curvature(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1, stepsize=stepsize) @@ -707,7 +707,7 @@ function calc_curve_param_given_arclen( ) n = length(s_arr) - t_arr = Array{Float64}(n) + t_arr = Array{Float64}(undef, n) s = s_arr[1] t = s/curve_length @@ -771,7 +771,7 @@ function calc_curve_param_given_arclen( @assert(size(spline_coeffs_y,2) == n_segments) n = length(s_arr) - t_arr = Array{Float64}(n) + t_arr = Array{Float64}(undef, n) s = s_arr[1] t = s/curve_length diff --git a/src/2d/utils/minkowski.jl b/src/2d/utils/minkowski.jl index 45a4f94..aa089bf 100644 --- a/src/2d/utils/minkowski.jl +++ b/src/2d/utils/minkowski.jl @@ -61,12 +61,15 @@ mutable struct ConvexPolygon npts::Int # number of pts that are currently used (since we preallocate a longer array) end -ConvexPolygon(npts::Int) = ConvexPolygon(Array{VecE2}(npts), 0) +ConvexPolygon(npts::Int) = ConvexPolygon(Array{VecE2}(undef, npts), 0) ConvexPolygon(pts::Vector{VecE2}) = ConvexPolygon(pts, length(pts)) -Base.start(poly::ConvexPolygon) = 1 -Base.done(poly::ConvexPolygon, i::Int) = i > length(poly) -Base.next(poly::ConvexPolygon, i::Int) = (poly.pts[i], i+1) +function Base.iterate(poly::ConvexPolygon, i::Int=1) + if i > length(poly) + return nothing + end + return (poly.pts[i], i+1) +end Base.length(poly::ConvexPolygon) = poly.npts Base.isempty(poly::ConvexPolygon) = poly.npts == 0 @@ -76,9 +79,9 @@ function Base.empty!(poly::ConvexPolygon) poly.npts = 0 poly end -function Base.copy!(dest::ConvexPolygon, src::ConvexPolygon) +function Base.copyto!(dest::ConvexPolygon, src::ConvexPolygon) dest.npts = src.npts - copy!(dest.pts, 1, src.pts, 1, src.npts) + copyto!(dest.pts, 1, src.pts, 1, src.npts) dest end function Base.push!(poly::ConvexPolygon, v::VecE2) @@ -86,7 +89,7 @@ function Base.push!(poly::ConvexPolygon, v::VecE2) poly.npts += 1 poly end -function Base.contains(poly::ConvexPolygon, v::VecE2) +function Base.in(v::VecE2, poly::ConvexPolygon) # does NOT include pts on the physical boundary @@ -110,7 +113,7 @@ function Base.contains(poly::ConvexPolygon, v::VecE2) end true end -Base.contains(poly::ConvexPolygon, v::VecSE2) = contains(poly, convert(VecE2, v)) +Base.in(v::VecSE2, poly::ConvexPolygon) = in(convert(VecE2, v), poly) function Base.show(io::IO, poly::ConvexPolygon) @printf(io, "ConvexPolygon: len %d (max %d pts)\n", length(poly), length(poly.pts)) for i in 1 : length(poly) @@ -121,7 +124,7 @@ end AutomotiveDrivingModels.get_center(poly::ConvexPolygon) = sum(poly.pts) / poly.npts function Vec.get_distance(poly::ConvexPolygon, v::VecE2; solid::Bool=true) - if solid && contains(poly, v) + if solid && in(v, poly) 0.0 else min_dist = Inf @@ -145,7 +148,7 @@ function ensure_pts_sorted_by_min_polar_angle!(poly::ConvexPolygon, npts::Int=po for i in 1 : npts seg = get_edge(poly.pts, i, npts) - θ = atan2(seg.B.y - seg.A.y, seg.B.x - seg.A.x) + θ = atan(seg.B.y - seg.A.y, seg.B.x - seg.A.x) if θ < 0.0 θ += 2π end @@ -251,11 +254,11 @@ end function is_colliding(P::ConvexPolygon, Q::ConvexPolygon, temp::ConvexPolygon=ConvexPolygon(length(P) + length(Q))) minkowski_difference!(temp, P, Q) - contains(temp, VecE2(0,0)) + in(VecE2(0,0), temp) end function Vec.get_distance(P::ConvexPolygon, Q::ConvexPolygon, temp::ConvexPolygon=ConvexPolygon(length(P) + length(Q))) minkowski_difference!(temp, P, Q) - get_distance(temp, VecE2(0,0)) + get_distance(VecE2(0,0), temp) end function to_oriented_bounding_box!(retval::ConvexPolygon, center::VecSE2, len::Float64, wid::Float64) @@ -337,9 +340,9 @@ function get_time_and_dist_of_closest_approach(a::Vehicle, b::Vehicle, mem::CPAM rel_pos = convert(VecE2, b.state.posG) - a.state.posG rel_velocity = polar(b.state.v, b.state.posG.θ) - polar(a.state.v, a.state.posG.θ) ray_speed = norm(VecE2(rel_velocity)) - ray = VecSE2(rel_pos, atan2(rel_velocity)) + ray = VecSE2(rel_pos, atan(rel_velocity)) - if contains(mem.mink, convert(VecE2, ray)) + if in(convert(VecE2, ray), mem.mink) return (0.0, 0.0) end @@ -398,7 +401,7 @@ end """ Loops through the scene and finds the first collision between a vehicle and scene[target_index] """ -function get_first_collision{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, target_index::Int, mem::CPAMemory=CPAMemory()) +function get_first_collision(scene::EntityFrame{S,D,I}, target_index::Int, mem::CPAMemory=CPAMemory()) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} A = target_index vehA = scene[A] vehA = convert(Vehicle,vehA) @@ -419,7 +422,7 @@ end """ Loops through the scene and finds the first collision between any two vehicles """ -function get_first_collision{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_indeces::AbstractVector{Int}, mem::CPAMemory=CPAMemory()) +function get_first_collision(scene::EntityFrame{S,D,I}, vehicle_indeces::AbstractVector{Int}, mem::CPAMemory=CPAMemory()) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} N = length(vehicle_indeces) for (a,A) in enumerate(vehicle_indeces) @@ -441,9 +444,9 @@ function get_first_collision{S<:VehicleState,D<:Union{VehicleDef, BicycleModel}, CollisionCheckResult(false, 0, 0) end -get_first_collision{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, mem::CPAMemory=CPAMemory()) = get_first_collision(scene, 1:length(scene), mem) -is_collision_free{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, mem::CPAMemory=CPAMemory()) = !(get_first_collision(scene, mem).is_colliding) -is_collision_free{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_indeces::AbstractVector{Int}, mem::CPAMemory=CPAMemory()) = get_first_collision(scene, vehicle_indeces, mem).is_colliding +get_first_collision(scene::EntityFrame{S,D,I}, mem::CPAMemory=CPAMemory()) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} = get_first_collision(scene, 1:length(scene), mem) +is_collision_free(scene::EntityFrame{S,D,I}, mem::CPAMemory=CPAMemory()) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} = !(get_first_collision(scene, mem).is_colliding) +is_collision_free(scene::EntityFrame{S,D,I}, vehicle_indeces::AbstractVector{Int}, mem::CPAMemory=CPAMemory()) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} = get_first_collision(scene, vehicle_indeces, mem).is_colliding ### @@ -455,13 +458,13 @@ Terminates the simulation once a collision occurs @with_kw struct CollisionCallback mem::CPAMemory=CPAMemory() end -function run_callback{S,D,I,R,M<:DriverModel}( +function run_callback( callback::CollisionCallback, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int, - ) + ) where {S,D,I,R,M<:DriverModel} return !is_collision_free(rec[0], callback.mem) end diff --git a/src/2d/utils/trajdata_cleaning.jl b/src/2d/utils/trajdata_cleaning.jl index 1c4ae00..ef35de4 100644 --- a/src/2d/utils/trajdata_cleaning.jl +++ b/src/2d/utils/trajdata_cleaning.jl @@ -26,8 +26,8 @@ end function Base.convert(::Type{Trajdata}, tdem::TrajdataEditMode) roadway = tdem.roadway vehicles = Dict{Int, VehicleDef}() - states = Array{TrajdataState}(sum(s->length(s), tdem.scenes)) - frames = Array{TrajdataFrame}(length(tdem.scenes)) + states = Array{TrajdataState}(undef, sum(s->length(s), tdem.scenes)) + frames = Array{TrajdataFrame}(undef, length(tdem.scenes)) states_index = 0 for (frame_index, scene) in enumerate(tdem.scenes) @@ -78,7 +78,7 @@ end function interpolate_to_timestep!(tdem::TrajdataEditMode, timestep::Float64) time = collect(tdem.time[1]:timestep:tdem.time[end]) - scenes = Array{Scene}(length(time)) + scenes = Array{Scene}(undef, length(time)) scenes[1] = tdem.scenes[1] frame_old = 1 @@ -102,7 +102,7 @@ function interpolate_to_timestep!(tdem::TrajdataEditMode, timestep::Float64) for veh_lo in scene_lo id = veh_lo.id veh_index = findfirst(scene_hi, id) - if veh_index != 0 + if veh_index != nothing veh_hi = scene_hi[veh_index] @assert(veh_lo.def == veh_hi.def) veh_interp = Vehicle(lerp(veh_lo.state, veh_hi.state, γ, tdem.roadway), veh_lo.def) diff --git a/src/2d/vehicles/scenes.jl b/src/2d/vehicles/scenes.jl index dc8fe6c..8eedddb 100644 --- a/src/2d/vehicles/scenes.jl +++ b/src/2d/vehicles/scenes.jl @@ -36,7 +36,7 @@ get_targetpoint_delta(::VehicleTargetPointRear, veh::Vehicle) = -veh.def.length/ const VEHICLE_TARGET_POINT_CENTER = VehicleTargetPointCenter() -function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_fore_along_lane( scene::EntityFrame{S,D,I}, roadway::Roadway, tag_start::LaneTag, @@ -45,8 +45,7 @@ function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_fore::Float64 = 250.0, # max distance to search forward [m] index_to_ignore::Int=-1, - ) - + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} best_ind = 0 best_dist = max_distance_fore tag_target = tag_start @@ -62,16 +61,13 @@ function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc s_adjust = NaN if veh.state.posF.roadind.tag == tag_target s_adjust = 0.0 - elseif is_between_segments_hi(veh.state.posF.roadind.ind, lane.curve) && is_in_entrances(roadway[tag_target], veh.state.posF.roadind.tag) - distance_between_lanes = norm(VecE2(roadway[tag_target].curve[1].pos - roadway[veh.state.posF.roadind.tag].curve[end].pos)) s_adjust = -(roadway[veh.state.posF.roadind.tag].curve[end].s + distance_between_lanes) elseif is_between_segments_lo(veh.state.posF.roadind.ind) && is_in_exits(roadway[tag_target], veh.state.posF.roadind.tag) - distance_between_lanes = norm(VecE2(roadway[tag_target].curve[end].pos - roadway[veh.state.posF.roadind.tag].curve[1].pos)) s_adjust = roadway[tag_target].curve[end].s + distance_between_lanes end @@ -107,7 +103,7 @@ function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc NeighborLongitudinalResult(best_ind, best_dist) end -function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_fore_along_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -115,7 +111,7 @@ function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} veh_ego = scene[vehicle_index] tag_start = veh_ego.state.posF.roadind.tag @@ -125,7 +121,7 @@ function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_primary, targetpoint_valid, max_distance_fore=max_distance_fore, index_to_ignore=vehicle_index) end -function get_neighbor_fore_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_fore_along_left_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -133,7 +129,7 @@ function get_neighbor_fore_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} retval = NeighborLongitudinalResult(0, max_distance_fore) @@ -153,7 +149,7 @@ function get_neighbor_fore_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, retval end -function get_neighbor_fore_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_fore_along_right_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -161,7 +157,7 @@ function get_neighbor_fore_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} retval = NeighborLongitudinalResult(0, max_distance_fore) @@ -182,46 +178,46 @@ function get_neighbor_fore_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, retval end -function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_fore_along_lane( scene::EntityFrame{S,D,I}, roadway::Roadway, tag_start::LaneTag, s_base::Float64; max_distance_fore::Float64 = 250.0, # max distance to search forward [m] index_to_ignore::Int=-1, - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore, index_to_ignore=index_to_ignore) end -function get_neighbor_fore_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; +function get_neighbor_fore_along_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_fore_along_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore) end -function get_neighbor_fore_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; +function get_neighbor_fore_along_left_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_fore_along_left_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore) end -function get_neighbor_fore_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; +function get_neighbor_fore_along_right_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_fore::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_fore_along_right_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore) end -function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_lane( scene::EntityFrame{S,D,I}, roadway::Roadway, tag_start::LaneTag, @@ -230,13 +226,13 @@ function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_rear::Float64 = 250.0, # max distance to search rearward [m] index_to_ignore::Int=-1, - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} best_ind = 0 best_dist = max_distance_rear tag_target = tag_start - ignore = Set{Int}() + ignore = Set{I}() dist_searched = 0.0 while dist_searched < max_distance_rear @@ -297,7 +293,7 @@ function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc NeighborLongitudinalResult(best_ind, best_dist) end -function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -305,7 +301,7 @@ function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_rear::Float64 = 250.0 # max distance to search rearward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} veh_ego = scene[vehicle_index] tag_start = veh_ego.state.posF.roadind.tag @@ -315,7 +311,7 @@ function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, Bicyc targetpoint_primary, targetpoint_valid, max_distance_rear=max_distance_rear, index_to_ignore=vehicle_index) end -function get_neighbor_rear_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_left_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -323,7 +319,7 @@ function get_neighbor_rear_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_rear::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} retval = NeighborLongitudinalResult(0, max_distance_rear) @@ -343,7 +339,7 @@ function get_neighbor_rear_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, retval end -function get_neighbor_rear_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_right_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway, @@ -351,7 +347,7 @@ function get_neighbor_rear_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle max_distance_rear::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} retval = NeighborLongitudinalResult(0, max_distance_rear) @@ -372,45 +368,45 @@ function get_neighbor_rear_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, retval end -function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_lane( scene::EntityFrame{S,D,I}, roadway::Roadway, tag_start::LaneTag, s_base::Float64; max_distance_rear::Float64 = 250.0, # max distance to search rearward [m] index_to_ignore::Int=-1, - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear, index_to_ignore=index_to_ignore) end -function get_neighbor_rear_along_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; +function get_neighbor_rear_along_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_rear::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_rear_along_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear) end -function get_neighbor_rear_along_left_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_left_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_rear::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_rear_along_left_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear) end -function get_neighbor_rear_along_right_lane{S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I}( +function get_neighbor_rear_along_right_lane( scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway; max_distance_rear::Float64 = 250.0 # max distance to search forward [m] - ) + ) where {S<:VehicleState,D<:Union{VehicleDef, BicycleModel},I} get_neighbor_rear_along_right_lane(scene, vehicle_index, roadway, VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER, diff --git a/src/2d/vehicles/vehicles.jl b/src/2d/vehicles/vehicles.jl index f22bcb4..fdb886b 100644 --- a/src/2d/vehicles/vehicles.jl +++ b/src/2d/vehicles/vehicles.jl @@ -2,14 +2,14 @@ struct VehicleState posG::VecSE2 # global posF::Frenet # lane-relative frame v::Float64 - - VehicleState() = new(VecSE2(), NULL_FRENET, NaN) - VehicleState(posG::VecSE2, v::Float64) = new(posG, NULL_FRENET, v) - VehicleState(posG::VecSE2, posF::Frenet, v::Float64) = new(posG, posF, v) - VehicleState(posG::VecSE2, roadway::Roadway, v::Float64) = new(posG, Frenet(posG, roadway), v) - VehicleState(posG::VecSE2, lane::Lane, roadway::Roadway, v::Float64) = new(posG, Frenet(posG, lane, roadway), v) - VehicleState(posF::Frenet, roadway::Roadway, v::Float64) = new(get_posG(posF, roadway), posF, v) end + +VehicleState() = VehicleState(VecSE2(), NULL_FRENET, NaN) +VehicleState(posG::VecSE2, v::Float64) = VehicleState(posG, NULL_FRENET, v) +VehicleState(posG::VecSE2, roadway::Roadway, v::Float64) = VehicleState(posG, Frenet(posG, roadway), v) +VehicleState(posG::VecSE2, lane::Lane, roadway::Roadway, v::Float64) = VehicleState(posG, Frenet(posG, lane, roadway), v) +VehicleState(posF::Frenet, roadway::Roadway, v::Float64) = VehicleState(get_posG(posF, roadway), posF, v) + Base.show(io::IO, s::VehicleState) = print(io, "VehicleState(", s.posG, ", ", s.posF, ", ", @sprintf("%.3f", s.v), ")") function Base.write(io::IO, ::MIME"text/plain", s::VehicleState) @printf(io, "%.16e %.16e %.16e", s.posG.x, s.posG.y, s.posG.θ) diff --git a/src/AutomotiveDrivingModels.jl b/src/AutomotiveDrivingModels.jl index d8cb350..0fde550 100644 --- a/src/AutomotiveDrivingModels.jl +++ b/src/AutomotiveDrivingModels.jl @@ -1,9 +1,9 @@ -__precompile__() - module AutomotiveDrivingModels using Reexport using Parameters +using Printf +using LinearAlgebra @reexport using Records @reexport using Distributions diff --git a/src/interface/actions.jl b/src/interface/actions.jl index 2df0aff..6da1d55 100644 --- a/src/interface/actions.jl +++ b/src/interface/actions.jl @@ -4,7 +4,7 @@ Take an entity of type {S,D,I} and move it over Δt seconds to produce a new entity based on the action on the given roadway. """ -propagate{S,D,I,A,R}(veh::Entity{S,D,I}, action::A, roadway::R, Δt::Float64) = error("propagate not implemented for Entity{$S, $D, $I}, actions $A, and roadway $R") +propagate(veh::Entity{S,D,I}, action::A, roadway::R, Δt::Float64) where {S,D,I,A,R} = error("propagate not implemented for Entity{$S, $D, $I}, actions $A, and roadway $R") -propagate{S,D,I,R}(veh::Entity{S,D,I}, state::S, roadway::R, Δt::Float64) = state \ No newline at end of file +propagate(veh::Entity{S,D,I}, state::S, roadway::R, Δt::Float64) where {S,D,I,R}= state \ No newline at end of file diff --git a/src/interface/callbacks.jl b/src/interface/callbacks.jl index 1f61d51..707ffe8 100644 --- a/src/interface/callbacks.jl +++ b/src/interface/callbacks.jl @@ -3,16 +3,16 @@ export # run callback and return whether simlation should terminate -run_callback{S,D,I,R,M<:DriverModel}(callback::Any, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int) = error("run_callback not implemented for callback $(typeof(callback))") +run_callback(callback::Any, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int) where {S,D,I,R,M<:DriverModel} = error("run_callback not implemented for callback $(typeof(callback))") -function _run_callbacks{S,D,I,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}(callbacks::C, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int) +function _run_callbacks(callbacks::C, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int) where {S,D,I,R,M<:DriverModel,C<:Tuple{Vararg{Any}}} isdone = false for callback in callbacks isdone |= run_callback(callback, rec, roadway, models, tick) end return isdone end -function simulate!{S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}( +function simulate!( ::Type{A}, rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, @@ -20,7 +20,7 @@ function simulate!{S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}( models::Dict{I,M}, nticks::Int, callbacks::C, - ) + ) where {S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}} empty!(rec) update!(rec, scene) @@ -30,7 +30,7 @@ function simulate!{S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}( return rec end - actions = Array{A}(length(scene)) + actions = Array{A}(undef, length(scene)) for tick in 1 : nticks get_actions!(actions, scene, roadway, models) tick!(scene, roadway, actions, get_timestep(rec)) @@ -42,14 +42,14 @@ function simulate!{S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}( return rec end -function simulate!{S,D,I,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}( +function simulate!( rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M}, nticks::Int, callbacks::C, - ) + ) where {S,D,I,R,M<:DriverModel,C<:Tuple{Vararg{Any}}} return simulate!(Any, rec, scene, roadway, models, nticks, callbacks) end \ No newline at end of file diff --git a/src/interface/driver_models.jl b/src/interface/driver_models.jl index fcbd35e..7f3677d 100644 --- a/src/interface/driver_models.jl +++ b/src/interface/driver_models.jl @@ -13,15 +13,15 @@ export abstract type DriverModel{DriveAction} end get_name(::DriverModel) = "???" -action_type{A}(::DriverModel{A}) = A +action_type(::DriverModel{A}) where {A} = A set_desired_speed!(model::DriverModel, v_des::Float64) = model # do nothing by default reset_hidden_state!(model::DriverModel) = model # do nothing by default -observe!{S,D,I,R}(model::DriverModel, scene::EntityFrame{S,D,I}, roadway::R, egoid::Int) = model # do nothing by default +observe!(model::DriverModel, scene::EntityFrame{S,D,I}, roadway::R, egoid::Int) where {S,D,I,R} = model # do nothing by default Base.rand(model::DriverModel) = error("rand not implemented for model $model") -Distributions.pdf{A}(model::DriverModel{A}, a::A) = error("pdf not implemented for model $model") -Distributions.logpdf{A}(model::DriverModel{A}, a::A) = error("logpdf not implemented for model $model") +Distributions.pdf(model::DriverModel{A}, a::A) where {A} = error("pdf not implemented for model $model") +Distributions.logpdf(model::DriverModel{A}, a::A) where {A} = error("logpdf not implemented for model $model") -function prime_with_history!{S,D,I,R}( +function prime_with_history!( model::DriverModel, trajdata::ListRecord{Entity{S,D,I}}, roadway::R, @@ -29,7 +29,7 @@ function prime_with_history!{S,D,I,R}( frame_end::Int, egoid::I, scene::EntityFrame{S,D,I} = allocate_frame(trajdata), - ) + ) where {S,D,I,R} reset_hidden_state!(model) @@ -40,10 +40,10 @@ function prime_with_history!{S,D,I,R}( return model end -function prime_with_history!{S,D,I,R}(model::DriverModel, rec::EntityQueueRecord{S,D,I}, roadway::R, egoid::I; +function prime_with_history!(model::DriverModel, rec::EntityQueueRecord{S,D,I}, roadway::R, egoid::I; pastframe_start::Int=1-nframes(rec), pastframe_end::Int=0, - ) + ) where {S,D,I,R} reset_hidden_state!(model) @@ -63,9 +63,9 @@ struct StaticDriver{A,P<:ContinuousMultivariateDistribution} <: DriverModel{A} end get_name(::StaticDriver) = "StaticDriver" -function Base.rand{A,P}(model::StaticDriver{A,P}) +function Base.rand(model::StaticDriver{A,P}) where {A,P} a = rand(model.distribution) return convert(A, a) end -Distributions.pdf{A}(model::StaticDriver{A}, a::A) = pdf(model.distribution, convert(Vector{Float64}, a)) -Distributions.logpdf{A}(model::StaticDriver{A}, a::A) = logpdf(model.distribution, convert(Vector{Float64}, a)) \ No newline at end of file +Distributions.pdf(model::StaticDriver{A}, a::A) where {A} = pdf(model.distribution, convert(Vector{Float64}, a)) +Distributions.logpdf(model::StaticDriver{A}, a::A) where {A} = logpdf(model.distribution, convert(Vector{Float64}, a)) \ No newline at end of file diff --git a/src/interface/features.jl b/src/interface/features.jl index ded6d03..b831e16 100644 --- a/src/interface/features.jl +++ b/src/interface/features.jl @@ -70,7 +70,7 @@ function generate_feature_functions( @eval begin export $const_name - immutable $feature_name <: AbstractFeature end + struct $feature_name <: AbstractFeature end const $const_name = ($feature_name)() units( ::$feature_name) = $(units) inherent_type( ::$feature_name) = $(inherent_type) @@ -83,14 +83,14 @@ function generate_feature_functions( end end -function _get_feature_derivative_backwards{S,D,I,R}( +function _get_feature_derivative_backwards( f::AbstractFeature, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0, frames_back::Int=1, - ) + ) where {S,D,I,R} id = rec[pastframe][vehicle_index].id @@ -102,7 +102,7 @@ function _get_feature_derivative_backwards{S,D,I,R}( veh_index_curr = vehicle_index veh_index_prev = findfirst(rec[pastframe2], id) - if veh_index_prev != 0 + if veh_index_prev != nothing curr = convert(Float64, get(f, rec, roadway, veh_index_curr, pastframe)) past = convert(Float64, get(f, rec, roadway, veh_index_prev, pastframe2)) Δt = get_elapsed_time(rec, pastframe2, pastframe) @@ -122,26 +122,26 @@ generate_feature_functions("VelFs", :velFs, Float64, "m/s") generate_feature_functions("VelFt", :velFt, Float64, "m/s") generate_feature_functions("Acc", :acc, Float64, "m/s^2") -function Base.get{S,D,I,R}(::Feature_Acc, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_Acc, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(SPEED, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("AccFs", :accFs, Float64, "m/s²") -function Base.get{S,D,I,R}(::Feature_AccFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_AccFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(VELFS, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("AccFt", :accFt, Float64, "m/s²") -function Base.get{S,D,I,R}(::Feature_AccFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_AccFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(VELFT, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("Jerk", :jerk, Float64, "m/s³") -function Base.get{S,D,I,R}(::Feature_Jerk, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_Jerk, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(ACC, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("JerkFs", :jerkFs, Float64, "m/s³") -function Base.get{S,D,I,R}(::Feature_JerkFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_JerkFs, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(ACCFS, rec, roadway, vehicle_index, pastframe) end generate_feature_functions("JerkFt", :jerkFt, Float64, "m/s³") -function Base.get{S,D,I,R}(::Feature_JerkFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) +function Base.get(::Feature_JerkFt, rec::EntityQueueRecord{S,D,I}, roadway::R, vehicle_index::Int, pastframe::Int=0) where {S,D,I,R} _get_feature_derivative_backwards(ACCFT, rec, roadway, vehicle_index, pastframe) end \ No newline at end of file diff --git a/src/interface/simulation.jl b/src/interface/simulation.jl index c60357f..f8e3a87 100644 --- a/src/interface/simulation.jl +++ b/src/interface/simulation.jl @@ -1,9 +1,9 @@ -function get_actions!{S,D,I,A,R,M<:DriverModel}( +function get_actions!( actions::Vector{A}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I, M}, # id → model - ) + ) where {S,D,I,A,R,M<:DriverModel} for (i,veh) in enumerate(scene) @@ -15,12 +15,12 @@ function get_actions!{S,D,I,A,R,M<:DriverModel}( actions end -function tick!{S,D,I,A,R}( +function tick!( scene::EntityFrame{S,D,I}, roadway::R, actions::Vector{A}, Δt::Float64, - ) + ) where {S,D,I,A,R} for i in 1 : length(scene) veh = scene[i] @@ -31,7 +31,7 @@ function tick!{S,D,I,A,R}( return scene end -function reset_hidden_states!{M<:DriverModel}(models::Dict{Int,M}) +function reset_hidden_states!(models::Dict{Int,M}) where {M<:DriverModel} for model in values(models) reset_hidden_state!(model) end @@ -41,18 +41,18 @@ end """ Run nticks of simulation and place all nticks+1 scenes into the QueueRecord """ -function simulate!{S,D,I,A,R,M<:DriverModel}( +function simulate!( ::Type{A}, rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M}, nticks::Int, - ) + ) where {S,D,I,A,R,M<:DriverModel} empty!(rec) update!(rec, scene) - actions = Array{A}(length(scene)) + actions = Array{A}(undef, length(scene)) for tick in 1 : nticks get_actions!(actions, scene, roadway, models) @@ -62,13 +62,13 @@ function simulate!{S,D,I,A,R,M<:DriverModel}( return rec end -function simulate!{S,D,I,R,M<:DriverModel}( +function simulate!( rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M}, nticks::Int, - ) + ) where {S,D,I,R,M<:DriverModel} return simulate!(Any, rec, scene, roadway, models, nticks) end @@ -79,7 +79,7 @@ end Only the ego vehicle is simulated; the other vehicles are as they were in the provided trajdata Other vehicle states will be interpolated """ -function simulate!{S,D,I}( +function simulate!( rec::EntityQueueRecord{S,D,I}, model::DriverModel, egoid::I, @@ -88,7 +88,7 @@ function simulate!{S,D,I}( frame_end::Int; prime_history::Int=0, # no prime-ing scene::EntityFrame{S,D,I} = allocate_frame(trajdata), - ) + ) where {S,D,I} @assert(isapprox(get_timestep(rec), get_timestep(trajdata))) diff --git a/test/2d/test_actions.jl b/test/2d/test_actions.jl index 94f65ba..e8f3daf 100644 --- a/test/2d/test_actions.jl +++ b/test/2d/test_actions.jl @@ -11,7 +11,7 @@ let let a = AccelTurnrate(0.1,0.2) @test a == convert(AccelTurnrate, [0.1,0.2]) - @test copy!([NaN, NaN], AccelTurnrate(0.1,0.2)) == [0.1,0.2] + @test copyto!([NaN, NaN], AccelTurnrate(0.1,0.2)) == [0.1,0.2] s = propagate(veh, AccelTurnrate(0.0,0.0), roadway, 1.0) @test isapprox(s.posG.x, veh.state.v) @@ -22,7 +22,7 @@ let let a = AccelDesang(0.1,0.2) @test a == convert(AccelDesang, [0.1,0.2]) - @test copy!([NaN, NaN], AccelDesang(0.1,0.2)) == [0.1,0.2] + @test copyto!([NaN, NaN], AccelDesang(0.1,0.2)) == [0.1,0.2] s = propagate(veh, AccelDesang(0.0,0.0), roadway, 1.0) @test isapprox(s.posG.x, veh.state.v*1.0) @@ -34,7 +34,7 @@ let let a = LatLonAccel(0.1,0.2) @test a == convert(LatLonAccel, [0.1,0.2]) - @test copy!([NaN, NaN], LatLonAccel(0.1,0.2)) == [0.1,0.2] + @test copyto!([NaN, NaN], LatLonAccel(0.1,0.2)) == [0.1,0.2] Δt = 0.1 s = propagate(veh, LatLonAccel(0.0,0.0), roadway, Δt) diff --git a/test/2d/vehicles/test_scenes.jl b/test/2d/vehicles/test_scenes.jl index 2e1bc79..768f542 100644 --- a/test/2d/vehicles/test_scenes.jl +++ b/test/2d/vehicles/test_scenes.jl @@ -22,7 +22,7 @@ let empty!(scene2) @test length(scene2) == 0 - copy!(scene2, scene) + copyto!(scene2, scene) @test length(scene2) == 2 for (i,veh) in enumerate(scene2) @test scene2[i].state == get_state(trajdata, i, 1) diff --git a/test/2d/vehicles/test_vehicles.jl b/test/2d/vehicles/test_vehicles.jl index 38a7751..ab9b33f 100644 --- a/test/2d/vehicles/test_vehicles.jl +++ b/test/2d/vehicles/test_vehicles.jl @@ -63,10 +63,12 @@ end let + # dummy test for the constructor + roadway=gen_straight_roadway(1, 50.0, lane_width=3.0) ped=SidewalkPedestrianModel(timestep=0.1, - crosswalk=Frenet(NULL_ROADINDEX, 0.0, 0.0, 0.0), - sw_origin = Frenet(NULL_ROADINDEX, 0.0, 0.0, 0.0), - sw_dest = Frenet(NULL_ROADINDEX, 0.0, 0.0, 0.0) + crosswalk= roadway[LaneTag(1,1)], + sw_origin = roadway[LaneTag(1,1)], + sw_dest = roadway[LaneTag(1,1)] ) @test ped.ttc_threshold >= 1.0 end diff --git a/test/runtests.jl b/test/runtests.jl index 2909460..aebd054 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,4 +1,4 @@ -using Base.Test +using Test using AutomotiveDrivingModels diff --git a/test/test_feature_extractors.jl b/test/test_feature_extractors.jl index 89dbce4..4b46bcd 100644 --- a/test/test_feature_extractors.jl +++ b/test/test_feature_extractors.jl @@ -13,7 +13,7 @@ let ext = DumbFeatureExtractor() @test rec_length(ext) == 1 @test_throws Exception length(ext) - @test_throws Exception pull_features!(ext, Array{Float64}(0), rec, roadway, 1) + @test_throws Exception pull_features!(ext, Array{Float64}(undef, 0), rec, roadway, 1) ext = FeatureExtractor(AbstractFeature[SPEED, POSFT], 2) @test rec_length(ext) == 2