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