Commit 63e2754c by 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