uwaft · andrewmourcos · Nov 4, 2018
@@ -0,0 +1,272 @@
+%Andrew_s_Simulation - Returns sensor detections
+%    allData = Andrew_s_Simulation returns sensor detections in a structure
+%    with time for an internally defined scenario and sensor suite.
+%
+%    [allData, scenario, sensors] = Andrew_s_Simulation optionally returns
+%    the drivingScenario and detection generator objects.
+
+% Generated by MATLAB(R) 9.5 and Automated Driving System Toolbox 1.3.
+% Generated on: 04-Nov-2018 11:48:59
+
+% Create the drivingScenario object and ego car
+[scenario, egoCar] = createDrivingScenario;
+
+% Create all the sensors
+[sensors, numSensors] = createSensors(scenario);
+
+allData = struct('Time', {}, 'ActorPoses', {}, 'ObjectDetections', {}, 'LaneDetections', {});
+
+BEP = createDemoDisplay(egoCar, sensors);
+running = true;
+while running
+
+    % Generate the target poses of all actors relative to the ego car
+    poses = targetPoses(egoCar);
+    time  = scenario.SimulationTime;
+
+    objectDetections = {};
+    laneDetections   = [];
+    isValidTime      = false(1, numSensors);
+
+    % Generate detections for each sensor
+    for sensorIndex = 1:numSensors
+        [objectDets, numObjects, isValidTime(sensorIndex)] = sensors{sensorIndex}(poses, time);
+        objectDetections = [objectDetections; objectDets(1:numObjects)]; %#ok<AGROW>
+    end
+
+    % Aggregate all detections into a structure for later use
+    if any(isValidTime)
+        allData(end + 1) = struct( ...
+            'Time',       scenario.SimulationTime, ...
+            'ActorPoses', actorPoses(scenario), ...
+            'ObjectDetections', {objectDetections}, ...
+            'LaneDetections',   {laneDetections}); 
+    end
+
+    % Advance the scenario one time step and exit the loop if the scenario is complete
+    running = advance(scenario);
+    updateBEP(BEP, egoCar, detections, confirmedTracks, positionSelector, velocitySelector);
+end
+
+% Restart the driving scenario to return the actors to their initial positions.
+restart(scenario);
+
+% Release all the sensor objects so they can be used again.
+for sensorIndex = 1:numSensors
+    release(sensors{sensorIndex});
+end
+
+%%%%%%%%%%%%%%%%%%%%
+% Helper functions %
+%%%%%%%%%%%%%%%%%%%%
+
+% Units used in createSensors and createDrivingScenario
+% Distance/Position - meters
+% Speed             - meters/second
+% Angles            - degrees
+% RCS Pattern       - dBsm
+
+function [sensors, numSensors] = createSensors(scenario)
+% createSensors Returns all sensor objects to generate detections
+
+% Assign into each sensor the physical and radar profiles for all actors
+profiles = actorProfiles(scenario);
+sensors{1} = visionDetectionGenerator('SensorIndex', 1, ...
+    'SensorLocation', [3.7 0], ...
+    'MaxRange', 100, ...
+    'DetectorOutput', 'Objects only', ...
+    'Intrinsics', cameraIntrinsics([1814.81018227767 1814.81018227767],[320 240],[480 640]), ...
+    'ActorProfiles', profiles);
+sensors{2} = visionDetectionGenerator('SensorIndex', 2, ...
+    'SensorLocation', [-1 0], ...
+    'Yaw', -180, ...
+    'MaxRange', 100, ...
+    'DetectorOutput', 'Objects only', ...
+    'Intrinsics', cameraIntrinsics([1814.81018227767 1814.81018227767],[320 240],[480 640]), ...
+    'ActorProfiles', profiles);
+sensors{3} = radarDetectionGenerator('SensorIndex', 3, ...
+    'SensorLocation', [1.9 0], ...
+    'ActorProfiles', profiles);
+sensors{4} = radarDetectionGenerator('SensorIndex', 4, ...
+    'SensorLocation', [0 0.9], ...
+    'Yaw', 90, ...
+    'MaxRange', 50, ...
+    'FieldOfView', [90 5], ...
+    'ActorProfiles', profiles);
+sensors{5} = radarDetectionGenerator('SensorIndex', 5, ...
+    'SensorLocation', [0 -0.9], ...
+    'Yaw', -90, ...
+    'MaxRange', 50, ...
+    'FieldOfView', [90 5], ...
+    'ActorProfiles', profiles);
+sensors{6} = radarDetectionGenerator('SensorIndex', 6, ...
+    'SensorLocation', [2.8 -0.9], ...
+    'Yaw', -90, ...
+    'MaxRange', 50, ...
+    'FieldOfView', [90 5], ...
+    'ActorProfiles', profiles);
+sensors{7} = radarDetectionGenerator('SensorIndex', 7, ...
+    'SensorLocation', [2.8 0.9], ...
+    'Yaw', 90, ...
+    'MaxRange', 50, ...
+    'FieldOfView', [90 5], ...
+    'ActorProfiles', profiles);
+numSensors = 7;
+end
+
+function [scenario, egoCar] = createDrivingScenario
+% createDrivingScenario Returns the drivingScenario defined in the Designer
+
+% Construct a drivingScenario object.
+scenario = drivingScenario;
+
+% Add all road segments
+roadCenters = [6 5.4 0;
+               35.8 7.5 0;
+               26.5 -12.3 0;
+               45.3 -13.4 0];
+marking = [laneMarking('Solid', 'Color', [0.98 0.86 0.36])
+laneMarking('Dashed', 'Length', 8, 'Space', 2)
+laneMarking('Solid')];
+laneSpecification = lanespec(2, 'Width', 4.925, 'Marking', marking);
+road(scenario, roadCenters, 'Lanes', laneSpecification);
+
+% Add the ego car
+egoCar = vehicle(scenario, ...
+    'ClassID', 1, ...
+    'Position', [12.1 8.4 0]);
+waypoints = [12.1 8.4 0;
+             18.7 12.6 0;
+             27.1 13.6 0;
+             32.5 11 0;
+             32.9 5.1 0;
+             24.9 -4.7 0;
+             23.5 -9.9 0;
+             25.3 -15.7 0;
+             31 -19.1 0;
+             37.5 -19 0;
+             44.7 -16.6 0];
+speed = 30;
+trajectory(egoCar, waypoints, speed);
+
+% Add the non-ego actors
+truck = vehicle(scenario, ...
+    'ClassID', 2, ...
+    'Length', 8.2, ...
+    'Width', 2.5, ...
+    'Height', 3.5, ...
+    'Position', [37.5 12.5 0]);
+waypoints = [37.5 12.5 0;
+             36.2 1.3 0;
+             31.2 -4.2 0;
+             28.6 -9.9 0;
+             31.3 -13.8 0;
+             37.2 -13.8 0;
+             43.7 -11.5 0];
+speed = 5;
+trajectory(truck, waypoints, speed);
+
+car1 = vehicle(scenario, ...
+    'ClassID', 1, ...
+    'Position', [6.4 9.9 0]);
+waypoints = [6.4 9.9 0;
+             13 14.9 0;
+             25.4 19 0;
+             34.8 15.4 0;
+             32.4 8.2 0;
+             27.2 -0.9 0;
+             23.6 -8.9 0;
+             25.7 -16.9 0;
+             35.4 -19.9 0;
+             44.7 -16.7 0];
+speed = 60;
+trajectory(car1, waypoints, speed);
+end
+
+function BEP = createDemoDisplay(egoCar, sensors)
+    % Make a figure
+    hFigure = figure('Position', [0, 0, 1200, 640], 'Name', 'Sensor Fusion with Synthetic Data Example');
+    movegui(hFigure, [0 -1]); % Moves the figure to the left and a little down from the top
+
+    % Add a car plot that follows the ego vehicle from behind
+    hCarViewPanel = uipanel(hFigure, 'Position', [0 0 0.5 0.5], 'Title', 'Chase Camera View');
+    hCarPlot = axes(hCarViewPanel);
+    chasePlot(egoCar, 'Parent', hCarPlot);
+
+    % Add a car plot that follows the ego vehicle from a top view
+    hTopViewPanel = uipanel(hFigure, 'Position', [0 0.5 0.5 0.5], 'Title', 'Top View');
+    hCarPlot = axes(hTopViewPanel);
+    chasePlot(egoCar, 'Parent', hCarPlot, 'ViewHeight', 130, 'ViewLocation', [0 0], 'ViewPitch', 90);
+
+    % Add a panel for a bird's-eye plot
+    hBEVPanel = uipanel(hFigure, 'Position', [0.5 0 0.5 1], 'Title', 'Bird''s-Eye Plot');
+
+    % Create bird's-eye plot for the ego car and sensor coverage
+    hBEVPlot = axes(hBEVPanel);
+    frontBackLim = 60;
+    BEP = birdsEyePlot('Parent', hBEVPlot, 'Xlimits', [-frontBackLim frontBackLim], 'Ylimits', [-35 35]);
+
+    % Plot the coverage areas for radars
+    for i = 1:7
+        cap = coverageAreaPlotter(BEP,'FaceColor','red','EdgeColor','red');
+        plotCoverageArea(cap, sensors{i}.SensorLocation,...
+            sensors{i}.MaxRange, sensors{i}.Yaw, sensors{i}.FieldOfView(1));
+    end
+
+    % Plot the coverage areas for vision sensors
+    for i = 1:2
+        cap = coverageAreaPlotter(BEP,'FaceColor','blue','EdgeColor','blue');
+        plotCoverageArea(cap, sensors{i}.SensorLocation,...
+            sensors{i}.MaxRange, sensors{i}.Yaw, 45);
+    end
+
+    % Create a vision detection plotter put it in a struct for future use
+    detectionPlotter(BEP, 'DisplayName','vision', 'MarkerEdgeColor','blue', 'Marker','^');
+
+    % Combine all radar detections into one entry and store it for later update
+    detectionPlotter(BEP, 'DisplayName','radar', 'MarkerEdgeColor','red');
+
+    % Add road borders to plot
+    laneMarkingPlotter(BEP, 'DisplayName','lane markings');
+
+    % Add the tracks to the bird's-eye plot. Show last 10 track updates.
+    trackPlotter(BEP, 'DisplayName','track', 'HistoryDepth',10);
+
+    axis(BEP.Parent, 'equal');
+    xlim(BEP.Parent, [-frontBackLim frontBackLim]);
+    ylim(BEP.Parent, [-40 40]);
+
+    % Add an outline plotter for ground truth
+    outlinePlotter(BEP, 'Tag', 'Ground truth');
+end
+
+function updateBEP(BEP, egoCar, detections, confirmedTracks, psel, vsel)
+    % Update road boundaries and their display
+    [lmv, lmf] = laneMarkingVertices(egoCar);
+    plotLaneMarking(findPlotter(BEP,'DisplayName','lane markings'),lmv,lmf);
+
+    % update ground truth data
+    [position, yaw, length, width, originOffset, color] = targetOutlines(egoCar);
+    plotOutline(findPlotter(BEP,'Tag','Ground truth'), position, yaw, length, width, 'OriginOffset', originOffset, 'Color', color);
+
+    % Prepare and update detections display
+    N = numel(detections);
+    detPos = zeros(N,2);
+    isRadar = true(N,1);
+    for i = 1:N
+        detPos(i,:) = detections{i}.Measurement(1:2)';
+        if detections{i}.SensorIndex > 6 % Vision detections
+            isRadar(i) = false;
+        end
+    end
+    plotDetection(findPlotter(BEP,'DisplayName','vision'), detPos(~isRadar,:));
+    plotDetection(findPlotter(BEP,'DisplayName','radar'), detPos(isRadar,:));
+
+    % Prepare and update tracks display
+    trackIDs = {confirmedTracks.TrackID};
+    labels = cellfun(@num2str, trackIDs, 'UniformOutput', false);
+    [tracksPos, tracksCov] = getTrackPositions(confirmedTracks, psel);
+    tracksVel = getTrackVelocities(confirmedTracks, vsel);
+    plotTrack(findPlotter(BEP,'DisplayName','track'), tracksPos, tracksVel, tracksCov, labels);
+end
+
@@ -255,4 +255,4 @@ function updateBEP(BEP, egoCar, detections, confirmedTracks, psel, vsel)
     [tracksPos, tracksCov] = getTrackPositions(confirmedTracks, psel);
     tracksVel = getTrackVelocities(confirmedTracks, vsel);
     plotTrack(findPlotter(BEP,'DisplayName','track'), tracksPos, tracksVel, tracksCov, labels);
-end
+end