Commit 63e2754c authored by Markus Koschi's avatar Markus Koschi

multiple minor bug fixes

parent 6002cb6c
classdef Shape < matlab.mixin.Heterogeneous & handle
% Shape - class for describing 2D shapes
classdef (Abstract) Shape < matlab.mixin.Heterogeneous & handle
% Shape - abstract class for describing 2D shapes
% (Inherits from matlab.mixin.Heterogeneous to combine instances of the
% subclasses into heterogeneous arrays)
%
......@@ -12,17 +12,16 @@ classdef Shape < matlab.mixin.Heterogeneous & handle
% Author: Markus Koschi
% Written: 08-February-2017
% Last update: 08-June-2017
% Last update: 25-August-2017
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (SetAccess = protected, GetAccess = public)
end
methods
methods (Abstract)
% visualization methods
disp(obj)
plot(varargin)
......
function [angle] = calcAngleOfVerticesAtPosition(position, vertices)
% calcAngleOfVerticesAtPosition - calculates the angle (orientation) of
% calcAngleOfVerticesAtPosition - calculates the angle (orientation) of
% the line segment of the vertices which has the minimal distance to the
% given position.
% The angle is measured in the closed interval [-pi, pi] relative to the
......@@ -23,7 +23,7 @@ function [angle] = calcAngleOfVerticesAtPosition(position, vertices)
% Author: Markus Koschi
% Written: 18-May-2017
% Last update:
% Last update: 23-August-2017
%
% Last revision:---
......@@ -33,10 +33,12 @@ function [angle] = calcAngleOfVerticesAtPosition(position, vertices)
[indexClosestVertex, ~] = geometry.findClosestVertexOfPosition(position, vertices);
if indexClosestVertex == length(vertices)
indexClosestVertex = indexClosestVertex - 1;
indexClosestVertex = indexClosestVertex - 1;
end
%ToDo: use flag_segment of calcProjectedDistance
angle = atan2(vertices(2,indexClosestVertex+1) - vertices(2,indexClosestVertex), ...
vertices(1,indexClosestVertex+1) - vertices(1,indexClosestVertex));
vertices(1,indexClosestVertex+1) - vertices(1,indexClosestVertex));
end
......
......@@ -27,7 +27,7 @@ function [distance, nLamda] = calcProjectedDistance(i, bound, position)
% Author: Markus Koschi
% Written: 20-Oktober-2016
% Last update: 23-May-2017
% Last update: 23-August-2017
%
% Last revision:---
......@@ -37,33 +37,19 @@ function [distance, nLamda] = calcProjectedDistance(i, bound, position)
vi = bound.vertices(:,i);
% find the previous and next vertices on the lane bound:
% (note that the bound of these vertices might not be the inner one)
% set v(i-2) and v(i-1)
if (i-2) > 0
viminus2 = bound.vertices(:,i-2);
viminus1 = bound.vertices(:,i-1);
elseif (i-2) == 0
viminus2 = bound.vertices(:,i-1);
viminus1 = viminus2;
else % i == 1
viminus1 = vi;
end
% set v(i+1) and v(i+2)
if (i+2) <= length(bound.vertices)
viplus2 = bound.vertices(:,i+2);
viplus1 = bound.vertices(:,i+1);
elseif (i+1) == length(bound.vertices)
viplus2 = bound.vertices(:,i+1);
viplus1 = viplus2;
elseif i == length(bound.vertices)
viplus2 = bound.vertices(:,i);
viplus1 = viplus2;
if ~exist('viminus2','var') % iBorder == 1 && iBorder == length(innerBound.vertices)
error(['Lane %i is too short to properly calculate the projection of'...
% (note that these vertices might not be the on the inner bound)
if i == 1 && i == size(bound.vertices,2)
error(['Lane %i is too short to properly calculate the projection of'...
' the object on the lane border'], bound.id(1));
end
end
% set v(i-2) and v(i-1)
iminus1 = geometry.getNextIndex(bound.vertices, i, 'backward', bound.distances);
viminus1 = bound.vertices(:,iminus1);
viminus2 = bound.vertices(:,geometry.getNextIndex(bound.vertices, iminus1, 'backward', bound.distances));
% set v(i+1) and v(i+2)
iplus1 = geometry.getNextIndex(bound.vertices, i, 'forward', bound.distances);
viplus1 = bound.vertices(:,iplus1);
viplus2 = bound.vertices(:,geometry.getNextIndex(bound.vertices, iplus1, 'forward', bound.distances));
% choose the segment in which the object's position is projected in:
% calculate the angle between the points (vi, p, vi-1) and (vi, p, vi+1)
......
......@@ -34,6 +34,8 @@ end
[distanceToVertex, indexOfVertex] = min(distance);
%knnsearch
end
%------------- END CODE --------------
\ No newline at end of file
function [nextIndex] = getNextIndex(vertices, currentIndex, direction, distances)
% getNextIndex - returns the next index in the specified direction while
% omitting vertices with zero distance
%
% Syntax:
% [nextIndex] = getNextIndex(vertices, currentIndex, direction, distances)
%
% Inputs:
% vertices - 2D array of vertices describing the polyline
% currentIndex - index in respect to the vertices array
% direction - direction of next: 'forward' or 'backward'
% distances - optional: distances between vertices
%
% Outputs:
% nextIndex - index of the next vertex
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Markus Koschi
% Written: 03-August-2017
% Last update:
%
% Last revision:---
%------------- BEGIN CODE --------------
% define epsilon for distance check
epsilon = 10e-6;
% input handling
if nargin < 3 || nargin > 4
error('Wrong input.')
end
if nargin < 4
distances = geometry.calcPathDistances(vertices(1,:), vertices(2,:));
end
% find next vertex in the specified direction
if strcmp(direction,'backward')
if currentIndex == 1
nextIndex = currentIndex;
else
k = currentIndex - 1;
while distances(k+1) <= epsilon && k > 1
k = k - 1;
end
nextIndex = k;
end
elseif strcmp(direction,'forward')
if currentIndex == size(vertices,2)
nextIndex = currentIndex;
else
k = currentIndex + 1;
while distances(k) <= epsilon && k < size(vertices,2)
k = k + 1;
end
nextIndex = k;
end
else
error('Wrong input.')
end
end
%------------- END CODE --------------
\ No newline at end of file
......@@ -57,7 +57,7 @@ classdef GoalRegion
end
methods (Static)
[goalRegionRectangle] = createGRRFromPosition(lanes, position, width, ratio)
[goalRegionRectangle] = createGRRFromPosition(lanes, position, width, ratio, inlane_idx)
end
end
......
function [ goalRegionRectangle ] = createGRRFromPosition( lanes, position, width, ratio )
%createGRRFromPosition This function creates a rectangle as position for
function [ goalRegionRectangle ] = createGRRFromPosition( lanes, position, width, ratio, inLane_idx )
%createGRRFromPosition - This function creates a rectangle as position for
% goalregion with width as percentage from lane width. Ratio as ratio
% between width and length of rectangle: >1 length bigger than
% width, <1 length smaller than width
......@@ -11,23 +11,22 @@ function [ goalRegionRectangle ] = createGRRFromPosition( lanes, position, width
% width (0-1 scalar)
% ratio (scalar)
%
%
% Outputs:
% goalRegionRectangle - rectangle object
%
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% Author: Lukas Braunstorfer
% Written: 20-April-2017
% Last update: 20-April-2017
% Last update: 16-August-2017
%
% Last revision:---
% ___Begin of Code___
if nargin<2 || nargin>4
%------------- BEGIN CODE --------------
if nargin<2 || nargin>5
error('wrong number of input arguments');
elseif nargin == 2
width = 0.5;
......@@ -37,28 +36,28 @@ elseif nargin == 3
end
inLane = lanes.findLaneByPosition(position);
numClosestVertice = dsearchn(inLane.center.vertices',position');
closestVertice = [inLane.center.vertices(1,numClosestVertice);...
inLane.center.vertices(2,numClosestVertice)];
numClosestVertice = dsearchn(inLane(inLane_idx).center.vertices',position');
closestVertice = [inLane(inLane_idx).center.vertices(1,numClosestVertice);...
inLane(inLane_idx).center.vertices(2,numClosestVertice)];
if numClosestVertice>length(inLane.center.vertices)
nextVertice = [inLane.center.vertices(1,numClosestVertice-1);...
inLane.center.vertices(2,numClosestVertice-1)];
if numClosestVertice>length(inLane(inLane_idx).center.vertices)
nextVertice = [inLane(inLane_idx).center.vertices(1,numClosestVertice-1);...
inLane(inLane_idx).center.vertices(2,numClosestVertice-1)];
orientation = atan2(closestVertice(2)-nextVertice(2),closestVertice(1)-nextVertice(1));
else
nextVertice = [inLane.center.vertices(1,numClosestVertice+1);...
inLane.center.vertices(2,numClosestVertice+1)];
nextVertice = [inLane(inLane_idx).center.vertices(1,numClosestVertice+1);...
inLane(inLane_idx).center.vertices(2,numClosestVertice+1)];
orientation = atan2(nextVertice(2)-closestVertice(2),nextVertice(1)-closestVertice(1));
end
laneWidth = 2*(sqrt((closestVertice(1)-inLane.leftBorder.vertices(1,numClosestVertice))^2+...
(closestVertice(2)-inLane.leftBorder.vertices(2,numClosestVertice))^2));
laneWidth = 2*(sqrt((closestVertice(1)-inLane(inLane_idx).leftBorder.vertices(1,numClosestVertice))^2+...
(closestVertice(2)-inLane(inLane_idx).leftBorder.vertices(2,numClosestVertice))^2));
widthRec = width*laneWidth;
lengthRec = ratio*width*laneWidth;
goalRegionRectangle = geometry.Rectangle(lengthRec,widthRec,closestVertice,orientation);
%goalRegionRectangle = geometry.Rectangle(lengthRec,widthRec,closestVertice,orientation);
goalRegionRectangle = geometry.Rectangle(lengthRec,widthRec,position,orientation);
end
......@@ -16,7 +16,7 @@ function plot(obj)
% Author: Markus Koschi
% Written: 18-April-2017
% Last update:
% Last update: 16-August-2018
%
% Last revision:---
......@@ -24,24 +24,21 @@ function plot(obj)
for i = 1:numel(obj)
% plot the position of the goal region
if isa(obj(i).position, 'geometry.Shape')
obj(i).position.plot('k');
else
obj(i).position.plot();
if globalPck.PlotProperties.SHOW_GOALREGION && ~isa(obj(i).position, 'world.Lanelet')
obj(i).position.plot(globalPck.PlotProperties.COLOR_EGO_VEHICLE);
end
% % print description
% if globalPck.PlotProperties.SHOW_OBJECT_NAMES && ~isempty(obj(i).position)
% if ~isa(obj(i).position, 'geometry.Rectangle')
% pos = obj(i).position.position;
% else
% continue
% end
%
% string = sprintf('goal Region');
% text(pos(1), pos(2), 0, string);
% %text(pos(1), pos(2), 0, ['\leftarrow ', string]);
% end
% print description
if globalPck.PlotProperties.SHOW_OBJECT_NAMES && ~isempty(obj(i).position)
if ~isa(obj(i).position, 'world.Lanelet') && ...
(~isa(obj(i).position, 'geometry.Shape') || isa(obj(i).position, 'geometry.Rectangle'))
pos = obj(i).position.position;
string = sprintf('goal Region');
text(pos(1), pos(2), 0, string);
%text(pos(1), pos(2), 0, ['\leftarrow ', string]);
end
end
end
end
......
......@@ -84,9 +84,9 @@ classdef TimeInterval < globalPck.Dynamic
end
idx_start = round((ts - obj.ts)/obj.dt) + 1;
%HACK to catch invalid index
if idx_start < 1 || isnan(idx_start)
idx_start = 1;
% catch invalid index and return 0
if idx_start < 0 || isnan(idx_start)
idx_start = 0;
end
end
......
......@@ -20,7 +20,7 @@ classdef Trajectory < handle
% Author: Markus Koschi
% Written: 15-November-2016
% Last update:
% Last update: 16-August-2017
%
% Last revision:---
......@@ -53,44 +53,54 @@ classdef Trajectory < handle
end
% get methods
function position = getPosition(obj,t)
if(~isempty(obj.position))
function position = getPosition(obj, t)
try
position = obj.position(:,obj.timeInterval.getIndex(t));
else
catch
position = [];
end
end
function orientation = getOrientation(obj,t)
if(~isempty(obj.orientation))
orientation = obj.orientation(obj.timeInterval.getIndex(t));
else
orientation = [];
function orientation = getOrientation(obj, t)
try
orientation = obj.orientation(:,obj.timeInterval.getIndex(t));
catch
orientation = [];
end
end
function velocity = getVelocity(obj,t)
if(~isempty(obj.velocity))
function velocity = getVelocity(obj, t)
try
velocity = obj.velocity(:,obj.timeInterval.getIndex(t));
else
catch
velocity = [];
end
end
function acceleration = getAcceleration(obj,t)
if(~isempty(obj.acceleration))
function acceleration = getAcceleration(obj, t)
try
acceleration = obj.acceleration(:,obj.timeInterval.getIndex(t));
else
catch
acceleration = [];
end
end
% set methods
function setOrientation(obj, orientation)
if numel(obj.orientation) == numel(orientation)
obj.orientation = orientation;
else
warning('number of orientation points does not fit');
end
end
% methods in seperate files:
% set trajectory
setTrajectory(obj, t, x, y, theta, v, a)
% create trajectory along center vertices with constant speed
defineTrajectory(obj, obstacle, timeInterval)
defineTrajectory(obj, obstacle, timeInterval, inLane_idx)
% visualization methods
disp(obj)
......
function defineTrajectory(obj, obstacle, timeInterval)
function defineTrajectory(obj, obstacle, timeInterval, inLane_idx)
% defineTrajectory - creates a trajectory with constant velocity along the
% path of the center vertices of the lane obstacle.inLane(inLane_idx)
%
% Syntax:
% defineTrajectory(obj, obstacle, timeInterval, inLane_idx)
%
% Inputs:
% obj - Trajectory object
% obstacle - obstacle (or ego vehicle) for which the trajecotry is
% created
% timeInterval - time interval of the intended trajectory
% inLane_idx - index of the inLane property of the obstacle to
% specify in which lane the trajectory shall be defined
%
% Outputs:
% none (trajectory is saved in the trajectory object)
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
% Author: Mona Beikirch, Markus Koschi
% Written: 20-March-2017
% Last update: 16-August-2017
%
% Last revision:---
%------------- BEIN CODE --------------
% get the time interval properties
[ts, dt, tf] = timeInterval.getTimeInterval();
numTimeIntervals = length(ts:dt:(tf-dt));
% get start position of obstacle
% for i = 1:length(obstacle.inLane.center.vertices)
% if (obstacle.inLane.center.vertices(1,i) == obstacle.position(1) && obstacle.inLane.center.vertices(2,i) == obstacle.position(2))
% startPos = i;
% break;
% end
% end
distance = 99 * ones(1,length(obstacle.inLane.center.vertices));
for j = 1:length(obstacle.inLane.center.vertices)
distance(j) = norm(obstacle.inLane.center.vertices(:,j) - obstacle.position);
distance = 99 * ones(1,length(obstacle.inLane(inLane_idx).center.vertices));
for j = 1:length(obstacle.inLane(inLane_idx).center.vertices)
distance(j) = norm(obstacle.inLane(inLane_idx).center.vertices(:,j) - obstacle.position);
end
[~,startPos] = min(distance);
% distances along center line
centerLineDistances = geometry.calcPathDistances(obstacle.inLane.center.vertices(1,:), obstacle.inLane.center.vertices(2,:));
[startDistance,startPos] = min(distance);
% properties of trajectory
positionArray = obstacle.position;
orientationArray = obstacle.orientation;
velocityArray = obstacle.velocity;
accelerationArray = obstacle.acceleration;
inLaneArray = obstacle.inLane;
inLaneArray = obstacle.inLane(inLane_idx);
%nextCenterPointIdxArray = startPos;
% overtaking with constant speed e.g. constant distance traveled in each
% trajectory with constant speed, i.e. constant distance traveled in each
% time interval
travelDistance = obstacle.velocity*dt;
travelDistance = obstacle.velocity * dt;
% distances along center line
centerLineDistances = geometry.calcPathDistances(obstacle.inLane(inLane_idx).center.vertices(1,:), obstacle.inLane(inLane_idx).center.vertices(2,:));
% compute trajectory points e.g. start position and end position of each
% compute trajectory points, i.e. start position and end position for each
% time interval
if norm(obstacle.inLane(inLane_idx).center.vertices(:,startPos+1) - obstacle.position) > centerLineDistances(startPos+1)
distance = startDistance;
else
distance = -startDistance; % obstacle is infront of startPos
end
position = startPos;
distance = 0; % start at distance 0
for i = 1:numTimeIntervals % for each time interval compute end position
for i = 1:numTimeIntervals
while (distance < travelDistance && position < length(centerLineDistances)) % travel along center line
distance = distance + centerLineDistances(position+1);
position = position + 1;
end
if position >= length(centerLineDistances)
warning('Trajectory to long!');
warning('Trajectory to long! (Obstacle id = %i)', obstacle.id);
break;
end
% compute the exact position between two center line points with
% theorem of intersection lines
x_1 = obstacle.inLane.center.vertices(1,position-1);
x_2 = obstacle.inLane.center.vertices(1,position);
y_1 = obstacle.inLane.center.vertices(2,position-1);
y_2 = obstacle.inLane.center.vertices(2,position);
if position < 1
position = 1;
end
x_1 = obstacle.inLane(inLane_idx).center.vertices(1,position-1);
x_2 = obstacle.inLane(inLane_idx).center.vertices(1,position);
y_1 = obstacle.inLane(inLane_idx).center.vertices(2,position-1);
y_2 = obstacle.inLane(inLane_idx).center.vertices(2,position);
orientation_obst = atan2(y_2-y_1,x_2-x_1);
d = centerLineDistances(position);
d_i = travelDistance - (distance-centerLineDistances(position));
d_i = travelDistance - (distance - centerLineDistances(position));
delta_x = d_i/d * (x_2-x_1);
delta_y = d_i/d * (y_2-y_1);
trajectoryPointX = x_1 + delta_x;
trajectoryPointY = y_1 + delta_y;
% fill trajectory properties
next_position = [trajectoryPointX; trajectoryPointY];
positionArray = [positionArray, next_position];
orientationArray(end+1) = orientation_obst;
velocityArray(end+1) = obstacle.velocity;
accelerationArray(end+1) = obstacle.acceleration;
inLaneArray(end+1) = obstacle.inLane;
%nextCenterPointIdxArray(end+1) = position;
inLaneArray(end+1) = obstacle.inLane(inLane_idx);
% set initial distance for next time interval which may be different
% from 0
......@@ -80,12 +109,11 @@ end
obj.timeInterval = timeInterval;
%obj.time = ts;
obj.position = positionArray;
obj.orientation = obstacle.orientation * ones(1,size(positionArray,2));
obj.orientation = orientationArray;
obj.velocity = velocityArray;
obj.acceleration = accelerationArray;
%obj.inLane = inLaneArray;
%obj.nextCenterPointIdx = nextCenterPointIdxArray;
end
%------------- END CODE --------------
\ No newline at end of file
......@@ -11,54 +11,47 @@ classdef (Abstract) PlotProperties < handle
% Author: Markus Koschi
% Written: 15-November-2016
% Last update:
% Last update: 16-August-2017
%
% Last revision:---
%------------- BEGIN CODE --------------
properties (Constant, GetAccess = public)
% general plot
SHOW_GRID = false;
SHOW_AXIS = true;
SHOW_OBJECT_NAMES = true;
SHOW_INITAL_CONFIGURATION = true;
PLOT_OCC_SNAPSHOTS = false;
PLOT_DYNAMIC = false;
PRINT_FIGURE = false;
FACE_TRANSPARENCY = 0.1;%1;%0.2;%0; % (of occupancies)
% lanes and lanelets
SHOW_LANES = true;
SHOW_LANE_NAMES = false;
SHOW_LANE_BORDER_VERTICES = false;
SHOW_LANE_BORDER_VERTICES_NUMBERS = false;
SHOW_LANE_CENTER_VERTICES = false;
SHOW_LANE_FACES = false;
SHOW_INITAL_CONFIGURATION = true;
% obstacles
SHOW_OBJECT_NAMES = true;
SHOW_OBSTACLES = true;
SHOW_PREDICTED_OCCUPANCIES = true;
SHOW_OBSTACLES_CENTER = false;
SHOW_TRAJECTORIES = false;
COLOR_OBSTACLES = ['c', 'b', 'r', 'g', 'y', 'm'];
COLOR_OBSTACLES_STATIC = [0.5 0.5 0.5];
% ego vehicle
SHOW_EGO_VEHICLE = true;
SHOW_GOAL_REGION = false;
SHOW_TRAJECTORIES = false;
SHOW_TRAJECTORIES_EGO = true;
SHOW_OBSTACLES_CENTER = false;
% predicted occupancy
SHOW_PREDICTED_OCCUPANCIES = true;
SHOW_TRAJECTORY_OCCUPANCIES_EGO = false;
SHOW_PREDICTED_OCCUPANCIES_EGO = false;
FACE_TRANSPARENCY = 0.1;%1;%0.2;%0;
PLOT_OCC_SNAPSHOTS = false;
PLOT_DYNAMIC = false;
PRINT_FIGURE = false;
% currently unused
COLOR_STATIC_OBSTACLES = [0 0 0];
COLOR_DYNAMIC_OBSTACLES = [1 0 0];
COLOR_PREDICTION = [1 0 0];
COLOR_REACH_SAMPLE = [0 0 0];
COLOR_EGO_VEHICLE = [0 0 1];
SHOW_COORDINATE_AXES = false;
LINEWIDTH_STATIC_OBSTACLES = 0.5;
LINEWIDTH_DYNAMIC_OBSTACLES = 0.5;
LINEWIDTH_PREDICTION = 0.5;
LINEWIDTH_REACH_SAMPLE = 0.5;
SHOW_GOALREGION = true;
COLOR_EGO_VEHICLE = [0 0 0];
COLOR_TRAJECTORY_OCCUPANCIES_EGO = [1 0 0];
end
end
......
......@@ -72,25 +72,7 @@ for i = 0:(nodeList.getLength()-1)
% convert from lat and lon in degrees to UTM format in meters)
[nodes(i+1).x, nodes(i+1).y, ~] = geometry.deg2utm(lat', lon');
% translate and rotate all nodes
if strcmp(filename, 'scenarios/markus/Example_straight_2lanes_extended_PNR.osm') || ...
strcmp(filename, 'scenarios/markus/Example_straight_2lanes_PNR.osm') || ...
strcmp(filename, 'scenarios/markus/Example_straight_2lanes.osm')
vertices = geometry.rotateAndTranslateVertices([nodes(i+1).x; nodes(i+1).y], [-8.691005394443008e+05 + 76.149251932755107; -5.311334845415503e+06 + 4.493494204689860], -0.033254436121817);
nodes(i+1).x = vertices(1);
nodes(i+1).y = vertices(2);
elseif strcmp(filename, 'scenarios/markus/Intersection_Leopold_Hohenzollern_adj_v3_PNR.osm')
vertices = geometry.rotateAndTranslateVertices([nodes(i+1).x; nodes(i+1).y], [+5.328317058270099e+06; +7.580935097911237e+05], 1.841107616763556 + 4.9108e-06);
nodes(i+1).x = vertices(1);
nodes(i+1).y = vertices(2);
elseif strcmp(filename, 'scenarios/markus/Intersection_Leopold_Hohenzollern_adj_v3.osm') || ...
strcmp(filename, 'scenarios/markus/Intersection_Leopold_Hohenzollern_v1.osm')
vertices = geometry.translateAndRotateVertices([nodes(i+1).x; nodes(i+1).y], [-6.922647376035838e+05; -5.337298994084798e+06], 0); %1.841107616763556 + 4.9108e-06);
nodes(i+1).x = vertices(1);
nodes(i+1).y = vertices(2);
end
% extract all tags, which are specified for trajectory nodes
% (orientation, velocity, acceleration, and time stamp)
nodeTagList = nodeList.item(i).getElementsByTagName('tag');
......
......@@ -41,7 +41,7 @@ function [lanelets, obstacles, egoVehicles, scenarioData] = readXMLFile(filename
% Author: Markus Koschi
% Written: 29-March-2017
% Last update:
% Last update: 25-August-2017
%
% Last revision:---
......@@ -67,15 +67,13 @@ scenarioData.timeStepSize = str2double(commonRoad.getAttribute('timeStepSize'));
%% -- Road Network ---
% extract all lanelets
laneletList = xDoc.getElementsByTagName('lanelet');
lanelets(laneletList.getLength()).id = [];
%lanelets(laneletList.getLength()).id = [];
for i = 0:(laneletList.getLength()-1)
% id
if ~strcmp(laneletList.item(i).getAttribute('id'),'')
lanelets(i+1).id = str2double(laneletList.item(i).getAttribute('id'));
else
% if element is not a proper lanelet, remove this row from struct
lanelets(i+1) = [];
break
continue
end