Avoid Moving Obstacles Based on Radar Detections

rng(0) % For repeatable results.

% Create a scenario that runs for 10 seconds.
s = uavScenario("StopTime",30,"HistoryBufferSize",200);

% Create a fixed-wing UAV that moves from [30 0 0] to [20 10 0].
obstacleUAV = uavPlatform("Obstacle",s,"Trajectory",waypointTrajectory([30 0 0; 0 30 0],"TimeOfArrival",[0 30]));
updateMesh(obstacleUAV,"fixedwing",{1},[1 0 0],eul2tform([0 0 pi]));

% Create a quadrotor as the ego vehicle.
egoVelocity = [1 1 0];
egoMultirotor = uavPlatform("EgoVehicle",s,"InitialVelocity", egoVelocity);
updateMesh(egoMultirotor,"quadrotor",{1},[0 1 0],eul2tform([0 0 pi]));

% Create a radar sensor and set up its properties.
radarSensorObject = radarDataGenerator("no scanning","SensorIndex",1,"UpdateRate",10,...
    "FieldOfView",[120 80],...
    "HasElevation", true,...
    "ElevationResolution", 3,...
    "AzimuthResolution", 1, ...
    "RangeResolution", 10, ... meters
    "RangeRateResolution",3,...
    "RangeLimits", [0 750],...
    "TargetReportFormat","Tracks",...
    "TrackCoordinates",'Scenario',...
    "HasINS", true,...
    "HasFalseAlarms",true,...
    "FalseAlarmRate",1e-5,...
    "HasRangeRate",true,...
    "FalseAlarmRate", 1e-7);

% Mount the radar sensor on the ego vehicle. ExampleHelperUAVRadar inherits
% from the uav.SensorAdaptor class.
radar = uavSensor("Radar",egoMultirotor,ExampleHelperUAVRadar(radarSensorObject),"MountingAngles", [0 0 0]);

% Set up the 3D view of the scenario.
[ax, plotFrames] = show3D(s);
% Represent the ego UAV as a green marker.
egoLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame);
% Represent the other UAV as a red marker.
obstacleUAVLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","red","Parent",plotFrames.Obstacle.BodyFrame);
legend([egoLine obstacleUAVLine], ["Ego", "Obstacle"])
xlim([-5,35]);
ylim([-5,35]);

% Show the radar field of view.
radarDirection = hgtransform("Parent",plotFrames.EgoVehicle.BodyFrame,"Matrix",eye(4));
coverageAngles = linspace(-radarSensorObject.FieldOfView(1)/360*pi,radarSensorObject.FieldOfView(1)/360*pi,128);
patch("XData",[0 radarSensorObject.RangeLimits(2)*cos(coverageAngles) 0], ...
    "YData",[0 radarSensorObject.RangeLimits(2)*sin(coverageAngles) 0], ...
    FaceAlpha=0.3,Parent=radarDirection);

% Start simulation.
setup(s);
while advance(s)
    % Move ego UAV along its velocity vector.
    motion = read(egoMultirotor);
    motion(1:2) = motion(1:2) + motion(4:5)/s.UpdateRate;
    move(egoMultirotor, motion);

    show3D(s,"FastUpdate", true,"Parent",ax);
    pause(0.02);
end

% Set up safety radius
radiusUAV = 0.5;  % UAV radius (m)
radiusObs = 2;  % Obstacle radius (m)
safetyDist = 2;  % Collision safety radius (m) - same for all obstacles
effectiveRadius = radiusUAV+radiusObs+safetyDist;

% Time Horizon (time until collision to perform avoidance maneuver)
Th = 10;  % sec

% Set up the 3D view of the scenario.
[ax,plotFrames] = show3D(s);
% Represent the ego UAV as a green marker.
egoLine = plot3(0,0,0,"Marker","diamond","MarkerFaceColor","green","Parent",plotFrames.EgoVehicle.BodyFrame);
% Represent the other UAV as a red marker.
obstacleUAVLine = plot3(0,0,0,"Marker","diamond","MarkerEdgeColor","red","MarkerFaceColor", "none", "Parent",plotFrames.Obstacle.BodyFrame);
xlim([-5,35]);
ylim([-5,35]);
zlim([-20,20]);

% Represent the radar detection.
detectionLine = plot3(nan,nan,nan,"Marker","o","MarkerFaceColor","blue", "MarkerSize", 4, "Parent",plotFrames.NED);
legend([egoLine obstacleUAVLine detectionLine], ["Ego", "Obstacle", "Detection"])

% Show the radar field of view.
radarDirection = hgtransform("Parent",plotFrames.EgoVehicle.BodyFrame,"Matrix",eye(4));
coverageAngles = linspace(-radarSensorObject.FieldOfView(1)/360*pi,radarSensorObject.FieldOfView(1)/360*pi,128);
patch("XData",[0 radarSensorObject.RangeLimits(2)*cos(coverageAngles) 0], ...
    "YData",[0 radarSensorObject.RangeLimits(2)*sin(coverageAngles) 0], ...
    FaceAlpha=0.3,Parent=radarDirection)

% Start simulation.
restart(s);
setup(s);
desVel = egoVelocity(1:2);
while advance(s)

    % Move the ego UAV along its velocity vector.
    motion = read(egoMultirotor);
    motion(1:2) = motion(1:2) + desVel/s.UpdateRate;
    motion(4:5) = desVel;
    move(egoMultirotor, motion);

    % Update sensor readings and read data.
    updateSensors(s);

    % Obtain detections from the radar.
    [isUpdated,time,confTracks,numTracks,config] = read(radar);

    % Perform obstacle avoidance maneuver by adjusting ego vehicle's
    % velocity
    desVel = egoVelocity(1:2);
    if numTracks > 0
        % Detect imminent collision using
        % exampleHelperDetectImminentCollision function.
        obstacleStates = [confTracks.State];
        detectionLine.XData = obstacleStates(1,:);
        detectionLine.YData = obstacleStates(3,:);
        detectionLine.ZData = obstacleStates(5,:);
        [isOnCollisionPath, VOFrontAngle, VOBackAngle, VOMinAngle, VOMaxAngle] ...
            = exampleHelperDetectImminentCollision(motion(1:2)', motion(4:5)', obstacleStates([1,3],:), ...
            obstacleStates([2,4],:),effectiveRadius,Th);

        % Find obstacles that are on imminent collision path.
        collisionObsIdx = find(isOnCollisionPath);
        if any(isOnCollisionPath)
            % Compute the velocity required to avoid collision using the
            % exampleHelperCVHeuristic2D function.
            desVel = exampleHelperCVHeuristic2D(norm(egoVelocity), ...
                motion(1:2)', ...
                motion(4:5)', ...
                obstacleStates([1,3],collisionObsIdx), ...
                obstacleStates([2,4],collisionObsIdx), ...
                effectiveRadius, ...
                VOFrontAngle(collisionObsIdx), ...
                VOBackAngle(collisionObsIdx), ...
                VOMinAngle(collisionObsIdx), ...
                VOMaxAngle(collisionObsIdx), Th);
            desVel = desVel';
        end
    end

    show3D(s,"FastUpdate", true,"Parent",ax);
    pause(0.02);
end