init commit

DrivingScenarioEnv.m

@@ -0,0 +1,193 @@
+classdef DrivingScenarioEnv < rl.env.MATLABEnvironment
+    % Copyright 2020 The MathWorks, Inc.
+    %MYENVIRONMENT: Template for defining custom environment in MATLAB.    
+
+    % parameters for simulation environment
+    properties
+        scenario
+        network
+        traffic
+        cars
+        state
+        driver
+        InjectionRate
+        TurnRatio
+        N = 3 % number of road
+        phaseDuration = 50 % time duration for each of the phase
+        T 
+    end
+
+    % simulation doesn't have yellow light
+    % manually set up clearning phase here if needed
+    properties
+        clearingPhase = false
+        clearingPhaseTime = 0
+        TrafficSignalDesign
+        ObservationSpaceDesign
+    end
+
+    % parameter for reward definition
+    properties
+       rewardForPass = 0
+       vehicleEnterJunction % keep record of cars pass the intersection
+       hitPenalty = 20
+       penaltyForFreqSwitch = 1
+       safeDistance = 2.25 % check collision 
+       slowSpeedThreshold = 3.5 % check whether car is waiting
+    end
+
+    properties
+       recordVid = false
+       vid 
+    end
+
+    properties
+        discrete_action = [0 1 2];
+        dim =10;
+    end
+
+    properties(Access = protected)
+        IsDone = false        
+    end
+
+    %% Necessary Methods
+    methods              
+        function this = DrivingScenarioEnv()
+            % Initialize Observation settings
+            ObservationInfo = rlNumericSpec([10, 1]); % # of state
+            ObservationInfo.Name = 'real-time traffic information';
+            ObservationInfo.Description = '';
+
+            % Initialize action settings   
+            ActionInfo = rlFiniteSetSpec([0 1 2]); % three phases
+            ActionInfo.Name = 'traffic signal phases';
+
+            % The following line implements built-in functions of the RL environment
+            this = [email protected](ObservationInfo,ActionInfo);
+        end
+
+        function [state, Reward,IsDone,LoggedSignals] = step(this, Action)
+            Action = getForce(this, Action);
+            % update the action
+            pre_phase = this.traffic.IsOpen;
+            if this.TrafficSignalDesign == 1
+                cur_phase = signalPhaseDesign1(Action);
+            elseif this.TrafficSignalDesign == 2
+                cur_phase = signalPhaseDesign2(Action);
+            elseif this.TrafficSignalDesign == 3
+                cur_phase = signalPhaseDesign3(Action);
+            end
+
+            % Reward: penalty for signal phase switch
+            changed = ~isequal(pre_phase, cur_phase);
+            Reward = this.penaltyForFreqSwitch * (1 - changed);
+
+            % (yellow light time)add clearing phase when signal phase switch 
+            if changed && this.clearingPhase
+                for i = 1:this.clearingPhaseTime
+                    this.traffic.IsOpen = [0, 0, 0, 0, 0, 0];
+                    advance(this.scenario);   
+                    this.T = this.T + this.scenario.SampleTime;
+                    notifyEnvUpdated(this);
+                    % check terminal condition
+                    IsHit = checkCollision(this);
+                    Reward = Reward - IsHit * this.hitPenalty;
+                    this.IsDone = IsHit || this.T+0.5 >= this.scenario.StopTime;
+                    if this.IsDone
+                        break
+                    end
+                end
+            end
+
+            % (green light time)simulate the signal phase based on the action by RL
+            this.traffic.IsOpen = cur_phase;
+            if ~this.IsDone
+                for i = 1:this.phaseDuration 
+                    % update traffic state
+                    advance(this.scenario);   
+                    this.T = this.T + this.scenario.SampleTime;
+                    % update visulization
+                    notifyEnvUpdated(this);
+                    % check terminal condition
+                    IsHit = checkCollision(this);
+                    Reward = Reward - IsHit * this.hitPenalty;
+                    this.IsDone = IsHit || this.T+0.5 >= this.scenario.StopTime;
+                    if this.IsDone
+                        break
+                    end
+                    % obtain reward
+                    Reward = Reward + obtainReward(this, cur_phase);
+                end
+            end
+            if this.ObservationSpaceDesign == 1
+                state = observationSpace1(this, Action);
+            else
+                state = observationSpace2(this, Action);
+            end
+            this.state = state;
+            IsDone = this.IsDone;
+            LoggedSignals = [];
+        end
+
+
+        function InitialState = reset(this)
+            % flag for record simulation 
+            this.recordVid = false;
+            % Initialize scenario
+            this.scenario = createTJunctionScenario();
+            this.scenario.StopTime = 100;
+            this.scenario.SampleTime = 0.05;
+            this.T = 0;
+            % initialize network
+            this.network = createTJunctionNetwork(this.scenario);
+            this.traffic = trafficControl.TrafficController(this.network(7:12));
+            % car parameters
+            this.InjectionRate = [250, 250, 250]; % veh/hour
+            this.TurnRatio = [50, 50];
+            this.cars = createVehiclesForTJunction(this.scenario, this.network, this.InjectionRate, this.TurnRatio);
+            this.vehicleEnterJunction = [];
+            % obtain state from traffic and network
+            if this.ObservationSpaceDesign == 1
+                InitialState = observationSpace1(this, 0);
+            else
+                InitialState = observationSpace2(this, 0);
+            end
+            % visulization
+            notifyEnvUpdated(this);            
+        end
+    end
+
+    methods               
+        function force = getForce(this,action)
+            if ~ismember(action,this.ActionInfo.Elements)
+                error('Action must be integer from 1 to numAction');
+            end
+            force = action;           
+        end
+        % update the action info based on max force
+        function updateActionInfo(this)        
+              this.ActionInfo.Elements = this.discrete_action;
+        end        
+    end
+
+    methods (Access = protected)
+        function envUpdatedCallback(this)
+            if this.T == 0
+                close all;
+                plot(this.scenario)
+                set(gcf,'Visible','On');
+                if this.recordVid
+                    this.vid = VideoWriter('baseRLlearningProcess33');
+                    this.vid.FrameRate=20;
+                    open(this.vid)
+                end
+            end
+            if this.recordVid
+                frame = getframe(gcf);
+                writeVideo(this.vid,frame);
+            end 
+            this.traffic.plotOpenPaths()
+            drawnow
+        end
+    end
+end

