%% Create Trajectory with Wait Time at Intersection

% Copyright 2020 The MathWorks, Inc.

%%
% Create a driving scenario containing a four-way intersection.
scenario = drivingScenario('SampleTime',0.02,'StopTime',20);

roadCenters = [0 0; 50 0];
laneSpecification = lanespec([1 1]);
road(scenario,roadCenters,'Lanes',laneSpecification);

roadCenters = [25 25; 25 -25];
road(scenario,roadCenters,'Lanes',laneSpecification);

%%
% Add the ego vehicle, which travels north but waits for one second at the
% intersection.
ego = vehicle(scenario,'ClassID',1,'Position',[2 -2 0]);
waypoints = [2 -2; 17.5 -2; 45 -2];
speed = [5 0 5];
waittime = [0 1 0];
smoothTrajectory(ego,waypoints,speed,waittime);

%%
% Add a bicyclist that travels east through the intersection at a constant
% speed without stopping.
bicycle = actor(scenario, ...
    'ClassID',3, ...
    'Length',1.7, ...
    'Width',0.45, ...
    'Height',1.7, ...
    'Position',[23 23 0]);
waypoints = [23 23; 23 -23];
speed = 4;
smoothTrajectory(bicycle,waypoints,speed);

%%
% Plot the scenario. The vehicle stops at the intersection for one second,
% then resumes driving after the bicyclist crosses the intersection.
plot(scenario)
while advance(scenario)
    pause(scenario.SampleTime)
end