MATLAB: Variable Occupation Maps for trajectory planning

Question

Hello,

how could I create a custom scenario for this example https://www.mathworks.com/help/nav/ug/optimal-trajectory-generation-for-urban-driving.html#mw_rtc_OptimalTrajectoryGenerationForUrbanDrivingExample_B94A9390 ?

I´ve simplified the example algorithm for collision-free motion and would like to test this in some other scenarios (different road layouts, different reference paths, no obstacles/static obstacles).

Thank you in Advance!

Here is the code, which works, in the example directory:

% Load data from urbanScenarioData.mat file, initialize required variables
clc 
clear
[otherVehicles,globalPlanPoints,stateValidator] = exampleHelperUrbanSetup;
% Set positions A, B, C and D
positionA =  [5.1, -1.8, 0];
positionB =  [60, -1.8, 0];
positionC =  [74.45, -30.0, 0];
positionD =  [74.45, -135, 0];
goalPoint =  [145.70, -151.8, 0];
% Set the initial state of the ego vehicle
egoInitPose     = positionA;
egoInitVelocity = [10, -0.3, 0];
egoInitYaw      = -0.165;
currentEgoState = [egoInitPose(1), egoInitPose(2), deg2rad(egoInitYaw),...
    0, norm(egoInitVelocity), 0];
vehicleLength = 4.7; % in meters
% Replan interval in number of simulation steps
% (default 50 simulation steps)
%%Distance to GOAL
distanceToGoal = norm(currentEgoState(1:3) - goalPoint);
goalRadius = 10; 
% Initialize Visualization
exampleHelperInitializeVisualization;
localPlanner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator); 
%Terminal States
localPlanner.TerminalStates.Longitudinal = [5 15 30 45]; %Longitudinal sampling distance in m
localPlanner.TerminalStates.Lateral = [-2 -1 0 1 2]; % Lateral deviation in meters from globalPlanPoints
localPlanner.TerminalStates.Time = 7; %Time in sec to reach end of trajectory
localPlanner.TerminalStates.Speed = 10; %Velocity in m/s
localPlanner.TerminalStates.Acceleration; % Acceleration at the end of trajectory in m/s^2
%Feasibility Parameters
localPlanner.FeasibilityParameters.MaxCurvature = 0.1; %Max feasible curvature value in m^-1
localPlanner.FeasibilityParameters.MaxAcceleration = 2.5; %Maximum feasible acceleration in m/s^2
%Weights 
planner = trajectoryOptimalFrenet(globalPlanPoints, stateValidator, 'Time',1,'Deviation',1,'LateralSmoothness',3,'LongitudinalSmoothness',5);
%% Time
timeResolution=0.01; 
localPlanner.TimeResolution = timeResolution; 
replanInterval=25; 
% Simulate till the ego vehicle reaches position B
simStep = 1;
% Check only for X as there is no change in Y.
% Simulate till the ego vehicle reaches position D
% Set Lane Width
laneWidth = [5 4 3 2.5 2 1.5 1 0 -1 -2];
% Check only for Y as there is no change in X at D
while (distanceToGoal > goalRadius)
    
    % Replan at every "replanInterval" simulation step
    if rem(simStep, replanInterval) == 0 || simStep == 1
        % Compute the replanning time
        previousReplanTime = simStep*timeResolution;
        
        % Updating occupancy map with vehicle information
        exampleHelperUpdateOccupancyMap(otherVehicles,simStep,currentEgoState);
        
        % TerminalState settings for negotiating Lane change
        localPlanner.TerminalStates.Longitudinal = 20:5:40;
        localPlanner.TerminalStates.Lateral = laneWidth;
        localPlanner.TerminalStates.Speed = 10;
        desiredTimeBound = localPlanner.TerminalStates.Longitudinal/...
            ((currentEgoState(1,5) + localPlanner.TerminalStates.Speed)/2);
        localPlanner.TerminalStates.Time = desiredTimeBound;
        localPlanner.FeasibilityParameters.MaxCurvature = 0.5;
        localPlanner.FeasibilityParameters.MaxAcceleration = 15;
        
        % Generate optimal trajectory for current set of parameters
        currentStateInFrenet = cart2frenet(localPlanner,[currentEgoState(1:5) 0]);
        trajectory = plan(localPlanner,currentStateInFrenet);
        
        % Visualize the ego-centric occupancy map
        show(egoMap,"Parent",hAxes1)
        title("Ego Centric Occupancy Map","Parent",hAxes1)
        
        % Visualize ego vehicle on occupancy map
        egoCenter = currentEgoState(1:2);
        egoPolygon = exampleHelperTransformPointtoPolygon(rad2deg(currentEgoState(3)), egoCenter);
        hold(hAxes1,"on")
        fill(egoPolygon(1, :),egoPolygon(2, :),"g","Parent",hAxes1)
        
        % Visualize the Trajectory reference path and trajectory
        show(localPlanner,"Trajectory","optimal","Parent",hAxes1)
    end
    
    % Execute and Update Visualization
    [isGoalReached, currentEgoState] = ...
        exampleHelperExecuteAndVisualize(currentEgoState,simStep,...
        trajectory,previousReplanTime);
    if(isGoalReached)
        break;
    end
    
    % Update the simulation step for the next iteration
    simStep = simStep + 1;
    pause(0.01);
end

Answer 1

Ok, I´ve soved the problem. The Workspace Directory had to be updated. It is possible to create scenarios with the autonomous driving app and extract the relevant workspace data.