www.gusucode.com > Mobile Robotics Simulation Toolbox > src/environment/LidarSensor.m

    classdef LidarSensor < matlab.System & matlab.system.mixin.CustomIcon & matlab.system.mixin.Propagates
    % LIDARSENSOR 2D Lidar simulator
    %
    % Returns the angles and ranges of a simulated lidar sensor based on
    % the input map (occupancy grid), scan offsets/angles, and maximum range.
    % Range values outside the maximum range will return NaN.
    %
    % For more information, see <a href="matlab:edit mrsDocLidarSensor">the documentation page</a>
    %
    % Copyright 2018-2019 The MathWorks, Inc.

    %% PROPERTIES
    % Public (user-visible) properties
    properties(Nontunable)
        mapName = ''; % Map
    end
    properties
        sensorOffset = [0,0]; % Lidar sensor offset (x,y) [m]
        scanAngles = [-pi/4,0,pi/4]; % Scan angles [rad]
        maxRange = 5; % Maximum range [m]
    end
    properties(Nontunable,Logical)
        isMultiRobot = false;   % Enable multi-robot detections
    end
    properties
        robotIdx = 1;       % Robot index
    end
    
    % Private properties
    properties(Access = private)
        env;     % MultiRobotEnv object
        map;     % Occupancy grid
    end

    %% METHODS
    methods
        % Constructor: Optionally accepts the following arguments:
        % 1: MultiRobotEnv object (defaults to global variable for Simulink)
        % 2: Robot index (defaults to 1)
        function obj = LidarSensor(varargin)
            if nargin > 1
               obj.robotIdx = varargin{2}; 
            else
               obj.robotIdx = 1;
            end
            if nargin > 0 
                env = varargin{1};
                setEnvironment(obj,env);
            else
            end
        end
        
        % Sets private property 'env' to a MultiRobotEnv object
        function setEnvironment(obj,env)
            if isa(env,'MultiRobotEnv')
                obj.env = env;
                obj.isMultiRobot = true;
            else
               error('Specify a MultiRobotEnv object as the environment.'); 
            end
        end
        
    end
    
    methods(Access = protected)
        
        % Setup method: Initializes all necessary graphics objects
        function setupImpl(obj)
            
            % Load the occupancy grid
            obj.map = internal.createMapFromName(obj.mapName);
            
            if obj.isMultiRobot && isempty(obj.env)
                % Get to this step only if the isMultiRobot flag is true, 
                % but no visualizer name was specified. Defaults to behavior
                % compatible with Simulink blocks.
                try
                    global slMultiRobotEnv  
                    release(slMultiRobotEnv);
                    setEnvironment(obj,slMultiRobotEnv);
                catch
                    error('No Multi-Robot Environment available');
                end
            end
            
        end

        % Step method: Outputs simulated lidar ranges based on map,
        % robot pose, and scan angles/max range
        function ranges = stepImpl(obj,pose)       
            
            % Initialize the ranges to the invalid value (NaN)
            ranges = nan(numel(obj.scanAngles),1);
            
            % If the map is empty or the pose is outside the map limits,
            % return all NaN
            if isempty(obj.map)
                warning('Robot map is empty. Returning NaN for lidar ranges.')
                return;
            elseif (pose(1)<obj.map.XWorldLimits(1)) || (pose(1)>obj.map.XWorldLimits(2)) || ...
                   (pose(2)<obj.map.YWorldLimits(1)) || (pose(2)>obj.map.YWorldLimits(2))
                warning('Robot pose outside world limits. Returning NaN for lidar ranges.');
                return;
            end

            % Find the sensor pose
            theta = pose(3);
            offsetVec = [cos(theta) -sin(theta); ...
                         sin(theta)  cos(theta)]*obj.sensorOffset';
            sensorLoc = pose(1:2)' + offsetVec';                                 

            % Cast a ray from the robot pose through the max range

            % If there is a single sensor offset, use that value and find
            % the intersection points/ranges in a single command
            if size(sensorLoc,1) == 1
               sensorPose = [sensorLoc, theta];
               interPts = rayIntersection(obj.map, sensorPose, ...
                                          obj.scanAngles, obj.maxRange);
               offsets = interPts-sensorPose(1:2);
               ranges = sqrt(sum(offsets.^2,2));
               
               % Check for multi-robot intersections
               if obj.isMultiRobot
                   targetPoses = obj.env.Poses;
                   targetRadii = obj.env.robotRadius;
                   for rIdx = 1:obj.env.numRobots
                       if (rIdx ~= obj.robotIdx) && (~isempty(targetPoses)) && (targetRadii(rIdx)>0)
                           robotRanges = internal.circleLineIntersection( ...
                               sensorPose,obj.scanAngles,obj.maxRange, ...
                               targetPoses(:,rIdx),targetRadii(rIdx));
                           ranges = min(ranges,robotRanges);
                       end
                   end
               end
               
            % Else, loop through each sensor location and find the 
            % intersection points/ranges one by one
            else
               
               if obj.isMultiRobot
                    targetPoses = obj.env.Poses;
                    targetRadii = obj.env.robotRadius;
               end
               
               for idx = 1:numel(ranges)
                    sensorPose = [sensorLoc(idx,:), theta]; 
                    interPts = rayIntersection(obj.map, sensorPose, ...
                                    obj.scanAngles(idx), obj.maxRange);
                    offsets = interPts-sensorPose(1:2);
                    ranges(idx) = sqrt(sum(offsets^2,2));
                    
                    % Check for multi-robot intersections
                    if obj.isMultiRobot
                        for rIdx = 1:obj.env.numRobots
                            if (rIdx ~= obj.robotIdx) && (~isempty(targetPoses)) && (targetRadii(rIdx)>0)
                                robotRanges = internal.circleLineIntersection( ...
                                    sensorPose,obj.scanAngles(idx),obj.maxRange(idx), ...
                                    targetPoses(:,rIdx),targetRadii(rIdx));
                                ranges(idx) = min(ranges(idx),robotRanges);
                            end
                        end
                    end
               end
            end
        end         

        % More methods needed for the Simulink block to inherit its output
        % sizes from the scan angle parameter provided.
        function sz = getOutputSizeImpl(obj)
            sz = numel(obj.scanAngles);
        end
        
        function fx = isOutputFixedSizeImpl(~)
           fx = true;
        end
        
        function dt = getOutputDataTypeImpl(obj)
            dt = propagatedInputDataType(obj,1);
        end

        function cp = isOutputComplexImpl(~)
            cp = false;
        end
        
        % Define icon for System block
        function icon = getIconImpl(~)
            icon = {'Lidar','Sensor'};
        end
          
        % Inactivate properties based on multirobot
        function flag = isInactivePropertyImpl(obj,prop)
             flag = false;
             switch prop
                 case 'robotIdx'
                     flag = ~obj.isMultiRobot;
             end
        end
        
    end
    
    methods (Static, Access = protected)
        % Do not show "Simulate using" option
        function flag = showSimulateUsingImpl
            flag = false;
        end
        % Always run in interpreted mode
        function simMode = getSimulateUsingImpl
            simMode = 'Interpreted execution';
        end
    end
    
end