%% Import Zenrin Japan Map 3.0 (Itsumo NAVI API 3.0) Roads Using Specified Coordinates
% Import Zenrin Japan Map 3.0 (Itsumo NAVI API 3.0) road network data that is nearest to the coordinates of a specified driving route into a driving scenario. Plot a vehicle following this route in the driving scenario.
% Load a sequence of geographic coordinates that correspond to a driving route.

data = load('tokyoSequence.mat');
lat = data.latitude;
lon = data.longitude;

% Display the route by streaming the coordinates on a geographic player. Set the zoom level to 17 and configure the player to display all points in its history. To speed up the streaming, plot only every tenth coordinate in the route.

zoomLevel = 17;
player = geoplayer(lat(1),lon(1),zoomLevel,'HistoryDepth',Inf);
timestep = 10;

for i = 1:timestep:length(lat)
    plotPosition(player,lat(i),lon(i));
end

%
% Create a driving scenario. Import the Zenrin Japan Map 3.0 (Itsumo NAVI API 3.0) road data that is nearest to the driving route into the scenario.

scenario = drivingScenario;
roadNetwork(scenario,'ZenrinJapanMap',lat,lon)

%
% Use the latlon2local function to convert the driving route from geographic coordinates to local east-north-up (ENU) Cartesian coordinates used in the driving scenario. For the origin of the ENU coordinate system, use the geographic road network origin stored in the GeoReference property of the scenario. The origin is the first coordinate specified in the driving route.

alt = 0;
origin = scenario.GeoReference;
[xEast,yNorth,zUp] = latlon2local(lat,lon,alt,origin);

%
% Add a vehicle to the driving scenario. Specify the converted driving route as the trajectory of the vehicle. Set a vehicle speed of 30 meters per second.

v = vehicle(scenario,'ClassID',1);
speed = 30;
smoothTrajectory(v,[xEast,yNorth,zUp],speed);

%
% Plot the scenario and pause every 0.01 seconds to slow down the simulation. To maintain the same alignment with geographic coordinate displays, the X-axis is on the bottom and the Y-axis is on the left. In driving scenarios not imported from maps, the X-axis is on the left and the Y-axis is on the bottom. This alignment is consistent with the Automated Driving Toolbox™ world coordinate system.

figure 
plot(scenario)
while advance(scenario)
    pause(0.01)
end

% Copyright 2020 The MathWorks, Inc.