gusucode.com > Mobile Robotics Simulation Toolbox > examples/matlab/sensors/mrsExampleObjectDetector.m

    %% Object Detector Example
% Copyright 2018 The MathWorks, Inc.
clc
close all

%% Create environment
load exampleMap

% Create objects [x,y,label]
objects = [2, 8, 1; ...
           8, 11, 2; ...
           8, 4, 3];

% Create object Detector sensor
detector = ObjectDetector;
detector.fieldOfView = pi/4;

% Create visualizer
viz = Visualizer2D;
viz.mapName = 'map';
attachObjectDetector(viz,detector);
viz.objectColors = [1 0 0;0 1 0;0 0 1];
viz.objectMarkers = 'so^';

%% Simulation parameters
sampleTime = 0.05;             % Sample time [s]
initPose = [6.5; 8; 0];        % Initial pose (x y theta)

% Initialize time, input, and pose arrays
tVec = 0:sampleTime:10;         % Time array
vxRef = zeros(size(tVec));      % Reference x speed
vyRef = zeros(size(tVec));  % Reference y speed
wRef = 0.75*ones(size(tVec));   % Reference angular speed
ref = [vxRef;vyRef;wRef];
pose = zeros(3,numel(tVec));    % Pose matrix
pose(:,1) = initPose;

%% Simulation loop
r = robotics.Rate(1/sampleTime);
for idx = 2:numel(tVec)   
    % Convert the reference speeds to world coordinates
    vel = bodyToWorld(ref(:,idx-1),pose(:,idx-1));
    
    % Perform forward discrete integration step
    pose(:,idx) = pose(:,idx-1) + vel*sampleTime;
    
    % Update object detector and visualization
    detections = detector(pose(:,idx),objects);
    viz(pose(:,idx),objects)
    
    % Display object detections every 10th iteration
    if mod(idx,10) == 0
        if ~isempty(detections)
            nearestLabel = detections(1,3);
            disp(['Nearest object is of label ' num2str(nearestLabel)]); 
        else
            disp('No objects detected'); 
        end
    end
    waitfor(r);
    
end