OpenTrafficLab/+drivingBehavior/gippsDriverModel.m

@@ -0,0 +1,25 @@
+function [acc,v_b,v_a] = gippsDriverModel(spacing,speed,speedDiff,varargin)
+
+desiredSpeed = 10; %Desired speed
+maxSpeed = 20; %max. speed
+minSpeed = 0; % min. speed
+minAcc = -3; %min. acceleration
+maxAcc = 3; %max. acceleration
+minAccEstimate = minAcc;
+reactionTime = 0.8;
+S0 = 2;
+
+v_now = speed;
+v_infront = speed+speedDiff;
+
+v_a = v_now + 2.5*maxAcc*reactionTime*(1-v_now/desiredSpeed)*sqrt(0.025+v_now/desiredSpeed);
+v_b = minAcc*reactionTime + sqrt((minAcc*reactionTime)^2-minAcc*(2*(spacing-S0)-v_now*reactionTime-v_infront^2/minAccEstimate));
+
+v_b = max(v_b,minSpeed);
+v_a = min(v_a,maxSpeed);
+v_new = min([v_a,v_b]);
+
+acc = (v_new-v_now)/reactionTime;
+
+end
+

OpenTrafficLab/+drivingBehavior/intelligentDriverModel.m

@@ -0,0 +1,30 @@
+function [acc] = intelligentDriverModel(spacing,speed,speedDiff,varargin)
+    % Parameters
+        % NAME          REALISTIC BOUNDS      DEFAULT     UNITS
+        % desiredSpeed       [0,11]              33         m/s
+        % safeHeadway        [1,3]               1.6          s
+        % accelMax           [0.5,2]             0.73     m/s^2
+        % decelConf          [0.5,2]             1.67     m/s^2
+        % beta                                    4
+        % minJamSpacing      [0,5]                2           m
+        % nonLinJamSpacing   [0,5]                3           m
+
+    % Need to program input parse to pass new parameters
+    desiredSpeed       = 10;
+    safeHeadway        = 1.6;
+    accelMax           = 0.73;
+    decelConf          = 1.67;
+    beta               = 4;
+    minJamSpacing      = 2;
+    nonLinJamSpacing   = 0;
+
+    desiredSpacing = minJamSpacing + nonLinJamSpacing*sqrt(speed/desiredSpeed)...
+                     +speed*safeHeadway-speed*speedDiff/2/sqrt(accelMax*decelConf);
+
+    acc = accelMax*(1-(speed/desiredSpeed)^beta-(desiredSpacing/spacing)^2);
+
+    if acc<-10 || acc>3 || isnan(acc)
+        acc;
+    end
+end
+

