Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
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 terms that are used across the package.
See the specific section of the documentation for a more thorough explanation.

- **Entity**: An entity is a traffic participant 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 actions. Given a scene, each entity can update 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 a default structure for representing entity states, entities, scenes, driver models and actions.
However it has been designed to support custom types.
Each section of the documentation contains an interface, which is a list of functions that a user must implement to use its own types.

```@contents
```
47 changes: 38 additions & 9 deletions docs/src/states.md
Original file line number Diff line number Diff line change
@@ -1,17 +1,48 @@
# States

In this section of the documentation we explain the default vehicle state type provided by `AutomotiveDrivingModels`
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.
as well as the data types used to represent a driving scene. Most of the underlying structures are defined in `Records.jl`.
The data structures 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`.
The entity data type has three field: a state, a definition, and an id.
Entities are represented by the `Entity` data type provided by `Records.jl` (https://github.com/sisl/Records.jl/blob/master/src/entities.jl).
The `Entity` data type has three fields: 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.
Two state data structures are provided.

## 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 of couple functions that need 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 custom state type based on existing ones
struct MyVehicleState
veh::VehicleState
acc::Float64
end

# define the functions 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

Expand All @@ -22,13 +53,11 @@ Two states data structure are provided.

### 2D states and vehicles

Here we list useful functions to interact with vehicle states and retrieve interesting information like the position of the front of the vehicle or the lane to which the vehicle belong.
Here we list useful functions to interact with vehicle states and retrieve interesting information like the position of the front of the vehicle or the lane to which the vehicle belongs.

```@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
Loading