Merge bb44f40 into 310c088

examples/Tintersection.jl

@@ -1,6 +1,6 @@
-# This example shows the T-intersection scenario -- nominal and failure scenario
 using AdversarialDriving
 using POMDPs, POMDPPolicies, POMDPSimulators
+using AutomotiveVisualization
 
 sut_agent = BlinkerVehicleAgent(up_left(id=1, s=25., v=15.), TIDM(Tint_TIDM_template, noisy_observations = true))
 adv1 = BlinkerVehicleAgent(left_straight(id=2, s=0., v=15.0), TIDM(Tint_TIDM_template))
@@ -10,6 +10,14 @@ adv4 = BlinkerVehicleAgent(right_turnleft(id=5, s=30., v=20.0), TIDM(Tint_TIDM_t
 
 mdp = AdversarialDrivingMDP(sut_agent, [adv1, adv2, adv3, adv4], Tint_roadway, 0.1)
 
+
+s = rand(initialstate(mdp))
+cam = StaticCamera(position=(-10,-10), zoom=10.)
+c = render([mdp.roadway, s,
+        FancyCar(car = s[4], color = colorant"blue")
+                ], camera = cam, surface=AutomotiveVisualization.CairoPDFSurface(IOBuffer(), AutomotiveVisualization.canvas_width(cam), AutomotiveVisualization.canvas_height(cam)))
+write("T_intersection.pdf", c)
+
 null_action = actions(mdp)[1]
 blinker_action = actions(mdp)[7]
 

examples/ped_crosswalk.jl

@@ -1,19 +1,30 @@
-# This example shows the pedestrain and crosswalk scenario -- nominal and failure scenario
 using AdversarialDriving
 using POMDPs, POMDPPolicies, POMDPSimulators
+using AutomotiveVisualization
 
 sut_agent = BlinkerVehicleAgent(get_ped_vehicle(id=1, s=5., v=15.), TIDM(ped_TIDM_template, noisy_observations = true))
 adv_ped = NoisyPedestrianAgent(get_pedestrian(id=2, s=7., v=2.0), AdversarialPedestrian())
-mdp = AdversarialDrivingMDP(sut_agent, [adv_ped], ped_roadway, 0.1)
+mdp = AdversarialDrivingMDP(sut_agent, [adv_ped], ped_roadway, 0.2)
+
+# Render a single frame
+s = rand(initialstate(mdp))
+c = render([mdp.roadway,
+                crosswalk,
+                VelocityArrow(entity = s[1], color = colorant"black"),
+                VelocityArrow(entity = s[2], color = colorant"black"),
+                FancyCar(car = s[2], color = colorant"blue"),
+                FancyPedestrian(ped = s[1])],
+                surface=AutomotiveVisualization.CairoPDFSurface(IOBuffer(), DEFAULT_CANVAS_WIDTH, DEFAULT_CANVAS_HEIGHT))
+write("pedestrian_crosswalk.pdf", c)
 
 null_action = Disturbance[PedestrianControl()]
 noisy_action = Disturbance[PedestrianControl(noise = Noise((-10.,0.), -2))]
 
 # Nominal Behavior
 hist = POMDPSimulators.simulate(HistoryRecorder(), mdp, FunctionPolicy((s) -> null_action))
-scenes_to_gif(state_hist(hist), mdp.roadway, "ped_crosswalk_nominal.gif", others = [crosswalk])
+scenes_to_gif([s.s for s in hist], mdp.roadway, "ped_crosswalk_nominal.gif", others = [crosswalk])
 
 # Behavior with noise
 hist = POMDPSimulators.simulate(HistoryRecorder(), mdp, FunctionPolicy((s) -> noisy_action))
-scenes_to_gif(state_hist(hist), mdp.roadway, "ped_crosswalk_failure.gif", others = [crosswalk])
+scenes_to_gif([s.s for s in hist], mdp.roadway, "ped_crosswalk_failure.gif", others = [crosswalk])
 

examples/ped_crosswalk_with_blindspot.jl

@@ -2,15 +2,28 @@ using AdversarialDriving
 using AutomotiveSimulator, AutomotiveVisualization
 using POMDPs, POMDPPolicies, POMDPSimulators
 
-blindspot = Blindspot(π/12 + 0.07, π/6)
+blindspot = Blindspot(π/12 - 0.05, π/13)
 sut_agent = BlinkerVehicleAgent(get_ped_vehicle(id=1, s=5., v=15.), TIDM(ped_TIDM_template, noisy_observations = true, blindspot = blindspot))
-adv_ped = NoisyPedestrianAgent(get_pedestrian(id=2, s=10.7, v=0.), AdversarialPedestrian(idm=IntelligentDriverModel(v_des = 0)))
+adv_ped = NoisyPedestrianAgent(get_pedestrian(id=2, s=10.8, v=0.), AdversarialPedestrian(idm=IntelligentDriverModel(v_des = 0)))
 mdp = AdversarialDrivingMDP(sut_agent, [adv_ped], ped_roadway, 0.1)
 
-# Sample blindspot behavior
-hist = POMDPSimulators.simulate(HistoryRecorder(max_steps = 25), mdp, FunctionPolicy((s) -> Disturbance[PedestrianControl()]))
 
 # Make the renderable object to visualize the blindspot
-rb = (s) -> RenderableBlindspot(posg(get_by_id(s, sutid(mdp))), sut(mdp).model.blindspot, 30, colorant"blue")
-scenes_to_gif(state_hist(hist), mdp.roadway, "blindspot.gif", others = [crosswalk], others_fn = [rb])
+rb = (s) -> RenderableBlindspot(posg(get_by_id(s, sutid(mdp))), sut(mdp).model.blindspot, 30, colorant"black")
+
+# Render a single frame
+s = rand(initialstate(mdp))
+c = render([mdp.roadway,
+                crosswalk,
+                rb(s),
+                FancyCar(car = s[2], color = colorant"blue"),
+                FancyPedestrian(ped = s[1])],
+                surface=AutomotiveVisualization.CairoPDFSurface(IOBuffer(), DEFAULT_CANVAS_WIDTH, DEFAULT_CANVAS_HEIGHT))
+write("blindspot.pdf", c)
+
+# Example blindspot behavior
+hist = POMDPSimulators.simulate(HistoryRecorder(max_steps = 25), mdp, FunctionPolicy((s) -> Disturbance[PedestrianControl()]))
+
+# Write to gif
+scenes_to_gif([s.s for s in hist], mdp.roadway, "blindspot.gif", others = [crosswalk], others_fn = [rb])
 

src/AdversarialDriving.jl

@@ -2,6 +2,7 @@ module AdversarialDriving
     using POMDPs
     using POMDPPolicies
     using POMDPSimulators
+    using POMDPModelTools
     using AutomotiveSimulator
     using AutomotiveVisualization
     using Distributions
@@ -31,7 +32,8 @@ module AdversarialDriving
     # MDP
     export Agent, BlinkerVehicleAgent, NoisyPedestrianAgent, id,
            AdversarialDrivingMDP, action_probability, step_scene,
-           agents, adversaries, model, sut, sutid, update_adversary!, DrivingMDP
+           agents, adversaries, model, sut, sutid, update_adversary!, DrivingMDP,
+           default_policy
     include("mdp.jl")
 
     # states
@@ -43,7 +45,8 @@ module AdversarialDriving
     export PEDESTRIAN_DISTURBANCE_DIM, PedestrianControl_to_vec, vec_to_PedestrianControl,
            BLINKERVEHICLE_DISTURBANCE_DIM, BlinkerVehicleControl_to_vec, vec_to_BlinkerVehicleControl,
            BV_ACTIONS, BV_ACTION_PROB, get_actions, get_actionindex, DiscreteActionModel,
-           combine_disturbance_models, combine_continuous, combine_discrete, get_bv_actions, get_rand_bv_actions
+           combine_disturbance_models, combine_continuous, combine_discrete,
+           get_bv_actions, get_rand_bv_actions, get_ped_actions
     include("actions.jl")
 
     #helpers

src/actions.jl

@@ -131,14 +131,18 @@ end
 
 ## default actions
 # Function to setup default actions for the blinker verhicle
-get_bv_actions(med_accel = 1.5, large_accel = 3.0, med_prob = 1e-2, large_prob = 1e-3) = DiscreteActionModel(
+get_bv_actions(med_accel = 1.5, large_accel = 3.0, med_prob = 1e-3, large_prob = 1e-4) = DiscreteActionModel(
                       Vector{Vector{Disturbance}}([ [BlinkerVehicleControl(0, 0., false, false, Noise())],
-                        [BlinkerVehicleControl(0, -large_accel, false, false, Noise())],
-                        [BlinkerVehicleControl(0, -med_accel, false, false, Noise())],
-                        [BlinkerVehicleControl(0, med_accel, false, false, Noise())],
-                        [BlinkerVehicleControl(0, large_accel, false, false, Noise())],
-                        [BlinkerVehicleControl(0, 0., true, false, Noise())], # toggle goal
-                        [BlinkerVehicleControl(0, 0., false, true, Noise())] # toggle blinker
+                        [BlinkerVehicleControl(da = -large_accel)],
+                        [BlinkerVehicleControl(da = -med_accel)],
+                        [BlinkerVehicleControl(da = med_accel)],
+                        [BlinkerVehicleControl(da = large_accel)],
+                        [BlinkerVehicleControl(toggle_goal = true)], # toggle goal
+                        [BlinkerVehicleControl(toggle_blinker = true)], # toggle blinker
+                        # [BlinkerVehicleControl(noise = Noise(pos = VecE2(-1, 0.)))],
+                        # [BlinkerVehicleControl(noise = Noise(pos = VecE2(1, 0.)))],
+                        # [BlinkerVehicleControl(noise = Noise(vel = -2))],
+                        # [BlinkerVehicleControl(noise = Noise(vel = 2))]
                        ]),
                         [1 - (4*large_prob + 2*med_prob), large_prob, med_prob, med_prob, large_prob, large_prob, large_prob]
                         )
@@ -166,3 +170,19 @@ end
 
 
 # TODO Add default distributions as needed
+
+get_ped_actions(accel = 1., noise_pos = 1., p = 1e-2) = DiscreteActionModel(
+                      Vector{Vector{Disturbance}}([
+                        [PedestrianControl()],
+                        [PedestrianControl(da = VecE2(accel, 0.))],
+                        [PedestrianControl(da = VecE2(-accel, 0.))],
+                        [PedestrianControl(da = VecE2(0., accel))],
+                        [PedestrianControl(da = VecE2(0., -accel))],
+                        # [PedestrianControl(noise = Noise(pos = VecE2(-noise_pos, 0.)))],
+                        # [PedestrianControl(noise = Noise(pos = VecE2(noise_pos, 0.)))],
+                        # [PedestrianControl(noise = Noise(pos = VecE2(0., noise_pos)))],
+                        # [PedestrianControl(noise = Noise(pos = VecE2(0., -noise_pos)))],
+                       ]),
+                        [1 - (4*p), p, p, p, p]#, p, p, p, p]
+                        )
+

src/driving_models.jl

@@ -62,7 +62,7 @@ end
 function noisy_entity(ent, roadway::Roadway)
     f = posf(ent)
     Δs, Δt = noise(ent).pos
-    noisy_f = Frenet(get_lane(roadway, ent), f.s + Δs, f.t + Δt, f.ϕ)
+    noisy_f = Frenet(roadway[f.roadind.tag], f.s + Δs, f.t + Δt, f.ϕ)
     noisy_g = posg(noisy_f, roadway)
     noisy_v = vel(ent) + noise(ent).vel
     noisy_vs = VehicleState(noisy_g, noisy_f, noisy_v)
@@ -130,6 +130,7 @@ function AutomotiveSimulator.propagate(ped::Entity{NoisyPedState, D, I}, action:
     vs_entity = Entity(ped.state.veh_state, ped.def, ped.id)
     a_lat_lon = reverse(action.a + action.da)
     vs = propagate(vs_entity, LatLonAccel(a_lat_lon...), roadway, Δt)
+    vs = VehicleState(vs.posG, vs.posF, clamp(vs.v, -3., 3.)) # Max pedestrian speed
     nps = NoisyPedState(set_lane(vs, laneid(ped), roadway), action.noise)
     @assert starting_lane == laneid(nps)
     nps
@@ -166,14 +167,19 @@ end
 
 # Define the wrapper for the adversarial pedestrian
 @with_kw mutable struct AdversarialPedestrian <: DriverModel{PedestrianControl}
-    idm::IntelligentDriverModel = IntelligentDriverModel(v_des= 2.0)
+    idm::IntelligentDriverModel = IntelligentDriverModel(v_des= 1.0)
     next_action::PedestrianControl = PedestrianControl()
+    ignore_idm = false
 end
 
 # Sample an action from AdversarialPedestrian model
 function Base.rand(rng::AbstractRNG, model::AdversarialPedestrian)
     na = model.next_action
-    PedestrianControl((model.idm.a, 0), na.da, na.noise)
+    if !model.ignore_idm
+        return PedestrianControl((model.idm.a, 0), na.da, na.noise)
+    else
+        return na
+    end
 end
 
 # Observe function for AdversarialPedestrian model
@@ -226,7 +232,7 @@ end
 
 # Define a driving model for a T-intersection IDM model
 @with_kw mutable struct TIDM <: DriverModel{BlinkerVehicleControl}
-    idm::IntelligentDriverModel = IntelligentDriverModel() # underlying idm
+    idm::IntelligentDriverModel = IntelligentDriverModel(v_des = 15.) # underlying idm
     noisy_observations::Bool = false # Whether or not this model gets noisy observations
     ttc_threshold = 7 # threshold through intersection
     next_action::BlinkerVehicleControl = BlinkerVehicleControl() # The next action that the model will do (for controllable vehicles)
@@ -259,23 +265,35 @@ function AutomotiveSimulator.observe!(model::TIDM, input_scene::Scene, roadway::
 
     # Compute ego headway to intersection and time to cross
     intrsxn_Δs = distance_to_point(ego, roadway, model.intersection_enter_loc[li])
+    intrsxn_exit_Δs = distance_to_point(ego, roadway, model.intersection_exit_loc[li])
     time_to_cross = time_to_cross_distance_const_acc(ego, model.idm, distance_to_point(ego, roadway, model.intersection_exit_loc[li]))
 
     # Get headway to the forward car
     fore = find_neighbor(scene, roadway, ego, targetpoint_ego = VehicleTargetPointFront(), targetpoint_neighbor = VehicleTargetPointRear())
-    fore_v, fore_Δs = isnothing(fore.ind) ? (NaN, NaN) : (vel(scene[fore.ind]), fore.Δs)
+    fore_v, fore_Δs = isnothing(fore.ind) ? (NaN, Inf) : (vel(scene[fore.ind]), fore.Δs)
 
     # Check to see if ego car has right of way
     has_right_of_way = true
     lanes_to_yield_to = model.yields_way[li]
     vehicles_to_yield_to = []
     for (i,veh) in enumerate(scene)
-        # Check if the other entity is in the blind spot.
-        if !isnothing(model.blindspot) && in_blindspot(posg(ego), model.blindspot, posg(veh))
-            println("here!")
-            continue;
+        # if the vehicle is the ego vehicle then move on
+        veh.id == egoid && continue
+
+        # Check if the other entity is in the blind spot. If so, move one
+        !isnothing(model.blindspot) && in_blindspot(posg(ego), model.blindspot, posg(veh)) && continue
+
+        # Check to see if the vehicle is in the ego's lane without the same laneid
+        if laneid(ego) != laneid(veh)
+            Δs_inlane = compute_inlane_headway(ego, veh, roadway)
+            if Δs_inlane < fore_Δs
+                fore_Δs = Δs_inlane
+                fore_v = vel(veh)
+            end
         end
-        if veh.id != egoid && lane_belief(veh, model, roadway) in lanes_to_yield_to
+
+        # If the vehicle is in a lane that the ego should yield to, store it
+        if lane_belief(veh, model, roadway) in lanes_to_yield_to
             has_right_of_way = false
             push!(vehicles_to_yield_to, veh)
         end
@@ -288,23 +306,36 @@ function AutomotiveSimulator.observe!(model::TIDM, input_scene::Scene, roadway::
 
     next_idm = track_longitudinal!(model.idm, ego_v, fore_v, fore_Δs)
     # If the vehicle does not have right of way then stop before the intersection
-    if !has_right_of_way
-        # Compare ttc
-        exit_time = [time_to_cross_distance_const_acc(veh,  model.idm, distance_to_point(veh, roadway, model.intersection_exit_loc[laneid(veh)])) for veh in vehicles_to_yield_to]
-        enter_time = [time_to_cross_distance_const_acc(veh,  model.idm, distance_to_point(veh, roadway, model.intersection_enter_loc[laneid(veh)])) for veh in vehicles_to_yield_to]
-        Δs_in_lane = [compute_inlane_headway(ego, veh, roadway, model.blindspot) for veh in vehicles_to_yield_to]
+    if !has_right_of_way && intrsxn_exit_Δs > 0
+        Nv = length(vehicles_to_yield_to)
+        in_intersection = Vector{Bool}(undef, Nv)
+        exit_time = Vector{Float64}(undef, Nv)
+        enter_time = Vector{Float64}(undef, Nv)
+        Δs_in_lane = Vector{Float64}(undef, Nv)
+        for (i, veh) in zip(1:Nv, vehicles_to_yield_to)
+            _exit = distance_to_point(veh, roadway, model.intersection_exit_loc[laneid(veh)])
+            _enter = distance_to_point(veh, roadway, model.intersection_enter_loc[laneid(veh)])
+            in_intersection[i] = (_enter < 2 && _exit > 0)
+            exit_time[i] = time_to_cross_distance_const_vel(veh, _exit)
+            enter_time[i] = time_to_cross_distance_const_vel(veh, _enter)
+            Δs_in_lane[i] = compute_inlane_headway(ego, veh, roadway)
+        end
+
         # The intersection is clear of car i if, it exited the intersection in the past, or
         # it will enter the intersection after you have crossed it, or
         # it will have exited a while before you crossed
-        intersection_clear = (exit_time .< 0) .| (enter_time .> time_to_cross) .| (exit_time .+ model.ttc_threshold .< time_to_cross)
+        intersection_clear = (exit_time .<= 0) .| (enter_time .> time_to_cross) .| (exit_time .+ model.ttc_threshold .< time_to_cross)
+        intersection_clear = intersection_clear .& (.!in_intersection)
+        intersection_clear = intersection_clear .& (Δs_in_lane .> intrsxn_exit_Δs)
         if !all(intersection_clear)
             # yield to oncoming traffic
             minΔs_to_yield = minimum(Δs_in_lane[.!intersection_clear]) # headways of cars you care about
             # If there isn't a leading car, or if it is past the intersection, use intersection point, otherwise use car
-            if isnan(fore_Δs) || minΔs_to_yield < fore_Δs || (intrsxn_Δs > 0 && intrsxn_Δs < fore_Δs)
+            if isinf(fore_Δs) || minΔs_to_yield < fore_Δs || (intrsxn_Δs > 0 && intrsxn_Δs < fore_Δs)
                 next_idm = track_longitudinal!(model.idm, ego_v, 0., min(minΔs_to_yield, intrsxn_Δs))
             end
         end
+
     end
     model.idm = next_idm
     model
@@ -315,7 +346,7 @@ end
 #         if veh.id != egoid &&
 # end
 
-function compute_inlane_headway(ego, veh, roadway, blindspot)
+function compute_inlane_headway(ego, veh, roadway)
     elane = laneid(ego)
     v = Entity(set_lane(veh.state.veh_state, elane, roadway), veh.def, veh.id)
     f = posf(v)
@@ -328,7 +359,6 @@ end
 function lane_belief(veh::Entity, model::TIDM, roadway::Roadway)
     possible_lanes = model.goals[laneid(veh)]
     length(possible_lanes) == 1 && return possible_lanes[1]
-
     possible_lanes = possible_lanes[[can_have_goal(veh, l, roadway) for l in possible_lanes]]
     length(possible_lanes) == 1 && return possible_lanes[1]
 
@@ -416,7 +446,7 @@ function ego_collides(egoid, scene)
 
     ego = get_by_id(scene, egoid)
     for (i,veh) in enumerate(scene)
-        if egoid != veh.id && collision_checker(ego, veh)
+        if egoid != veh.id && collision_checker(ego, veh) && vel(ego) > 0.1
             return true
         end
     end

src/helpers.jl

@@ -60,12 +60,13 @@ get_rand_pedestrian(;id::Int64, s_dist, v_dist) = (rng::AbstractRNG = Random.GLO
 
 ## Create gif from rollout
 
-function scenes_to_gif(scenes, roadway, filename; others = [], others_fn = [], fps = 10, camera = nothing, canvas = (1200, 800), repeat_last_frame = 1 )
+function scenes_to_gif(scenes, roadway, filename; others = [], others_fn = [], fps = 10, camera = nothing, canvas = (1200, 800), repeat_last_frame = 1, verbose = false )
     push!(scenes, fill(scenes[end], repeat_last_frame)...)
     files = []
     for i=1:length(scenes)
-        frame = render([roadway, others..., [f(scenes[i]) for f in others_fn]..., scenes[i]], canvas_width=canvas[1], canvas_height=canvas[2], camera = camera)
-        file = string("/tmp/test_", lpad(i,2, "0"), ".png")
+        verbose && println("frame $i")
+        frame = AutomotiveVisualization.render([roadway, others..., [f(scenes[i]) for f in others_fn]..., scenes[i]], canvas_width=canvas[1], canvas_height=canvas[2], camera = camera)
+        file = string("/tmp/test_", lpad(i,4, "0"), ".png")
         push!(files, file)
         write(file, frame)
     end
@@ -76,11 +77,12 @@ end
 
 # Create a random IntelligentDriverModel
 function random_IDM(rng::AbstractRNG)
-    headway_t = max(0.5, rand(rng, Normal(1.5, 0.5))) # desired time headway [s]
-    v_des = max(15.0, rand(rng, Normal(20.0, 5.0))) # desired speed [m/s]
-    s_min = max(1.0, rand(rng, Normal(5.0, 1.0))) # minimum acceptable gap [m]
-    a_max = max(2.0, rand(rng, Normal(3.0, 1.0))) # maximum acceleration ability [m/s²]
-    IntelligentDriverModel(T = headway_t, v_des = v_des, s_min = s_min, a_max = a_max)
+    headway_t = rand(rng, Uniform(0.5, 1.5)) # desired time headway [s]
+    v_des = rand(rng, Uniform(10., 15.))
+    s_min = rand(rng, Uniform(2.0, 5.0)) # minimum acceptable gap [m]
+    d_cmf = rand(rng, Uniform(1.5, 2.5)) # comfortable deceleration [m/s²] (positive)
+    d_max = rand(rng, Uniform(5.0, 9.0)) # maximum deceleration [m/s²] (positive)
+    IntelligentDriverModel(T = headway_t, v_des = v_des, s_min = s_min, d_cmf = d_cmf, d_max = d_max)
 end
 
 ## action conversion function