%% Import OpenStreetMap Road Network and Plot Route
% Import roads from the OpenStreetMap(R) web service into a driving
% scenario. Then, plot a vehicle following a route in the imported road
% network.

% Copyright 2020 The MathWorks, Inc.

%%
% Import a road network of the MathWorks(R) Apple Hill campus into an empty
% driving scenario. The file was downloaded from
% https://www.openstreetmap.org, which provides access to crowd-sourced
% map data all over the world. The data is licensed under the Open Data
% Commons Open Database License (ODbL),
% https://opendatacommons.org/licenses/odbl/.
%
% Plot the imported road network. 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(TM) world coordinate
% system.

scenario = drivingScenario;
roadNetwork(scenario,'OpenStreetMap','applehill.osm');
plot(scenario)
%% 
% Load the latitude and longitude coordinates for a driving route in this
% road network.

data = load('geoRouteAH.mat');
lat = data.latitude;
lon = data.longitude;
%%
% Use the |<docid:driving_ref#mw_80e7a72c-09f3-40cc-a4c7-2472f5c534d9
% 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. Because the driving route contains only latitudinal and
% longitudinal data, set the altitude to 0.
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. Plot the vehicle trajectory and pause every 0.01 seconds to
% slow down the simulation.
v = vehicle(scenario,'ClassID',1);
speed = 30;
smoothTrajectory(v,[xEast,yNorth,zUp],speed);

while advance(scenario)
    pause(0.01)
end