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