├── .DS_Store ├── OpenTrafficLabIntroductoryExample.mlx ├── Testing ├── T-Junction-with-lights-trimmed.gif ├── test.m ├── createDrivingScenario.m ├── testTjunction.m ├── testFourWayjunction.m ├── createTJunctionNetwork.m ├── createVehiclesForFourWayJunction.m ├── createVehiclesForTJunction.m ├── createFourWayJunctionNetwork.m ├── createFourWayJunctionScenario.m └── createTJunctionScenario.m ├── SECURITY.md ├── +drivingBehavior ├── generatePoissonEntryTimes.m ├── gippsDriverModel.m └── intelligentDriverModel.m ├── +trafficControl ├── StopSign.m ├── TrafficLight.m └── TrafficController.m ├── license.txt ├── @Node ├── roadDistanceToCenterAndLeft.m └── Node.m ├── README.md └── @DrivingStrategy ├── DrivingStrategy.asv └── DrivingStrategy.m /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/OpenTrafficLab/HEAD/.DS_Store -------------------------------------------------------------------------------- /OpenTrafficLabIntroductoryExample.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/OpenTrafficLab/HEAD/OpenTrafficLabIntroductoryExample.mlx -------------------------------------------------------------------------------- /Testing/T-Junction-with-lights-trimmed.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks/OpenTrafficLab/HEAD/Testing/T-Junction-with-lights-trimmed.gif -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | # Reporting Security Vulnerabilities 2 | 3 | If you believe you have discovered a security vulnerability, please report it to 4 | [security@mathworks.com](mailto:security@mathworks.com). Please see 5 | [MathWorks Vulnerability Disclosure Policy for Security Researchers](https://www.mathworks.com/company/aboutus/policies_statements/vulnerability-disclosure-policy.html) 6 | for additional information. -------------------------------------------------------------------------------- /Testing/test.m: -------------------------------------------------------------------------------- 1 | % Copyright 2020 - 2020 The MathWorks, Inc. 2 | 3 | s = createDrivingScenario(); 4 | s.StopTime = 50; 5 | net = Node(s,s.RoadSegments(1),1); 6 | initVel = 10; 7 | plot(s) 8 | 9 | tInjection = 0; 10 | while advance(s) 11 | if tInjection>3 12 | car = vehicle(s,'EntryTime',s.SimulationTime+1,'Velocity',[10,0,0]); 13 | ms = DrivingStrategy(car,'NextNode',net(1),'StoreData',true); 14 | tInjection = 0; 15 | end 16 | tInjection =tInjection+s.SampleTime; 17 | end -------------------------------------------------------------------------------- /+drivingBehavior/generatePoissonEntryTimes.m: -------------------------------------------------------------------------------- 1 | function entryTimes = generatePoissonEntryTimes(tMin,tMax,mu) 2 | %generatePoissonEntryTimes generates a monotonically increassing vector of 3 | %entry times of vehicles arriving with a rate mu (in vehicles per hour) between times tMin and tMax. 4 | %The minHeadway guarantees that vehicles have a certain minimum time 5 | %headway in seconds. 6 | 7 | % Copyright 2020 - 2020 The MathWorks, Inc. 8 | 9 | t=tMin; 10 | minHeadway = 1; 11 | entryTimes = []; 12 | while t3 || isnan(acc) 31 | acc; 32 | end 33 | end 34 | 35 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020, The MathWorks, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 3. In all cases, the software is, and all modifications and derivatives of the software shall be, licensed to you solely for use in conjunction with MathWorks products and service offerings. 9 | 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | -------------------------------------------------------------------------------- /@Node/roadDistanceToCenterAndLeft.m: -------------------------------------------------------------------------------- 1 | function [center, left, kappa, dkappa, dx_y] = roadDistanceToCenterAndLeft(xr, hcd, hl, hip, course, k0, k1, vpp, bpp) 2 | 3 | % Copyright 2017 The MathWorks, Inc. 4 | 5 | % find index/indices into table 6 | idx = discretize(xr, hcd); 7 | % If nan values come at end of the indices, it replace the maximum value at end of 8 | % the indices. 9 | if isnan(idx(end)) 10 | idx(isnan(idx)) = idx(find(isnan(idx)==0, 1, 'last' )); 11 | end 12 | % fetch clothoid segment at index and initial position. 13 | dkappa = (k1(idx)-k0(idx))./hl(idx); 14 | kappa0 = k0(idx); 15 | theta = course(idx); 16 | p0 = hip(idx); 17 | 18 | % get length and curvature into clothoid segment 19 | l = xr-hcd(idx); 20 | kappa = kappa0 + l.*dkappa; 21 | 22 | % get corresponding points in complex plane and derivative w.r.t. road length 23 | x_y = matlabshared.tracking.internal.scenario.fresnelg2(l, dkappa, kappa0, theta); 24 | dx_y = matlabshared.tracking.internal.scenario.dfresnelg2(l, dkappa, kappa0, theta); 25 | 26 | % get elevation and derivative w.r.t road length 27 | zp = ppval(vpp, xr); 28 | 29 | % get banking angles 30 | bank = ppval(bpp, xr); 31 | 32 | % assemble the 3D positions of the road centers. This corresponds to (xr, 0) in road coordinates. 33 | center = [real(x_y+p0) imag(x_y+p0) zp]; 34 | 35 | % assemble unit tangent to xy in xy plane (neglecting derivative of elevation) 36 | forward = [real(dx_y) imag(dx_y) zeros(length(x_y),1)]; 37 | forward = forward ./ sqrt(sum(forward.^2,2)); 38 | up = repmat([0 0 1],length(x_y),1); 39 | left = cross(up,forward,2); 40 | left = left ./ sqrt(sum(left.^2,2)); 41 | 42 | % apply bank angles 43 | left = [left(:,1).*cos(bank) left(:,2).*cos(bank) sin(bank)]; -------------------------------------------------------------------------------- /Testing/createTJunctionNetwork.m: -------------------------------------------------------------------------------- 1 | function [net] = createTJunctionNetwork(scenario) 2 | %This funcion creates the network of Node objects for the 3 | %T-Junction scenario. It takes as input the scenario created by the function 4 | %createTJunctionScenario, and it outputs the vector containing the 12 nodes 5 | %associated with that road network. 6 | % 7 | % First each Node object is created using the constructor of the Node 8 | % class. We create Nodes for each of the lanes that feeds the intersection 9 | % first, then Nodes for the lanes that leave the intersection, and finally 10 | % Nodes for the lanes that make up the turns and motions within the 11 | % intersection 12 | 13 | % Copyright 2020 - 2020 The MathWorks, Inc. 14 | 15 | % Creating the Nodes 16 | for i =1:3 17 | net(i) = Node(scenario,scenario.RoadSegments(i),-1); 18 | end 19 | 20 | for i =1:3 21 | net(3+i) = Node(scenario,scenario.RoadSegments(i),1); 22 | end 23 | 24 | for i = 1:6 25 | rs = scenario.RoadSegments(i+3); 26 | net(i+6) = Node(scenario,rs,1); 27 | end 28 | 29 | net(1).ConnectsTo = net(7); 30 | net(1).ConnectsTo = net(8); 31 | net(1).SharesRoadWith = net(4); 32 | 33 | net(2).ConnectsTo = net(11); 34 | net(2).ConnectsTo = net(12); 35 | net(2).SharesRoadWith = net(5); 36 | 37 | net(3).ConnectsTo = net(9); 38 | net(3).ConnectsTo = net(10); 39 | net(3).SharesRoadWith = net(6); 40 | 41 | net(10).ConnectsTo = net(4); 42 | net(11).ConnectsTo = net(4); 43 | net(10).SharesRoadWith = net([7:9,11,12]); 44 | net(11).SharesRoadWith = net([7:11,12]); 45 | 46 | net(8).ConnectsTo = net(5); 47 | net(9).ConnectsTo = net(5); 48 | net(8).SharesRoadWith = net([7,9:12]); 49 | net(9).SharesRoadWith = net([7,8,10:12]); 50 | 51 | 52 | net(7).ConnectsTo = net(6); 53 | net(12).ConnectsTo = net(6); 54 | net(7).SharesRoadWith = net([8:12]); 55 | net(12).SharesRoadWith = net([7:11]); 56 | 57 | 58 | end 59 | 60 | -------------------------------------------------------------------------------- /Testing/createVehiclesForFourWayJunction.m: -------------------------------------------------------------------------------- 1 | function cars = createVehiclesForFourWayJunction(s,net,InjectionRate,TurnRatio) 2 | 3 | % Copyright 2020 - 2020 The MathWorks, Inc. 4 | 5 | rng(0); 6 | numEntryRoads = 4; 7 | numTurns = 3; 8 | 9 | [s,net,InjectionRate,TurnRatio] = checkInputs(s,net,InjectionRate,TurnRatio); 10 | 11 | cars = driving.scenario.Vehicle.empty; 12 | 13 | for i=1:numEntryRoads 14 | entryTimes = generatePoissonEntryTimes(s.SimulationTime,s.StopTime,InjectionRate(i)); 15 | 16 | for entryTime =entryTimes 17 | j = discretize(rand, [0 cumsum(TurnRatio)]); 18 | path = [net(i), net(i).ConnectsTo(j), net(i).ConnectsTo(j).ConnectsTo(1)]; 19 | pos = path(1).getRoadCenterFromStation(0); 20 | [station,direction,offset]=path(1).getStationDistance(pos(1:2)); 21 | car = vehicle(s,'Position',pos,'EntryTime',entryTime,'Velocity',[10,0,0]); 22 | car.ForwardVector = [direction,0]; 23 | ms = DrivingStrategy(car,'NextNode',path); 24 | cars(end+1)=car; 25 | end 26 | end 27 | 28 | end 29 | 30 | function entryTimes = generatePoissonEntryTimes(tMin,tMax,mu) 31 | 32 | t=tMin; 33 | minHeadway = 1; 34 | entryTimes = []; 35 | while tTM, namely, the DrivingScenarioDesigner app and the drivingScenario object it generates. These tools can be used efficiently to represent road networks, populate them with vehicles and specify their trajectories. However, the motion of the vehicles is meant to be specified before the simulation is ran by specifying waypoints to the vehicles' trajectories manually. OpenTrafficLab is meant to extend this functionality by simulating traffic in closed loop simulation, where the vehicles' speed trajectories are not given apriori but are the product of a state dependent control logic. 10 | 11 | **Driving Scenario Designer Application** is part of Automated Driving Toolbox. Refer to the documentation [here](https://www.mathworks.com/help/driving/ref/drivingscenariodesigner-app.html) for more information. 12 | 13 | 14 | ## MATLAB® Toolbox Dependencies 15 | This model has been tested with MATLAB R2020b. The version tested with MATLAB R2020a is being developed. 16 | 17 | To run this model, you need: MATLAB, Automated Driving ToolboxTM. 18 | 19 | ## Getting Started 20 | To get started, users can go through [OpenTrafficLabIntroductoryExample.mlx]() for a documented example covering scenario creation, junction controller creation, vehicle generation, and running a simulation using the provided car following models. 21 | 22 | 23 | ## Architecture 24 | OpenTrafficLab is built using an Object Oriented Programming (OOP) architecture. Concepts of OOP, and OOP in MATLAB can be reviewed [here](https://www.mathworks.com/products/matlab/object-oriented-programming.html?s_tid=srchtitle_object%20oriented%20programming_1). The structure of the simulator makes use existing classes of the Driving Scenario Designer App, like drivingScenario, Vehicle, and Actor. Crucially, three classes that need to be understood are added: 25 | 26 | * A [Node]() class that is used for three main purposes: 27 | 1. To represent the network as a directed graph of connected lanes and turns that vehicles can use to specify their routes through the network. 28 | 2. To provide a mapping between global position, the station distance along the length of the lane, and the direction and curvature of the lane 29 | 3. To connect vehicles and traffic controllers. 30 | * A [DrivingStrategy]() class that controls the vehicles in the network. 31 | 1. The DrivingStrategy object implements car following longitudinal control, and tracks the center lane statically, that is, it assumes no dynamics for lane keeping. 32 | 2. Two car following models, the Gipps Car Following Model and the Intelligent Driver Model, are pre-programmed into the DrivingStrategy class and they can be used to benchmark againts a human driven scenario. These models guarantee safe front to back driving, but rely on proper junction controllers to avoid lateral collisions. 33 | 3. To implement user defined driving logic, DrivingStrategy can be used as a parent class and the relevant methods can be overriden. 34 | * A [TrafficController]() class that is used to model and implement traffic rules and signalization. 35 | 1. A TrafficController object is associated with all Nodes (i.e. lanes or turns) it is meant to control. It controls them by setting the Node as open or closed. A vehicle should react to an upcoming closed Node by not entering it. 36 | 2. To implement user defined traffic control logic, TrafficController can be used as a parent class and the relevant methods can be overriden. The repository includes one such controller, in a [TrafficLight]() class that inherits from TrafficController 37 | 38 | ## Simulating Traffic 39 | 40 | To set up a simulation users must create a road network and specify the driving direction and connectivity of lanes and turns. The drivingScenario object containing the road network can be created using the DrivingScenarioDesigner or its programatic API. Functions to create two sample scenarios are included in the repository, one for a T-Junction ([createTJunctionScenario.m]()) and one for a four-way junction ([createFourWayJunctionScenario.m]()). The network of Node objects is then built by associating each lane or turn in the road network to a Node, and linking the Nodes together accordingly. Functions for creating the Nodes and their connectivity for the the two examples are also provided (see [createTJunctionNetwork.m]() and [createFourWayJunctionNetwork.m]()). 41 | 42 | Next vehicles and their entry times must be generated, along with the DrivingStrategy objects that will control them. Entry times can be generated using the provided Poisson arrival process model, which is implemented in the [generatePoissonEntryTimes.m]() function, within the drivingBehavior package. Functions that create vehicle and drivers for the two nominal examples are provided (see [createVehiclesForTJunction.m]() and [createVehiclesForFourWayJunction.m]()). 43 | 44 | A traffic controller, such as a traffic light, can be created to dynamically control when Nodes (i.e. lanes or turns) can be entered by vehicles. 45 | 46 | Finally, the simulation can be executed by calling the [advance](https://www.mathworks.com/help/driving/ref/drivingscenario.advance.html?s_tid=srchtitle) function on the drivingScenario object. 47 | 48 | 49 | -------------------------------------------------------------------------------- /Testing/createTJunctionScenario.m: -------------------------------------------------------------------------------- 1 | function [scenario, egoVehicle,net] = createTJunctionScenario(roadLength) 2 | % createDrivingScenario Returns the drivingScenario defined in the Designer 3 | 4 | % Generated by MATLAB(R) 9.9 (R2020b) and Automated Driving Toolbox 3.2 (R2020b). 5 | % Generated on: 16-Jun-2020 13:01:51 6 | 7 | % Copyright 2020 - 2020 The MathWorks, Inc. 8 | 9 | % Construct a drivingScenario object. 10 | scenario = drivingScenario; 11 | if nargin<1 12 | roadLength = [50 50 50]; 13 | end 14 | % Add all road segments 15 | roadCenters = [-401.4941 2185.809 0; 16 | -414.7725 2170.853 0]; 17 | roadDirection = diff(roadCenters)./norm(diff(roadCenters)); 18 | roadCenters(2,:) = roadCenters(1,:)+roadLength(1)*roadDirection; 19 | marking = [laneMarking('Unmarked') 20 | laneMarking('Unmarked') 21 | laneMarking('Solid', 'Width', 0.13) 22 | laneMarking('Dashed', 'Width', 0.13) 23 | laneMarking('Solid', 'Width', 0.13) 24 | laneMarking('Unmarked') 25 | laneMarking('Unmarked')]; 26 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 27 | road(scenario, roadCenters, 'Lanes', laneSpecification); 28 | 29 | roadCenters = [-402.2 2206.4 0; 30 | -417.7083 2220.263 0]; 31 | roadDirection = diff(roadCenters)./norm(diff(roadCenters)); 32 | roadCenters(2,:) = roadCenters(1,:)+roadLength(2)*roadDirection; 33 | marking = [laneMarking('Unmarked') 34 | laneMarking('Unmarked') 35 | laneMarking('Solid', 'Width', 0.13) 36 | laneMarking('Dashed', 'Width', 0.13) 37 | laneMarking('Solid', 'Width', 0.13) 38 | laneMarking('Unmarked') 39 | laneMarking('Unmarked')]; 40 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 41 | road(scenario, roadCenters, 'Lanes', laneSpecification); 42 | 43 | roadCenters = [-381.8 2208.1 0; 44 | -368.298 2223.199 0]; 45 | roadDirection = diff(roadCenters)./norm(diff(roadCenters)); 46 | roadCenters(2,:) = roadCenters(1,:)+roadLength(3)*roadDirection; 47 | marking = [laneMarking('Unmarked') 48 | laneMarking('Unmarked') 49 | laneMarking('Solid', 'Width', 0.13) 50 | laneMarking('Dashed', 'Width', 0.13) 51 | laneMarking('Solid', 'Width', 0.13) 52 | laneMarking('Unmarked') 53 | laneMarking('Unmarked')]; 54 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 55 | road(scenario, roadCenters, 'Lanes', laneSpecification); 56 | 57 | %rg = driving.scenario.RoadGroup('Name', 'T junction'); 58 | roadCenters = [-401.4941 2185.809 0; 59 | -381.5764 2208.243 0]; 60 | marking = [laneMarking('Unmarked') 61 | laneMarking('Unmarked') 62 | laneMarking('Solid', 'Width', 0.13) 63 | laneMarking('Dashed', 'Width', 0.13) 64 | laneMarking('Solid', 'Width', 0.13) 65 | laneMarking('Unmarked') 66 | laneMarking('Unmarked')]; 67 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 68 | road(scenario, roadCenters, 'Lanes', laneSpecification); 69 | 70 | roadCenters = [-401.4941 2185.809 0; 71 | -400.5184 2186.908 0; 72 | -398.6231 2189.23 0; 73 | -399.4963 2203.927 0; 74 | -401.6534 2206.009 0; 75 | -402.7523 2206.984 0]; 76 | marking = [laneMarking('Unmarked') 77 | laneMarking('Unmarked') 78 | laneMarking('Solid', 'Width', 0.13) 79 | laneMarking('Dashed', 'Width', 0.13) 80 | laneMarking('Solid', 'Width', 0.13) 81 | laneMarking('Unmarked') 82 | laneMarking('Unmarked')]; 83 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 84 | road(scenario, roadCenters, 'Lanes', laneSpecification); 85 | 86 | roadCenters = [-381.5764 2208.243 0; 87 | -382.5521 2207.144 0; 88 | -384.6341 2204.987 0; 89 | -399.3306 2204.114 0; 90 | -401.6534 2206.009 0; 91 | -402.7523 2206.984 0]; 92 | marking = [laneMarking('Unmarked') 93 | laneMarking('Unmarked') 94 | laneMarking('Solid', 'Width', 0.13) 95 | laneMarking('Dashed', 'Width', 0.13) 96 | laneMarking('Solid', 'Width', 0.13) 97 | laneMarking('Unmarked') 98 | laneMarking('Unmarked')]; 99 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 100 | road(scenario, roadCenters, 'Lanes', laneSpecification); 101 | 102 | roadCenters = [-381.5764 2208.243 0; 103 | -401.4941 2185.809 0]; 104 | marking = [laneMarking('Unmarked') 105 | laneMarking('Unmarked') 106 | laneMarking('Solid', 'Width', 0.13) 107 | laneMarking('Dashed', 'Width', 0.13) 108 | laneMarking('Solid', 'Width', 0.13) 109 | laneMarking('Unmarked') 110 | laneMarking('Unmarked')]; 111 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 112 | road(scenario, roadCenters, 'Lanes', laneSpecification); 113 | 114 | roadCenters = [-402.7523 2206.984 0; 115 | -401.6534 2206.009 0; 116 | -399.4963 2203.927 0; 117 | -398.6231 2189.23 0; 118 | -400.5184 2186.908 0; 119 | -401.4941 2185.809 0]; 120 | marking = [laneMarking('Unmarked') 121 | laneMarking('Unmarked') 122 | laneMarking('Solid', 'Width', 0.13) 123 | laneMarking('Dashed', 'Width', 0.13) 124 | laneMarking('Solid', 'Width', 0.13) 125 | laneMarking('Unmarked') 126 | laneMarking('Unmarked')]; 127 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 128 | road(scenario, roadCenters, 'Lanes', laneSpecification); 129 | 130 | roadCenters = [-402.7523 2206.984 0; 131 | -401.6534 2206.009 0; 132 | -399.3306 2204.114 0; 133 | -384.6341 2204.987 0; 134 | -382.5521 2207.144 0; 135 | -381.5764 2208.243 0]; 136 | marking = [laneMarking('Unmarked') 137 | laneMarking('Unmarked') 138 | laneMarking('Solid', 'Width', 0.13) 139 | laneMarking('Dashed', 'Width', 0.13) 140 | laneMarking('Solid', 'Width', 0.13) 141 | laneMarking('Unmarked') 142 | laneMarking('Unmarked')]; 143 | laneSpecification = lanespec([3 3], 'Width', [1.5 0.15 3.65 3.65 0.15 1.5], 'Marking', marking); 144 | road(scenario, roadCenters, 'Lanes', laneSpecification); 145 | 146 | 147 | 148 | 149 | 150 | -------------------------------------------------------------------------------- /@Node/Node.m: -------------------------------------------------------------------------------- 1 | classdef Node < handle 2 | %Node 3 | % The Node class is used to create an intermediate representation of 4 | % the road bnetwork in a drivingScenario object 5 | % The Node class 6 | % Copyright 2020 - 2020 The MathWorks, Inc. 7 | properties 8 | 9 | ConnectsTo = Node.empty % Nodes it spills into 10 | ConnectsFrom = Node.empty % Nodes that feeds it 11 | SharesRoadWith = Node.empty % Road or Junction it is a part of 12 | 13 | end 14 | 15 | properties (SetAccess = protected) 16 | Scenario 17 | RoadSegment 18 | Lane 19 | Length 20 | Mapping 21 | Vehicles = driving.scenario.Vehicle.empty 22 | PlotHandle 23 | end 24 | 25 | properties (SetAccess = {?trafficControl.TrafficController}) 26 | TrafficController 27 | end 28 | 29 | %% Constructor and Setup methods 30 | methods 31 | function obj = Node(scenario,rs,lane) 32 | obj.Scenario = scenario; 33 | obj.RoadSegment = rs; 34 | obj.Lane = lane; 35 | obj.setMapping; 36 | end 37 | 38 | function setMapping(obj) 39 | rs = obj.RoadSegment; 40 | xr = 0:0.1:rs.hcd(end); 41 | map = nan(length(xr),6); 42 | if obj.Lane < 0 43 | xr = fliplr(xr); 44 | end 45 | for idx = 1:length(xr) 46 | 47 | [center, left, kappa, dkappa, dx_y] = obj.roadDistanceToCenterAndLeft(xr(idx), rs.hcd, rs.hl, rs.hip, rs.course, rs.k0, rs.k1, rs.vpp, rs.bpp); 48 | left = left*(-obj.Lane); 49 | dx_y = dx_y*(obj.Lane); 50 | center = center + left*3.65/2; 51 | 52 | if obj.Lane == -1 53 | map(idx,:) = [xr(end+1-idx),center,real(dx_y),imag(dx_y)]; 54 | else 55 | map(idx,:) = [xr(idx),center,real(dx_y),imag(dx_y)]; 56 | end 57 | end 58 | %map = sortrows(map); 59 | obj.Mapping = map; 60 | obj.Length = max(xr); 61 | end 62 | 63 | function set.ConnectsTo(this,those) 64 | for that=those 65 | if ~any(this.ConnectsTo==that) 66 | this.ConnectsTo(end+1) = that; 67 | end 68 | if ~any(that.ConnectsFrom==this) 69 | that.ConnectsFrom(end+1) = this; 70 | end 71 | end 72 | end 73 | 74 | function set.SharesRoadWith(this,those) 75 | for that = those 76 | if ~any(this.SharesRoadWith==that) 77 | this.SharesRoadWith(end+1) = that; 78 | end 79 | if ~any(that.SharesRoadWith==this) 80 | that.SharesRoadWith(end+1) = this; 81 | end 82 | end 83 | end 84 | 85 | end 86 | %% Public facing methods 87 | methods 88 | 89 | function [dist,forwardVector,offsetVector] = getStationDistance(obj,pos) 90 | % Given a position, this function returns the station distance 91 | % along the road, along with the direction of the road, and 92 | % the distance vector from the point to the road. 93 | distanceToPoints = sqrt(sum((obj.Mapping(:,2:3)-pos).^2,2)); 94 | [~,idx] = min(distanceToPoints); 95 | sampleToPosVector = pos-obj.Mapping(idx,2:3); 96 | forwardVector = obj.Mapping(idx,5:6); 97 | dist = obj.Mapping(idx,1)+dot(forwardVector,sampleToPosVector); 98 | sideVector = cross([forwardVector,0],[0,0,1]); 99 | offsetVector = -dot(sideVector(1:2),sampleToPosVector)*sideVector(1:2); 100 | end 101 | 102 | function [center,indexes] = getRoadCenterFromStation(obj,stations) 103 | center = zeros(length(stations),3); 104 | indexes = zeros(size(stations)); 105 | for idx = 1:length(stations) 106 | s = stations(idx); 107 | dist = (obj.Mapping(:,1)-s).^2; 108 | [~,ii] = min(dist); 109 | center(idx,:) = obj.Mapping(ii,2:4); 110 | indexes(idx)= ii; 111 | end 112 | end 113 | 114 | function length = getRoadSegmentLength(obj) 115 | length = obj.Length; 116 | end 117 | 118 | function added = addVehicle(obj,vehicle) 119 | if ~any(obj.Vehicles==vehicle) 120 | obj.Vehicles(end+1)=vehicle; 121 | added = true; 122 | else 123 | added = true; 124 | %error('Vehicle is already in the node'); 125 | end 126 | 127 | end 128 | 129 | function removed = removeVehicle(obj,vehicle) 130 | if any(obj.Vehicles==vehicle) 131 | obj.Vehicles(obj.Vehicles==vehicle)=[]; 132 | removed = true; 133 | else 134 | error('Vehicle is not in the node') 135 | end 136 | 137 | end 138 | 139 | function actors = getActiveVehicles(obj) 140 | actors = obj.Vehicles; 141 | end 142 | 143 | function [s,veh] = getTrailingVehicleStation(obj,time) 144 | if nargin<2 %If no time is given assume current sim time 145 | time = obj.Scenario.SimulationTime; 146 | end 147 | drivers = [obj.Vehicles.MotionStrategy]; 148 | if isempty(drivers) 149 | s = obj.getRoadSegmentLength(); 150 | veh = driving.scenario.Vehicle.empty; 151 | return 152 | end 153 | [s,idx] = min(getStationDistance(drivers,time)); 154 | veh = drivers(idx).EgoActor; 155 | end 156 | 157 | function s = getLeadingVehicleStation(obj,time) 158 | if nargin<2 %If no time is given assusme current sim time 159 | time = obj.Scenario.SimulationTime; 160 | end 161 | drivers = [obj.Vehicles.MotionStrategy]; 162 | if isempty(drivers) 163 | s = 0; 164 | return 165 | end 166 | s = max(getStationDistance(drivers,time)); 167 | end 168 | 169 | function l = plotPath(obj,ax) 170 | % This function plots a line along the nominal trajectory 171 | % associated with the Nodes passed to it. It outputs the plot 172 | % handle for each line 173 | if nargin<2 174 | if ~isempty(obj(1).Scenario.Plots) 175 | ax = obj(1).Scenario.Plots(end).Parent; 176 | hold on 177 | else 178 | plot(obj(1).Scenario.Plots) 179 | ax=gca; 180 | hold on 181 | end 182 | end 183 | for idx=1:length(obj) 184 | node = obj(idx); 185 | 186 | l(idx) = plot3(ax,node.Mapping(:,2),node.Mapping(:,3),node.Mapping(:,4)+10); 187 | end 188 | end 189 | 190 | function state = getNodeState(obj) 191 | state = obj.TrafficController.getNodeState(obj); 192 | end 193 | 194 | function clearVehicles(obj) 195 | for idx = 1:length(obj) 196 | obj(idx).Vehicles = []; 197 | end 198 | end 199 | end 200 | %% Helper methods 201 | methods(Static) 202 | [c, l, k, d, dx_y] = roadDistanceToCenterAndLeft(xr, hcd, hl, hip, course, k0, k1, vpp, bpp) 203 | end 204 | end 205 | 206 | -------------------------------------------------------------------------------- /@DrivingStrategy/DrivingStrategy.asv: -------------------------------------------------------------------------------- 1 | classdef DrivingStrategy < driving.scenario.MotionStrategy... 2 | & driving.scenario.mixin.PropertiesInitializableInConstructor 3 | %DrivingStrategy Driving Strategy 4 | % DrivingStrategy is used to control vehicles in an OppenTrafficLab 5 | % simulation. The driving logic consists of car following behavior, 6 | % suitably modified to react to open or closed lanes ahead. 7 | % To implemnent a custom driving strategy, use this class as a parent 8 | % class and override the relevant methods 9 | % 10 | % strategy = DrivingStrategy(vehicle) creates a nominal driving strategy 11 | % for the actor vehicle 12 | % 13 | % strategy = DrivingStrategy(...,Name,Value) creates a nominal driving strategy 14 | % for the actor vehicle 15 | % 16 | % Overview 17 | % DrivingStrategy controls the vehicles in its move method, which is 18 | % called every time the advance method of the drivingScenario being 19 | % simulated is called. The move method moves the vehicle to the requested 20 | % time. It does so by setting all states and state dependent variables to 21 | % the immediatly previous time step, computing the control input for that 22 | % set of states, and integrating the states forward in time by one time 23 | % step. The move method perform the following steps 24 | % 25 | % 1. Check if vehicle has entered the network or is currently in the network. If not break anf continue 26 | % 2. Set the vehicle's states nadd state dependent variables to the 27 | % correct their value at the correct time step (i.e one time step before the target time the vehicle is being moved to) 28 | % 3. Call determineDrivingMode 29 | % 4. Call determineDrivingInput 30 | % 5. Integrate states from to the requested time 31 | % 6. Update state dependent variables based on the new value for the 32 | % states 33 | % 34 | % DrivingStrategy properties: 35 | % 36 | % NextNode - list of Nodes the vehicle will traverse in its path 37 | % UDStates - user defined states 38 | % Mode - flag indicating driving mode (i.e. 'CarFollowing', 'ApproachingRedLight') 39 | % Data - data structure for saving time series info. 40 | % 41 | % Parameters: 42 | % 43 | % DesiredSpeed - 44 | % ReactionTime - 45 | % SpeedBounds - 46 | % AccBounds - 47 | % 48 | % States: 49 | % 50 | % Position - 51 | % Velocity - 52 | % ForwardVector - 53 | % 54 | % State dependent variable: 55 | % 56 | % Station - 57 | % Speed - 58 | % Node - 59 | % 60 | % DrivingStrategy methods: 61 | % 62 | % Public Access me 63 | % getStationDistance - 64 | % getSpeed - 65 | % getPositon - 66 | % getUDStates - 67 | % 68 | % 69 | % Note: DrivingStrategy inherits from a MATLAB class meant for internal 70 | % use. It has been tested in MATLAB 2020b, and may not work in future or 71 | % earlier releases 72 | % 73 | % Copyright 2020 - 2020 The MathWorks, Inc. 74 | properties 75 | %Scenario drivingScenario object the 76 | Scenario 77 | %Data structure to store the time series data of vehicle's states. 78 | % Data is a structure where DrivingStrategy store the state information. 79 | % Set the StoreData property to 'true' to store data at all 80 | % time steps. The default is to only store the current and previous time 81 | % step, because this is needed for simulation. 82 | Data = struct('Time',[],... 83 | 'Station',[],... 84 | 'Speed',[],... 85 | 'Node',Node.empty,... 86 | 'UDStates',[],... 87 | 'Position',[],... 88 | 'Yaw',[]); 89 | 90 | %NextNode a vector of type Node containing the vehicle's planned path 91 | NextNode = Node.empty 92 | 93 | %UDStates a vector of doubles used for user defined states. 94 | % UDStates and the methods initializeUDStates and updateUDStates are 95 | % placeholders for states users might want to define in child 96 | % classes of DrivingStrategy, and their dynamics. 97 | % initializeUDStates is called when a vehicle enters the network 98 | % and updateUDstates is called at the end of each time step, when 99 | % all other states have been updated. 100 | UDStates = [nan] 101 | 102 | %Mode string flag indicating driving logic 103 | % Mode is a flag of type string that is set at each timestep by the method 104 | % determineDrivingMode. The possible values in the nominal 105 | % DrivingStrategy class are 'CarFollowing','ApproachingGreenLight', 106 | % and 'ApproachingRedLight'. 107 | Mode = ''; 108 | 109 | % Parameters for car following 110 | 111 | DesiredSpeed = 10; % [m/s] 112 | ReactionTime = 0.8; % [s] 113 | SpeedBounds = [0,15]; 114 | AccBounds = [-5,3]; 115 | 116 | % CarFollowingModel is a flag property of type string indicating 117 | % which car following model to use. Options: 'IDM','Gipps' 118 | CarFollowingModel = 'Gipps'; 119 | 120 | % Flags 121 | 122 | %StoreData flag to indicate wether or not to store the time series data of the states 123 | % Set to true to store time series data in the Data structure 124 | StoreData = true; 125 | 126 | %StaticLaneKeeping flag to indicate wether or not the lateral 127 | %dynamics are enabled 128 | % Set to true to have the vehicle follow the center of the lane 129 | % without worrying about control the sterring of the vehicle. 130 | % The lateral control of the vehicle has not yet been tested. 131 | StaticLaneKeeping = true; 132 | end 133 | 134 | properties (Dependent,Access = protected) 135 | % Vehicle States 136 | Position(3,1) double {mustBeReal, mustBeFinite} 137 | Velocity(3,1) double {mustBeReal, mustBeFinite} 138 | ForwardVector(3,1)double {mustBeReal, mustBeFinite} 139 | Speed 140 | end 141 | 142 | properties (Access = protected) 143 | % Input States 144 | Acceleration(1,1) double {mustBeReal, mustBeFinite} = 0 145 | AngularAcceleration(1,1) double {mustBeReal, mustBeFinite} = 0 146 | end 147 | properties (Access = protected) 148 | % Position Dependent Variables 149 | Node = Node.empty; 150 | Station = []; 151 | % Environment Dependent Variables 152 | IsLeader = false; 153 | Leader = driving.scenario.Vehicle.empty; 154 | LeaderSpacing = nan; 155 | end 156 | 157 | %% Constructor 158 | methods 159 | function obj = DrivingStrategy(egoActor,varargin) 160 | obj@driving.scenario.MotionStrategy(egoActor); 161 | obj@driving.scenario.mixin.PropertiesInitializableInConstructor(varargin{:}); 162 | egoActor.MotionStrategy = obj; 163 | obj.Scenario = egoActor.Scenario; 164 | obj.Speed = obj.DesiredSpeed; 165 | egoActor.IsVisible = false; 166 | end 167 | end 168 | %% Set and Get methods 169 | methods 170 | % Position 171 | function set.Position(obj,pos) 172 | obj.EgoActor.Position = pos; 173 | end 174 | function pos = get.Position(obj) 175 | pos = nan(length(obj),3); 176 | for idx = 1:length(obj) 177 | pos(idx,:) = obj(idx).EgoActor.Position; 178 | end 179 | end 180 | % Velocity 181 | function set.Velocity(obj,vel) 182 | obj.EgoActor.Velocity = vel; 183 | end 184 | function vel = get.Velocity(obj) 185 | vel = nan(length(obj),3); 186 | for idx = 1:length(obj) 187 | vel(idx,:) = obj(idx).EgoActor.Velocity; 188 | end 189 | end 190 | % Forward Vector 191 | function set.ForwardVector(obj,dir) 192 | if all(size(dir)==[3,1]) 193 | dir=dir'; 194 | end 195 | obj.EgoActor.ForwardVector = dir; 196 | end 197 | function dir = get.ForwardVector(obj) 198 | dir = nan(length(obj),3); 199 | for idx = 1:length(obj) 200 | dir(idx,:) = obj(idx).EgoActor.ForwardVector; 201 | end 202 | end 203 | % Speed 204 | function s = get.Speed(obj) 205 | s = obj.ForwardVector*obj.Velocity'; 206 | end 207 | function set.Speed(obj,speed) 208 | obj.EgoActor.Velocity = obj.EgoActor.ForwardVector*speed; 209 | end 210 | 211 | % Node 212 | function set.Node(obj,node) 213 | if node == obj.Node 214 | return 215 | end 216 | 217 | if ~isempty(obj.Node) 218 | removeVehicle(obj.Node,obj.EgoActor); 219 | if ~isempty(node) 220 | addVehicle(node,obj.EgoActor); 221 | end 222 | obj.Node = node; 223 | else 224 | if ~isempty(node) 225 | addVehicle(node,obj.EgoActor); 226 | end 227 | obj.Node = node; 228 | end 229 | 230 | 231 | end 232 | 233 | end 234 | %% Public Access Methods 235 | methods (Access = public) 236 | function s = getStationDistance(obj,time) 237 | if nargin<2 %If no time is given assume current sim time 238 | time = obj(1).Scenario.SimulationTime; 239 | end 240 | dt = obj(1).Scenario.SampleTime; 241 | for idx = 1:length(obj) 242 | tIdx = round((time-obj(idx).Data.Time(1))/dt)+1; 243 | if tIdx>0 && tIdx0 && tIdx0 && tIdx0 && tIdx0 && tIdx2 346 | obj.Speed = speed; 347 | end 348 | obj.Station = 0; 349 | initializeUDStates(obj,t); 350 | addData(obj,t) 351 | obj.EgoActor.IsVisible=true; 352 | 353 | end 354 | 355 | function addData(obj,t) 356 | if ~isempty(obj.Data.Time) 357 | if t~=obj.Data.Time(end)+obj.Scenario.SampleTime 358 | error('Can only add data to the following time step') 359 | end 360 | end 361 | 362 | obj.Data.Time(end+1) = t; 363 | obj.Data.Station(end+1) = obj.Station; 364 | obj.Data.Node(end+1) = obj.Node; 365 | obj.Data.Speed(end+1) = obj.Speed; 366 | obj.Data.Position(end+1,:) = obj.Position; 367 | obj.Data.Yaw(end+1) = obj.EgoActor.Yaw; 368 | obj.Data.UDStates(end+1,:) = obj.UDStates; 369 | 370 | % Delete first entry if store data flag is false 371 | if ~obj.StoreData && length(obj.Data.Time)>2 372 | obj.Data.Time(1) = []; 373 | obj.Data.Station(1) = []; 374 | obj.Data.Node(1) = []; 375 | obj.Data.Speed(1) = []; 376 | obj.Data.Yaw(1) = []; 377 | obj.Data.Position(1,:) = []; 378 | obj.Data.UDStates(1,:) = []; 379 | 380 | 381 | end 382 | end 383 | 384 | function orientEgoActor(obj,direction,offset) 385 | if nargin == 1 386 | [s,direction,offset]= obj.getLaneInformation(); 387 | end 388 | obj.ForwardVector = [direction,0]; 389 | obj.Position = obj.Position + [offset,0]; 390 | end 391 | 392 | function [leader,leaderSpacing] = getLeader(obj,tNow) 393 | % Get other actors in the node and their stations 394 | actors = getVehiclesInSegment(obj); 395 | drivers = [actors.MotionStrategy]; 396 | selfIdx = find(drivers == obj); 397 | stations = [drivers.Station]; 398 | deltaStations = stations - stations(selfIdx); 399 | deltaStations(deltaStations<0.1)=inf; 400 | [leaderSpacing,idx]=min(deltaStations); 401 | if isinf(leaderSpacing) 402 | leaderSpacing = nan; 403 | leader = driving.scenario.Vehicle.empty; 404 | if ~isempty(obj.NextNode) 405 | [sLeader,leader] = obj.NextNode(1).getTrailingVehicleStation(tNow); 406 | if ~isempty(leader) 407 | leaderSpacing = sLeader-leader.Length +(obj.getSegmentLength()-stations(selfIdx)); 408 | end 409 | end 410 | 411 | else 412 | leader = actors(idx); 413 | leaderSpacing = leaderSpacing-leader.Length; 414 | end 415 | 416 | end 417 | 418 | function actors = getVehiclesInSegment(obj) 419 | actors = obj.Node.getActiveVehicles(); 420 | end 421 | 422 | function state = getNextNodeState(obj) 423 | state = 1; 424 | if ~isempty(obj.NextNode) 425 | if ~isempty(obj.NextNode(1).TrafficController) 426 | state = obj.NextNode(1).getNodeState(); 427 | end 428 | end 429 | end 430 | end 431 | %% Nominal Driving Logic 432 | methods 433 | function mode = determineDrivingMode(obj,tNow) 434 | % This function decides which mode(s) the driver is following, 435 | % and the creates and outputs the function handle to that mode 436 | 437 | segLength = obj.getSegmentLength(); 438 | leader = obj.Leader; 439 | leaderSpacing = obj.LeaderSpacing; 440 | distToEnd = segLength-obj.Station; 441 | state = obj.getNextNodeState(); 442 | isLeader = isempty(leader); 443 | 444 | 445 | % Determine the mode 446 | if state 447 | if isLeader 448 | mode = 'ApproachingGreenLight'; 449 | else 450 | mode = 'CarFollowing'; 451 | end 452 | else 453 | if isLeader 454 | mode = 'ApproachingRedLight'; 455 | else 456 | leaderIsPastRoadEnd = getNode(leader.MotionStrategy,tNow)~=obj.Node; 457 | if leaderIsPastRoadEnd 458 | mode = 'ApproachingRedLight'; 459 | else 460 | mode = 'CarFollowing'; 461 | end 462 | end 463 | end 464 | end 465 | 466 | function inputs = determineDrivingInputs(obj,tNow) 467 | 468 | segLength = obj.getSegmentLength(); 469 | leader = obj.Leader; 470 | distToEnd = segLength-obj.Station; 471 | 472 | switch obj.Mode 473 | case 'CarFollowing' 474 | delVel = leader.MotionStrategy.Speed-obj.Speed; 475 | leaderSpacing = obj.LeaderSpacing; 476 | case 'ApproachingRedLight' 477 | delVel = 0 - obj.Speed; 478 | leaderSpacing = distToEnd; 479 | case 'ApproachingGreenLight' 480 | delVel = 0; 481 | leaderSpacing = inf; 482 | end 483 | acc = carFollowing(obj,leaderSpacing,obj.Speed,delVel); 484 | 485 | %Saturate acc 486 | %With velocity bounds 487 | accBounds = (obj.SpeedBounds-obj.Speed)/obj.Scenario.SampleTime; 488 | % With acceleration bounds 489 | accBounds = [max(accBounds(1),obj.AccBounds(1)), min(accBounds(2),obj.AccBounds(2))]; 490 | 491 | acc = min(max(accBounds(1),acc),accBounds(2)); 492 | 493 | inputs = [acc,0]; 494 | 495 | end 496 | 497 | function acc = carFollowing(obj,spacing,speed,speedDiff) 498 | if strcmp(obj.CarFollowingModel,'IDM') 499 | acc = drivingBehavior.intelligentDriverModel(spacing,speed,speedDiff); 500 | end 501 | 502 | if strcmp(obj.CarFollowingModel,'Gipps') 503 | if mod(obj.Scenario.SimulationTime,obj.ReactionTime)=car.ExitTime || tNow0)&&(tNow - car.EntryTime1 549 | speed = min(v_b,v_a); 550 | injectVehicle(obj,tNow,speed); 551 | else 552 | car.EntryTime = car.EntryTime+dt; 553 | running = true; 554 | return; 555 | end 556 | end 557 | %% 558 | %c = discretize(s-5,[-inf,5,10,inf]); 559 | % switch c 560 | % case 1 % 561 | % car.EntryTime = car.EntryTime+dt; 562 | % running = true; 563 | % return; 564 | % case 2 565 | % if ~isempty(leader) 566 | % speed = leader.MotionStrategy.getSpeed(tNow); 567 | % else 568 | % speed=obj.Speed; 569 | % end 570 | % injectVehicle(obj,tNow); 571 | % case 3 572 | % injectVehicle(obj,tNow,obj.Speed); 573 | % 574 | % end 575 | end 576 | 577 | %% Set vehicle's state and variables to tNow 578 | % State 579 | obj.Position = getPosition(obj,tNow); 580 | obj.Speed = getSpeed(obj,tNow); 581 | % State dependent variables 582 | obj.Station = getStationDistance(obj,tNow); 583 | obj.Node = getNode(obj,tNow); 584 | obj.UDStates = getUDStates(obj,tNow); 585 | 586 | % Environment Dependent Variables 587 | [obj.Leader,obj.LeaderSpacing]=getLeader(obj,tNow); 588 | if isempty(obj.Leader) 589 | obj.IsLeader = true; 590 | end 591 | %% Determine Driving Mode 592 | obj.Mode = determineDrivingMode(obj,tNow); 593 | 594 | %% Get Inputs 595 | inputs = determineDrivingInputs(obj,tNow); 596 | 597 | obj.Acceleration = inputs(1); 598 | obj.AngularAcceleration = inputs(2); 599 | 600 | %% Integrate 601 | 602 | % Position and Velocity 603 | obj.Position = obj.Position + dt*car.Velocity; 604 | obj.Speed = obj.Speed + dt*obj.Acceleration; 605 | 606 | % Forward Vector 607 | %obj.ForwardVector = ... to be implemented 608 | 609 | %% Update state dependent variables 610 | % Get new station distance, node and lane information 611 | [station, direction, offset] = getLaneInformation(obj); 612 | 613 | if station > getSegmentLength(obj) % Check if the veh entered a new node 614 | goToNextNode(obj,tNext) 615 | if isempty(obj.Node) 616 | % The vehicle finished its path 617 | running = false; 618 | return 619 | else 620 | % Get new station andlane information 621 | [station, direction, offset] = getLaneInformation(obj); 622 | end 623 | 624 | end 625 | obj.Station = station; 626 | updateUDStates(obj,tNow); 627 | 628 | if obj.StaticLaneKeeping %Orient veh along lane if lateral control is deactivated 629 | obj.orientEgoActor(direction,offset); 630 | end 631 | 632 | addData(obj,tNext); 633 | running = true; 634 | end 635 | 636 | function restart(obj) 637 | % Needs to be programmed 638 | 639 | end 640 | end 641 | end 642 | 643 | -------------------------------------------------------------------------------- /@DrivingStrategy/DrivingStrategy.m: -------------------------------------------------------------------------------- 1 | classdef DrivingStrategy < driving.scenario.MotionStrategy... 2 | & driving.scenario.mixin.PropertiesInitializableInConstructor 3 | %DrivingStrategy Driving Strategy 4 | % DrivingStrategy is used to control vehicles in an OppenTrafficLab 5 | % simulation. The driving logic consists of car following behavior, 6 | % suitably modified to react to open or closed lanes ahead. 7 | % To implemnent a custom driving strategy, use this class as a parent 8 | % class and override the relevant methods 9 | % 10 | % strategy = DrivingStrategy(vehicle) creates a nominal driving strategy 11 | % for the actor vehicle 12 | % 13 | % strategy = DrivingStrategy(...,Name,Value) creates a nominal driving strategy 14 | % for the actor vehicle 15 | % 16 | % Overview 17 | % DrivingStrategy controls the vehicles in its move method, which is 18 | % called every time the advance method of the drivingScenario being 19 | % simulated is called. The move method moves the vehicle to the requested 20 | % time. It does so by setting all states and state dependent variables to 21 | % the immediatly previous time step, computing the control input for that 22 | % set of states, and integrating the states forward in time by one time 23 | % step. The move method perform the following steps 24 | % 25 | % 1. Check if vehicle has entered the network or is currently in the network. If not break anf continue 26 | % 2. Set the vehicle's states nadd state dependent variables to the 27 | % correct their value at the correct time step (i.e one time step before the target time the vehicle is being moved to) 28 | % 3. Call determineDrivingMode 29 | % 4. Call determineDrivingInput 30 | % 5. Integrate states from to the requested time 31 | % 6. Update state dependent variables based on the new value for the 32 | % states 33 | % 34 | % DrivingStrategy properties: 35 | % 36 | % NextNode - list of Nodes the vehicle will traverse in its path 37 | % UDStates - user defined states 38 | % Mode - flag indicating driving mode (i.e. 'CarFollowing', 'ApproachingRedLight') 39 | % Data - data structure for saving time series info. 40 | % 41 | % Parameters: 42 | % 43 | % DesiredSpeed - 44 | % ReactionTime - 45 | % SpeedBounds - 46 | % AccBounds - 47 | % 48 | % States: 49 | % 50 | % Position - 51 | % Velocity - 52 | % ForwardVector - 53 | % 54 | % State dependent variable: 55 | % 56 | % Station - 57 | % Speed - 58 | % Node - 59 | % 60 | % DrivingStrategy methods: 61 | % 62 | % Public Access methods: 63 | % 64 | % getStationDistance - 65 | % getSpeed - 66 | % getPositon - 67 | % getUDStates - 68 | % 69 | % User defined methods: 70 | % 71 | % updateUDStates - 72 | % initializeUDStates - 73 | % 74 | % Driving Logic methods: 75 | % 76 | % determineDrivingMode - 77 | % determineDrivingInputs - 78 | % carFollowing - 79 | % 80 | % 81 | % Note: DrivingStrategy inherits from a MATLAB class meant for internal 82 | % use. It has been tested in MATLAB 2020b, and may not work in future or 83 | % earlier releases 84 | % 85 | % Copyright 2020 - 2020 The MathWorks, Inc. 86 | %% Properties 87 | properties 88 | %Scenario drivingScenario object the 89 | Scenario 90 | %Data structure to store the time series data of vehicle's states. 91 | % Data is a structure where DrivingStrategy store the state information. 92 | % Set the StoreData property to 'true' to store data at all 93 | % time steps. The default is to only store the current and previous time 94 | % step, because this is needed for simulation. 95 | Data = struct('Time',[],... 96 | 'Station',[],... 97 | 'Speed',[],... 98 | 'Node',Node.empty,... 99 | 'UDStates',[],... 100 | 'Position',[],... 101 | 'Yaw',[]); 102 | 103 | %NextNode a vector of type Node containing the vehicle's planned path 104 | NextNode = Node.empty 105 | 106 | %UDStates a vector of doubles used for user defined states. 107 | % UDStates and the methods initializeUDStates and updateUDStates are 108 | % placeholders for states users might want to define in child 109 | % classes of DrivingStrategy, and their dynamics. 110 | % initializeUDStates is called when a vehicle enters the network 111 | % and updateUDstates is called at the end of each time step, when 112 | % all other states have been updated. 113 | UDStates = [nan] 114 | 115 | %Mode string flag indicating driving logic 116 | % Mode is a flag of type string that is set at each timestep by the method 117 | % determineDrivingMode. The possible values in the nominal 118 | % DrivingStrategy class are 'CarFollowing','ApproachingGreenLight', 119 | % and 'ApproachingRedLight'. 120 | Mode = ''; 121 | 122 | % Parameters for car following 123 | 124 | DesiredSpeed = 10; % [m/s] 125 | ReactionTime = 0.8; % [s] 126 | SpeedBounds = [0,15]; 127 | AccBounds = [-5,3]; 128 | 129 | % CarFollowingModel is a flag property of type string indicating 130 | % which car following model to use. Options: 'IDM','Gipps' 131 | CarFollowingModel = 'Gipps'; 132 | 133 | % Flags 134 | 135 | %StoreData flag to indicate wether or not to store the time series data of the states 136 | % Set to true to store time series data in the Data structure 137 | StoreData = true; 138 | 139 | %StaticLaneKeeping flag to indicate wether or not the lateral 140 | %dynamics are enabled 141 | % Set to true to have the vehicle follow the center of the lane 142 | % without worrying about control the sterring of the vehicle. 143 | % The lateral control of the vehicle has not yet been tested. 144 | StaticLaneKeeping = true; 145 | end 146 | 147 | properties (Dependent,Access = protected) 148 | % Vehicle States 149 | Position(3,1) double {mustBeReal, mustBeFinite} 150 | Velocity(3,1) double {mustBeReal, mustBeFinite} 151 | ForwardVector(3,1)double {mustBeReal, mustBeFinite} 152 | Speed 153 | end 154 | 155 | properties (Access = protected) 156 | % Input States 157 | Acceleration(1,1) double {mustBeReal, mustBeFinite} = 0 158 | AngularAcceleration(1,1) double {mustBeReal, mustBeFinite} = 0 159 | end 160 | properties (Access = protected) 161 | % Position Dependent Variables 162 | Node = Node.empty; 163 | Station = []; 164 | % Environment Dependent Variables 165 | IsLeader = false; 166 | Leader = driving.scenario.Vehicle.empty; 167 | LeaderSpacing = nan; 168 | end 169 | 170 | %% Constructor 171 | methods 172 | function obj = DrivingStrategy(egoActor,varargin) 173 | obj@driving.scenario.MotionStrategy(egoActor); 174 | obj@driving.scenario.mixin.PropertiesInitializableInConstructor(varargin{:}); 175 | egoActor.MotionStrategy = obj; 176 | obj.Scenario = egoActor.Scenario; 177 | obj.Speed = obj.DesiredSpeed; 178 | egoActor.IsVisible = false; 179 | end 180 | end 181 | %% Set and Get methods 182 | methods 183 | % Position 184 | function set.Position(obj,pos) 185 | obj.EgoActor.Position = pos; 186 | end 187 | function pos = get.Position(obj) 188 | pos = nan(length(obj),3); 189 | for idx = 1:length(obj) 190 | pos(idx,:) = obj(idx).EgoActor.Position; 191 | end 192 | end 193 | % Velocity 194 | function set.Velocity(obj,vel) 195 | obj.EgoActor.Velocity = vel; 196 | end 197 | function vel = get.Velocity(obj) 198 | vel = nan(length(obj),3); 199 | for idx = 1:length(obj) 200 | vel(idx,:) = obj(idx).EgoActor.Velocity; 201 | end 202 | end 203 | % Forward Vector 204 | function set.ForwardVector(obj,dir) 205 | if all(size(dir)==[3,1]) 206 | dir=dir'; 207 | end 208 | obj.EgoActor.ForwardVector = dir; 209 | end 210 | function dir = get.ForwardVector(obj) 211 | dir = nan(length(obj),3); 212 | for idx = 1:length(obj) 213 | dir(idx,:) = obj(idx).EgoActor.ForwardVector; 214 | end 215 | end 216 | % Speed 217 | function s = get.Speed(obj) 218 | s = obj.ForwardVector*obj.Velocity'; 219 | end 220 | function set.Speed(obj,speed) 221 | obj.EgoActor.Velocity = obj.EgoActor.ForwardVector*speed; 222 | end 223 | 224 | % Node 225 | function set.Node(obj,node) 226 | if node == obj.Node 227 | return 228 | end 229 | 230 | if ~isempty(obj.Node) 231 | removeVehicle(obj.Node,obj.EgoActor); 232 | if ~isempty(node) 233 | addVehicle(node,obj.EgoActor); 234 | end 235 | obj.Node = node; 236 | else 237 | if ~isempty(node) 238 | addVehicle(node,obj.EgoActor); 239 | end 240 | obj.Node = node; 241 | end 242 | 243 | 244 | end 245 | 246 | end 247 | %% Public Access Methods 248 | methods (Access = public) 249 | function s = getStationDistance(obj,time) 250 | if nargin<2 %If no time is given assume current sim time 251 | time = obj(1).Scenario.SimulationTime; 252 | end 253 | dt = obj(1).Scenario.SampleTime; 254 | for idx = 1:length(obj) 255 | tIdx = round((time-obj(idx).Data.Time(1))/dt)+1; 256 | if tIdx>0 && tIdx0 && tIdx0 && tIdx0 && tIdx0 && tIdx2 359 | obj.Speed = speed; 360 | end 361 | obj.Station = 0; 362 | initializeUDStates(obj,t); 363 | addData(obj,t) 364 | obj.EgoActor.IsVisible=true; 365 | 366 | end 367 | 368 | function addData(obj,t) 369 | if ~isempty(obj.Data.Time) 370 | if t~=obj.Data.Time(end)+obj.Scenario.SampleTime 371 | error('Can only add data to the following time step') 372 | end 373 | end 374 | 375 | obj.Data.Time(end+1) = t; 376 | obj.Data.Station(end+1) = obj.Station; 377 | obj.Data.Node(end+1) = obj.Node; 378 | obj.Data.Speed(end+1) = obj.Speed; 379 | obj.Data.Position(end+1,:) = obj.Position; 380 | obj.Data.Yaw(end+1) = obj.EgoActor.Yaw; 381 | obj.Data.UDStates(end+1,:) = obj.UDStates; 382 | 383 | % Delete first entry if store data flag is false 384 | if ~obj.StoreData && length(obj.Data.Time)>2 385 | obj.Data.Time(1) = []; 386 | obj.Data.Station(1) = []; 387 | obj.Data.Node(1) = []; 388 | obj.Data.Speed(1) = []; 389 | obj.Data.Yaw(1) = []; 390 | obj.Data.Position(1,:) = []; 391 | obj.Data.UDStates(1,:) = []; 392 | 393 | 394 | end 395 | end 396 | 397 | function orientEgoActor(obj,direction,offset) 398 | if nargin == 1 399 | [s,direction,offset]= obj.getLaneInformation(); 400 | end 401 | obj.ForwardVector = [direction,0]; 402 | obj.Position = obj.Position + [offset,0]; 403 | end 404 | 405 | function [leader,leaderSpacing] = getLeader(obj,tNow) 406 | % Get other actors in the node and their stations 407 | actors = getVehiclesInSegment(obj); 408 | drivers = [actors.MotionStrategy]; 409 | selfIdx = find(drivers == obj); 410 | stations = [drivers.Station]; 411 | deltaStations = stations - stations(selfIdx); 412 | deltaStations(deltaStations<0.1)=inf; 413 | [leaderSpacing,idx]=min(deltaStations); 414 | if isinf(leaderSpacing) 415 | leaderSpacing = nan; 416 | leader = driving.scenario.Vehicle.empty; 417 | if ~isempty(obj.NextNode) 418 | [sLeader,leader] = obj.NextNode(1).getTrailingVehicleStation(tNow); 419 | if ~isempty(leader) 420 | leaderSpacing = sLeader-leader.Length +(obj.getSegmentLength()-stations(selfIdx)); 421 | end 422 | end 423 | 424 | else 425 | leader = actors(idx); 426 | leaderSpacing = leaderSpacing-leader.Length; 427 | end 428 | 429 | end 430 | 431 | function actors = getVehiclesInSegment(obj) 432 | actors = obj.Node.getActiveVehicles(); 433 | end 434 | 435 | function state = getNextNodeState(obj) 436 | state = 1; 437 | if ~isempty(obj.NextNode) 438 | if ~isempty(obj.NextNode(1).TrafficController) 439 | state = obj.NextNode(1).getNodeState(); 440 | end 441 | end 442 | end 443 | end 444 | %% Nominal Driving Logic 445 | methods 446 | function mode = determineDrivingMode(obj,tNow) 447 | % This function decides which mode(s) the driver is following, 448 | % and the creates and outputs the function handle to that mode 449 | 450 | segLength = obj.getSegmentLength(); 451 | leader = obj.Leader; 452 | leaderSpacing = obj.LeaderSpacing; 453 | distToEnd = segLength-obj.Station; 454 | state = obj.getNextNodeState(); 455 | isLeader = isempty(leader); 456 | 457 | 458 | % Determine the mode 459 | if state 460 | if isLeader 461 | mode = 'ApproachingGreenLight'; 462 | else 463 | mode = 'CarFollowing'; 464 | end 465 | else 466 | if isLeader 467 | mode = 'ApproachingRedLight'; 468 | else 469 | leaderIsPastRoadEnd = getNode(leader.MotionStrategy,tNow)~=obj.Node; 470 | if leaderIsPastRoadEnd 471 | mode = 'ApproachingRedLight'; 472 | else 473 | mode = 'CarFollowing'; 474 | end 475 | end 476 | end 477 | end 478 | 479 | function inputs = determineDrivingInputs(obj,tNow) 480 | 481 | segLength = obj.getSegmentLength(); 482 | leader = obj.Leader; 483 | distToEnd = segLength-obj.Station; 484 | 485 | switch obj.Mode 486 | case 'CarFollowing' 487 | delVel = leader.MotionStrategy.Speed-obj.Speed; 488 | leaderSpacing = obj.LeaderSpacing; 489 | case 'ApproachingRedLight' 490 | delVel = 0 - obj.Speed; 491 | leaderSpacing = distToEnd; 492 | case 'ApproachingGreenLight' 493 | delVel = 0; 494 | leaderSpacing = inf; 495 | end 496 | acc = carFollowing(obj,leaderSpacing,obj.Speed,delVel); 497 | 498 | %Saturate acc 499 | %With velocity bounds 500 | accBounds = (obj.SpeedBounds-obj.Speed)/obj.Scenario.SampleTime; 501 | % With acceleration bounds 502 | accBounds = [max(accBounds(1),obj.AccBounds(1)), min(accBounds(2),obj.AccBounds(2))]; 503 | 504 | acc = min(max(accBounds(1),acc),accBounds(2)); 505 | 506 | inputs = [acc,0]; 507 | 508 | end 509 | 510 | function acc = carFollowing(obj,spacing,speed,speedDiff) 511 | if strcmp(obj.CarFollowingModel,'IDM') 512 | acc = drivingBehavior.intelligentDriverModel(spacing,speed,speedDiff); 513 | end 514 | 515 | if strcmp(obj.CarFollowingModel,'Gipps') 516 | if mod(obj.Scenario.SimulationTime,obj.ReactionTime)=car.ExitTime || tNow0)&&(tNow - car.EntryTime1 562 | speed = min(v_b,v_a); 563 | injectVehicle(obj,tNow,speed); 564 | else 565 | car.EntryTime = car.EntryTime+dt; 566 | running = true; 567 | return; 568 | end 569 | end 570 | %% 571 | %c = discretize(s-5,[-inf,5,10,inf]); 572 | % switch c 573 | % case 1 % 574 | % car.EntryTime = car.EntryTime+dt; 575 | % running = true; 576 | % return; 577 | % case 2 578 | % if ~isempty(leader) 579 | % speed = leader.MotionStrategy.getSpeed(tNow); 580 | % else 581 | % speed=obj.Speed; 582 | % end 583 | % injectVehicle(obj,tNow); 584 | % case 3 585 | % injectVehicle(obj,tNow,obj.Speed); 586 | % 587 | % end 588 | end 589 | 590 | %% Set vehicle's state and variables to tNow 591 | % State 592 | obj.Position = getPosition(obj,tNow); 593 | obj.Speed = getSpeed(obj,tNow); 594 | % State dependent variables 595 | obj.Station = getStationDistance(obj,tNow); 596 | obj.Node = getNode(obj,tNow); 597 | obj.UDStates = getUDStates(obj,tNow); 598 | 599 | % Environment Dependent Variables 600 | [obj.Leader,obj.LeaderSpacing]=getLeader(obj,tNow); 601 | if isempty(obj.Leader) 602 | obj.IsLeader = true; 603 | end 604 | %% Determine Driving Mode 605 | obj.Mode = determineDrivingMode(obj,tNow); 606 | 607 | %% Get Inputs 608 | inputs = determineDrivingInputs(obj,tNow); 609 | 610 | obj.Acceleration = inputs(1); 611 | obj.AngularAcceleration = inputs(2); 612 | 613 | %% Integrate 614 | 615 | % Position and Velocity 616 | obj.Position = obj.Position + dt*car.Velocity; 617 | obj.Speed = obj.Speed + dt*obj.Acceleration; 618 | 619 | % Forward Vector 620 | %obj.ForwardVector = ... to be implemented 621 | 622 | %% Update state dependent variables 623 | % Get new station distance, node and lane information 624 | [station, direction, offset] = getLaneInformation(obj); 625 | 626 | if station > getSegmentLength(obj) % Check if the veh entered a new node 627 | goToNextNode(obj,tNext) 628 | if isempty(obj.Node) 629 | % The vehicle finished its path 630 | running = false; 631 | return 632 | else 633 | % Get new station andlane information 634 | [station, direction, offset] = getLaneInformation(obj); 635 | end 636 | 637 | end 638 | obj.Station = station; 639 | updateUDStates(obj,tNow); 640 | 641 | if obj.StaticLaneKeeping %Orient veh along lane if lateral control is deactivated 642 | obj.orientEgoActor(direction,offset); 643 | end 644 | 645 | addData(obj,tNext); 646 | running = true; 647 | end 648 | 649 | function restart(obj) 650 | % Needs to be programmed 651 | 652 | end 653 | end 654 | end 655 | 656 | --------------------------------------------------------------------------------