diff --git a/Project.toml b/Project.toml
index a399cf6..fa27222 100644
--- a/Project.toml
+++ b/Project.toml
@@ -10,7 +10,6 @@ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a"
Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
-Records = "5984c134-fa48-5ed5-a57f-fc2f6936871f"
Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
Tricks = "410a4b4d-49e4-4fbc-ab6d-cb71b17b3775"
diff --git a/docs/src/examples/intersection.md b/docs/src/examples/intersection.md
index dec4953..ee75a74 100644
--- a/docs/src/examples/intersection.md
+++ b/docs/src/examples/intersection.md
@@ -155,7 +155,7 @@ Let's populate the intersection
```@example intersection
vs0 = VehicleState(B + polar(50.0,-π), roadway, 8.0) # initial state of the vehicle
-scene = Scene([Vehicle(vs0, VehicleDef(), 1)])
+scene = Scene([Entity(vs0, VehicleDef(), 1)])
snapshot = render([roadway, scene])
write("intersection_populated.svg", snapshot) # hide
@@ -213,7 +213,7 @@ timestep = 0.1
nticks = 100
vs0 = VehicleState(B + polar(50.0,-π), roadway, 8.0)
-scene = Scene([Vehicle(vs0, VehicleDef(), 1)])
+scene = Scene([Entity(vs0, VehicleDef(), 1)])
models = Dict(1 => InterDriver(LaneSpecificAccelLatLon(0.0,0.0)))
scenes = simulate!(scene, roadway, models, nticks, timestep)
diff --git a/legacy/dxf_roadways.jl b/legacy/dxf_roadways.jl
deleted file mode 100644
index 2b6b3ca..0000000
--- a/legacy/dxf_roadways.jl
+++ /dev/null
@@ -1,279 +0,0 @@
-
-############################################
-
-function _fit_curve(
- pts::AbstractMatrix{Float64}, # 2×n
- desired_distance_between_samples::Real;
- max_iterations::Int=50,
- epsilon::Float64=1e-4,
- n_intervals_in_arclen::Int=100,
- )
-
- @assert(size(pts, 1) == 2)
-
- spline_coeffs = fit_cubic_spline(pts)
-
- L = calc_curve_length(spline_coeffs[1], spline_coeffs[2], n_intervals_per_segment=n_intervals_in_arclen)
- n = round(Int, L/desired_distance_between_samples, RoundNearestTiesUp)+1
-
- s_arr = collect(linspace(0.0,L,n))
- t_arr = calc_curve_param_given_arclen(spline_coeffs[1], spline_coeffs[2], s_arr,
- curve_length=L, max_iterations=max_iterations, epsilon=epsilon, n_intervals_in_arclen=n_intervals_in_arclen)
-
- x_arr = sample_spline(spline_coeffs[1], t_arr)
- y_arr = sample_spline(spline_coeffs[2], t_arr)
- θ_arr = sample_spline_theta(spline_coeffs[1], spline_coeffs[2], t_arr)
-
- κ_arr = sample_spline_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)
- κd_arr = sample_spline_derivative_of_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)
-
- @assert(!any(s->isnan(s), s_arr))
- @assert(!any(s->isnan(s), x_arr))
- @assert(!any(s->isnan(s), y_arr))
- @assert(!any(s->isnan(s), θ_arr))
-
- 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])
- end
- curve
-end
-
-"""
- read_dxf(io::IO, ::Type{Roadway})
-Return a Roadway generated from a DXF file
-
- Layers with names such as seg001 will contain LWPOLYLINEs.
- Each LWPOLYLINE corresponds to a lane centerline, which together
- are all neighbored.
-"""
-function read_dxf(io::IO, ::Type{Roadway};
- dist_threshold_lane_connect::Float64 = 0.25, # [m]
- desired_distance_between_curve_samples::Float64 = 1.0 # [m]
- )
-
- lines = readlines(io)
-
- i = findfirst(isequal("ENTITIES\n"), lines)
- i != nothing || error("ENTITIES section not found")
-
- ###################################################
- # Pull pts for each lane
- lane_pts_dict = Dict{LaneTag, Vector{VecE2}}()
-
- i = findnext(lines, "LWPOLYLINE\n", i)
- while i != 0
- i = findnext(lines, " 8\n", i)
- if i != 0 # segment identifier found in LWPOLYLINE
-
- if ismatch(r"(?<=seg)(\d*)", lines[i+1])
- segid = parse(Int, match(r"(?<=seg)(\d*)", lines[i+1]).match)
-
- i = findnext(lines, "AcDbPolyline\n", i)
- i != 0 || error("AcDbPolyline not found in LWPOLYLINE!")
- i = findnext(lines, " 90\n", i)
- i != 0 || error("Number of vertices not found in AcDbPolyline!")
-
- N = parse(Int, lines[i+1])
- N > 0 || error("Empty line segment!")
-
- pts = Array{VecE2}(undef, N)
-
- i = findnext(lines, " 10\n", i)
- i != 0 || error("First point not found in AcDbPolyline!")
-
- for j in 1 : N
- x = parse(Float64, lines[i+1])
- y = parse(Float64, lines[i+3])
- i += 4
- pts[j] = VecE2(x,y)
- end
-
- laneid = 1
- for tag in keys(lane_pts_dict)
- if tag.segment == segid
- laneid += 1
- end
- end
- lane_pts_dict[LaneTag(segid, laneid)] = pts
- end
-
- i = findnext(lines, "LWPOLYLINE\n", i)
- end
- end
-
- ###################################################
- # Shift pts to connect to previous / next pts
- lane_next_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()
- lane_prev_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()
-
- for (tag, pts) in lane_pts_dict
- # see if can connect to next
- best_tag = NULL_LANETAG
- best_ind = -1
- best_sq_dist = dist_threshold_lane_connect
- for (tag2, pts2) in lane_pts_dict
- if tag2.segment != tag.segment
- for (ind,pt) in enumerate(pts2)
- sq_dist = normsquared(VecE2(pt - pts[end]))
- if sq_dist < best_sq_dist
- best_sq_dist = sq_dist
- best_ind = ind
- best_tag = tag2
- end
- end
- end
- end
- if best_tag != NULL_LANETAG
- # remove our last pt and set next to pt to their pt
- pop!(pts)
- lane_next_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
- if best_ind == 1 # set connect prev as well
- lane_prev_dict[best_tag] = (pts[end], tag)
- end
- end
- end
- for (tag, pts) in lane_pts_dict
- # see if can connect to prev
- if !haskey(lane_prev_dict, tag)
- best_tag = NULL_LANETAG
- best_ind = -1
- best_sq_dist = dist_threshold_lane_connect
- for (tag2, pts2) in lane_pts_dict
- if tag2.segment != tag.segment
- for (ind,pt) in enumerate(pts2)
- sq_dist = normsquared(VecE2(pt - pts[1]))
- if sq_dist < best_sq_dist
- best_sq_dist = sq_dist
- best_ind = ind
- best_tag = tag2
- end
- end
- end
- end
- if best_tag != NULL_LANETAG
- # connect 'em
- shift!(pts)
- lane_prev_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
- end
- end
- end
-
- ###################################################
- # Build the roadway
- retval = Roadway()
- for (tag, pts) in lane_pts_dict
- if !has_segment(retval, tag.segment)
- push!(retval.segments, RoadSegment(tag.segment))
- end
- end
-
- lane_new_dict = Dict{LaneTag, LaneTag}() # old -> new tag
- for seg in retval.segments
-
- # pull lanetags for this seg
- lanetags = LaneTag[]
- for tag in keys(lane_pts_dict)
- if tag.segment == seg.id
- push!(lanetags, tag)
- end
- end
-
- # sort the lanes such that the rightmost lane is lane 1
- # do this by taking the first lane,
- # then project each lane's midpoint to the perpendicular at the midpoint
-
- @assert(!isempty(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, atan(hi - lo) + π/2) # direction perpendicular (left) of lane
-
- for (i,tag) in enumerate(lanetags)
- pts = lane_pts_dict[tag]
- n = length(pts)
- midpt = (pts[div(n,2)] + pts[div(n,2)+1])/2
- proj_positions[i] = proj(midpt - midpt_orig, dir, Float64)
- end
-
- for (i,j) in enumerate(sortperm(proj_positions))
-
- tag = lanetags[j]
- boundary_left = i == length(proj_positions) ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)
- boundary_right = i == 1 ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)
-
- pts = lane_pts_dict[tag]
- 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
- end
-
- println("fitting curve ", length(pts), " "); tic()
- curve = _fit_curve(pt_matrix, desired_distance_between_curve_samples)
- toc()
-
- tag_new = LaneTag(seg.id, length(seg.lanes)+1)
- lane = Lane(tag_new, curve,
- boundary_left = boundary_left,
- boundary_right = boundary_right)
- push!(seg.lanes, lane)
- lane_new_dict[tag] = tag_new
- end
- end
-
- ###################################################
- # Connect the lanes
- for (tag_old, tup) in lane_next_dict
- next_pt, next_tag_old = tup
- lane = retval[lane_new_dict[tag_old]]
- next_tag_new = lane_new_dict[next_tag_old]
- dest = retval[next_tag_new]
- roadproj = proj(VecSE2(next_pt, 0.0), dest, retval)
-
- # println("connecting $(lane.tag) to $(dest.tag)")
-
- cindS = curveindex_end(lane.curve)
- cindD = roadproj.curveproj.ind
-
- if cindD == CURVEINDEX_START # a standard connection
- connect!(lane, dest)
- # remove any similar connection from lane_prev_dict
- if haskey(lane_prev_dict, next_tag_old) && lane_prev_dict[next_tag_old][2] == tag_old
- delete!(lane_prev_dict, next_tag_old)
- end
- else
- # otherwise connect as before
- pushfirst!(lane.exits, LaneConnection(true, cindS, RoadIndex(cindD, dest.tag)))
- push!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, lane.tag)))
- end
- end
- for (tag_old, tup) in lane_prev_dict
- prev_pt, prev_tag_old = tup
- lane = retval[lane_new_dict[tag_old]]
- prev_tag_new = lane_new_dict[prev_tag_old]
- prev = retval[prev_tag_new]
- roadproj = proj(VecSE2(prev_pt, 0.0), prev, retval)
-
- # println("connecting $(lane.tag) from $(prev.tag)")
-
- cindS = roadproj.curveproj.ind
- cindD = CURVEINDEX_START
-
- if cindS == curveindex_end(prev) # a standard connection
- @assert(!has_prev(prev))
- connect!(prev, lane)
- else
- # a standard connection
- push!(prev.exits, LaneConnection(true, cindS, RoadIndex(cindD, lane.tag)))
- pushfirst!(lane.entrances, LaneConnection(false, cindD, RoadIndex(cindS, prev.tag)))
- end
- end
-
- retval
-end
\ No newline at end of file
diff --git a/legacy/splines.jl b/legacy/splines.jl
deleted file mode 100644
index b3329be..0000000
--- a/legacy/splines.jl
+++ /dev/null
@@ -1,824 +0,0 @@
-
-
-# export fit_cubic_spline,
-# sample_spline,
-# sample_spline_derivative,
-# sample_spline_derivative2,
-# sample_spline_speed,
-# sample_spline_theta,
-# sample_spline_curvature,
-# sample_spline_derivative_of_curvature,
-# calc_curve_length,
-# arclength,
-# calc_curve_param_given_arclen
-
-function _integrate_simpsons(f::Function, a::Real, b::Real, n::Int)
- # integrate using Composite Simpson's rule
- # reference: https://en.wikipedia.org/wiki/Simpson%27s_rule
-
- @assert(n > 0) # number of intervals
- @assert(mod(n,2) == 0) # n must be even
-
- h = (b-a)/n
- retval = f(a) + f(b)
- flip = true
- for i = 1 : n-1
- retval += f(a+i*h) * (flip ? 4 : 2)
- flip = !flip
- end
- return h/3*retval
-end
-
-function _fit_open(pts::AbstractVector{Float64} )
- # fits the 1-D spline such that:
- # spline goes through each point
- # first and second derivatives match at each inner point
- # the second derivative at the ends is zero
- # see: http://mathworld.wolfram.com/CubicSpline.html
-
- # this function returns a 4×(n-1) spline coefficient matrix, where n = |pts|
-
- n = length(pts)-1
- @assert(n > 0)
-
- M = spzeros(n+1,n+1)
- for i = 1 : n
- M[i,i] = 4
- M[i,i+1] = 1
- M[i+1,i] = 1
- end
- M[n+1,n+1] = 2
- M[1,1] = 2
-
- Y = Array{Float64}(undef, n+1)
- for i = 1 : n+1
- ind_hi = min(i+1,n)
- ind_lo = max(1,i-1)
- Y[i] = 3*(pts[ind_hi] - pts[ind_lo])
- end
-
- D = M\Y
-
- 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]
- spline_coeffs[4,:] = 2*(pts[1:n] - pts[2:n+1]) + D[1:n] + D[2:n+1]
-
- spline_coeffs
-end
-function _fit_closed(pts::AbstractVector{Float64} )
- # fits the 1-D spline such that:
- # spline goes through each point
- # first and second derivatives match at each inner point
- # first the second derivative at the ends match
- # see: http://mathworld.wolfram.com/CubicSpline.html
-
- # this function returns a 4×n spline coefficient matrix, where n = |pts|
-
- n = length(pts)-1
- @assert(n > 0)
-
- M = spzeros(n+1,n+1)
- for i = 1 : n
- M[i,i] = 4
- M[i,i+1] = 1
- M[i+1,i] = 1
- end
- M[n+1,n+1] = 4
- M[1,n+1] = 1
- M[n+1,1] = 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])
- end
- Y[end] = 3*(pts[1] - pts[n])
-
- D = M\Y
-
- 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]
- spline_coeffs[4,1:n] = 2*(pts[1:n] - pts[2:n+1]) + D[1:n] + D[2:n+1]
- spline_coeffs[3,n+1] = 3*(pts[1] - pts[n+1]) -2*D[n+1]-D[1]
- spline_coeffs[4,n+1] = 2*(pts[n+1] - pts[1]) + D[n+1] + D[1]
-
- spline_coeffs
-end
-function _fit_open(pts::Matrix{Float64}) # 2×n {x,y}
-
- # see http://mathworld.wolfram.com/CubicSpline.html
-
- d,n = size(pts)
- n -= 1
-
- Y = Array{Float64}(undef, n+1)
-
- M = sparse(Int[], Int[], Float64[], n+1,n+1)
- for i in 1 : n
- M[i,i] = 4.0
- M[i,i+1] = 1.0
- M[i+1,i] = 1.0
- end
- M[n+1,n+1] = 2.0
- M[1,1] = 2.0
-
- retval = Array{Matrix{Float64}}(undef, d)
- for k in 1 : d
-
- for i in 1 : n+1
- ind_hi = min(i+1,n)
- ind_lo = max(1,i-1)
- Y[i] = 3*(pts[k,ind_hi] - pts[k,ind_lo])
- end
-
- D = M \ Y
-
- 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'₁
- spline_coeffs[4,:] = 2*(pts[k,1:n]' - pts[k,2:n+1]') + D[1:n] + D[2:n+1] # 2x₀ - 2x₁ + x'₀ + x'₁
-
- retval[k] = spline_coeffs
- end
- retval
-end
-function _fit_closed(pts::AbstractMatrix{Float64})
- d = size(pts,1)
- retval = Array{Matrix{Float64}}(undef, d)
- for i = 1 : d
- retval[i] = _fit_closed(vec(pts[i,:]))
- end
- retval
-end
-
-function fit_cubic_spline(pts::AbstractArray{Float64}; open::Bool=true)
- if open
- return _fit_open(pts)
- else
- return _fit_closed(pts)
- end
-end
-
-function sample_spline(spline_coeffs::AbstractVector{Float64}, t::Float64)
- # here t is generally expected to be t ∈ [0,1]
- return spline_coeffs[1] + t*(spline_coeffs[2] + t*(spline_coeffs[3] + t*spline_coeffs[4]))
-end
-function sample_spline(spline_coeffs::AbstractMatrix{Float64}, t::Float64)
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2))
- sample_spline(spline_coeffs[:,col_ind], t-col_ind+1)
-end
-function sample_spline(spline_coeffs::AbstractVector{Float64}, t_arr::AbstractVector{Float64})
- # here t is generally expected to be t ∈ [0,1]
-
- a = spline_coeffs[1]
- b = spline_coeffs[2]
- c = spline_coeffs[3]
- d = spline_coeffs[4]
-
- retval = Array{Float64}(undef, length(t_arr))
- for (i,t) in enumerate(t_arr)
- retval[i] = a + t*(b + t*(c + t*d))
- end
- retval
-end
-function sample_spline(spline_coeffs::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- 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)
- end
- retval
-end
-
-function sample_spline_derivative(spline_coeffs::AbstractVector{Float64}, t::Float64)
- # here t is generally expected to be t ∈ [0,1]
- return spline_coeffs[2] + t*(2spline_coeffs[3] + t*3spline_coeffs[4])
-end
-function sample_spline_derivative(spline_coeffs::AbstractMatrix{Float64}, t::Float64)
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2))
- sample_spline_derivative(spline_coeffs[:,col_ind], t-col_ind+1)
-end
-function sample_spline_derivative(spline_coeffs::AbstractVector{Float64}, t_arr::AbstractVector{Float64})
- # here t is generally expected to be t ∈ [0,1]
-
- b = spline_coeffs[2]
- c = spline_coeffs[3]
- d = spline_coeffs[4]
-
- retval = Array{Float64}(undef, length(t_arr))
- for (i,t) in enumerate(t_arr)
- retval[i] = b + t*(2c + t*3d)
- end
- retval
-end
-function sample_spline_derivative(spline_coeffs::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- 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)
- end
- retval
-end
-
-function sample_spline_derivative2(spline_coeffs::AbstractVector{Float64}, t::Float64)
- # here t is generally expected to be t ∈ [0,1]
- return 2spline_coeffs[3] + t*6spline_coeffs[4]
-end
-function sample_spline_derivative2(spline_coeffs::AbstractMatrix{Float64}, t::Float64)
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- col_ind = clamp(ceil(Int, t), 1, size(spline_coeffs,2))
- sample_spline_derivative2(spline_coeffs[:,col_ind], t-col_ind+1)
-end
-function sample_spline_derivative2(spline_coeffs::AbstractVector{Float64}, t_arr::AbstractVector{Float64})
- # here t is generally expected to be t ∈ [0,1]
-
- b = spline_coeffs[2]
- c = spline_coeffs[3]
- d = spline_coeffs[4]
-
- retval = Array{Float64}(undef, length(t_arr))
- for (i,t) in enumerate(t_arr)
- retval[i] = 2c + t*6d
- end
- retval
-end
-function sample_spline_derivative2(spline_coeffs::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- @assert(size(spline_coeffs, 1) == 4)
- 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)
- end
- retval
-end
-
-function sample_spline_speed(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t::Float64)
- dxdt = sample_spline_derivative(spline_coeffs_x, t)
- dydt = sample_spline_derivative(spline_coeffs_y, t)
- hypot(dxdt, dydt)
-end
-function sample_spline_speed(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t::Float64)
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- col_ind = clamp(ceil(Int, t), 1, n)::Int
- sample_spline_speed(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1)
-end
-function sample_spline_speed(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t_arr::AbstractVector{Float64})
- # here t is generally expected to be t ∈ [0,1]
-
- bx = spline_coeffs_x[2]
- cx = spline_coeffs_x[3]
- dx = spline_coeffs_x[4]
-
- by = spline_coeffs_y[2]
- cy = spline_coeffs_y[3]
- dy = spline_coeffs_y[4]
-
- 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)
- retval[i] = hypot(dxdt, dydt)
- end
- retval
-end
-function sample_spline_speed(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- 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)
- end
- retval
-end
-
-function sample_spline_theta(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t::Float64;
- stepsize=1e-4
- )
-
- # compute the angle from positive x-axis (counter-clockwise positive) of the curve in the positive t direction at t
- # uses an approximation via small step size instead of derivative due to zero-derivative issues
- # uses the forward derivative approximation unless it would put it out of range
- # result returned is in radians
-
- t_lo, t_hi = t, t+stepsize
- if t_hi > 1.0
- t_lo, t_hi = t-min(1000stepsize,0.1), t
- end
-
- x1 = sample_spline(spline_coeffs_x, t_lo)
- x2 = sample_spline(spline_coeffs_x, t_hi)
- 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, ($(atan(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]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- col_ind = clamp(ceil(Int, t), 1, n)
- sample_spline_theta(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1)
-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}(undef, length(t_arr))
- for (i,t) in enumerate(t_arr)
- retval[i] = sample_spline_theta(spline_coeffs_x, spline_coeffs_y, t)
- end
- retval
-end
-function sample_spline_theta(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- 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)
- end
- retval
-end
-
-function sample_spline_curvature(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t::Float64)
- # computes the signed curvature
-
- dx = sample_spline_derivative( spline_coeffs_x, t)
- dy = sample_spline_derivative( spline_coeffs_y, t)
- ddx = sample_spline_derivative2(spline_coeffs_x, t)
- ddy = sample_spline_derivative2(spline_coeffs_y, t)
-
- (dx*ddy - dy*ddx)/(dx*dx + dy*dy)^1.5
-end
-function sample_spline_curvature(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t::Float64)
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- col_ind = clamp(ceil(Int, t), 1, n)
- sample_spline_curvature(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1)
-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}(undef, length(t_arr))
- for (i,t) in enumerate(t_arr)
- retval[i] = sample_spline_curvature(spline_coeffs_x, spline_coeffs_y, t)
- end
- retval
-end
-function sample_spline_curvature(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64})
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- 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)
- end
- retval
-end
-
-function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t::Float64;
- stepsize=1e-4
- )
-
- # computes the derivative of the signed curvature
-
- t_lo, t_hi = t, t+stepsize
- if t_hi > 1.0
- t_lo, t_hi = t-stepsize, t
- end
-
- κ_hi = sample_spline_curvature(spline_coeffs_x, spline_coeffs_y, t_hi)
- κ_lo = sample_spline_curvature(spline_coeffs_x, spline_coeffs_y, t_lo)
-
- (κ_hi - κ_lo) / stepsize
-end
-function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t::Float64;
- stepsize=1e-4
- )
-
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- col_ind = clamp(ceil(Int, t), 1, n)
- sample_spline_derivative_of_curvature(spline_coeffs_x[:,col_ind], spline_coeffs_y[:,col_ind], t-col_ind+1, stepsize=stepsize)
-end
-function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64}, t_arr::AbstractVector{Float64};
- stepsize=1e-4
- )
-
- # here t is generally expected to be t ∈ [0,1]
-
- 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
- retval
-end
-function sample_spline_derivative_of_curvature(spline_coeffs_x::AbstractMatrix{Float64}, spline_coeffs_y::AbstractMatrix{Float64}, t_arr::AbstractVector{Float64};
- stepsize=1e-4
- )
-
- # for t ∈ (-∞,1] we use spline_coeffs[:,1]
- # for t ∈ [1,2] we use spline_coeffs[:,2]
- # etc.
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x, 1) == 4)
- @assert(size(spline_coeffs_y, 1) == 4)
- @assert(n == size(spline_coeffs_y, 2))
- 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)
- end
- retval
-end
-
-function calc_curve_length(spline_coeffs_x::AbstractVector{Float64}, spline_coeffs_y::AbstractVector{Float64};
- n_intervals::Int = 100
- )
-
- # integrate using Simpson's rule
- # _integrate_simpsons(t->sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t), 0.0, 1.0, n_intervals)
-
- a = 0.0
- b = 1.0
- n = n_intervals
-
- h = (b-a)/n
- retval = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, a) + sample_spline_speed(spline_coeffs_x, spline_coeffs_y, b)
- flip = true
- for i = 1 : n-1
- retval += sample_spline_speed(spline_coeffs_x, spline_coeffs_y, a+i*h) * (flip ? 4 : 2)
- flip = !flip
- end
- return h/3*retval
-end
-function calc_curve_length(
- spline_coeffs_x::AbstractMatrix{Float64},
- spline_coeffs_y::AbstractMatrix{Float64};
- n_intervals_per_segment::Int = 100
- )
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_y, 2) == n)
- @assert(size(spline_coeffs_x, 1) == size(spline_coeffs_y, 1) == 4)
-
- len = 0.0
- for i = 1 : n
- len += calc_curve_length(spline_coeffs_x[:,i], spline_coeffs_y[:,i], n_intervals = n_intervals_per_segment)
- end
- len
-end
-
-function arclength(
- spline_coeffs_x::AbstractVector{Float64},
- spline_coeffs_y::AbstractVector{Float64},
- t_min::Real = 0.0,
- t_max::Real = 1.0,
- n_intervals::Int = 100
- )
-
- if isapprox(t_min, t_max)
- return 0.0
- end
-
- # _integrate_simpsons(t->sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t), t_min, t_max, n_intervals)
-
- a = t_min
- b = t_max
- n = n_intervals
-
- h = (b-a)/n
- retval = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, a) + sample_spline_speed(spline_coeffs_x, spline_coeffs_y, b)
- flip = true
- for i = 1 : n-1
- retval += sample_spline_speed(spline_coeffs_x, spline_coeffs_y, a+i*h) * (flip ? 4 : 2)
- flip = !flip
- end
- return h/3*retval
-end
-function arclength(
- spline_coeffs_x::AbstractMatrix{Float64},
- spline_coeffs_y::AbstractMatrix{Float64},
- t_min::Real = 0.0,
- t_max::Real = size(spline_coeffs_x, 2),
- n_intervals_per_segment::Int = 100
- )
-
- n = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_y, 2) == n)
- @assert(size(spline_coeffs_x, 1) == size(spline_coeffs_y, 1) == 4)
-
- if isapprox(t_min, t_max)
- return 0.0
- end
-
- # println("tmin/tmax: $t_min / $t_max")
-
- len = 0.0
- for i = floor(Int, t_min) : min(floor(Int, t_max), n-1)
- t_lo, t_hi = float(i), i+1.0
-
- spline_ind = i+1
- t_in_min = max(t_lo, t_min) - t_lo
- t_in_max = min(t_hi, t_max) - t_lo
- # println("($i) t_lo: $t_lo, t_hi: $t_hi, : $t_in_min → $t_in_max")
- len += arclength(spline_coeffs_x[:,spline_ind], spline_coeffs_y[:,spline_ind], t_in_min, t_in_max, n_intervals_per_segment)
- end
- # println("len: ", len)
- len
-end
-
-function calc_curve_param_given_arclen(
- spline_coeffs_x :: AbstractVector{Float64},
- spline_coeffs_y :: AbstractVector{Float64},
- s :: Float64;
- max_iterations :: Int=100,
- curve_length :: Float64 = calc_curve_length(spline_coeffs_x, spline_coeffs_y),
- epsilon::Float64 = 1e-4 # tolerance required before termination
- )
-
- # finds t such that p(t) is a distance s from start of curve
- # returns t=0 if s ≤ 0.0 and t=1 if s > L
- if s ≤ 0.0
- return 0.0
- elseif s ≥ curve_length
- return 1.0
- end
-
- t = s/curve_length
- lo, hi = 0.0, 1.0
-
- # @printf("%10s %10s %10s %10s %10s %10s\n", "iter", "lo", "hi", "t", "s", "F")
- # println("-"^65)
-
- for iter = 1 : max_iterations
- F = arclength(spline_coeffs_x, spline_coeffs_y, 0.0, t) - s
-
- # @printf("%10d %10.5f %10.5f %10.5f %10.5f %10.5f\n", iter-1, lo, hi, t, s, F)
-
- if abs(F) < epsilon
- # |F(t)| is close enough to zero, report it
- return t
- end
-
- DF = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t)
- tCandidate = t - F/DF
- if F > 0
- hi = t
- t = tCandidate ≤ lo ? 0.5*(lo+hi) : tCandidate
- else
- lo = t
- t = tCandidate ≥ hi ? 0.5*(lo+hi) : tCandidate
- end
- end
-
- # @printf("%10d %10.5f %10.5f %10.5f %10.5f %10s\n", max_iterations, lo, hi, t, s, "-")
-
- t
-end
-function calc_curve_param_given_arclen(
- spline_coeffs_x :: AbstractMatrix{Float64},
- spline_coeffs_y :: AbstractMatrix{Float64},
- s :: Float64;
- max_iterations :: Int=100,
- n_integration_intervals :: Int=100, # must be multiple of 2
- curve_length :: Float64 = calc_curve_length(spline_coeffs_x, spline_coeffs_y),
- epsilon::Float64 = 1e-4 # tolerance required before termination
- )
-
- # finds t such that p(t) is a distance s from start of curve
- # returns t=0 if s ≤ 0.0 and t=t_max if s > L
-
- n_segments = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x,1) == size(spline_coeffs_y,1) == 4)
- @assert(size(spline_coeffs_y,2) == n_segments)
-
- if s ≤ 0.0
- return 0.0
- elseif s ≥ curve_length
- return float(n_segments)
- end
-
- t = s/curve_length
- lo, hi = 0.0, float(n_segments)
-
- # @printf("%10s %10s %10s %10s %10s %10s\n", "iter", "lo", "hi", "t", "s", "F")
- # println("-"^65)
-
- for iter = 1 : max_iterations
- F = arclength(spline_coeffs_x, spline_coeffs_y, 0.0, t, n_integration_intervals) - s
-
- # @printf("%10d %10.5f %10.5f %10.5f %10.5f %10.5f\n", iter-1, lo, hi, t, s, F)
-
- if abs(F) < epsilon
- return t
- end
-
- DF = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t)
- tCandidate = t - F/DF
- if F > 0
- hi = t
- t = tCandidate ≤ lo ? 0.5*(lo+hi) : tCandidate
- else
- lo = t
- t = tCandidate ≥ hi ? 0.5*(lo+hi) : tCandidate
- end
- end
-
- # @printf("%10d %10.5f %10.5f %10.5f %10.5f %10s\n", max_iterations, lo, hi, t, s, "-")
-
- t
-end
-function calc_curve_param_given_arclen(
- spline_coeffs_x :: AbstractVector{Float64},
- spline_coeffs_y :: AbstractVector{Float64},
- s_arr :: AbstractVector{Float64}; # assumes s_arr is sorted
- max_iterations :: Int=100,
- curve_length :: Float64 = calc_curve_length(spline_coeffs_x, spline_coeffs_y),
- epsilon::Float64 = 1e-4 # tolerance required before termination
- )
-
- n = length(s_arr)
- t_arr = Array{Float64}(undef, n)
-
- s = s_arr[1]
- t = s/curve_length
- if s ≤ 0.0
- t = 0.0
- elseif s ≥ curve_length
- t = 1.0
- end
-
- lo = 0.0
-
- for (i,s) in enumerate(s_arr)
-
- if s ≤ 0.0
- t = 0.0
- t_arr[i], lo = t, t
- continue
- elseif s ≥ curve_length
- t = 1.0
- t_arr[i], lo = t, t
- continue
- end
-
- hi = 1.0
- for iter = 1 : max_iterations
- F = arclength(spline_coeffs_x, spline_coeffs_y, 0.0, t) - s
-
- if abs(F) < epsilon
- t_arr[i], lo = t, t
- continue
- end
-
- DF = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t)
- tCandidate = t - F/DF
- if F > 0
- hi = t
- t = tCandidate ≤ lo ? 0.5*(lo+hi) : tCandidate
- else
- lo = t
- t = tCandidate ≥ hi ? 0.5*(lo+hi) : tCandidate
- end
- end
-
- t_arr[i], lo = t, t
- end
-
- t_arr
-end
-function calc_curve_param_given_arclen(
- spline_coeffs_x :: AbstractMatrix{Float64},
- spline_coeffs_y :: AbstractMatrix{Float64},
- s_arr :: AbstractVector{Float64}; # assumes s_arr is sorted
- max_iterations :: Int = 50,
- curve_length :: Float64 = calc_curve_length(spline_coeffs_x, spline_coeffs_y),
- epsilon::Float64 = 1e-4, # tolerance required before termination
- n_intervals_in_arclen::Int = 100
- )
-
- n_segments = size(spline_coeffs_x, 2)
- @assert(size(spline_coeffs_x,1) == size(spline_coeffs_y,1) == 4)
- @assert(size(spline_coeffs_y,2) == n_segments)
-
- n = length(s_arr)
- t_arr = Array{Float64}(undef, n)
-
- s = s_arr[1]
- t = s/curve_length
- if s ≤ 0.0
- t = 0.0
- elseif s ≥ curve_length
- return float(n_segments)
- end
-
- lo = 0.0
- # println("L: ", curve_length)
- # println("s_max: ", s_arr[end])
- for (i,s) in enumerate(s_arr)
-
- # println("\ns: ", s)
-
- if s ≤ 0.0
- t = 0.0
- t_arr[i] = lo = t
- continue
- elseif s ≥ curve_length
- t = float(n_segments)
- t_arr[i] = lo = t
- continue
- end
-
- hi = float(n_segments)
- for iter = 1 : max_iterations
- F = arclength(spline_coeffs_x, spline_coeffs_y, 0.0, t, n_intervals_in_arclen) - s
-
- if abs(F) < epsilon
- break
- end
-
- DF = sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t)
- tCandidate = t - F/DF
- if F > 0
- hi = t
- t = tCandidate ≤ lo ? 0.5*(lo+hi) : tCandidate
- else
- lo = t
- t = tCandidate ≥ hi ? 0.5*(lo+hi) : tCandidate
- end
- end
-
- t_arr[i] = lo = t
- end
-
- t_arr
-end
\ No newline at end of file
diff --git a/src/AutomotiveDrivingModels.jl b/src/AutomotiveDrivingModels.jl
index 3a85d50..f0e32b3 100644
--- a/src/AutomotiveDrivingModels.jl
+++ b/src/AutomotiveDrivingModels.jl
@@ -10,19 +10,11 @@ using Random
using DataFrames
using Tricks: static_hasmethod
-@reexport using Records
-
-include("vec/Vec.jl")
+include(joinpath(@__DIR__, "Vec", "Vec.jl"))
@reexport using .Vec
# Roadways
-export StraightRoadway,
- mod_position_to_roadway,
- get_headway
-
-include("roadways/straight_1d_roadways.jl")
-
export CurvePt,
Curve,
CurveIndex,
@@ -108,35 +100,30 @@ export
include("agent-definitions/agent_definitions.jl")
export
+ Entity,
+ Frame,
+ EntityFrame,
+ capacity,
+ id2index,
+ get_by_id,
+ get_first_available_id,
posf,
posg,
vel,
velf,
velg,
VehicleState,
- Vehicle,
- get_vel_s,
- get_vel_t,
get_center,
get_footpoint,
get_front,
get_rear,
get_lane
+include("states/entities.jl")
+include("states/frames.jl")
include("states/interface.jl")
include("states/vehicle_state.jl")
-export Trajdata
-
-include("states/trajdatas.jl")
-
-export
- Scene,
- SceneRecord
-
-include("states/scenes.jl")
-
-
## Collision Checkers
export
@@ -219,9 +206,6 @@ export
targetpoint_delta,
find_neighbor,
NeighborLongitudinalResult,
- get_neighbor_fore,
- get_neighbor_rear,
- get_headway,
FrenetRelativePosition,
get_frenet_relative_position,
dist_to_front_neighbor,
@@ -247,6 +231,7 @@ include("feature-extraction/lidar_sensor.jl")
export
propagate,
+ EntityAction,
LaneFollowingAccel,
AccelTurnrate,
AccelDesang,
@@ -265,12 +250,11 @@ include("actions/pedestrian_lat_lon_accel.jl")
export
DriverModel,
StaticDriver,
- get_name,
action_type,
set_desired_speed!,
observe!,
reset_hidden_state!,
- prime_with_history!
+ reset_hidden_states!
include("behaviors/interface.jl")
@@ -308,24 +292,16 @@ include("behaviors/tim_2d_driver.jl")
include("behaviors/sidewalk_pedestrian_model.jl")
export
- get_actions!,
- tick!,
- reset_hidden_states!,
simulate,
simulate!,
- EntityAction,
run_callback,
- CollisionCallback
+ CollisionCallback,
+ observe_from_history!,
+ simulate_from_history!,
+ simulate_from_history
include("simulation/simulation.jl")
include("simulation/callbacks.jl")
-
-
-export
- State1D,
- Vehicle1D,
- Scene1D
-
-include("deprecated.jl")
+include("simulation/simulation_from_history.jl")
end # AutomotiveDrivingModels
diff --git a/src/vec/Vec.jl b/src/Vec/Vec.jl
similarity index 100%
rename from src/vec/Vec.jl
rename to src/Vec/Vec.jl
diff --git a/src/vec/common.jl b/src/Vec/common.jl
similarity index 100%
rename from src/vec/common.jl
rename to src/Vec/common.jl
diff --git a/src/vec/coordinate_transforms.jl b/src/Vec/coordinate_transforms.jl
similarity index 100%
rename from src/vec/coordinate_transforms.jl
rename to src/Vec/coordinate_transforms.jl
diff --git a/src/vec/geom/1d.jl b/src/Vec/geom/1d.jl
similarity index 100%
rename from src/vec/geom/1d.jl
rename to src/Vec/geom/1d.jl
diff --git a/src/vec/geom/geom.jl b/src/Vec/geom/geom.jl
similarity index 100%
rename from src/vec/geom/geom.jl
rename to src/Vec/geom/geom.jl
diff --git a/src/vec/geom/hyperplanes.jl b/src/Vec/geom/hyperplanes.jl
similarity index 100%
rename from src/vec/geom/hyperplanes.jl
rename to src/Vec/geom/hyperplanes.jl
diff --git a/src/vec/geom/line_segments.jl b/src/Vec/geom/line_segments.jl
similarity index 100%
rename from src/vec/geom/line_segments.jl
rename to src/Vec/geom/line_segments.jl
diff --git a/src/vec/geom/lines.jl b/src/Vec/geom/lines.jl
similarity index 100%
rename from src/vec/geom/lines.jl
rename to src/Vec/geom/lines.jl
diff --git a/src/vec/geom/projectiles.jl b/src/Vec/geom/projectiles.jl
similarity index 100%
rename from src/vec/geom/projectiles.jl
rename to src/Vec/geom/projectiles.jl
diff --git a/src/vec/geom/rays.jl b/src/Vec/geom/rays.jl
similarity index 100%
rename from src/vec/geom/rays.jl
rename to src/Vec/geom/rays.jl
diff --git a/src/vec/geom/solids.jl b/src/Vec/geom/solids.jl
similarity index 100%
rename from src/vec/geom/solids.jl
rename to src/Vec/geom/solids.jl
diff --git a/src/vec/quat.jl b/src/Vec/quat.jl
similarity index 100%
rename from src/vec/quat.jl
rename to src/Vec/quat.jl
diff --git a/src/vec/vecE2.jl b/src/Vec/vecE2.jl
similarity index 100%
rename from src/vec/vecE2.jl
rename to src/Vec/vecE2.jl
diff --git a/src/vec/vecE3.jl b/src/Vec/vecE3.jl
similarity index 100%
rename from src/vec/vecE3.jl
rename to src/Vec/vecE3.jl
diff --git a/src/vec/vecSE2.jl b/src/Vec/vecSE2.jl
similarity index 100%
rename from src/vec/vecSE2.jl
rename to src/Vec/vecSE2.jl
diff --git a/src/actions/interface.jl b/src/actions/interface.jl
index 74ac334..a365ada 100644
--- a/src/actions/interface.jl
+++ b/src/actions/interface.jl
@@ -32,9 +32,13 @@ function Base.findfirst(id, frame::Frame{A}) where {A<:EntityAction}
end
return nothing
end
-function Records.id2index(frame::Frame{A}, id) where {A<:EntityAction}
+
+function id2index(frame::Frame{A}, id) where {A<:EntityAction}
entity_index = findfirst(id, frame)
- if (entity_index === nothing) throw(BoundsError(frame, [id])) end
+ if (entity_index === nothing)
+ throw(BoundsError(frame, [id]))
+ end
return entity_index
end
-Records.get_by_id(frame::Frame{A}, id) where {A<:EntityAction} = frame[id2index(frame, id)]
+
+get_by_id(frame::Frame{A}, id) where {A<:EntityAction} = frame[id2index(frame, id)]
diff --git a/src/agent-definitions/agent_definitions.jl b/src/agent-definitions/agent_definitions.jl
index 3523386..2cc24a9 100644
--- a/src/agent-definitions/agent_definitions.jl
+++ b/src/agent-definitions/agent_definitions.jl
@@ -67,8 +67,8 @@ function Base.show(io::IO, d::VehicleDef)
"UNKNOWN"
@printf(io, "VehicleDef(%s, %.3f, %.3f)", class, d.length, d.width)
end
-Base.write(io::IO, ::MIME"text/plain", def::VehicleDef) = @printf(io, "%d %.16e %.16e", def.class, def.length, def.width)
-function Base.read(io::IO, ::MIME"text/plain", ::Type{VehicleDef})
+Base.write(io::IO, def::VehicleDef) = @printf(io, "%d %.16e %.16e", def.class, def.length, def.width)
+function Base.read(io::IO, ::Type{VehicleDef})
tokens = split(strip(readline(io)), ' ')
class = parse(Int, tokens[1])
length = parse(Float64, tokens[2])
diff --git a/src/behaviors/MOBIL.jl b/src/behaviors/MOBIL.jl
index e3bf2d7..0287275 100644
--- a/src/behaviors/MOBIL.jl
+++ b/src/behaviors/MOBIL.jl
@@ -30,11 +30,6 @@ function MOBIL(
return MOBIL(DIR_MIDDLE, mlon, safe_decel, politeness, advantage_threshold)
end
-"""
-Return the name of the lane changing model
-"""
-get_name(::MOBIL) = "MOBIL"
-
"""
Set the desired speed of the longitudinal model within MOBIL
"""
diff --git a/src/behaviors/intelligent_driver_model.jl b/src/behaviors/intelligent_driver_model.jl
index 335c039..69fb7c2 100644
--- a/src/behaviors/intelligent_driver_model.jl
+++ b/src/behaviors/intelligent_driver_model.jl
@@ -34,7 +34,7 @@ around the non-errorable IDM output.
d_cmf::Float64 = 2.0 # comfortable deceleration [m/s²] (positive)
d_max::Float64 = 9.0 # maximum deceleration [m/s²] (positive)
end
-get_name(::IntelligentDriverModel) = "IDM"
+
function set_desired_speed!(model::IntelligentDriverModel, v_des::Float64)
model.v_des = v_des
model
diff --git a/src/behaviors/interface.jl b/src/behaviors/interface.jl
index 2253e22..8cdfe75 100644
--- a/src/behaviors/interface.jl
+++ b/src/behaviors/interface.jl
@@ -9,12 +9,6 @@ The DriverModel type is an abstract type! Custom driver models should inherit fr
"""
abstract type DriverModel{DriveAction} end
-"""
- get_name(::DriverModel)
-returns the name of the driver model
-"""
-function get_name end
-
"""
action_type(::DriverModel{A}) where {A}
returns the type of the actions that are sampled from the model
@@ -34,6 +28,17 @@ Resets the hidden states of the model.
"""
function reset_hidden_state! end
+"""
+ reset_hidden_states!(models::Dict{I,M}) where {M<:DriverModel}
+reset hidden states of all driver models in `models`
+"""
+function reset_hidden_states!(models::Dict{I,M}) where {I, M<:DriverModel}
+ for model in values(models)
+ reset_hidden_state!(model)
+ end
+ return models
+end
+
"""
observe!(model::DriverModel, scene, roadway, egoid)
Observes the scene and updates the model states accordingly.
@@ -48,40 +53,6 @@ Samples an action from the model.
Base.rand(model::DriverModel) = rand(Random.GLOBAL_RNG, model)
Base.rand(rng::AbstractRNG, model::DriverModel) = error("AutomotiveDrivingModelsError: Base.rand(::AbstractRNG, ::$(typeof(model))) not implemented")
-function prime_with_history!(
- model::DriverModel,
- trajdata::ListRecord{S,D,I},
- roadway::R,
- frame_start::Int,
- frame_end::Int,
- egoid::I,
- scene::EntityFrame{S,D,I} = allocate_frame(trajdata),
- ) where {S,D,I,R}
-
- reset_hidden_state!(model)
-
- for frame in frame_start : frame_end
- get!(scene, trajdata, frame)
- observe!(model, scene, roadway, egoid)
- end
-
- return model
-end
-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)
-
- for pastframe in pastframe_start : pastframe_end
- scene = rec[pastframe]
- observe!(model, scene, roadway, egoid)
- end
-
- model
-end
-
####
"""
@@ -97,7 +68,6 @@ struct StaticDriver{A,P<:ContinuousMultivariateDistribution} <: DriverModel{A}
distribution::P
end
-get_name(::StaticDriver) = "StaticDriver"
function Base.rand(rng::AbstractRNG, model::StaticDriver{A,P}) where {A,P}
a = rand(rng, model.distribution)
return convert(A, a)
diff --git a/src/behaviors/lane_following_drivers.jl b/src/behaviors/lane_following_drivers.jl
index 978787b..f8d5410 100644
--- a/src/behaviors/lane_following_drivers.jl
+++ b/src/behaviors/lane_following_drivers.jl
@@ -26,7 +26,6 @@ mutable struct StaticLaneFollowingDriver <: LaneFollowingDriver
end
StaticLaneFollowingDriver() = StaticLaneFollowingDriver(LaneFollowingAccel(0.0))
StaticLaneFollowingDriver(a::Float64) = StaticLaneFollowingDriver(LaneFollowingAccel(a))
-get_name(::StaticLaneFollowingDriver) = "ProportionalSpeedTracker"
Base.rand(rng::AbstractRNG, model::StaticLaneFollowingDriver) = model.a
Distributions.pdf(model::StaticLaneFollowingDriver, a::LaneFollowingAccel) = isapprox(a.a, model.a.a) ? Inf : 0.0
Distributions.logpdf(model::StaticLaneFollowingDriver, a::LaneFollowingAccel) = isapprox(a.a, model.a.a) ? Inf : -Inf
diff --git a/src/behaviors/lat_lon_separable_driver.jl b/src/behaviors/lat_lon_separable_driver.jl
index 777e99a..5d60568 100644
--- a/src/behaviors/lat_lon_separable_driver.jl
+++ b/src/behaviors/lat_lon_separable_driver.jl
@@ -5,7 +5,6 @@ end
LatLonSeparableDriver(mlat::LateralDriverModel, mlon::LaneFollowingDriver) = LatLonSeparableDriver{LatLonAccel}(mlat, mlon)
-get_name(model::LatLonSeparableDriver) = @sprintf("%s + %s", get_name(model.mlat), get_name(model.mlon))
function set_desired_speed!(model::LatLonSeparableDriver, v_des::Float64)
set_desired_speed!(model.mlon, v_des)
model
diff --git a/src/behaviors/lateral_driver_models.jl b/src/behaviors/lateral_driver_models.jl
index e3d9148..21b89c7 100644
--- a/src/behaviors/lateral_driver_models.jl
+++ b/src/behaviors/lateral_driver_models.jl
@@ -22,8 +22,6 @@ A controller that executes the lane change decision made by the `lane change mod
kd::Float64 = 2.0 # derivative constant for lane tracking
end
-get_name(::ProportionalLaneTracker) = "ProportionalLaneTracker"
-
function track_lateral!(model::ProportionalLaneTracker, laneoffset::Float64, lateral_speed::Float64)
model.a = -laneoffset*model.kp - lateral_speed*model.kd
model
diff --git a/src/behaviors/princeton_driver.jl b/src/behaviors/princeton_driver.jl
index 8cf7fea..9986afb 100644
--- a/src/behaviors/princeton_driver.jl
+++ b/src/behaviors/princeton_driver.jl
@@ -14,7 +14,7 @@ A lane following driver model that controls longitudinal speed by following a fr
k::Float64 = 1.0 # proportional constant for speed tracking [s⁻¹]
v_des::Float64 = 29.0 # desired speed [m/s]
end
-get_name(::PrincetonDriver) = "PrincetonDriver"
+
function set_desired_speed!(model::PrincetonDriver, v_des::Float64)
model.v_des = v_des
model
diff --git a/src/behaviors/sidewalk_pedestrian_model.jl b/src/behaviors/sidewalk_pedestrian_model.jl
index 21a0a00..44c6536 100644
--- a/src/behaviors/sidewalk_pedestrian_model.jl
+++ b/src/behaviors/sidewalk_pedestrian_model.jl
@@ -65,7 +65,6 @@ Walks along the sidewalk until approaching the crosswalk. Waits for the cars to
phases = Int[]
end
-AutomotiveDrivingModels.get_name(model::SidewalkPedestrianModel) = "SidewalkPedestrianModel"
Base.rand(rng::AbstractRNG, model::SidewalkPedestrianModel) = model.a
function AutomotiveDrivingModels.observe!(model::SidewalkPedestrianModel, scene::Frame{Entity{VehicleState, D, I}}, roadway::Roadway, egoid::I) where {D, I}
diff --git a/src/behaviors/speed_trackers.jl b/src/behaviors/speed_trackers.jl
index f1e2371..6f5e92c 100644
--- a/src/behaviors/speed_trackers.jl
+++ b/src/behaviors/speed_trackers.jl
@@ -14,7 +14,7 @@ Longitudinal proportional speed control.
k::Float64 = 1.0# proportional constant for speed tracking [s⁻¹]
v_des::Float64 = 29.0 # desired speed [m/s]
end
-get_name(::ProportionalSpeedTracker) = "ProportionalSpeedTracker"
+
function set_desired_speed!(model::ProportionalSpeedTracker, v_des::Float64)
model.v_des = v_des
model
diff --git a/src/behaviors/tim_2d_driver.jl b/src/behaviors/tim_2d_driver.jl
index 44f5337..324efc9 100644
--- a/src/behaviors/tim_2d_driver.jl
+++ b/src/behaviors/tim_2d_driver.jl
@@ -10,26 +10,18 @@ Driver that combines longitudinal driver and lateral driver into one model.
- `mlat::LateralDriverModel = ProportionalLaneTracker()` Lateral driving model
- `mlane::LaneChangeModel =TimLaneChanger` Lane change model
"""
-mutable struct Tim2DDriver <: DriverModel{LatLonAccel}
- mlon::LaneFollowingDriver
- mlat::LateralDriverModel
- mlane::LaneChangeModel
-end
-function Tim2DDriver(
- timestep::Float64;
- mlon::LaneFollowingDriver=IntelligentDriverModel(),
- mlat::LateralDriverModel=ProportionalLaneTracker(),
- mlane::LaneChangeModel=TimLaneChanger(timestep),
- )
- return Tim2DDriver(mlon, mlat, mlane)
+@with_kw mutable struct Tim2DDriver <: DriverModel{LatLonAccel}
+ mlon::LaneFollowingDriver = IntelligentDriverModel()
+ mlat::LateralDriverModel = ProportionalLaneTracker()
+ mlane::LaneChangeModel = TimLaneChanger()
end
-get_name(::Tim2DDriver) = "Tim2DDriver"
function set_desired_speed!(model::Tim2DDriver, v_des::Float64)
set_desired_speed!(model.mlon, v_des)
set_desired_speed!(model.mlane, v_des)
model
end
+
function track_longitudinal!(driver::LaneFollowingDriver, scene::Frame{Entity{VehicleState, D, I}}, roadway::Roadway, vehicle_index::Int64, fore::NeighborLongitudinalResult) where {D, I}
v_ego = vel(scene[vehicle_index].state)
if fore.ind != nothing
@@ -39,6 +31,7 @@ function track_longitudinal!(driver::LaneFollowingDriver, scene::Frame{Entity{Ve
end
return track_longitudinal!(driver, v_ego, v_oth, headway)
end
+
function observe!(driver::Tim2DDriver, scene::Frame{Entity{S, D, I}}, roadway::Roadway, egoid::I) where {S, D, I}
observe!(driver.mlane, scene, roadway, egoid)
@@ -68,6 +61,9 @@ function observe!(driver::Tim2DDriver, scene::Frame{Entity{S, D, I}}, roadway::R
driver
end
+
Base.rand(rng::AbstractRNG, driver::Tim2DDriver) = LatLonAccel(rand(rng, driver.mlat), rand(rng, driver.mlon).a)
+
Distributions.pdf(driver::Tim2DDriver, a::LatLonAccel) = pdf(driver.mlat, a.a_lat) * pdf(driver.mlon, a.a_lon)
+
Distributions.logpdf(driver::Tim2DDriver, a::LatLonAccel) = logpdf(driver.mlat, a.a_lat) * logpdf(driver.mlon, a.a_lon)
diff --git a/src/behaviors/tim_lane_changer.jl b/src/behaviors/tim_lane_changer.jl
index efbd81f..b3a4488 100644
--- a/src/behaviors/tim_lane_changer.jl
+++ b/src/behaviors/tim_lane_changer.jl
@@ -8,54 +8,30 @@ Has not been published anywhere, so first use in a paper would have to describe
See MOBIL if you want a lane changer you can cite.
# Constructors
- TimLaneChanger(timestep::Float64;v_des::Float64=29.0,rec::SceneRecord=SceneRecord(2,timestep),threshold_fore::Float64 = 50.0,threshold_lane_change_gap_fore::Float64 = 10.0, threshold_lane_change_gap_rear::Float64 = 10.0,dir::Int=DIR_MIDDLE)
+ TimLaneChanger(v_des::Float64=29.0, threshold_fore::Float64 = 50.0,threshold_lane_change_gap_fore::Float64 = 10.0, threshold_lane_change_gap_rear::Float64 = 10.0,dir::Int=DIR_MIDDLE)
# Fields
- `dir::Int = DIR_MIDDLE` the desired lane to go to eg: left,middle (i.e. stay in same lane) or right
-- `rec::SceneRecord` TODO
- `v_des::Float64 = 29.0` desired velocity
- `threshold_fore::Float64 = 50.0` Distance from lead vehicle
- `threshold_lane_change_gap_fore::Float64 = 10.0` Space in front
- `threshold_lane_change_gap_rear::Float64 = 10.0` Space rear
"""
-mutable struct TimLaneChanger <: LaneChangeModel{LaneChangeChoice}
- dir::Int
- rec::SceneRecord
-
- v_des::Float64
- threshold_fore::Float64
- threshold_lane_change_gap_fore::Float64
- threshold_lane_change_gap_rear::Float64
-
- function TimLaneChanger(
- timestep::Float64;
- v_des::Float64=29.0,
- rec::SceneRecord=SceneRecord(2,timestep),
- threshold_fore::Float64 = 50.0,
- threshold_lane_change_gap_fore::Float64 = 10.0,
- threshold_lane_change_gap_rear::Float64 = 10.0,
- dir::Int=DIR_MIDDLE,
- )
-
- retval = new()
- retval.dir = dir
- retval.rec = rec
- retval.v_des = v_des
- retval.threshold_fore = threshold_fore
- retval.threshold_lane_change_gap_fore = threshold_lane_change_gap_fore
- retval.threshold_lane_change_gap_rear = threshold_lane_change_gap_rear
- retval
- end
+@with_kw mutable struct TimLaneChanger <: LaneChangeModel{LaneChangeChoice}
+ dir::Int = DIR_MIDDLE
+ v_des::Float64 = 29.0
+ threshold_fore::Float64 = 50.0
+ threshold_lane_change_gap_fore::Float64 = 10.0
+ threshold_lane_change_gap_rear::Float64 = 10.0
end
-get_name(::TimLaneChanger) = "TimLaneChanger"
+
function set_desired_speed!(model::TimLaneChanger, v_des::Float64)
model.v_des = v_des
model
end
+
function observe!(model::TimLaneChanger, scene::Frame{Entity{S, D, I}}, roadway::Roadway, egoid::I) where {S, D, I}
- rec = model.rec
- update!(rec, scene)
vehicle_index = findfirst(egoid, scene)
veh_ego = scene[vehicle_index]
@@ -117,4 +93,5 @@ function observe!(model::TimLaneChanger, scene::Frame{Entity{S, D, I}}, roadway:
model
end
+
Base.rand(rng::AbstractRNG, model::TimLaneChanger) = LaneChangeChoice(model.dir)
diff --git a/src/deprecated.jl b/src/deprecated.jl
deleted file mode 100644
index 3ae4dd0..0000000
--- a/src/deprecated.jl
+++ /dev/null
@@ -1,194 +0,0 @@
-@deprecate get_vel_s velf(state).s
-@deprecate get_vel_t velf(state).t
-@deprecate get_name x->@sprintf("%s", typeof(x))
-
-@deprecate propagate(veh, state, roadway, Δt) propagate(veh, state, a, roadway, Δt)
-
-function simulate!(
- scene::Frame{E},
- roadway::R,
- models::Dict{I,M},
- nticks::Int64,
- timestep::Float64;
- rng::AbstractRNG = Random.GLOBAL_RNG,
- callbacks = nothing
-) where {E<:Entity,A,R,I,M<:DriverModel}
- Base.depwarn(
-"`simulate!` without specifying a pre-allocated data structure for `scenes` is now deprecated.\n
-You probably want to use the `simulate` function instead.\n
-Alternatively, you can provide a pre-allocated data structure via the `scenes=` keyword",
- :simulate_no_prealloc
- )
- return simulate(scene, roadway, models, nticks, timestep, rng=rng, callbacks=callbacks)
-end
-
-"""
- # TODO: this should be removed, but where to document the function then?
- run_callback(callback::Any, scenes::Vector{F}, roadway::R, models::Dict{I,M}, tick::Int) where {F,I,R,M<:DriverModel}
- run_callback(callback::Any, rec::EntityQueueRecord{S,D,I}, roadway::R, models::Dict{I,M}, tick::Int) where {S,D,I,R,M<:DriverModel}
-run callback and return whether simlation should terminate
-A new method should be implemented when defining a new callback object.
-"""
-function run_callback(callback, scenes, actions::Union{Nothing, Vector{Frame{A}}}, roadway, models, tick) where {A<:EntityAction}
- Base.depwarn(
-"Using a deprecated version of `run_callback`. Since v0.7.10, user-defined callback functions should also take an `actions` argument.
- If you have implemented `run_callback` with an actions argument, make sure the method signature is more specific than this one.\n
-Check the section `Simulation > Callbacks` in the package documentation for more information
- on how to define callback functions.\n
-Your function call is being forwarded to `run_callback` without actions argument.",
- :run_callback_no_actions
- )
- return run_callback(callback, scenes, roadway, models, tick)
-end
-
-# 1D state
-
-state_1d_dep_msgs = """
- State1D, Vehicle1D, and Scene1D are deprecated, use VehicleState, Vehicle, and Scene instead.
-"""
-
-"""
- State1D
-
-A data type to represent one dimensional states
-
-# Fields
- - `s::Float64` position
- - `v::Float64` speed [m/s]
-"""
-struct State1D
- s::Float64 # position
- v::Float64 # speed [m/s]
-end
-
-function vel(s::State1D)
- Base.depwarn(state_1d_dep_msgs, :vel)
- return s.v
-end
-
-posf(s::State1D) = VecSE2{Float64}(s.s)
-posg(s::State1D) = VecSE2{Float64}(s.s, 0., 0.) # TODO: how to derive global position & angle
-velf(s::State1D) = (s=s.v, t=0.)
-velg(s::State1D) = (x=s.v*cos(posg(s).θ), y=s.v*sin(posg(s).θ))
-
-function Base.write(io::IO, ::MIME"text/plain", s::State1D)
- Base.depwarn(state_1d_dep_msgs, :write)
- @printf(io, "%.16e %.16e", s.s, s.v)
-end
-function Base.read(io::IO, ::MIME"text/plain", ::Type{State1D})
- Base.depwarn(state_1d_dep_msgs, :read)
- i = 0
- tokens = split(strip(readline(io)), ' ')
- s = parse(Float64, tokens[i+=1])
- v = parse(Float64, tokens[i+=1])
- return State1D(s,v)
-end
-
-"""
- Vehicle1D
-A specific instance of the Entity type defined in Records.jl to represent vehicles in 1d environments.
-"""
-const Vehicle1D = Entity{State1D, VehicleDef, Int64}
-
-"""
- Scene1D
-
-A specific instance of the Frame type defined in Records.jl to represent a list of vehicles in 1d environments.
-
-# constructors
- Scene1D(n::Int=100)
- Scene1D(arr::Vector{Vehicle1D})
-"""
-const Scene1D = Frame{Vehicle1D}
-
-function Scene1D(n::Int=100)
- Base.depwarn(state_1d_dep_msgs, :Scene1D)
- Frame(Vehicle1D, n)
-end
-
-function Scene1D(arr::Vector{Vehicle1D})
- Base.depwarn(state_1d_dep_msgs, :Scene1D)
- Frame{Vehicle1D}(arr, length(arr))
-end
-
-function get_center(veh::Vehicle1D)
- Base.depwarn(state_1d_dep_msgs, :get_center)
- veh.state.s
-end
-function get_footpoint(veh::Vehicle1D)
- Base.depwarn(state_1d_dep_msgs, :get_footpoint)
- veh.state.s
-end
-function get_front(veh::Vehicle1D)
- Base.depwarn(state_1d_dep_msgs, :get_front)
- veh.state.s + length(veh.def)/2
-end
-function get_rear(veh::Vehicle1D)
- Base.depwarn(state_1d_dep_msgs, :get_rear)
- veh.state.s - length(veh.def)/2
-end
-
-
-function get_headway(veh_rear::Vehicle1D, veh_fore::Vehicle1D, roadway::StraightRoadway)
- Base.depwarn(state_1d_dep_msgs, :get_headway)
- return get_headway(get_front(veh_rear), get_rear(veh_fore), roadway)
-end
-function get_neighbor_fore(scene::Frame{Entity{State1D, D, I}}, vehicle_index::I, roadway::StraightRoadway) where {D, I}
- Base.depwarn(state_1d_dep_msgs, :get_neighbor_fore)
- ego = scene[vehicle_index]
- best_ind = nothing
- best_gap = Inf
- for (i,veh) in enumerate(scene)
- if i != vehicle_index
- Δs = get_headway(ego, veh, roadway)
- if Δs < best_gap
- best_gap, best_ind = Δs, i
- end
- end
- end
- return NeighborLongitudinalResult(best_ind, best_gap)
-end
-function get_neighbor_rear(scene::Frame{Entity{State1D, D, I}}, vehicle_index::I, roadway::StraightRoadway) where {D, I}
- Base.depwarn(state_1d_dep_msgs, :get_neighbor_rear)
- ego = scene[vehicle_index]
- best_ind = nothing
- best_gap = Inf
- for (i,veh) in enumerate(scene)
- if i != vehicle_index
- Δs = get_headway(veh, ego, roadway)
- if Δs < best_gap
- best_gap, best_ind = Δs, i
- end
- end
- end
- return NeighborLongitudinalResult(best_ind, best_gap)
-end
-
-function propagate(veh::Vehicle1D, action::LaneFollowingAccel, roadway::StraightRoadway, Δt::Float64)
- Base.depwarn(state_1d_dep_msgs, :propagate)
- a = action.a
- s, v = veh.state.s, veh.state.v
-
- s′ = s + v*Δt + a*Δt*Δt/2
- v′ = max(v + a*Δt, 0.) # no negative velocities
-
- s′ = mod_position_to_roadway(s′, roadway)
-
- return State1D(s′, v′)
-end
-
-
-function observe!(model::LaneFollowingDriver, scene::Frame{Entity{State1D, D, I}}, roadway::StraightRoadway, egoid::I) where {D, I}
- Base.depwarn(state_1d_dep_msgs, :observe!)
- vehicle_index = findfirst(egoid, scene)
-
- fore_res = get_neighbor_fore(scene, vehicle_index, roadway)
-
- v_ego = vel(scene[vehicle_index].state)
- v_oth = vel(scene[fore_res.ind].state)
- headway = fore_res.Δs
-
- track_longitudinal!(model, v_ego, v_oth, headway)
-
- return model
-end
diff --git a/src/feature-extraction/features.jl b/src/feature-extraction/features.jl
index e616973..939f92d 100644
--- a/src/feature-extraction/features.jl
+++ b/src/feature-extraction/features.jl
@@ -59,14 +59,14 @@ The row correspond to the feature value for each scene (time history).
```julia
roadway = gen_straight_roadway(4, 100.0)
-scene_0 = Scene([
- Vehicle(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
+scene_0 = Frame([
+ Entity(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
])
-scene_1 = Scene([
- Vehicle(VehicleState(VecSE2( 10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(20.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
+scene_1 = Frame([
+ Entity(VehicleState(VecSE2( 10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(20.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
])
dfs = extract_features((posgx, iscolliding), roadway, [scene_0, scene_1], [1,2])
diff --git a/src/roadways/roadways.jl b/src/roadways/roadways.jl
index 05588cd..b2dc4ec 100644
--- a/src/roadways/roadways.jl
+++ b/src/roadways/roadways.jl
@@ -235,10 +235,10 @@ Roadway() = Roadway{Float64}()
Base.show(io::IO, roadway::Roadway) = @printf(io, "Roadway")
"""
- Base.write(io::IO, ::MIME"text/plain", roadway::Roadway)
+ Base.write(io::IO, roadway::Roadway)
write all the roadway information to a text file
"""
-function Base.write(io::IO, ::MIME"text/plain", roadway::Roadway)
+function Base.write(io::IO, roadway::Roadway)
# writes to a text file
println(io, "ROADWAY")
@@ -269,10 +269,10 @@ function Base.write(io::IO, ::MIME"text/plain", roadway::Roadway)
end
"""
- Base.read(io::IO, ::MIME"text/plain", ::Type{Roadway})
+ Base.read(io::IO, ::Type{Roadway})
extract roadway information from a text file and returns a roadway object.
"""
-function Base.read(io::IO, ::MIME"text/plain", ::Type{Roadway})
+function Base.read(io::IO, ::Type{Roadway})
lines = readlines(io)
line_index = 1
if occursin("ROADWAY", lines[line_index])
diff --git a/src/roadways/straight_1d_roadways.jl b/src/roadways/straight_1d_roadways.jl
deleted file mode 100644
index cd2d636..0000000
--- a/src/roadways/straight_1d_roadways.jl
+++ /dev/null
@@ -1,37 +0,0 @@
-"""
- StraightRoadway
-A simple type representing a one lane, one dimensional straight roadway
-# Fields
-- `length::Float64`
-"""
-struct StraightRoadway
- length::Float64
-end
-
-"""
- mod_position_to_roadway(s::Float64, roadway::StraightRoadway)
-performs a modulo of the position `s` with the length of `roadway`
-"""
-function mod_position_to_roadway(s::Float64, roadway::StraightRoadway)
- while s > roadway.length
- s -= roadway.length
- end
- while s < 0.0
- s += roadway.length
- end
- return s
-end
-
-"""
- get_headway(s_rear::Float64, s_fore::Float64, roadway::StraightRoadway)
-returns a positive distance between s_rear and s_fore.
-"""
-function get_headway(s_rear::Float64, s_fore::Float64, roadway::StraightRoadway)
- while s_fore < s_rear
- s_fore += roadway.length
- end
- while s_fore > s_rear + roadway.length
- s_fore -= roadway.length
- end
- return s_fore - s_rear # positive distance
-end
diff --git a/src/simulation/callbacks.jl b/src/simulation/callbacks.jl
index a04ab7a..861072c 100644
--- a/src/simulation/callbacks.jl
+++ b/src/simulation/callbacks.jl
@@ -1,7 +1,7 @@
"""
Run all callbacks
"""
-function _run_callbacks(callbacks::C, scenes::Union{EntityQueueRecord{S,D,I}, Vector{Frame{Entity{S,D,I}}}}, actions::Union{Nothing, Vector{Frame{A}}}, roadway::R, models::Dict{I,M}, tick::Int) where {S,D,I,A<:EntityAction,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}
+function _run_callbacks(callbacks::C, scenes::Vector{Frame{Entity{S,D,I}}}, actions::Union{Nothing, Vector{Frame{A}}}, roadway::R, models::Dict{I,M}, tick::Int) where {S,D,I,A<:EntityAction,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}
isdone = false
for callback in callbacks
isdone |= run_callback(callback, scenes, actions, roadway, models, tick)
@@ -9,54 +9,6 @@ function _run_callbacks(callbacks::C, scenes::Union{EntityQueueRecord{S,D,I}, Ve
return isdone
end
-function simulate!(
- ::Type{A},
- rec::EntityQueueRecord{S,D,I},
- scene::EntityFrame{S,D,I},
- roadway::R,
- models::Dict{I,M},
- nticks::Int,
- callbacks::C,
- ) where {S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}
- Base.depwarn(
-"`simulate!` using `EntityQueueRecord`s is deprecated since v0.7.10 and may be removed in future versions.
- You should pass a pre-allocated vector of entitites `scenes::Vector{Frame{Entity{S,D,I}}}` to `simulate!`
- or use the convenience function `simulate` without pre-allocation instead.",
- :simulate_rec
- )
-
- empty!(rec)
- update!(rec, scene)
-
- # potential early out right off the bat
- if _run_callbacks(callbacks, rec, nothing, roadway, models, 0)
- return rec
- end
-
- actions = Array{A}(undef, length(scene))
- for tick in 1 : nticks
- get_actions!(actions, scene, roadway, models)
- tick!(scene, roadway, actions, get_timestep(rec))
- update!(rec, scene)
- if _run_callbacks(callbacks, rec, nothing, roadway, models, tick)
- break
- end
- end
-
- return rec
-end
-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
-
## Implementations of useful callbacks
"""
@@ -68,17 +20,6 @@ Terminates the simulation once a collision occurs
mem::CPAMemory=CPAMemory()
end
-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
-
function run_callback(
callback::CollisionCallback,
scenes::Vector{Frame{E}},
diff --git a/src/simulation/simulation.jl b/src/simulation/simulation.jl
index ab1d0c3..24dc08a 100644
--- a/src/simulation/simulation.jl
+++ b/src/simulation/simulation.jl
@@ -1,117 +1,3 @@
-"""
- get_actions!(actions::Vector{A}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I, M},) where {S,D,I,A,R,M<:DriverModel}
-Fill in `actions` with the actions of each agent present in the scene. It calls `observe!`
-and `rand` for each driver models.
-`actions` will contain the actions to apply to update the state of each vehicle.
-"""
-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)
- model = models[veh.id]
- observe!(model, scene, roadway, veh.id)
- actions[i] = rand(model)
- end
-
- actions
-end
-
-"""
- tick!(scene::EntityFrame{S,D,I}, roadway::R, actions::Vector{A}, Δt::Float64) where {S,D,I,A,R}
-update `scene` in place by updating the state of each vehicle given their current action in `actions`.
-It calls the `propagate` method for each vehicle in the scene.
-"""
-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]
- state′ = propagate(veh, actions[i], roadway, Δt)
- scene[i] = Entity(state′, veh.def, veh.id)
- end
-
- return scene
-end
-
-"""
- reset_hidden_states!(models::Dict{Int,M}) where {M<:DriverModel}
-reset hidden states of all driver models in `models`
-"""
-function reset_hidden_states!(models::Dict{Int,M}) where {M<:DriverModel}
- for model in values(models)
- reset_hidden_state!(model)
- end
- return models
-end
-
-"""
-DEPRECATION WARNING: this version of `simulate!` is now deprecated.
-
- simulate!(scene::Frame{E}, roadway::R, models::Dict{I,M<:DriverModel}, nticks::Int64, timestep::Float64; rng::AbstractRNG = Random.GLOBAL_RNG, scenes::Vector{Frame{E}} = [Frame(E, length(scene)) for i=1:nticks+1], callbacks=nothing)
-
-Run `nticks` steps of simulation with time step `dt` and return a vector of scenes from time step 0 to nticks.
-
- simulate!(::Type{A}, rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M<:DriverModel}, nticks::Int)
- simulate!(rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M<:DriverModel}, nticks::Int)
-
-Run nticks of simulation and place all nticks+1 scenes into the QueueRecord
-
- simulate!(::Type{A},rec::EntityQueueRecord{S,D,I}, scene::EntityFrame{S,D,I}, roadway::R, models::Dict{I,M}, nticks::Int, callbacks::C) where {S,D,I,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}
- 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,A,R,M<:DriverModel,C<:Tuple{Vararg{Any}}}
-
-Callback objects can also be passed in the simulate! function.
-
-"""
-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}
- Base.depwarn(
-"`simulate!` using `EntityQueueRecord`s is deprecated since v0.7.10 and may be removed in future versions.
- You should pass a pre-allocated vector of entitites `scenes::Vector{Frame{Entity{S,D,I}}}` to `simulate!`
- or use the convenience function `simulate` without pre-allocation instead.",
- :simulate_rec
- )
-
- empty!(rec)
- update!(rec, scene)
- actions = Array{A}(undef, length(scene))
-
- for tick in 1 : nticks
- get_actions!(actions, scene, roadway, models)
- tick!(scene, roadway, actions, rec.timestep)
- update!(rec, scene)
- end
-
- return rec
-end
-
-
-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
-
-
"""
simulate(
scene::Frame{E}, roadway::R, models::Dict{I,M}, nticks::Int64, timestep::Float64;
@@ -138,7 +24,6 @@ function simulate(
return scenes[1:(n+1)]
end
-
"""
simulate!(
@@ -207,56 +92,3 @@ function simulate!(
end
return nticks
end
-
-"""
- Run a simulation and store the resulting scenes in the provided QueueRecord.
-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!(
- rec::EntityQueueRecord{S,D,I},
- model::DriverModel,
- egoid::I,
- trajdata::ListRecord{S,D,I},
- roadway::R,
- frame_start::Int,
- frame_end::Int;
- prime_history::Int=0, # no prime-ing
- scene::EntityFrame{S,D,I} = allocate_frame(trajdata),
- ) where {S,D,I,R}
-
- @assert(isapprox(get_timestep(rec), get_timestep(trajdata)))
-
- # prime with history
- prime_with_history!(model, trajdata, roadway, frame_start, frame_end, egoid, scene)
-
- # add current frame
- update!(rec, get!(scene, trajdata, frame_start))
- observe!(model, scene, roadway, egoid)
-
- # run simulation
- frame_index = frame_start
- ego_veh = get_by_id(scene, egoid)
- while frame_index < frame_end
-
- # pull original scene
- get!(scene, trajdata, frame_index)
-
- # propagate ego vehicle and set
- ego_action = rand(model)
- ego_state = propagate(ego_veh, ego_action, roadway, get_timestep(rec))
- ego_veh = Entity(ego_veh, ego_state)
- scene[findfirst(ego_veh.id, scene)] = ego_veh
-
- # update record
- update!(rec, scene)
-
- # observe
- observe!(model, scene, roadway, ego_veh.id)
-
- # update time
- frame_index += 1
- end
-
- return rec
-end
diff --git a/src/simulation/simulation_from_history.jl b/src/simulation/simulation_from_history.jl
new file mode 100644
index 0000000..0251c17
--- /dev/null
+++ b/src/simulation/simulation_from_history.jl
@@ -0,0 +1,80 @@
+"""
+ observe_from_history!(model::DriverModel, roadway::Roadway, trajdata::Vector{<:EntityFrame}, egoid, start::Int, stop::Int)
+
+Given a prerecorded trajectory `trajdata`, run the observe function of a driver model for the scenes between `start` and `stop` for the vehicle of id `egoid`.
+The ego vehicle does not take any actions, it just observe the scenes,
+"""
+function observe_from_history!(
+ model::DriverModel,
+ roadway::Roadway,
+ trajdata::Vector{<:EntityFrame},
+ egoid,
+ start::Int = 1,
+ stop::Int = length(trajdata))
+ reset_hidden_state!(model)
+
+ for i=start:stop
+ observe!(model, trajdata[i], roadway, egoid)
+ end
+
+ return model
+end
+
+function maximum_entities(trajdata::Vector{<:EntityFrame})
+ return maximum(capacity, trajdata)
+end
+
+function simulate_from_history(
+ model::DriverModel,
+ roadway::Roadway,
+ trajdata::Vector{Frame{E}},
+ egoid,
+ timestep::Float64,
+ start::Int = 1,
+ stop::Int = length(trajdata);
+ rng::AbstractRNG = Random.GLOBAL_RNG
+ ) where {E<:Entity}
+ scenes = [Frame(E, maximum_entities(trajdata)) for i=1:(stop - start + 1)]
+ n = simulate_from_history!(model, roadway, trajdata, egoid, timestep,
+ start, stop, scenes,
+ rng=rng)
+ return scenes[1:(n+1)]
+end
+
+function simulate_from_history!(
+ model::DriverModel,
+ roadway::Roadway,
+ trajdata::Vector{Frame{E}},
+ egoid,
+ timestep::Float64,
+ start::Int,
+ stop::Int,
+ scenes::Vector{Frame{E}};
+ actions::Union{Nothing, Vector{Frame{A}}} = nothing,
+ rng::AbstractRNG = Random.GLOBAL_RNG
+ ) where {E<:Entity, A<:EntityAction}
+
+ # run model (unsure why it is needed, it was in the old code )
+ observe_from_history!(model, roadway, trajdata, egoid, start, stop)
+
+ copyto!(scenes[1], trajdata[start])
+ for tick=1:(stop - start)
+
+ empty!(scenes[tick + 1])
+ if (actions !== nothing) empty!(actions[tick]) end
+
+ ego = get_by_id(scenes[tick], egoid)
+ observe!(model, scenes[tick], roadway, egoid)
+ a = rand(rng, model)
+
+ ego_state_p = propagate(ego, a, roadway, timestep)
+
+ copyto!(scenes[tick+1], trajdata[start+tick])
+ egoind = findfirst(egoid, scenes[tick+1])
+ scenes[tick+1][egoind] = Entity(ego_state_p, ego.def, egoid)
+
+ if (actions !== nothing) push!(actions[tick], EntityAction(a, egoid)) end
+
+ end
+ return (stop - start)
+end
diff --git a/src/states/entities.jl b/src/states/entities.jl
new file mode 100644
index 0000000..a467df7
--- /dev/null
+++ b/src/states/entities.jl
@@ -0,0 +1,26 @@
+"""
+ Entity{S,D,I}
+Immutable data structure to represent entities (vehicle, pedestrian, ...).
+Entities are defined by a state, a definition, and an id.
+The state of an entity usually models changing values while the definition and the id should not change.
+
+# Constructor
+
+`Entity(state, definition, id)`
+
+Copy constructor that keeps the definition and id but changes the state (a new object is still created):
+
+`Entity(entity::Entity{S,D,I}, s::S)`
+
+# Fields
+
+- `state::S`
+- `def::D`
+- `id::I`
+"""
+struct Entity{S,D,I} # state, definition, identification
+ state::S
+ def::D
+ id::I
+end
+Entity(entity::Entity{S,D,I}, s::S) where {S,D,I} = Entity(s, entity.def, entity.id)
diff --git a/src/states/frames.jl b/src/states/frames.jl
new file mode 100644
index 0000000..faf8b59
--- /dev/null
+++ b/src/states/frames.jl
@@ -0,0 +1,187 @@
+"""
+ Frame{E}
+
+Container to store a list of entities.
+The main difference from a regular array is that its size is defined at construction and is fixed.
+(`push!` is O(1))
+
+# Constructors
+
+- `Frame(arr::AbstractVector; capacity::Int=length(arr))`
+- `Frame(::Type{E}, capacity::Int=100) where {E}`
+
+
+# Fields
+
+To interact with `Frame` object it is preferable to use functions rather than accessing the fields directly.
+
+- `entities::Vector{E}`
+- `n::Int` current number of entities in the scene
+"""
+mutable struct Frame{E}
+ entities::Vector{E} # NOTE: I tried StaticArrays; was not faster
+ n::Int
+end
+function Frame(arr::AbstractVector{E}; capacity::Int=length(arr)) where {E}
+ capacity ≥ length(arr) || error("capacity cannot be less than entitiy count! (N ≥ length(arr))")
+ entities = Array{E}(undef, capacity)
+ copyto!(entities, arr)
+ return Frame{E}(entities, length(arr))
+end
+function Frame(::Type{E}, capacity::Int=100) where {E}
+ entities = Array{E}(undef, capacity)
+ return Frame{E}(entities, 0)
+end
+
+Base.show(io::IO, frame::Frame{E}) where {E}= @printf(io, "Frame{%s}(%d entities)", string(E), length(frame))
+
+"""
+ capacity(frame::Frame)
+returns the maximum number of entities that can be put in the frame.
+To get the current number of entities use `length` instead.
+"""
+capacity(frame::Frame) = length(frame.entities)
+
+Base.length(frame::Frame) = frame.n
+Base.getindex(frame::Frame, i::Int) = frame.entities[i]
+Base.eltype(frame::Frame{E}) where {E} = E
+
+Base.lastindex(frame::Frame) = frame.n
+function Base.setindex!(frame::Frame{E}, entity::E, i::Int) where {E}
+ frame.entities[i] = entity
+ return frame
+end
+function Base.empty!(frame::Frame)
+ frame.n = 0
+ return frame
+end
+function Base.deleteat!(frame::Frame, entity_index::Int)
+ for i in entity_index : frame.n - 1
+ frame.entities[i] = frame.entities[i+1]
+ end
+ frame.n -= 1
+ frame
+end
+
+function Base.iterate(frame::Frame{E}, i::Int=1) where {E}
+ if i > length(frame)
+ return nothing
+ end
+ return (frame.entities[i], i+1)
+end
+
+function Base.copyto!(dest::Frame{E}, src::Frame{E}) where {E}
+ for i in 1 : src.n
+ dest.entities[i] = src.entities[i]
+ end
+ dest.n = src.n
+ return dest
+end
+Base.copy(frame::Frame{E}) where {E} = copyto!(Frame(E, capacity(frame)), frame)
+
+function Base.push!(frame::Frame{E}, entity::E) where {E}
+ frame.n += 1
+ frame.entities[frame.n] = entity
+ return frame
+end
+
+
+####
+"""
+ EntityFrame{S,D,I} = Frame{Entity{S,D,I}}
+Alias for `Frame` when the entities in the frame are of type `Entity`
+
+# Constructors
+- `EntityFrame(::Type{S},::Type{D},::Type{I}) where {S,D,I}`
+- `EntityFrame(::Type{S},::Type{D},::Type{I},capacity::Int)`
+
+"""
+const EntityFrame{S,D,I} = Frame{Entity{S,D,I}}
+EntityFrame(::Type{S},::Type{D},::Type{I}) where {S,D,I} = Frame(Entity{S,D,I})
+EntityFrame(::Type{S},::Type{D},::Type{I},capacity::Int) where {S,D,I} = Frame(Entity{S,D,I}, capacity)
+
+Base.in(id::I, frame::EntityFrame{S,D,I}) where {S,D,I} = findfirst(id, frame) !== nothing
+
+function Base.findfirst(id::I, frame::EntityFrame{S,D,I}) where {S,D,I}
+ for entity_index in 1 : frame.n
+ entity = frame.entities[entity_index]
+ if entity.id == id
+ return entity_index
+ end
+ end
+ return nothing
+end
+
+function id2index(frame::EntityFrame{S,D,I}, id::I) where {S,D,I}
+ entity_index = findfirst(id, frame)
+ if entity_index === nothing
+ throw(BoundsError(frame, [id]))
+ end
+ return entity_index
+end
+
+"""
+ get_by_id(frame::EntityFrame{S,D,I}, id::I) where {S,D,I}
+Retrieve the entity by its `id`. This function uses `findfirst` which is O(n).
+"""
+get_by_id(frame::EntityFrame{S,D,I}, id::I) where {S,D,I} = frame[id2index(frame, id)]
+
+function get_first_available_id(frame::EntityFrame{S,D,I}) where {S,D,I}
+ ids = Set{I}(entity.id for entity in frame)
+ id_one = one(I)
+ id = id_one
+ while id ∈ ids
+ id += id_one
+ end
+ return id
+end
+function Base.push!(frame::EntityFrame{S,D,I}, s::S) where {S,D,I}
+ id = get_first_available_id(frame)
+ entity = Entity{S,D,I}(s, D(), id)
+ push!(frame, entity)
+end
+
+Base.delete!(frame::EntityFrame{S,D,I}, entity::Entity{S,D,I}) where {S,D,I} = deleteat!(frame, findfirst(entity.id, frame))
+function Base.delete!(frame::EntityFrame{S,D,I}, id::I) where {S,D,I}
+ entity_index = findfirst(id, frame)
+ if entity_index != nothing
+ deleteat!(frame, entity_index)
+ end
+ return frame
+end
+
+###
+
+function Base.write(io::IO, frames::Vector{EntityFrame{S,D,I}}) where {S,D,I}
+ println(io, length(frames))
+ for frame in frames
+ println(io, length(frame))
+ for entity in frame
+ write(io, entity.state)
+ print(io, "\n")
+ write(io, entity.def)
+ print(io, "\n")
+ write(io, string(entity.id))
+ print(io, "\n")
+ end
+ end
+end
+function Base.read(io::IO, ::Type{Vector{EntityFrame{S,D,I}}}) where {S,D,I}
+
+ n = parse(Int, readline(io))
+ frames = Array{EntityFrame{S,D,I}}(undef, n)
+
+ for i in 1 : n
+ m = parse(Int, readline(io))
+ frame = Frame(Entity{S,D,I}, m)
+ for j in 1 : m
+ state = read(io, S)
+ def = read(io, D)
+ id = parse(I, readline(io))
+ push!(frame, Entity(state,def,id))
+ end
+ frames[i] = frame
+ end
+
+ return frames
+end
diff --git a/src/states/scenes.jl b/src/states/scenes.jl
deleted file mode 100644
index 669d642..0000000
--- a/src/states/scenes.jl
+++ /dev/null
@@ -1,35 +0,0 @@
-"""
- Scene
-
-A Scene is a specific instance of the Frame type defined in Records. It represents a collection of vehicles at a given time.
-
-# Constructors
- - `Scene(n::Int=100)`
- - `Scene(arr::Vector{Vehicle})`
-"""
-const Scene = Frame{Vehicle}
-Scene(n::Int=100) = Frame(Vehicle, n)
-Scene(arr::Vector{Vehicle}) = Frame{Vehicle}(arr, length(arr))
-
-Base.show(io::IO, scene::Scene) = print(io, "Scene(with $(length(scene)) cars)")
-
-"""
- Base.convert(::Type{Vehicle}, veh::Entity{VehicleState, D, Int64}) where D<:AbstractAgentDefinition
-
-Converts an entity in Vehicle (it is converting the agent definition only)
-"""
-function Base.convert(::Type{Entity{VehicleState, VehicleDef, I}}, veh::Entity{VehicleState, D, I}) where {D<:AbstractAgentDefinition, I}
- vehdef = VehicleDef(class(veh.def), length(veh.def), width(veh.def))
- return Entity{VehicleState, VehicleDef, I}(veh.state, vehdef, veh.id)
-end
-
-"""
- SceneRecord
-A SceneRecord is a specific instance of the QueueRecord type defined in Records.jl. It represents a collection of Scenes.
-
-# constructor
- SceneRecord(capacity::Int, timestep::Float64, frame_capacity::Int=100)
-"""
-const SceneRecord = QueueRecord{Vehicle}
-SceneRecord(capacity::Int, timestep::Float64, frame_capacity::Int=100) = QueueRecord(Vehicle, capacity, timestep, frame_capacity)
-Base.show(io::IO, rec::SceneRecord) = print(io, "SceneRecord(nscenes=", nframes(rec), ")")
diff --git a/src/states/trajdatas.jl b/src/states/trajdatas.jl
deleted file mode 100644
index 8a91a19..0000000
--- a/src/states/trajdatas.jl
+++ /dev/null
@@ -1,13 +0,0 @@
-"""
- Trajdata
-Trajdata is a specific instance of ListRecord defined in Records.jl. It is a collection of Scenes
-"""
-const Trajdata = ListRecord{VehicleState, VehicleDef, Int}
-Trajdata(timestep::Float64) = ListRecord(timestep, VehicleState, VehicleDef, Int)
-function Trajdata(scenes::Vector{EntityFrame{S,D,I}}, timestep::Float64) where {S,D,I}
- trajdata = ListRecord(timestep, S, D, I)
- push!.(Ref(trajdata), scenes)
- return trajdata
-end
-
-Base.show(io::IO, trajdata::Trajdata) = @printf(io, "Trajdata(%d frames)", nframes(trajdata))
diff --git a/src/states/vehicle_state.jl b/src/states/vehicle_state.jl
index 8ca4648..f58cfa3 100644
--- a/src/states/vehicle_state.jl
+++ b/src/states/vehicle_state.jl
@@ -39,13 +39,13 @@ velg(veh::Entity) = velg(veh.state)
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)
+function Base.write(io::IO, s::VehicleState)
@printf(io, "%.16e %.16e %.16e", s.posG.x, s.posG.y, s.posG.θ)
@printf(io, " %d %.16e %d %d", s.posF.roadind.ind.i, s.posF.roadind.ind.t, s.posF.roadind.tag.segment, s.posF.roadind.tag.lane)
@printf(io, " %.16e %.16e %.16e", s.posF.s, s.posF.t, s.posF.ϕ)
@printf(io, " %.16e", s.v)
end
-function Base.read(io::IO, ::MIME"text/plain", ::Type{VehicleState})
+function Base.read(io::IO, ::Type{VehicleState})
tokens = split(strip(readline(io)), ' ')
i = 0
posG = VecSE2(parse(Float64, tokens[i+=1]), parse(Float64, tokens[i+=1]), parse(Float64, tokens[i+=1]))
@@ -66,17 +66,6 @@ function Vec.lerp(a::VehicleState, b::VehicleState, t::Float64, roadway::Roadway
VehicleState(posG, roadway, v)
end
-"""
- get_vel_s(s::VehicleState)
-returns the longitudinal velocity (along the lane)
-"""
-get_vel_s(s::VehicleState) = s.v * cos(s.posF.ϕ) # velocity along the lane
-"""
- get_vel_t(s::VehicleState)
-returns the lateral velocity (⟂ to lane)
-"""
-get_vel_t(s::VehicleState) = s.v * sin(s.posF.ϕ) # velocity ⟂ to lane
-
"""
move_along(vehstate::VehicleState, roadway::Roadway, Δs::Float64;
ϕ₂::Float64=vehstate.posF.ϕ, t₂::Float64=vehstate.posF.t, v₂::Float64=vehstate.v)
@@ -99,13 +88,6 @@ function move_along(vehstate::VehicleState, roadway::Roadway, Δs::Float64;
VehicleState(posG, roadway, v₂)
end
-"""
- Vehicle
-A specific instance of the Entity type defined in Records to represent Vehicles with
-state `VehicleState` , definition `VehicleDef` and id `Int64`
-"""
-const Vehicle = Entity{VehicleState,VehicleDef,Int64}
-
# XXX Should this go in features
"""
get_center(veh::Entity{VehicleState, D, I})
@@ -130,7 +112,6 @@ returns the position of the rear of the vehicle
"""
get_rear(veh::Entity{VehicleState, D, I}) where {D<:AbstractAgentDefinition, I} = veh.state.posG - polar(length(veh.def)/2, veh.state.posG.θ)
-
"""
get_lane(roadway::Roadway, vehicle::Entity)
get_lane(roadway::Roadway, vehicle::VehicleState)
@@ -143,3 +124,13 @@ function get_lane(roadway::Roadway, vehicle::VehicleState)
lane_tag = vehicle.posF.roadind.tag
return roadway[lane_tag]
end
+
+"""
+ Base.convert(::Type{Entity{S, VehicleDef, I}}, veh::Entity{S, D, I}) where {S,D<:AbstractAgentDefinition,I}
+
+Converts the definition of an entity
+"""
+function Base.convert(::Type{Entity{S, VehicleDef, I}}, veh::Entity{S, D, I}) where {S,D<:AbstractAgentDefinition,I}
+ vehdef = VehicleDef(class(veh.def), length(veh.def), width(veh.def))
+ return Entity{S, VehicleDef, I}(veh.state, vehdef, veh.id)
+end
diff --git a/test/collision_checkers_benchmark.jl b/test/collision_checkers_benchmark.jl
index 1678184..7f2a099 100644
--- a/test/collision_checkers_benchmark.jl
+++ b/test/collision_checkers_benchmark.jl
@@ -13,7 +13,7 @@ const ROADWAY = gen_straight_roadway(1, 20.0)
function create_vehicle(x::Float64, y::Float64, θ::Float64 = 0.0; id::Int64=1)
s = VehicleState(VecSE2(x, y, θ), ROADWAY, 0.0)
- return Vehicle(s, VehicleDef(), id)
+ return Entity(s, VehicleDef(), id)
end
const VEH_REF = create_vehicle(0.0, 0.0, 0.0, id=1)
@@ -109,4 +109,4 @@ println("Minkowski Sum performance on close range negative collisions: ")
@btime no_collisions_MI($xspace, $yspace, $thetaspace)
println(" ")
println(" ------------------------------------------")
-println(" ")
\ No newline at end of file
+println(" ")
diff --git a/test/runtests.jl b/test/runtests.jl
index dcc1940..2034c6d 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -10,6 +10,7 @@ end
@testset "AutomotiveDrivingModels" begin
include("test_roadways.jl")
include("test_agent_definitions.jl")
+ include("test_frames.jl")
include("test_states.jl")
include("test_collision_checkers.jl")
include("test_actions.jl")
diff --git a/test/test.txt b/test/test.txt
new file mode 100644
index 0000000..9b43d91
--- /dev/null
+++ b/test/test.txt
@@ -0,0 +1,29 @@
+4
+2
+0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 0.0000000000000000e+00 1 1 0.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00 1.0000000000000000e+01
+2 4.0000000000000000e+00 1.8000000000000000e+00
+1
+5.0000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 1.0000000000000009e-02 1 1 5.0000000000000000e+00 0.0000000000000000e+00 0.0000000000000000e+00 5.0000000000000000e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+2
+2
+9.5499999999999996e-01 0.0000000000000000e+00 0.0000000000000000e+00 1 1.9100000000000000e-03 1 1 9.5499999999999996e-01 0.0000000000000000e+00 0.0000000000000000e+00 9.0999999999999996e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+1
+5.5000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 1.1000000000000008e-02 1 1 5.5000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 5.0000000000000000e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+2
+2
+1.8199999999999998e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 3.6399999999999996e-03 1 1 1.8199999999999998e+00 0.0000000000000000e+00 0.0000000000000000e+00 8.1999999999999993e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+1
+6.0000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 1.2000000000000009e-02 1 1 6.0000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 5.0000000000000000e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+2
+2
+2.5949999999999998e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 5.1899999999999993e-03 1 1 2.5949999999999998e+00 0.0000000000000000e+00 0.0000000000000000e+00 7.2999999999999989e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+1
+6.5000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 1 1.3000000000000008e-02 1 1 6.5000000000000044e+00 0.0000000000000000e+00 0.0000000000000000e+00 5.0000000000000000e+00
+2 4.0000000000000000e+00 1.8000000000000000e+00
+2
diff --git a/test/test_actions.jl b/test/test_actions.jl
index 7480abf..0022436 100644
--- a/test/test_actions.jl
+++ b/test/test_actions.jl
@@ -1,7 +1,7 @@
@testset "action interface" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
s = VehicleState()
@test VehicleState() == propagate(veh, s, roadway, NaN)
end
@@ -9,7 +9,7 @@ end
@testset "AccelTurnrate" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
a = AccelTurnrate(0.1,0.2)
io = IOBuffer()
show(io, a)
@@ -29,7 +29,7 @@ end
@testset "AccelDesang" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
a = AccelDesang(0.1,0.2)
@test a == convert(AccelDesang, [0.1,0.2])
@test copyto!([NaN, NaN], AccelDesang(0.1,0.2)) == [0.1,0.2]
@@ -48,7 +48,7 @@ end
@testset "AccelSteeringAngle" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
a = AccelSteeringAngle(0.1,0.2)
io = IOBuffer()
show(io, a)
@@ -74,17 +74,17 @@ end
@testset "LaneFollowingAccel" begin
a = LaneFollowingAccel(1.0)
- roadway1d = StraightRoadway(20.0)
- s1d = State1D(10.0, 10.0)
- veh = Vehicle1D(s1d, VehicleDef(), 1)
- vehp = propagate(veh, a, roadway1d, 1.0)
- @test vehp.s == 0.5
+ roadway = gen_straight_roadway(3, 100.0)
+ s = VehicleState(VecSE2(0.0, 0.0, 0.0), roadway, 0.0)
+ veh = Entity(s, VehicleDef(), 1)
+ vehp = propagate(veh, a, roadway, 1.0)
+ @test posf(vehp).s == 0.5
end
@testset "LatLonAccel" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
a = LatLonAccel(0.1,0.2)
io = IOBuffer()
show(io, a)
@@ -104,7 +104,7 @@ end
@testset "Pedestrian LatLon" begin
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- veh = get(trajdata, 1, 1)
+ veh = trajdata[1][1]
a = PedestrianLatLonAccel(0.5,1.0, roadway[LaneTag(2,1)])
Δt = 1.0
s = propagate(veh, a, roadway, Δt)
diff --git a/test/test_behaviors.jl b/test/test_behaviors.jl
index 73246ee..63e12f4 100644
--- a/test/test_behaviors.jl
+++ b/test/test_behaviors.jl
@@ -8,8 +8,8 @@ struct FakeDriverModel <: DriverModel{FakeDriveAction} end
model = FakeDriverModel()
@test_throws MethodError reset_hidden_state!(model)
- @test_throws MethodError observe!(model, Scene(), roadway, 1)
- @test_throws MethodError prime_with_history!(model, trajdata, roadway, 1, 2, 1)
+ @test_throws MethodError observe!(model, Frame(Entity{VehicleState, VehicleDef, Int64}), roadway, 1)
+ @test_throws MethodError observe_from_history!(model, roadway, trajdata, 1, 2, 1)
@test action_type(model) <: FakeDriveAction
@test_throws MethodError set_desired_speed!(model, 0.0)
@@ -27,18 +27,14 @@ end
set_desired_speed!(models[2], 5.0)
@test models[2].v_des == 5.0
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 5.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 70.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
+ scene = Frame([veh1, veh2])
n_steps = 40
dt = 0.1
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
simulate(scene, roadway, models, n_steps, dt)
@test isapprox(get_by_id(scene, 2).state.v, models[2].v_des)
@@ -51,26 +47,19 @@ end
@test logpdf(models[1], LaneFollowingAccel(0.0)) < 0.0
n_steps = 40
dt = 0.1
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
- simulate(scene, roadway, models, n_steps, dt)
- prime_with_history!(IntelligentDriverModel(), rec, roadway, 2)
+ scenes = simulate(scene, roadway, models, n_steps, dt)
- println("There should be a warning here: ")
+ observe_from_history!(IntelligentDriverModel(), roadway, scenes, 2)
# initializing vehicles too close
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 5.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 3.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
+ scene = Frame([veh1, veh2])
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, 1)
simulate(scene, roadway, models, 1, dt)
end
@@ -86,7 +75,7 @@ struct FakeLaneChanger <: LaneChangeModel{LaneChangeChoice} end
model = FakeLaneChanger()
@test_throws MethodError reset_hidden_state!(model)
- @test_throws MethodError observe!(model, Scene(), roadway, 1)
+ @test_throws MethodError observe!(model, Frame(), roadway, 1)
@test_throws MethodError set_desired_speed!(model, 0.0)
@test_throws ErrorException rand(model)
@@ -101,19 +90,19 @@ end
roadway = gen_straight_roadway(3, 1000.0)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 0.0), roadway, 10.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 20.0), roadway, 2.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
dt = 0.5
n_steps = 10
models = Dict{Int, DriverModel}()
- models[1] = Tim2DDriver(dt, mlane=MOBIL(dt))
+ models[1] = Tim2DDriver(mlane=MOBIL(dt))
set_desired_speed!(models[1], 10.0)
- models[2] = Tim2DDriver(dt, mlane=MOBIL(dt))
+ models[2] = Tim2DDriver(mlane=MOBIL(dt))
set_desired_speed!(models[2], 2.0)
- scene = Scene([veh1, veh2])
+ scene = Frame([veh1, veh2])
scenes = simulate(scene, roadway, models, n_steps, dt)
@test posf(last(scenes)[1]).roadind.tag == LaneTag(1, 3)
@@ -123,7 +112,7 @@ end
@testset "Tim2DDriver" begin
timestep = 0.1
- drivermodel = Tim2DDriver(timestep)
+ drivermodel = Tim2DDriver()
set_desired_speed!(drivermodel,20.0)
@test drivermodel.mlon.v_des == 20.0
@@ -131,26 +120,24 @@ end
roadway = gen_straight_roadway(3, 1000.0)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 0.0), roadway, 10.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,2)], 10.0), roadway, 2.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
dt = 0.5
n_steps = 10
models = Dict{Int, DriverModel}()
- models[1] = Tim2DDriver(dt)
+ models[1] = Tim2DDriver()
set_desired_speed!(models[1], 10.0)
- models[2] = Tim2DDriver(dt)
+ models[2] = Tim2DDriver()
set_desired_speed!(models[2], 2.0)
- scene = Scene([veh1, veh2])
+ scene = Frame([veh1, veh2])
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
- simulate(scene, roadway, models, n_steps, dt)
+ scenes = simulate(scene, roadway, models, n_steps, dt)
- @test scene[1].state.posF.roadind.tag == LaneTag(1, 3)
- @test scene[2].state.posF.roadind.tag == LaneTag(1, 2)
+ @test scenes[end][1].state.posF.roadind.tag == LaneTag(1, 3)
+ @test scenes[end][2].state.posF.roadind.tag == LaneTag(1, 2)
end
@testset "lane following" begin
@@ -172,25 +159,21 @@ end
@test models[3].v_des == 5.0
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 5.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 70.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 130.0), roadway, 5.)
- veh3 = Vehicle(veh_state, VehicleDef(), 3)
+ veh3 = Entity(veh_state, VehicleDef(), 3)
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
- push!(scene, veh3)
+ scene = Frame([veh1, veh2, veh3])
n_steps = 40
dt = 0.1
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
- simulate(scene, roadway, models, n_steps, dt)
- @test isapprox(get_by_id(scene, 2).state.v, models[2].v_des, atol=1e-3)
- @test isapprox(get_by_id(scene, 3).state.v, models[3].v_des)
+ scenes = simulate(scene, roadway, models, n_steps, dt)
+
+ @test isapprox(get_by_id(scenes[end], 2).state.v, models[2].v_des, atol=1e-3)
+ @test isapprox(get_by_id(scenes[end], 3).state.v, models[3].v_des)
# same wth noise
models = Dict{Int, DriverModel}()
@@ -206,13 +189,11 @@ end
n_steps = 40
dt = 0.1
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
- simulate(scene, roadway, models, n_steps, dt)
+ scenes = simulate(scene, roadway, models, n_steps, dt)
- @test isapprox(get_by_id(scene, 2).state.v, models[2].v_des, atol=1.0)
- @test isapprox(get_by_id(scene, 3).state.v, models[3].v_des, atol=1.0)
+ @test isapprox(get_by_id(scenes[end], 2).state.v, models[2].v_des, atol=1.0)
+ @test isapprox(get_by_id(scenes[end], 3).state.v, models[3].v_des, atol=1.0)
end
function generate_sidewalk_env()
@@ -265,15 +246,13 @@ end
# Crossing pedestrian definition
ped_init_state = VehicleState(VecSE2(49.0,-3.0,0.), sidewalk[2], roadway, 1.3)
- ped = Vehicle(ped_init_state, VehicleDef(AgentClass.PEDESTRIAN, 1.0, 1.0), 1)
+ ped = Entity(ped_init_state, VehicleDef(AgentClass.PEDESTRIAN, 1.0, 1.0), 1)
# Car definition
car_initial_state = VehicleState(VecSE2(0.0, 0., 0.), roadway.segments[1].lanes[1],roadway, 8.0)
- car = Vehicle(car_initial_state, VehicleDef(), 2)
+ car = Entity(car_initial_state, VehicleDef(), 2)
- scene = Scene()
- push!(scene, ped)
- push!(scene, car)
+ scene = Frame([ped, car])
# Define a model for each entity present in the scene
models = Dict{Int, DriverModel}()
@@ -294,10 +273,7 @@ end
)
nticks = 300
- rec = SceneRecord(nticks+1, timestep)
- # Execute the simulation
- @test_deprecated simulate!(rec, scene, roadway, models, nticks)
- simulate(scene, roadway, models, nticks, timestep)
+ scenes = simulate(scene, roadway, models, nticks, timestep)
- ped = get_by_id(rec[0], ped_id)
+ ped = get_by_id(scenes[end], ped_id)
end
diff --git a/test/test_collision_checkers.jl b/test/test_collision_checkers.jl
index 24a344d..a25224e 100644
--- a/test/test_collision_checkers.jl
+++ b/test/test_collision_checkers.jl
@@ -2,7 +2,7 @@ const ROADWAY = gen_straight_roadway(1, 20.0)
function create_vehicle(x::Float64, y::Float64, θ::Float64 = 0.0; id::Int64=1)
s = VehicleState(VecSE2(x, y, θ), ROADWAY, 0.0)
- return Vehicle(s, VehicleDef(), id)
+ return Entity(s, VehicleDef(), id)
end
const VEH_REF = create_vehicle(0.0, 0.0, 0.0, id=1)
@@ -31,26 +31,26 @@ end
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- scene = Scene()
+ scene = Frame(Entity{VehicleState, VehicleDef, Int64})
- col = get_first_collision(get!(scene, trajdata, 1))
+ col = get_first_collision(trajdata[1])
@test col.is_colliding
@test col.A == 1
@test col.B == 2
- @test get_first_collision(get!(scene, trajdata, 2), CPAMemory()).is_colliding == true
- scene = Scene()
- @test is_collision_free(get!(scene, trajdata, 1)) == false
- @test is_collision_free(get!(scene, trajdata, 1), [1]) == false
+ @test get_first_collision(trajdata[1], CPAMemory()).is_colliding == true
+ scene = trajdata[1]
+ @test is_collision_free(scene) == false
+ @test is_collision_free(scene, [1]) == false
@test is_colliding(scene[1], scene[2])
@test get_distance(scene[1], scene[2]) == 0
- @test is_collision_free(get!(scene, trajdata, 3))
+ @test is_collision_free(trajdata[2])
get_distance(scene[1], scene[2])
roadway = gen_straight_roadway(2, 100.0)
- veh1 = Vehicle(VehicleState(VecSE2(0.0, 0.0, 0.0), roadway, 10.0), VehicleDef(), 1)
- veh2 = Vehicle(VehicleState(VecSE2(10.0, 0.0, 0.0), roadway, 5.0), VehicleDef(), 2)
- scene = Scene([veh1, veh2])
+ veh1 = Entity(VehicleState(VecSE2(0.0, 0.0, 0.0), roadway, 10.0), VehicleDef(), 1)
+ veh2 = Entity(VehicleState(VecSE2(10.0, 0.0, 0.0), roadway, 5.0), VehicleDef(), 2)
+ scene = Frame([veh1, veh2])
@test is_collision_free(scene)
@test get_distance(veh1, veh2) ≈ 6.0
end
@@ -77,7 +77,6 @@ end
roadway = get_test_roadway()
trajdata = get_test_trajdata(roadway)
- scene = Scene()
- get!(scene, trajdata, 1)
+ scene = trajdata[1]
@test collision_checker(scene, 1)
-end
\ No newline at end of file
+end
diff --git a/test/test_features.jl b/test/test_features.jl
index ecb1990..5844d28 100644
--- a/test/test_features.jl
+++ b/test/test_features.jl
@@ -1,5 +1,5 @@
@testset "neighbor features" begin
- scene=Frame(Entity{VehicleState, BicycleModel, Int},100)
+ scene=Frame(Entity{VehicleState, BicycleModel, Int}, 100)
roadway=gen_straight_roadway(3, 200.0, lane_width=3.0)
push!(scene,Entity(VehicleState(VecSE2(30.0,3.0,0.0), roadway, 0.0),
BicycleModel(VehicleDef(AgentClass.CAR, 4.826, 1.81)),1))
@@ -23,17 +23,15 @@
@test find_neighbor(scene, roadway, scene[1], lane=rightlane(roadway, scene[1]), rear=true) == NeighborLongitudinalResult(4,10.0)
trajdata = get_test_trajdata(roadway)
- scene = get!(Scene(), trajdata, 1)
+ scene = trajdata[1]
@test find_neighbor(scene, roadway, scene[1]) == NeighborLongitudinalResult(2, 3.0)
- scene = get!(Scene(), trajdata, 1)
@test find_neighbor(scene, roadway, scene[2]) == NeighborLongitudinalResult(nothing, 250.0)
- scene = get!(Scene(), trajdata, 2)
+ scene = trajdata[2]
@test find_neighbor(scene, roadway, scene[1]) == NeighborLongitudinalResult(2, 4.0)
- scene = get!(Scene(), trajdata, 2)
@test find_neighbor(scene, roadway, scene[2]) == NeighborLongitudinalResult(nothing, 250.0)
roadway = gen_stadium_roadway(1)
- scene = Scene(2)
+ scene = Frame(Entity{VehicleState, VehicleDef, Int64}, 2)
scene.n = 2
def = VehicleDef(AgentClass.CAR, 2.0, 1.0)
@@ -43,7 +41,7 @@
roadind = move_along(roadind, roadway, s)
frenet = Frenet(roadind, roadway[roadind].s, 0.0, 0.0)
state = VehicleState(frenet, roadway, 0.0)
- scene[i] = Vehicle(state, def, i)
+ scene[i] = Entity(state, def, i)
end
place_at!(1, 0.0)
@@ -121,8 +119,8 @@ end
@testset "feature extraction" begin
roadway = gen_straight_roadway(4, 100.0)
- scene = Scene([Vehicle(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
+ scene = Frame([Entity(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
])
# test each feature individually
@@ -134,7 +132,7 @@ end
@test pos1[1] == 0.0
@test pos2[2] == 10.0
- scene = Scene([Vehicle(VehicleState(VecSE2(1.1,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1)])
+ scene = Frame([Entity(VehicleState(VecSE2(1.1,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1)])
posy = extract_feature(featuretype(posgy), posgy, roadway, [scene], 1)
@test posy[1] == 1.2
posθ = extract_feature(featuretype(posgθ), posgθ, roadway, [scene], 1)
@@ -147,8 +145,8 @@ end
@test posϕ[1] == 0.0
- scene = Scene([Vehicle(VehicleState(VecSE2(1.1,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(1.5,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2)])
+ scene = Frame([Entity(VehicleState(VecSE2(1.1,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(1.5,1.2,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2)])
coll = extract_feature(featuretype(iscolliding), iscolliding, roadway, [scene], 1)
@test coll[1]
@@ -163,9 +161,9 @@ end
# extract multiple features
roadway = gen_straight_roadway(3, 1000.0, lane_width=1.0)
- scene = Scene([
- Vehicle(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
+ scene = Frame([
+ Entity(VehicleState(VecSE2( 0.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
])
dfs = extract_features((iscolliding, markerdist_left, markerdist_right), roadway, [scene], [1,2])
@@ -187,11 +185,11 @@ end
@test nrow(dfs[id]) == 2
end
- scene = Scene([
- Vehicle(VehicleState(VecSE2( 1.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
- Vehicle(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
- Vehicle(VehicleState(VecSE2(12.0,1.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 3),
- Vehicle(VehicleState(VecSE2( 0.0,1.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 4),
+ scene = Frame([
+ Entity(VehicleState(VecSE2( 1.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 1),
+ Entity(VehicleState(VecSE2(10.0,0.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 2),
+ Entity(VehicleState(VecSE2(12.0,1.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 3),
+ Entity(VehicleState(VecSE2( 0.0,1.0,0.0), roadway, 10.0), VehicleDef(AgentClass.CAR, 5.0, 2.0), 4),
])
dfs= extract_features((dist_to_front_neighbor, front_neighbor_speed, time_to_collision), roadway, [scene], [1,2,3,4])
@test isapprox(dfs[1][1,1], 9.0)
@@ -212,7 +210,7 @@ end # features
num_veh = 7
ego_index = 1
roadway = gen_straight_roadway(4, 400.)
- scene = Scene(num_veh)
+ scene = Frame(Entity{VehicleState,VehicleDef,Int64}, num_veh)
# order: ego, fore, rear, left, right, fore_fore, fore_fore_fore
speeds = [10., 15., 15., 0., -5, 20., 20.]
positions = [200., 220., 150., 200., 200., 240., 350.]
@@ -223,7 +221,7 @@ end # features
veh_state = VehicleState(Frenet(road_idx, roadway), roadway, speeds[i])
veh_state = move_along(veh_state, roadway, positions[i])
veh_def = VehicleDef(AgentClass.CAR, 2., 2.)
- push!(scene, Vehicle(veh_state, veh_def, i))
+ push!(scene, Entity(veh_state, veh_def, i))
end
# basic lidar with sufficient range for all vehicles
diff --git a/test/test_frames.jl b/test/test_frames.jl
new file mode 100644
index 0000000..2a82572
--- /dev/null
+++ b/test/test_frames.jl
@@ -0,0 +1,104 @@
+@testset "Frame" begin
+ @testset begin
+ frame = Frame([1,2,3])
+ @test length(frame) == 3
+ @test capacity(frame) == 3
+ for i in 1 : 3
+ @test frame[i] == i
+ end
+
+ frame = Frame([1,2,3], capacity=5)
+ @test length(frame) == 3
+ @test capacity(frame) == 5
+ for i in 1 : 3
+ @test frame[i] == i
+ end
+
+ @test_throws ErrorException Frame([1,2,3], capacity=2)
+
+ frame = Frame(Int)
+ @test length(frame) == 0
+ @test capacity(frame) > 0
+ @test lastindex(frame) == frame.n
+
+ frame = Frame(Int, 2)
+ @test length(frame) == 0
+ @test capacity(frame) == 2
+
+ frame[1] = 999
+ frame[2] = 888
+
+ @test frame[1] == 999
+ @test frame[2] == 888
+ @test length(frame) == 0 # NOTE: length does not change
+ @test capacity(frame) == 2
+
+ empty!(frame)
+ @test length(frame) == 0
+ @test capacity(frame) == 2
+
+ push!(frame, 999)
+ push!(frame, 888)
+ @test length(frame) == 2
+ @test capacity(frame) == 2
+
+ @test_throws BoundsError push!(frame, 777)
+
+ frame = Frame([999,888])
+ deleteat!(frame, 1)
+ @test length(frame) == 1
+ @test capacity(frame) == 2
+ @test frame[1] == 888
+
+ deleteat!(frame, 1)
+ @test length(frame) == 0
+ @test capacity(frame) == 2
+
+ frame = Frame([1,2,3])
+ frame2 = copy(frame)
+ for i in 1 : 3
+ @test frame[i] == frame2[i]
+ end
+ frame[1] = 999
+ @test frame2[1] == 1
+ end
+
+ @testset begin
+ frame = EntityFrame(Int, Float64, String)
+ frame = EntityFrame(Int, Float64, String, 10)
+ @test eltype(frame) == Entity{Int,Float64,String}
+ @test capacity(frame) == 10
+
+ frame = Frame([Entity(1,1,"A"),Entity(2,2,"B"),Entity(3,3,"C")])
+ @test in("A", frame)
+ @test in("B", frame)
+ @test in("C", frame)
+ @test !in("D", frame)
+ @test findfirst("A", frame) == 1
+ @test findfirst("B", frame) == 2
+ @test findfirst("C", frame) == 3
+ @test findfirst("D", frame) == nothing
+ @test id2index(frame, "A") == 1
+ @test id2index(frame, "B") == 2
+ @test id2index(frame, "C") == 3
+ @test_throws BoundsError id2index(frame, "D")
+
+ frame = Frame([Entity(1,1,"A"),Entity(2,2,"B"),Entity(3,3,"C")])
+ @test get_by_id(frame, "A") == frame[1]
+ @test get_by_id(frame, "B") == frame[2]
+ @test get_by_id(frame, "C") == frame[3]
+
+ delete!(frame, Entity(2,2,"B"))
+ @test frame[1] == Entity(1,1,"A")
+ @test frame[2] == Entity(3,3,"C")
+ @test length(frame) == 2
+
+ delete!(frame, "A")
+ @test frame[1] == Entity(3,3,"C")
+ @test length(frame) == 1
+
+ frame = Frame([Entity(1,1,1),Entity(2,2,2)], capacity=3)
+ @test get_first_available_id(frame) == 3
+
+ end
+end
diff --git a/test/test_idm.jl b/test/test_idm.jl
index 0925f31..863d2db 100644
--- a/test/test_idm.jl
+++ b/test/test_idm.jl
@@ -6,7 +6,7 @@ using Random
roadway = gen_straight_roadway(1, 500.0)
num_veh = 2
-scene = Scene(num_veh)
+scene = Frame(num_veh)
models = Dict{Int, DriverModel}()
@@ -21,7 +21,7 @@ road_idx = RoadIndex(proj(VecSE2(0.0, 0.0, 0.0), roadway))
base_speed = 10.
veh_state = VehicleState(Frenet(road_idx, roadway), roadway, base_speed)
veh_def = VehicleDef(AgentClass.CAR, 5., 2.)
-push!(scene, Vehicle(veh_state, veh_def, 1))
+push!(scene, Entity(veh_state, veh_def, 1))
# 2: second vehicle, in the middle, moving at intermediate speed
mlane = MOBIL(.1, politeness = politeness)
mlon = IntelligentDriverModel(k_spd = k_spd, σ = 0.0)
@@ -31,9 +31,9 @@ road_pos = 8.
veh_state = VehicleState(Frenet(road_idx, roadway), roadway, base_speed)
veh_state = move_along(veh_state, roadway, road_pos)
veh_def = VehicleDef(AgentClass.CAR, 5., 2.)
-push!(scene, Vehicle(veh_state, veh_def, 2))
+push!(scene, Entity(veh_state, veh_def, 2))
-rec = SceneRecord(500, 0.1, num_veh)
+rec = QueueRecord(500, 0.1, num_veh)
prime_time = .2
rng = MersenneTwister(1)
diff --git a/test/test_roadways.jl b/test/test_roadways.jl
index 0fe5cf7..7335185 100644
--- a/test/test_roadways.jl
+++ b/test/test_roadways.jl
@@ -60,25 +60,6 @@ function get_test_roadway()
roadway
end
-@testset "1d roadway" begin
- roadway = StraightRoadway(20.0)
- s = 10.0
- @test mod_position_to_roadway(s, roadway) == s
- s = 25.0
- @test mod_position_to_roadway(s, roadway) == 5.0
- s = 45.0
- @test mod_position_to_roadway(s, roadway) == 5.0
- s = -5.0
- @test mod_position_to_roadway(s, roadway) == 15.0
- s_rear = 10.0
- s_fore = 15.0
- @test get_headway(s_rear, s_fore, roadway) == 5.0
- s_fore = 25.0
- @test get_headway(s_rear, s_fore, roadway) == 15.0
- s_fore = 5.0
- @test get_headway(s_rear, s_fore, roadway) == 15.0
-end
-
@testset "Curves" begin
p = lerp(CurvePt(VecSE2(0.0,0.0,0.0), 0.0), CurvePt(VecSE2(1.0,2.0,3.0), 4.0), 0.25)
show(IOBuffer(), p)
@@ -497,7 +478,7 @@ end # roadway test
############
path, io = mktemp()
- write(io, MIME"text/plain"(), roadway)
+ write(io, roadway)
close(io)
lines = open(readlines, path)
@@ -554,7 +535,7 @@ end # roadway test
end
io = open(path)
- roadway2 = read(io, MIME"text/plain"(), Roadway)
+ roadway2 = read(io, Roadway)
close(io)
rm(path)
diff --git a/test/test_simulation.jl b/test/test_simulation.jl
index 2cfccec..fdc94a0 100644
--- a/test/test_simulation.jl
+++ b/test/test_simulation.jl
@@ -12,56 +12,52 @@ AutomotiveDrivingModels.run_callback(callback::WithActionCallback, scenes::Vecto
models[2] = IntelligentDriverModel(k_spd = 1.0, v_des = 5.0)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 5.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 70.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
+ scene = Frame([veh1, veh2])
n_steps = 40
dt = 0.1
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, n_steps)
- @test_deprecated simulate!(scene, roadway, models, n_steps, dt)
@inferred simulate(scene, roadway, models, n_steps, dt)
reset_hidden_states!(models)
# initializing vehicles too close
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 10.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 5.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
-
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, 10, (CollisionCallback(),))
+ scene = Frame([veh1, veh2])
scenes = @inferred simulate(scene, roadway, models, n_steps, dt, callbacks=(CollisionCallback(),))
@test length(scenes) < 10
+ open("test.txt", "w+") do io
+ write(io, scenes)
+ end
+ r = open("test.txt", "r") do io
+ read(io, typeof(scenes))
+ end
+ @test length(r) == length(scenes)
+ for (i, s) in enumerate(r)
+ for (j, veh) in enumerate(r[i])
+ @test posg(veh) ≈ posg(scenes[i][j])
+ end
+ end
+
# make sure warnings, errors and deprecations in run_callback work as expected
- @test_deprecated @test_throws MethodError simulate!(rec, scene, roadway, models, 10, (NoCallback(),))
- @test_deprecated simulate(scene, roadway, models, 10, .1, callbacks=(NoActionCallback(),))
@test_nowarn simulate(scene, roadway, models, 10, .1, callbacks=(WithActionCallback(),))
# collision right from start
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 0.0), roadway, 10.)
- veh1 = Vehicle(veh_state, VehicleDef(), 1)
+ veh1 = Entity(veh_state, VehicleDef(), 1)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 1.0), roadway, 5.)
- veh2 = Vehicle(veh_state, VehicleDef(), 2)
-
- scene = Scene()
- push!(scene, veh1)
- push!(scene, veh2)
+ veh2 = Entity(veh_state, VehicleDef(), 2)
- rec = SceneRecord(n_steps, dt)
- @test_deprecated simulate!(rec, scene, roadway, models, 10, (CollisionCallback(),))
+ scene = Frame([veh1, veh2])
scenes = @inferred simulate(scene, roadway, models, n_steps, dt, callbacks=(CollisionCallback(),))
@test length(scenes) == 1
@@ -72,10 +68,10 @@ end
trajdata = get_test_trajdata(roadway)
veh_state = VehicleState(Frenet(roadway[LaneTag(1,1)], 6.0), roadway, 10.)
- ego = Vehicle(veh_state, VehicleDef(), 2)
+ ego = Entity(veh_state, VehicleDef(), 2)
model = ProportionalSpeedTracker()
- dt = get_timestep(trajdata)
- rec = SceneRecord(3, dt)
- simulate!(rec, model, ego.id, trajdata, roadway, 1, 2)
- @test findfirst(ego.id, rec[0]) != nothing
+
+ scenes = simulate_from_history(model, roadway, trajdata, ego.id, 0.1)
+
+ @test findfirst(ego.id, scenes[end]) != nothing
end
diff --git a/test/test_states.jl b/test/test_states.jl
index 5b0c4b3..50f819c 100644
--- a/test/test_states.jl
+++ b/test/test_states.jl
@@ -1,42 +1,17 @@
function get_test_trajdata(roadway::Roadway)
- trajdata = Trajdata(0.1)
-
- trajdata.defs[1] = VehicleDef(AgentClass.CAR, 5.0, 3.0)
- trajdata.defs[2] = VehicleDef(AgentClass.CAR, 5.0, 3.0)
-
- push!(trajdata.states, RecordState{VehicleState, Int64}(VehicleState(VecSE2(0.0,0.0,0.0), roadway, 10.0), 1)) # car 1, frame 1
- push!(trajdata.states, RecordState{VehicleState, Int64}(VehicleState(VecSE2(3.0,0.0,0.0), roadway, 20.0), 2)) # car 2, frame 1
- push!(trajdata.states, RecordState{VehicleState, Int64}(VehicleState(VecSE2(1.0,0.0,0.0), roadway, 10.0), 1)) # car 1, frame 2
- push!(trajdata.states, RecordState{VehicleState, Int64}(VehicleState(VecSE2(5.0,0.0,0.0), roadway, 20.0), 2)) # car 2, frame 2
-
- push!(trajdata.frames, RecordFrame(1,2))
- push!(trajdata.frames, RecordFrame(3,4))
-
- trajdata
-end
-
-@testset "1d state" begin
- s = State1D(0.0, 0.0)
- path, io = mktemp()
- write(io, MIME"text/plain"(), s)
- close(io)
- io = open(path)
- s2 = read(io, MIME"text/plain"(), State1D)
- close(io)
- @test s == s2
-
- veh = Vehicle1D(s, VehicleDef(), 1)
- scene = Scene1D()
- push!(scene, veh)
- scene2 = Scene1D([veh])
- @test scene[1].state.s == 0.0
- @test first(scene2.entities) == first(scene.entities)
- @test scene2.n == scene.n == 1
- @test get_center(veh) == 0.0
- @test get_footpoint(veh) == 0.0
- @test get_front(veh) == veh.def.length/2
- @test get_rear(veh) == - veh.def.length/2
-
+ scene1 = Frame([
+ Entity(VehicleState(VecSE2(0.0,0.0,0.0), roadway, 10.0), VehicleDef(), 1),
+ Entity(VehicleState(VecSE2(3.0,0.0,0.0), roadway, 20.0), VehicleDef(), 2)
+ ]
+ )
+ scene2 = Frame([
+ Entity(VehicleState(VecSE2(1.0,0.0,0.0), roadway, 10.0), VehicleDef(), 1),
+ Entity(VehicleState(VecSE2(5.0,0.0,0.0), roadway, 20.0), VehicleDef(), 2)
+ ]
+ )
+
+ trajdata = [scene1, scene2]
+ return trajdata
end
@testset "VehicleState" begin
@@ -47,8 +22,6 @@ end
show(IOBuffer(), s)
s = VehicleState(VecSE2(0.0,0.0,0.0), Frenet(NULL_ROADINDEX, 0.0, 0.0, 0.1), 10.0)
- @test isapprox(get_vel_s(s), 10.0*cos(0.1))
- @test isapprox(get_vel_t(s), 10.0*sin(0.1))
@test isapprox(velf(s).s, 10.0*cos(0.1))
@test isapprox(velf(s).t, 10.0*sin(0.1))
@test isapprox(velg(s).x, 10.0)
@@ -56,7 +29,7 @@ end
@test isapprox(vel(s), 10.0)
vehdef = VehicleDef(AgentClass.CAR, 5.0, 3.0)
- veh = Vehicle(s, vehdef, 1)
+ veh = Entity(s, vehdef, 1)
@test isapprox(get_footpoint(veh), VecSE2(0.0,0.0,0.0))
show(IOBuffer(), vehdef)
show(IOBuffer(), veh)
@@ -70,7 +43,7 @@ end
roadway = gen_straight_roadway(3, 100.0)
veh = VehicleState(VecSE2(0.0, 0.0, 0.0), roadway, 0.0)
- veh = Vehicle(veh, VehicleDef(), 1)
+ veh = Entity(veh, VehicleDef(), 1)
@test get_lane(roadway, veh).tag == LaneTag(1,1)
@test get_lane(roadway, veh).tag == get_lane(roadway, veh.state).tag
veh = VehicleState(VecSE2(0.0, 3.0, 0.0), roadway, 0.0)
@@ -92,10 +65,9 @@ end
vehstate = VehicleState(VecSE2(0.0, 0.0, 0.0), roadway, 0.0)
vehstate1 = VehicleState(VecSE2(0.0, 0.0, 0.0), roadway[LaneTag(1,1)], roadway, 0.0)
@test vehstate1 == vehstate
- veh = Vehicle(vehstate, VehicleDef(), 1)
- scene1 = Scene()
- push!(scene1, veh)
- scene2 = Scene([veh])
+ veh = Entity(vehstate, VehicleDef(), 1)
+ scene1 = Frame([veh])
+ scene2 = Frame([veh])
@test first(scene1.entities) == first(scene2.entities)
@test scene1.n == scene2.n
@@ -104,28 +76,27 @@ end
close(io)
veh2 = Entity(vehstate, BicycleModel(VehicleDef()), 1)
- veh3 = convert(Vehicle, veh2)
+ veh3 = convert(Entity{VehicleState, VehicleDef, Int64}, veh2)
@test veh3 == veh
- rec = SceneRecord(1, 0.5)
- rec = SceneRecord(1, 0.5, 10)
+ scene = Frame([veh, veh2, veh3])
io = IOBuffer()
- show(io, rec)
+ show(io, scene)
close(io)
- scene = Scene()
- get!(scene, trajdata, 1)
+ scene = Frame(typeof(veh))
+ copyto!(scene, trajdata[1])
@test length(scene) == 2
for (i,veh) in enumerate(scene)
- @test scene[i].state == get_state(trajdata, i, 1)
- @test scene[i].def == get_def(trajdata, i)
+ @test scene[i].state == trajdata[1][i].state
+ @test scene[i].def == trajdata[1][i].def
end
- scene2 = Scene(deepcopy(scene.entities), 2)
+ scene2 = Frame(deepcopy(scene.entities), 2)
@test length(scene2) == 2
for (i,veh) in enumerate(scene2)
- @test scene2[i].state == get_state(trajdata, i, 1)
- @test scene2[i].def == get_def(trajdata, i)
+ @test scene2[i].state == trajdata[1][i].state
+ @test scene2[i].def == trajdata[1][i].def
end
@test get_by_id(scene, 1) == scene[1]
@@ -136,17 +107,17 @@ end
copyto!(scene2, scene)
@test length(scene2) == 2
for (i,veh) in enumerate(scene2)
- @test scene2[i].state == get_state(trajdata, i, 1)
- @test scene2[i].def == get_def(trajdata, i)
+ @test scene2[i].state == trajdata[1][i].state
+ @test scene2[i].def == trajdata[1][i].def
end
delete!(scene2, scene2[1])
@test length(scene2) == 1
- @test scene2[1].state == get_state(trajdata, 2, 1)
- @test scene2[1].def == get_def(trajdata, 2)
+ @test scene2[1].state == trajdata[1][2].state
+ @test scene2[1].def == trajdata[1][2].def
scene2[1] = deepcopy(scene[1])
- @test scene2[1].state == get_state(trajdata, 1, 1)
- @test scene2[1].def == get_def(trajdata, 1)
+ @test scene2[1].state == trajdata[1][1].state
+ @test scene2[1].def == trajdata[1][1].def
@test findfirst(1, scene) == 1
@test findfirst(2, scene) == 2
@@ -157,217 +128,8 @@ end
@test !in(3, scene)
veh = scene[2]
- @test veh.state == get_state(trajdata, 2, 1)
- @test veh.def == get_def(trajdata, 2)
-
- push!(scene, get_state(trajdata, 1, 1))
-end
-
-@testset "trajdata" begin
- roadway = get_test_roadway()
- trajdata = get_test_trajdata(roadway)
-
- @test nframes(trajdata) == 2
- @test !frame_inbounds(trajdata, 0)
- @test frame_inbounds(trajdata, 1)
- @test frame_inbounds(trajdata, 2)
- @test !frame_inbounds(trajdata, 3)
-
- @test n_objects_in_frame(trajdata, 1) == 2
- @test n_objects_in_frame(trajdata, 2) == 2
-
- @test nth_id(trajdata, 1) == 1
- @test nth_id(trajdata, 1, 2) == 2
- @test nth_id(trajdata, 2, 1) == 1
- @test nth_id(trajdata, 2, 2) == 2
-
- @test findfirst_frame_with_id(trajdata, 1) == 1
- @test findfirst_frame_with_id(trajdata, 2) == 1
- @test findfirst_frame_with_id(trajdata, -1) == nothing
- @test findlast_frame_with_id(trajdata, 1) == 2
- @test findlast_frame_with_id(trajdata, 2) == 2
- @test findlast_frame_with_id(trajdata, -1) == nothing
-
- @test sort!(get_ids(trajdata)) == [1,2]
-
- @test in(1, trajdata, 1)
- @test in(1, trajdata, 2)
- @test in(2, trajdata, 1)
- @test in(2, trajdata, 2)
- @test !in(3, trajdata, 1)
-
- @test isapprox(get_time(trajdata, 1), 0.0)
- @test isapprox(get_time(trajdata, 2), 0.1)
-
- @test isapprox(get_elapsed_time(trajdata, 1, 2), 0.1)
- @test isapprox(get_elapsed_time(trajdata, 2, 1), -0.1)
-
- @test get_timestep(trajdata) == 0.1
-
- veh = get(trajdata, 1, 1)
- @test veh.state == VehicleState(VecSE2(0.0,0.0,0.0), roadway, 10.0)
- @test_throws ArgumentError get(trajdata, 10, 1)
- @test_throws BoundsError get(trajdata, 1, 10)
-
- let
- iter = ListRecordStateByIdIterator(trajdata, 1)
- items = collect(iter) # list of (frame_index, state)
- @test length(items) == 2
- @test items[1][1] == 1
- @test items[1][2] == get_state(trajdata, 1, 1)
- @test items[2][1] == 2
- @test items[2][2] == get_state(trajdata, 1, 2)
-
- iter = ListRecordStateByIdIterator(trajdata, 2)
- items = collect(iter)
- @test length(items) == 2
- @test items[1][1] == 1
- @test items[1][2] == get_state(trajdata, 2, 1)
- @test items[2][1] == 2
- @test items[2][2] == get_state(trajdata, 2, 2)
- end
-
- path, io = mktemp()
- write(io, MIME"text/plain"(), trajdata)
- close(io)
-
- io = open(path)
- trajdata2 = read(io, MIME"text/plain"(), Trajdata)
- close(io)
- rm(path)
-
- @test nframes(trajdata2) == nframes(trajdata)
- for i in 1 : nframes(trajdata2)
- @test n_objects_in_frame(trajdata2, i) == n_objects_in_frame(trajdata, i)
- for j in 1 : n_objects_in_frame(trajdata, i)
- veh1 = get(trajdata, j, i)
- veh2 = get(trajdata2, j, i)
- @test veh1.id == veh2.id
- @test veh1.def.class == veh2.def.class
- @test isapprox(veh1.def.length, veh2.def.length)
- @test isapprox(veh1.def.width, veh2.def.width)
-
- @test isapprox(veh1.state.v, veh2.state.v)
- @test isapprox(veh1.state.posG, veh2.state.posG, atol=1e-3)
- @test isapprox(veh1.state.posF.s, veh2.state.posF.s, atol=1e-3)
- @test isapprox(veh1.state.posF.t, veh2.state.posF.t, atol=1e-3)
- @test isapprox(veh1.state.posF.ϕ, veh2.state.posF.ϕ, atol=1e-6)
- @test veh1.state.posF.roadind.tag == veh2.state.posF.roadind.tag
- @test veh1.state.posF.roadind.ind.i == veh2.state.posF.roadind.ind.i
- @test isapprox(veh1.state.posF.roadind.ind.t, veh2.state.posF.roadind.ind.t, atol=1e-3)
- end
- end
-
- trajdata3 = get_subinterval(trajdata2, 1, nframes(trajdata2))
- @test nframes(trajdata3) == nframes(trajdata2)
- for i in 1 : nframes(trajdata3)
- @test n_objects_in_frame(trajdata3, i) == n_objects_in_frame(trajdata2, i)
- for j in 1 : n_objects_in_frame(trajdata2, i)
- veh1 = get(trajdata2, j, i)
- veh2 = get(trajdata3, j, i)
- @test veh1.id == veh2.id
- @test veh1.def.class == veh2.def.class
- @test isapprox(veh1.def.length, veh2.def.length)
- @test isapprox(veh1.def.width, veh2.def.width)
-
- @test isapprox(veh1.state.v, veh2.state.v)
- @test isapprox(veh1.state.posG, veh2.state.posG, atol=1e-3)
- @test isapprox(veh1.state.posF.s, veh2.state.posF.s, atol=1e-3)
- @test isapprox(veh1.state.posF.t, veh2.state.posF.t, atol=1e-3)
- @test isapprox(veh1.state.posF.ϕ, veh2.state.posF.ϕ, atol=1e-6)
- @test veh1.state.posF.roadind.tag == veh2.state.posF.roadind.tag
- @test veh1.state.posF.roadind.ind.i == veh2.state.posF.roadind.ind.i
- @test isapprox(veh1.state.posF.roadind.ind.t, veh2.state.posF.roadind.ind.t, atol=1e-3)
- end
- end
-
- trajdata3 = get_subinterval(trajdata2, 1, 1)
- @test nframes(trajdata3) == 1
- let
- i = 1
- @test n_objects_in_frame(trajdata3, i) == n_objects_in_frame(trajdata2, i)
- for j in 1 : n_objects_in_frame(trajdata2, i)
- veh1 = get(trajdata2, j, i)
- veh2 = get(trajdata3, j, i)
- @test veh1.id == veh2.id
- @test veh1.def.class == veh2.def.class
- @test isapprox(veh1.def.length, veh2.def.length)
- @test isapprox(veh1.def.width, veh2.def.width)
-
- @test isapprox(veh1.state.v, veh2.state.v)
- @test isapprox(veh1.state.posG, veh2.state.posG, atol=1e-3)
- @test isapprox(veh1.state.posF.s, veh2.state.posF.s, atol=1e-3)
- @test isapprox(veh1.state.posF.t, veh2.state.posF.t, atol=1e-3)
- @test isapprox(veh1.state.posF.ϕ, veh2.state.posF.ϕ, atol=1e-6)
- @test veh1.state.posF.roadind.tag == veh2.state.posF.roadind.tag
- @test veh1.state.posF.roadind.ind.i == veh2.state.posF.roadind.ind.i
- @test isapprox(veh1.state.posF.roadind.ind.t, veh2.state.posF.roadind.ind.t, atol=1e-3)
- end
- end
-end
-
-@testset "SceneRecord" begin
- roadway = get_test_roadway()
- trajdata = get_test_trajdata(roadway)
-
- Δt = 0.1
- rec = SceneRecord(5, Δt)
- @test capacity(rec) == 5
- @test nframes(rec) == 0
- @test !pastframe_inbounds(rec, 0)
- @test !pastframe_inbounds(rec, -1)
- @test !pastframe_inbounds(rec, 1)
-
- scene = get!(Scene(), trajdata, 1)
- update!(rec, scene)
- @test nframes(rec) == 1
- @test pastframe_inbounds(rec, 0)
- @test !pastframe_inbounds(rec, -1)
- @test !pastframe_inbounds(rec, 1)
- @test isapprox(get_elapsed_time(rec, 0), 0)
- @test rec[0][1].state == get_state(trajdata, 1, 1)
- @test rec[0][1].def == get_def(trajdata, 1)
- @test rec[0][2].state == get_state(trajdata, 2, 1)
- @test rec[0][2].def == get_def(trajdata, 2)
- show(IOBuffer(), rec)
-
-
- get!(scene, trajdata, 2)
- update!(rec, scene)
- @test nframes(rec) == 2
- @test pastframe_inbounds(rec, 0)
- @test pastframe_inbounds(rec, -1)
- @test !pastframe_inbounds(rec, 1)
- @test isapprox(get_elapsed_time(rec, 0), 0)
- @test isapprox(get_elapsed_time(rec, -1), Δt)
- @test isapprox(get_elapsed_time(rec, -1, 0), Δt)
- @test rec[0][1].state == get_state(trajdata, 1, 2)
- @test rec[0][1].def == get_def(trajdata, 1)
- @test rec[0][2].state == get_state(trajdata, 2, 2)
- @test rec[0][2].def == get_def(trajdata, 2)
- @test rec[-1][1].state == get_state(trajdata, 1, 1)
- @test rec[-1][1].def == get_def(trajdata, 1)
- @test rec[-1][2].state == get_state(trajdata, 2, 1)
- @test rec[-1][2].def == get_def(trajdata, 2)
-
- scene2 = get!(Scene(), rec)
- @test scene2[1].state == get_state(trajdata, 1, 2)
- @test scene2[1].def == get_def(trajdata, 1)
- @test scene2[2].state == get_state(trajdata, 2, 2)
- @test scene2[2].def == get_def(trajdata, 2)
-
- get!(scene2, rec, -1)
- @test scene2[1].state == get_state(trajdata, 1, 1)
- @test scene2[1].def == get_def(trajdata, 1)
- @test scene2[2].state == get_state(trajdata, 2, 1)
- @test scene2[2].def == get_def(trajdata, 2)
-
- empty!(rec)
- @test nframes(rec) == 0
+ @test veh.state == trajdata[1][2].state
+ @test veh.def == trajdata[1][2].def
- test_veh_state = VehicleState(VecSE2(7.0,7.0,2.0), roadway, 10.0)
- test_veh_def = VehicleDef(AgentClass.CAR, 5.0, 3.0)
- test_veh = Vehicle(test_veh_state, test_veh_def, 999)
- rec[-1][1] = test_veh
- @test rec[-1][1].state == test_veh_state
+ push!(scene, trajdata[1][1].state)
end