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

    classdef RobotDetector < matlab.System & matlab.system.mixin.CustomIcon & matlab.system.mixin.Propagates & matlab.system.mixin.SampleTime
    % ROBOTDETECTOR Robot detector simulator
    %
    % Returns the angles, ranges, and labels of a simulated robot detector
    %
    % For more information, see <a href="matlab:edit mrsDocRobotDetector">the documentation page</a>
    %
    % Copyright 2018-2019 The MathWorks, Inc.
    
    %% PROPERTIES
    % Public (user-visible) properties
    properties
        robotIdx = 1;           % Robot index
        sensorOffset = [0,0];   % Robot detector offset (x,y) [m]
        sensorAngle = 0;        % Robot detector angle [rad]
        fieldOfView = pi/3;     % Sensor field of view [rad]
        maxRange = 5;           % Maximum range [m]
        maxDetections = 3;      % Maximum number of detections
    end
    
    properties(Nontunable)
       sampleTime = 0.1; % Sample time
    end

    % Private properties
    properties(Access = private)
        env;        % MultiRobotEnv object
        map;        % Occupancy grid
        hasMap;     % Binary flag for having a map
    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 = RobotDetector(varargin)
            if nargin > 1
               obj.robotIdx = varargin{2}; 
            else
               obj.robotIdx = 1;
            end
            if nargin > 0 
                obj.env = varargin{1};
            else
                % Get to this step only if the constructor is called
                % without arguments. Defaults to behavior compatible with
                % Simulink blocks.
                try
                    global slMultiRobotEnv  
                    obj.env = slMultiRobotEnv;
                catch
                    error('No Multi-Robot Environment available');
                end
            end
            
        end
    end
    
    methods(Access = protected)
      
        function setupImpl(obj)           
            
           if isempty(obj.env)
                global slMultiRobotEnv
                obj.env = slMultiRobotEnv;
           end
           
           % Attach the robot detector to the environment
           attachRobotDetector(obj.env,obj);
           
           % Ensure to use the same map as the visualizer
           obj.map = internal.createMapFromName(obj.env.mapName);
           obj.hasMap = ~isempty(obj.map);
           
        end
        
        % Step method: Wraps around the step method of the RobotDetector
        function detections = stepImpl(obj)                 
            % Initialize
            ranges = [];
            angles = [];
            labels = [];
            
            % Find the sensor pose and check if valid
            pose = obj.env.Poses(:,obj.robotIdx);
            theta = pose(3);
            offsetVec = [cos(theta) -sin(theta); ...
                         sin(theta)  cos(theta)]*obj.sensorOffset';
            sensorPose = pose + [offsetVec; obj.sensorAngle];  
            if ~obj.hasMap
                validPose = true;
            else
                validPose = sensorPose(1) >= obj.map.XWorldLimits(1) && ...
                            sensorPose(1) <= obj.map.XWorldLimits(2) && ... 
                            sensorPose(2) >= obj.map.YWorldLimits(1) && ... 
                            sensorPose(2) <= obj.map.YWorldLimits(2);
            end
            
            % Return the range and angle for all robots
            % First, find the offsets
            offsets = obj.env.Poses(1:2,:) - sensorPose(1:2);
            offsets(:,obj.robotIdx) = inf; % Take out the "self-offset"
            
            if ~isempty(offsets) && validPose
                % Extract ranges and angles
                ranges = sqrt(sum(offsets.^2,1));
                angles = wrapToPi(atan2(offsets(2,:),offsets(1,:))-sensorPose(3));
                
                % Filter by maximum range and field of vision
                validIdx = (ranges <= obj.maxRange) & ... 
                           (abs(angles) <= obj.fieldOfView/2);
                ranges = ranges(validIdx);
                angles = angles(validIdx);
                labels = find(validIdx);
                                
                % Use occupancy grid, if any, to account for obstacles
                if ~isempty(obj.map)
                    % Loop backwards since we're removing values
                    for idx = numel(ranges):-1:1  
                        intPts = rayIntersection(obj.map,sensorPose, ... 
                                                 angles(idx),ranges(idx));
                        % Delete the reading if the point is occupied
                        if ~isnan(intPts)
                            ranges(idx) = [];
                            angles(idx) = [];
                            labels(idx) = [];
                        end
                    end
                end
                
            end
            
            if ~isempty(ranges)
                % Sort from nearest
                [ranges,sortedIdx] = sort(ranges);
                angles = angles(sortedIdx);
                labels = labels(sortedIdx);
                if numel(ranges) > obj.maxDetections
                    ranges = ranges(1:obj.maxDetections);
                    angles = angles(1:obj.maxDetections);
                    labels = labels(1:obj.maxDetections);
                end
                
                % Pack the final results into the output
                detections = [ranges', angles', labels'];
            else
                detections = []; 
            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 = [obj.maxDetections,3];
        end
        
        function fx = isOutputFixedSizeImpl(~)
           fx = false;
        end
        
        function dt = getOutputDataTypeImpl(~)
            dt = 'double';
        end

        function cp = isOutputComplexImpl(~)
            cp = false;
        end
        
        % Define icon for System block
        function icon = getIconImpl(obj)
            icon = {['Robot ' num2str(obj.robotIdx)],'Detector'};
        end
        
        % Define sample time
        % Must be discrete since the output is a variable-sized signal
        function sts = getSampleTimeImpl(obj)
            if obj.sampleTime > 0
               sts = createSampleTime(obj,'Type','Discrete','SampleTime',obj.sampleTime);
            else
               sts = createSampleTime(obj,'Type','Inherited'); 
            end
        end
        
        % Save and load object methods to enable stepping
        function s = saveObjectImpl(obj)
            s = saveObjectImpl@matlab.System(obj);
            s.env = obj.env;
            s.map = obj.map;
        end
        function loadObjectImpl(obj,s,wasInUse)
            obj.map = s.map;
            obj.env = s.env;
            loadObjectImpl@matlab.System(obj,s,wasInUse);
        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