Skip to content
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 15 additions & 1 deletion docs/src/index.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,20 @@
# About

This is the documentation for AutomotiveDrivingModels.jl. Placeholder for now
This is the documentation for AutomotiveDrivingModels.jl.

## Concepts

This section defines a few term that are used accross the package.
See the specific section of the documentation for a more thorough explanation.

- **Entity**: An entity is a traffic participants that navigates in the environment, it is defined by a physical state (position, velocity, ...), an agent definition (whether it is a car or pedestrian, how large it is, ...), and an ID.
- **Scene**: A scene represents a snapshot in time of a driving situation, it essentially consists of a list of entities at a given time.
- **Driver Model**: A driver model is a distribution over action. Given a scene, each entity can updates its model, we call this process observation (the corresponding method is `observe!`). After observing the scene, an action can be sampled from the driver model (using `rand`).
- **Actions**: An action consists of a command applied to move the entity (e.g. longitudinal acceleration, steering). The state of the entity is updated using the `propagate` method which encodes the dynamics model.

This package provides default structure for representing entity states, entities, scenes, driver model and actions.
However it has been design to support custom types.
Each section of the documentation contains an interface, which is a list of function that a user must implement to use its own types.

```@contents
```
37 changes: 33 additions & 4 deletions docs/src/states.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,46 @@ In this section of the documentation we explain the default vehicle state type p
as well as the data types used to represent driving scene. Most of the underlying structures are defined in `Records.jl`.
The data structure provided in ADM.jl are concrete instances of parametric types defined in Records. It is possible in principle to define your custom state definition and use the interface defined in ADM.jl.

## Agent States
## Entity state

Agents are represented by the entity data type provided by `Records.jl`.
Entities are represented by the entity data type provided by `Records.jl`.
The entity data type has three field: a state, a definition, and an id.

The state of an entity usually describes physical quantity such as position and velocity.

Two states data structure are provided.
Comment thread
MaximeBouton marked this conversation as resolved.
Resolved

## Defining your own state type

You can define your own state type if the provided `VehicleState` does not contain the right information.
There are a couple functions that needs to be defined such that other functions in AutomotiveDrivingModels can work smoothly with your custom state type.

```@docs
posg()
posf
vel
velf
velg
```

**Example of a custom state type containing acceleration:**

```julia

# you can use composition to define your state type
struct MyVehicleState
veh::VehicleState
acc::Float64
end

# define the function from the interface
posg(s::MyVehicleState) = posg(s.veh) # those functions are implemented for the `VehicleState` type
posf(s::MyVehicleState) = posf(s.veh)
velg(s::MyVehicleState) = velg(s.veh)
velf(s::MyVehicleState) = velf(s.veh)
vel(s::MyVehicleState) = vel(s.veh)
```

### 1D states and vehicles

```@docs
Expand All @@ -27,8 +58,6 @@ Here we list useful functions to interact with vehicle states and retrieve inter
```@docs
VehicleState
Vec.lerp(a::VehicleState, b::VehicleState, t::Float64, roadway::Roadway)
get_vel_s
get_vel_t
move_along(vehstate::VehicleState, roadway::Roadway, Δs::Float64; ϕ₂::Float64=vehstate.posF.ϕ, ::Float64=vehstate.posF.t, v₂::Float64=vehstate.v)
Vehicle
get_front
Expand Down
9 changes: 8 additions & 1 deletion src/AutomotiveDrivingModels.jl
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ include("roadways/roadways.jl")

export
Frenet,
get_posG,
NULL_FRENET

include("roadways/frenet.jl")
Expand Down Expand Up @@ -112,6 +111,11 @@ export
include("states/1d_states.jl")

export
posf,
posg,
vel,
velf,
velg,
VehicleState,
Vehicle,
get_vel_s,
Expand All @@ -122,6 +126,7 @@ export
get_rear,
get_lane

include("states/interface.jl")
include("states/vehicle_state.jl")

export
Expand Down Expand Up @@ -295,4 +300,6 @@ export
include("simulation/simulation.jl")
include("simulation/callbacks.jl")

include("deprecated.jl")