OpenTrafficLab/+trafficControl/TrafficController.m

@@ -0,0 +1,86 @@
+classdef TrafficController  < driving.scenario.MotionStrategy...
+        & driving.scenario.mixin.PropertiesInitializableInConstructor
+
+
+    properties 
+        Scenario
+        Nodes = Node.empty % List of nodes the traffic controller manages
+        IsOpen % Boolean list indicating wether the node can be entered
+        PlotHandles = plot3([],[],[]);
+    end
+
+    methods
+        function obj = TrafficController(nodes,varargin)
+
+            [email protected](nodes(1).Scenario.actor);
+            [email protected](varargin{:});
+
+            obj.EgoActor.MotionStrategy = obj;
+            obj.EgoActor.IsVisible = false;
+            %obj.EgoActor.Position = nodes(1).getRoadCenterFromStation(10);
+
+            obj.Scenario = nodes(1).Scenario;
+            obj.Nodes = nodes;
+            obj.IsOpen = false(size(nodes));
+
+        end
+
+        function set.Nodes(obj,nodes)
+            for node = nodes
+                if ~any(obj.Nodes==node)
+                    obj.Nodes(end+1)=node;
+                    node.TrafficController=obj;
+                end
+            end
+        end
+
+        function running = move(obj,SimulationTime)
+
+            running = true;
+        end
+
+        function running = restart(obj,inputArg)
+            %METHOD1 Summary of this method goes here
+            %   Detailed explanation goes here
+            outputArg = obj.Property1 + inputArg;
+        end
+    end
+
+    methods
+        function state = getNodeState(obj,node)
+            state = obj.IsOpen(obj.Nodes==node);
+        end
+    end
+
+    methods % plot methods
+        function plotOpenPaths(obj,ax)
+            green = [0.4660 0.6740 0.1880];
+            red = [0.6350 0.0780 0.1840];
+            yellow = [0.9290 0.6940 0.1250];
+            if nargin<2
+                ax=gca;
+            end
+            hold on
+            if isempty(obj.PlotHandles)
+                for node=obj.Nodes
+                    obj.PlotHandles(end+1) = plot3(ax,node.Mapping(:,2),node.Mapping(:,3),node.Mapping(:,4)+10);
+                end
+            end
+            for idx = 1:length(obj.Nodes)
+                p = obj.PlotHandles(idx);
+                node = obj.Nodes(idx);
+                p.XData = node.Mapping(:,2);
+                p.YData = node.Mapping(:,3);
+                p.ZData = node.Mapping(:,4)+0;
+
+                p.LineWidth = 2;
+                if obj.IsOpen(idx)==true
+                    p.Color = [green,1];
+                else
+                    p.Color = [red,0.2];
+                end
+            end
+        end
+    end
+end
+

OpenTrafficLab/+trafficControl/TrafficLight.m

@@ -0,0 +1,28 @@
+classdef TrafficLight < trafficControl.TrafficController
+
+    properties
+        Cliques
+        Cycle
+        Phase
+    end
+
+    methods
+        function obj = TrafficLight(nodes,varargin)
+            [email protected](nodes,varargin{:});
+        end
+
+        function running = move(obj,SimulationTime)
+            obj.IsOpen = false(size(obj.Nodes));
+            t = mod(SimulationTime,obj.Cycle(end));
+            phase = discretize(t,obj.Cycle);
+            numPhases = max(obj.Cliques);
+            for i =1:numPhases
+                if phase==i
+                    obj.IsOpen(obj.Cliques==i)=true;
+                end
+            end
+            running = true;
+        end
+    end
+end
+