From 80704553df4b597c0ad858d3f81ffcfd76e045c7 Mon Sep 17 00:00:00 2001 From: HVjay Date: Thu, 8 Nov 2018 19:40:07 -0500 Subject: [PATCH] Changed curvature of road, added opposing lane with car and a bike --- MatlabChallenge.m | 311 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 311 insertions(+) create mode 100644 MatlabChallenge.m diff --git a/MatlabChallenge.m b/MatlabChallenge.m new file mode 100644 index 0000000..d86f94c --- /dev/null +++ b/MatlabChallenge.m @@ -0,0 +1,311 @@ +function [allData, scenario, sensors] = MatlabChallenge() +%MatlabChallenge - Returns sensor detections +% allData = MatlabChallenge returns sensor detections in a structure +% with time for an internally defined scenario and sensor suite. +% +% [allData, scenario, sensors] = MatlabChallenge optionally returns +% the drivingScenario and detection generator objects. + +% Generated by MATLAB(R) 9.5 and Automated Driving System Toolbox 1.3. +% Generated on: 08-Nov-2018 19:34:46 + +% Create the drivingScenario object and ego car +[scenario, egoCar] = createDrivingScenario; + +% Create all the sensors +[sensors, numSensors] = createSensors(scenario); + +allData = struct('Time', {}, 'ActorPoses', {}, 'ObjectDetections', {}, 'LaneDetections', {}); +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 + 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}); %#ok + end + + % Advance the scenario one time step and exit the loop if the scenario is complete + running = advance(scenario); +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', [1.9 0], ... + 'DetectorOutput', 'Objects only', ... + 'ActorProfiles', profiles); +sensors{2} = visionDetectionGenerator('SensorIndex', 2, ... + 'SensorLocation', [1.9 0], ... + 'Yaw', 180, ... + 'DetectorOutput', 'Objects only', ... + 'ActorProfiles', profiles); +sensors{3} = visionDetectionGenerator('SensorIndex', 3, ... + 'SensorLocation', [3.7 0], ... + 'Yaw', -30, ... + 'MaxRange', 100, ... + 'DetectorOutput', 'Objects only', ... + 'Intrinsics', cameraIntrinsics([1814.81018227767 1814.81018227767],[320 240],[480 640]), ... + 'ActorProfiles', profiles); +sensors{4} = radarDetectionGenerator('SensorIndex', 4, ... + 'SensorLocation', [1.67 -0.9], ... + 'Yaw', -90, ... + 'Pitch', 40, ... + 'MaxRange', 75, ... + 'FieldOfView', [80 5], ... + 'ActorProfiles', profiles); +sensors{5} = radarDetectionGenerator('SensorIndex', 5, ... + 'SensorLocation', [3.7 0], ... + 'MaxRange', 30, ... + 'FieldOfView', [170 2], ... + 'ActorProfiles', profiles); +sensors{6} = radarDetectionGenerator('SensorIndex', 6, ... + 'SensorLocation', [1.56 0.83], ... + 'Yaw', 90, ... + 'MaxRange', 75, ... + 'FieldOfView', [80 5], ... + 'ActorProfiles', profiles); +sensors{7} = radarDetectionGenerator('SensorIndex', 7, ... + 'SensorLocation', [3.7 0], ... + 'MaxRange', 100, ... + 'FieldOfView', [70 10], ... + 'ActorProfiles', profiles); +sensors{8} = radarDetectionGenerator('SensorIndex', 8, ... + 'SensorLocation', [-1 0], ... + 'Yaw', -180, ... + 'MaxRange', 100, ... + 'FieldOfView', [50 5], ... + 'ActorProfiles', profiles); +sensors{9} = radarDetectionGenerator('SensorIndex', 9, ... + 'SensorLocation', [-1 0], ... + 'Yaw', -180, ... + 'MaxRange', 30, ... + 'FieldOfView', [170 5], ... + 'ActorProfiles', profiles); +sensors{10} = radarDetectionGenerator('SensorIndex', 10, ... + 'SensorLocation', [1.63 0.88], ... + 'Yaw', 90, ... + 'MaxRange', 10, ... + 'FieldOfView', [180 5], ... + 'ActorProfiles', profiles); +sensors{11} = radarDetectionGenerator('SensorIndex', 11, ... + 'SensorLocation', [1.76 -0.9], ... + 'Yaw', -90, ... + 'MaxRange', 10, ... + 'FieldOfView', [180 5], ... + 'ActorProfiles', profiles); +numSensors = 11; + +function [scenario, egoCar] = createDrivingScenario +% createDrivingScenario Returns the drivingScenario defined in the Designer + +% Construct a drivingScenario object. +scenario = drivingScenario; + +% Add all road segments +roadCenters = [-10.8 38 0; + -15.9 51.9 0; + -25.5 63.4 0; + -59.5 67.7 0]; +laneSpecification = lanespec(3, 'Width', 3.28333); +road(scenario, roadCenters, 'Lanes', laneSpecification); + +roadCenters = [-11.2 36.2 0; + -3 9.4 0; + 22.9 -3.3 0; + 43.5 -2.9 0; + 50.9 55.1 0; + 69 92.6 0; + 98.6 87.6 0; + 106.4 62.1 0; + 108.9 27.5 0; + 107.3 -5.8 0; + 106.9 -53.1 0; + 132.2 -79.5 0; + 139.3 -42.6 0; + 151.1 -36.8 0; + 149.2 -4.6 0; + 150.1 44.4 0; + 150.5 75.7 0]; +marking = [laneMarking('Solid', 'Color', [0.98 0.86 0.36]) + laneMarking('DoubleSolid', 'Color', [0.98 0.86 0.36]) + laneMarking('Dashed') + laneMarking('Solid')]; +laneSpecification = lanespec(3, 'Width', 3.28333, 'Marking', marking); +road(scenario, roadCenters, 'Lanes', laneSpecification); + +% Add the ego car +egoCar = vehicle(scenario, ... + 'ClassID', 1, ... + 'Position', [-5.5 8.9 0]); +waypoints = [-5.5 8.9 0; + 6.4 -3.1 0; + 26 -7.1 0; + 36.7 -8.2 0; + 44.1 -6.3 0; + 48.5 -2.4 0; + 52.7 1 0; + 55.1 5.5 0; + 58 11.7 0; + 58.6 19.7 0; + 57.8 28.5 0; + 55.9 39.5 0; + 53.7 51.5 0; + 53.3 57.1 0; + 53.5 66.8 0; + 56.6 75.9 0; + 64.2 86.7 0; + 68.86 90.04 0.01; + 74.31 92.56 0.01; + 78.9 93.3 0; + 83.2 93.3 0; + 88.5 91.4 0; + 92.8 89.2 0; + 99.1 81.9 0; + 102.1 71.7 0; + 103.8 59.2 0; + 105.2 45.3 0; + 106.3 31.7 0; + 106 17.2 0; + 105.4 0.9 0; + 104.5 -13.8 0; + 103.9 -23.5 0; + 103.6 -36.9 0; + 103.5 -44.4 0; + 104.5 -52.7 0; + 105.2 -65.1 0; + 112.6 -80 0; + 122.8 -84.6 0; + 131.8 -83.5 0; + 137.8 -77.4 0; + 139.4 -67.2 0; + 138.8 -60.8 0; + 137.5 -50.7 0; + 141.4 -43.3 0; + 149.3 -41.6 0; + 156.2 -35.3 0; + 157.9 -30.1 0; + 157.6 -21.8 0; + 154.8 -12.2 0; + 153.6 -9.9 0]; +speed = 30; +trajectory(egoCar, waypoints, speed); + +% Add the non-ego actors +car1 = vehicle(scenario, ... + 'ClassID', 1, ... + 'Position', [-10.8 31.7 0]); +waypoints = [-10.8 31.7 0; + -6 13.9 0; + 4.3 3.1 0; + 16.1 -2 0; + 33.5 -4.5 0; + 39.8 -3.9 0; + 48.1 -0.3 0; + 53.6 8.6 0; + 55.8 20.4 0; + 54.2 36.1 0; + 51.4 48.9 0; + 51.4 62.3 0; + 51.8 70.1 0; + 58.7 85 0; + 69.3 93 0; + 81.8 96 0; + 90.3 93.4 0; + 95.7 90.9 0; + 101.9 83 0; + 105.9 71.9 0; + 106.9 59 0; + 107.5 46.4 0; + 109.2 23.3 0; + 108.2 8.2 0; + 107.3 -6.1 0; + 107.3 -16.3 0; + 106.3 -30.3 0; + 106.3 -38.5 0; + 106.6 -48.5 0; + 108.3 -61 0; + 112.1 -73 0; + 120.8 -80.9 0; + 135.3 -75.6 0; + 136 -67.6 0; + 136 -59.9 0; + 135.1 -53.1 0; + 136.6 -46.2 0; + 143.4 -40.1 0; + 148.6 -38.2 0; + 150.2 -36.1 0]; +speed = 40; +trajectory(car1, waypoints, speed); + +actor(scenario, ... + 'ClassID', 5, ... + 'Length', 2.4, ... + 'Width', 0.76, ... + 'Height', 0.8, ... + 'Position', [146 -33.4 0], ... + 'Yaw', 130); + +bicycle = actor(scenario, ... + 'ClassID', 3, ... + 'Length', 1.7, ... + 'Width', 0.45, ... + 'Height', 1.7, ... + 'Position', [104.2 84.8 0]); +waypoints = [104.2 84.8 0; + 102 88.1 0; + 98.1 93.5 0; + 92.7 96.7 0; + 87.9 98.6 0; + 83 99.4 0; + 75.1 99 0; + 68.1 96.4 0; + 61.5 93.2 0; + 58 88.2 0; + 54 82 0; + 50.8 73.7 0; + 48.8 60.4 0; + 51.3 42.2 0; + 54.3 26.6 0; + 53.1 14.5 0]; +speed = 10; +trajectory(bicycle, waypoints, speed); +