%% Generate Lidar Point Cloud Data of Multiple Actors
% Generate lidar point cloud data for a driving scenario with multiple actors by using the lidarPointCloudGenerator System object. Create the driving scenario by using drivingScenario object. It contains an ego-vehicle, pedestrian and two other vehicles.
%% Create and plot a driving scenario with multiple vehicles
% Create a driving scenario.

scenario = drivingScenario;

% Add a straight road to the driving scenario. The road has one lane in each direction.

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

% Add an ego vehicle to the driving scenario.

egoVehicle = vehicle(scenario,'ClassID',1,'Mesh',driving.scenario.carMesh);
waypoints = [1 -2 0; 35 -2 0];
smoothTrajectory(egoVehicle,waypoints,10);

% Add a truck, pedestrian, and bicycle to the driving scenario and plot the scenario.

truck = vehicle(scenario,'ClassID',2,'Length', 8.2,'Width',2.5,'Height',3.5, ...
  'Mesh',driving.scenario.truckMesh);
waypoints = [70 1.7 0; 20 1.9 0];
smoothTrajectory(truck,waypoints,15);

pedestrian = actor(scenario,'ClassID',4,'Length',0.24,'Width',0.45,'Height',1.7, ...
  'Mesh',driving.scenario.pedestrianMesh);
waypoints = [23 -4 0; 10.4 -4 0];
smoothTrajectory(pedestrian,waypoints,1.5);

bicycle = actor(scenario,'ClassID',3,'Length',1.7,'Width',0.45,'Height',1.7, ...
  'Mesh',driving.scenario.bicycleMesh);
waypoints = [12.7 -3.3 0; 49.3 -3.3 0];
smoothTrajectory(bicycle,waypoints,5);

plot(scenario,'Meshes','on')

%
%% Generate and plot lidar point cloud data
% Create a lidarPointCloudGenerator System object.

lidar = lidarPointCloudGenerator;

% Add actor profiles and the ego vehicle actor ID from the driving scenario to the System object.

lidar.ActorProfiles = actorProfiles(scenario);
lidar.EgoVehicleActorID = egoVehicle.ActorID;

% Plot the point cloud data.

bep = birdsEyePlot('Xlimits',[0 70],'YLimits',[-30 30]);
plotter = pointCloudPlotter(bep);
legend('off');
while advance(scenario)
    tgts = targetPoses(egoVehicle);
    rdmesh = roadMesh(egoVehicle);
    [ptCloud,isValidTime] = lidar(tgts,rdmesh,scenario.SimulationTime);
    if isValidTime
        plotPointCloud(plotter,ptCloud);
    end
end

% Copyright 2019 The MathWorks, Inc.