%% Obtain Target Poses Within Sensor Range
% Obtain the poses of targets that are within the maximum range of a sensor mounted to the ego vehicle.
% Create a driving scenario. The scenario contains a 75-meter straight road, an ego vehicle, and two target vehicles.
% The nearest target vehicle is 45 meters away and in the same lane as the ego vehicle.
% The farthest target vehicle is 65 meters away and in the opposite lane of the ego vehicle.
% Plot the driving scenario.

scenario = drivingScenario;

roadCenters = [0 0 0; 75 0 0];
laneSpecification = lanespec([1 1]);
road(scenario,roadCenters,'Lanes',laneSpecification,'Name','Road');

egoVehicle = vehicle(scenario, ...
    'ClassID',1, ...
    'Position',[4 -2 0], ...
    'Name','Ego');

vehicle(scenario, ...
    'ClassID',1, ...
    'Position',[45 -1.7 0], ...
    'Name','Near Target');

vehicle(scenario, ...
    'ClassID',1, ...
    'Position',[65 2 0], ...
    'Yaw',-180, ...
    'Name','Far Target');

plot(scenario)

%
% Create a vision sensor mounted to the front bumper of the ego vehicle. Configure the sensor to have a maximum detection range of 50 meters.

sensor = visionDetectionGenerator('SensorIndex',1, ...
    'SensorLocation',[3.7 0], ...
    'MaxRange',50);

%
% Plot the outlines of the vehicles and the coverage area of the sensor. The nearest target vehicle is within range of the sensor but the farthest target vehicle is not.

bep = birdsEyePlot;

olPlotter = outlinePlotter(bep);
[position,yaw,length,width,originOffset,color] = targetOutlines(egoVehicle);
plotOutline(olPlotter,position,yaw,length,width, ...
    'OriginOffset',originOffset,'Color',color)

caPlotter = coverageAreaPlotter(bep,'DisplayName','Coverage area','FaceColor','blue');
mountPosition = sensor.SensorLocation;
range = sensor.MaxRange;
orientation = sensor.Yaw;
fieldOfView = sensor.FieldOfView(1);
plotCoverageArea(caPlotter,mountPosition,range,orientation,fieldOfView);

% Obtain the poses of targets that are within the range of the sensor. The output structure contains the pose of only the nearest target, which is under 50 meters away from the ego vehicle.

poses = targetPoses(egoVehicle,range)

% Copyright 2020 The MathWorks, Inc.