end # AutomotiveDrivingModels
6 changes: 2 additions & 4 deletions src/actions/accel_desang.jl
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,8 @@ function propagate(veh::Entity{VehicleState,D,I}, action::AccelDesang, roadway::
a = action.a # accel
ϕdes = action.ϕdes # desired heading angle

x = veh.state.posG.x
y = veh.state.posG.y
θ = veh.state.posG.θ
v = veh.state.v
x, y, θ = posg(veh.state)
v = vel(veh.state)

δt = Δt/n_integration_steps

Expand Down
8 changes: 3 additions & 5 deletions src/actions/accel_steering.jl
Original file line number Diff line number Diff line change
Expand Up @@ -43,16 +43,14 @@ function propagate(veh::Entity{VehicleState, BicycleModel, Int}, action::AccelSt
a = action.a # accel [m/s²]
δ = action.δ # steering wheel angle [rad]

x = veh.state.posG.x
y = veh.state.posG.y
θ = veh.state.posG.θ
v = veh.state.v
x, y, θ = posg(veh.state)
v = vel(veh.state)

s = v*Δt + a*Δt*Δt/2 # distance covered
v′ = v + a*Δt

if abs(δ) < 0.01 # just drive straight
posG = veh.state.posG + polar(s, θ)
posG = posg(veh.state) + polar(s, θ)
else # drive in circle

R = L/tan(δ) # turn radius
Expand Down
6 changes: 2 additions & 4 deletions src/actions/accel_turnrate.jl
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,8 @@ function propagate(veh::Entity{VehicleState,D,I}, action::AccelTurnrate, roadway
a = action.a # accel
ω = action.ω # turnrate

x = veh.state.posG.x
y = veh.state.posG.y
θ = veh.state.posG.θ
v = veh.state.v
x, y, θ = posg(veh.state)
v = vel(veh.state)

δt = Δt / n_integration_steps

Expand Down
17 changes: 3 additions & 14 deletions src/actions/lane_following_accel.jl
Original file line number Diff line number Diff line change
Expand Up @@ -26,26 +26,15 @@ function propagate(veh::Entity{VehicleState,D,I}, action::LaneFollowingAccel, ro

a_lon = action.a

ds = veh.state.v
ds = vel(veh.state)

ΔT² = ΔT*ΔT
Δs = ds*ΔT + 0.5*a_lon*ΔT²

v₂ = ds + a_lon*ΔT

roadind = move_along(veh.state.posF.roadind, roadway, Δs)
roadind = move_along(posf(veh.state).roadind, roadway, Δs)
posG = roadway[roadind].pos
posF = Frenet(roadind, roadway, t=veh.state.posF.t, ϕ=veh.state.posF.ϕ)
posF = Frenet(roadind, roadway, t=posf(veh.state).t, ϕ=posf(veh.state).ϕ)
VehicleState(posG, posF, v₂)
end


#XXX these should probably be removed
# Base.show(io::IO, a::LaneFollowingAccel) = @printf(io, "LaneFollowingAccel(%6.3f)", a.a)
# Base.length(::Type{LaneFollowingAccel}) = 1
# Base.convert(::Type{LaneFollowingAccel}, v::Vector{Float64}) = LaneFollowingAccel(v[1])

# function Base.copyto!(v::Vector{Float64}, a::LaneFollowingAccel)
# v[1] = a.a
# v
# end
8 changes: 4 additions & 4 deletions src/actions/lat_lon_accel.jl
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,10 @@ function propagate(veh::Entity{VehicleState, D, I}, action::LatLonAccel, roadway
a_lat = action.a_lat
a_lon = action.a_lon

v = veh.state.v
ϕ = veh.state.posF.ϕ
v = vel(veh.state)
ϕ = posf(veh.state).ϕ
ds = v*cos(ϕ)
t = veh.state.posF.t
t = posf(veh.state).t
dt = v*sin(ϕ)

ΔT² = ΔT*ΔT
Expand All @@ -39,7 +39,7 @@ function propagate(veh::Entity{VehicleState, D, I}, action::LatLonAccel, roadway
v₂ = sqrt(dt₂*dt₂ + ds₂*ds₂) # v is the magnitude of the velocity vector
ϕ₂ = atan(dt₂, ds₂)

roadind = move_along(veh.state.posF.roadind, roadway, Δs)
roadind = move_along(posf(veh.state).roadind, roadway, Δs)
footpoint = roadway[roadind]
posG = VecE2{Float64}(footpoint.pos.x,footpoint.pos.y) + polar(t + Δt, footpoint.pos.θ + π/2)

Expand Down
4 changes: 2 additions & 2 deletions src/actions/pedestrian_lat_lon_accel.jl
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ function AutomotiveDrivingModels.propagate(ped::Entity{VehicleState, D, I},
ΔT::Float64=0.1
) where {D,I}
state = propagate(ped, LatLonAccel(action.a_lat, action.a_lon), roadway, ΔT)
roadproj = proj(state.posG, roadway[action.lane_des.tag], roadway, move_along_curves=false)
retval = VehicleState(Frenet(roadproj, roadway), roadway, state.v)
roadproj = proj(posg(state), roadway[action.lane_des.tag], roadway, move_along_curves=false)
retval = VehicleState(Frenet(roadproj, roadway), roadway, vel(state))
return retval
end
8 changes: 4 additions & 4 deletions src/behaviors/MOBIL.jl
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ function observe!(model::MOBIL, scene::Frame{Entity{S, D, I}}, roadway::Roadway,

vehicle_index = findfirst(egoid, rec[0])
veh_ego = scene[vehicle_index]
v = veh_ego.state.v
v = vel(veh_ego.state)
egostate_M = veh_ego.state

left_lane_exists = convert(Float64, get(N_LANE_LEFT, rec, roadway, vehicle_index)) > 0
Expand All @@ -72,11 +72,11 @@ function observe!(model::MOBIL, scene::Frame{Entity{S, D, I}}, roadway::Roadway,

# candidate position after lane change is over
footpoint = get_footpoint(veh_ego)
lane = roadway[veh_ego.state.posF.roadind.tag]
lane = get_lane(roadway, veh_ego)
lane_L = roadway[LaneTag(lane.tag.segment, lane.tag.lane + 1)]
roadproj = proj(footpoint, lane_L, roadway)
frenet_L = Frenet(RoadIndex(roadproj), roadway)
egostate_L = VehicleState(frenet_L, roadway, veh_ego.state.v)
egostate_L = VehicleState(frenet_L, roadway, vel(veh_ego.state))

Δaccel_n = 0.0
passes_safety_criterion = true
Expand Down Expand Up @@ -140,7 +140,7 @@ function observe!(model::MOBIL, scene::Frame{Entity{S, D, I}}, roadway::Roadway,
lane_R = roadway[LaneTag(lane.tag.segment, lane.tag.lane - 1)]
roadproj = proj(footpoint, lane_R, roadway)
frenet_R = Frenet(RoadIndex(roadproj), roadway)
egostate_R = VehicleState(frenet_R, roadway, veh_ego.state.v)
egostate_R = VehicleState(frenet_R, roadway, vel(veh_ego.state))

Δaccel_n = 0.0
passes_safety_criterion = true
Expand Down
2 changes: 1 addition & 1 deletion src/behaviors/lane_change_models.jl
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ Base.show(io::IO, a::LaneChangeChoice) = @printf(io, "LaneChangeChoice(%d)", a.d

function get_lane_offset(a::LaneChangeChoice, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0)
if a.dir == DIR_MIDDLE
rec[pastframe][vehicle_index].state.posF.t
posf(rec[pastframe][vehicle_index].state).t
elseif a.dir == DIR_LEFT
convert(Float64, get(LANEOFFSETLEFT, rec, roadway, vehicle_index, pastframe))
else
Expand Down
4 changes: 2 additions & 2 deletions src/behaviors/lane_following_drivers.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@ function observe!(model::LaneFollowingDriver, scene::Frame{Entity{State1D, D, I}

fore_res = get_neighbor_fore(scene, vehicle_index, roadway)

v_ego = scene[vehicle_index].state.v
v_oth = scene[fore_res.ind].state.v
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)
Expand Down
4 changes: 2 additions & 2 deletions src/behaviors/lat_lon_separable_driver.jl
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,12 @@ function observe!(model::LaneFollowingDriver, scene::Frame{Entity{S, D, I}}, roa

fore = get_neighbor_fore_along_lane(scene, vehicle_index, roadway, VehicleTargetPointFront(), VehicleTargetPointRear(), VehicleTargetPointFront())

v_ego = scene[vehicle_index].state.v
v_ego = vel(scene[vehicle_index].state)
v_oth = NaN
headway = NaN

if fore.ind != nothing
v_oth = scene[fore.ind].state.v
v_oth = vel(scene[fore.ind].state)
headway = fore.Δs
end

Expand Down
31 changes: 10 additions & 21 deletions src/behaviors/lateral_driver_models.jl
Original file line number Diff line number Diff line change
Expand Up @@ -15,37 +15,26 @@ A controller that executes the lane change decision made by the `lane change mod
- `kp::Float64 = 3.0` proportional constant for lane tracking
- `kd::Float64 = 2.0` derivative constant for lane tracking
"""
mutable struct ProportionalLaneTracker <: LateralDriverModel{Float64}
a::Float64 # predicted acceleration
σ::Float64 # optional stdev on top of the model, set to zero or NaN for deterministic behavior
kp::Float64 # proportional constant for lane tracking
kd::Float64 # derivative constant for lane tracking

function ProportionalLaneTracker(;
σ::Float64 = NaN,
kp::Float64 = 3.0,
kd::Float64 = 2.0,
)

retval = new()
retval.a = NaN
retval.σ = σ
retval.kp = kp
retval.kd = kd
retval
end
@with_kw mutable struct ProportionalLaneTracker <: LateralDriverModel{Float64}
a::Float64 = NaN # predicted acceleration
σ::Float64 = NaN # optional stdev on top of the model, set to zero or NaN for deterministic behavior
kp::Float64 = 3.0 # proportional constant for lane tracking
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
end

function observe!(model::ProportionalLaneTracker, scene::Frame{Entity{S, D, I}}, roadway::Roadway, egoid::I) where {S, D, I}

ego_index = findfirst(egoid, scene)
veh_ego = scene[ego_index]
t = veh_ego.state.posF.t # lane offset
dt = veh_ego.state.v * sin(veh_ego.state.posF.ϕ) # rate of change of lane offset
t = posf(veh_ego.state).t # lane offset
dt = velf(veh_ego.state).t # rate of change of lane offset
model.a = -t*model.kp - dt*model.kd

model
Expand Down
20 changes: 10 additions & 10 deletions src/behaviors/sidewalk_pedestrian_model.jl
Original file line number Diff line number Diff line change
Expand Up @@ -71,9 +71,9 @@ 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}
ped = scene[findfirst(egoid, scene)]

push!(model.pos_x, ped.state.posG.x)
push!(model.pos_y, ped.state.posG.y)
push!(model.velocities, ped.state.v)
push!(model.pos_x, posg(ped.state).x)
push!(model.pos_y, posg(ped.state).y)
push!(model.velocities, vel(ped.state))
push!(model.phases, model.phase)

if model.phase == APPROACHING
Expand Down Expand Up @@ -121,11 +121,11 @@ function update_appraising(ped::Entity{S, D, I},
end

# Check vehicle distance to the crosswalk
posG = veh.state.posG
posG = posg(veh.state)
cw_posF = Frenet(posG, model.crosswalk, roadway)
lane = get_lane(roadway, veh)
Δs = get_distance_to_lane(veh, model.crosswalk, roadway)
ttc = Δs/veh.state.v
ttc = Δs/vel(veh.state)

if 0.0 < ttc < model.ttc_threshold
return # Exit if any car is not within threshold
Expand Down Expand Up @@ -168,17 +168,17 @@ function update_leaving(ped::Entity{S, D, I},
end

function set_inst_vel(ped::Entity{VehicleState, D, I}, model::SidewalkPedestrianModel, lane_des::Lane, v_des::Float64) where {D, I}
model.a = PedestrianLatLonAccel(0.0, calc_a(ped.state.v, v_des, model.timestep), lane_des)
model.a = PedestrianLatLonAccel(0.0, calc_a(vel(ped.state), v_des, model.timestep), lane_des)
end

function calc_a(v_init::Float64, v::Float64, Δt::Float64)
return (v - v_init)/Δt
end

function set_accel(ped::Entity{VehicleState, D, I}, model::SidewalkPedestrianModel, v_des::Float64, lane_des::Lane) where {D, I}
if ped.state.v < v_des
if vel(ped.state) < v_des
model.a = PedestrianLatLonAccel(0.0, model.ped_accel, lane_des)
elseif ped.state.v > v_des
elseif vel(ped.state) > v_des
model.a = PedestrianLatLonAccel(0.0, model.ped_decel, lane_des)
else
model.a = PedestrianLatLonAccel(0.0, 0.0, lane_des)
Expand All @@ -191,8 +191,8 @@ end
function get_distance_to_lane(veh::Entity{VehicleState, D, I}, lane_des::Lane, roadway::Roadway) where {D, I}
vehlane = get_lane(roadway, veh)
lane_segment = LineSegment(lane_des.curve[1].pos, lane_des.curve[end].pos)
collision_point = intersect(Ray(veh.state.posG), lane_segment)
collision_point = intersect(Ray(posg(veh.state)), lane_segment)
lane_des_proj = Frenet(VecSE2(collision_point), vehlane, roadway)
Δs = lane_des_proj.s - veh.state.posF.s
Δs = lane_des_proj.s - posf(veh.state).s
return Δs
end
Loading