Added R2019b files

README.md

@@ -0,0 +1,57 @@
+# Getting Started with MATLAB, Simulink, and ROS
+### Copyright 2017-2019 The MathWorks, Inc.
+Resources for getting started with MATLAB and Simulink and the Robot Operating System (ROS). This functionality is provided by ROS Toolbox.
+
+## templateFiles Folder
+Contains files provide a basic structure for creating your own MATLAB and Simulink files.
+
+### MATLAB Templates
+* **loopTemplate:** Algorithm runs in a simple loop, with an optional pause between iterations
+* **rateTemplate:** Algorithm runs in a simple loop, which runs at a fixed rate dictated by either the wall clock or the global ROS node clock
+* **async/asyncTemplate:** Algorithm runs asynchronously, whenever a new ROS message is received
+* **timer/timerTemplate:** Algorithm is scheduled to run on a timer in the background
+
+### Simulink Templates
+* **simulationTemplate:** Model is slowed down using the wall clock, to run approximately in real time
+* **codeGenerationTemplate:** Model is configured to generate a standalone C++ ROS node using Embedded Coder
+* **multirateTemplate:** Model contains multiple subscribe and algorithm rates, as well as displays the color-coded rates in the model
+* **architecture/mdlArchTemplate:** Model contains MATLAB, Simulink, and Stateflow elements. Links to reusable MATLAB files and Simulink block library in the **architecture** folder
+* **paramServerGetterTemplate + paramServerSetterTemplate**: The "getter" model can run on the desktop or deployed standalone, and receives parameters from the ROS parameter server. To modify the parameter value, you can either run the "setter" model, or use **rosparam** in MATLAB or a Terminal window.
+
+## demoFiles Folder
+Contains filled out versions of the templates above, that will run on several platforms.
+
+### Algorithms
+
+#### Object Detection
+* Uses camera information to detect a blue object and return its position and size
+* MATLAB code can be found in **Algorithms\detectCircle.m**
+* The same function is called in the Simulink examples using a MATLAB Function block
+
+#### Object Tracking
+* Uses the output of the object detection algorithm to assign linear and angular velocities such that the object is centered in the field of vision and has a certain pixel size (or distance)
+* MATLAB code can be found in **Algorithms\trackCircle.m**
+* Simulink and Stateflow versions of the same algorithm can be found in **Algorithms\controlLib.slx**
+
+### Target Platforms
+
+#### Webcam
+These examples have been tested on Windows using the MATLAB Support Package for USB Webcams at https://www.mathworks.com/matlabcentral/fileexchange/45182-matlab-support-package-for-usb-webcams
+
+#### Gazebo Examples
+These examples have been tested using the Linux virtual machine at https://www.mathworks.com/support/product/robotics/v3-installation-instructions.html.
+
+When you open the virtual machine, you can track blue objects in either the **Gazebo Playground** or **Gazebo TurtleBot World** worlds. To test the tracking algorithm, you can use Gazebo to move the robot and/or the blue objects in the world.
+
+#### TurtleBot
+These examples have been tested using the TurtleBot 2. 
+
+You can find a printable blue circle in the **Images** folder.
+
+### MATLAB Examples
+The **MATLAB** subfolder contains loop examples, as well as a **timer** subfolder showing the timer object and handle class approach.
+
+### Simulink Examples
+The **Simulink** subfolder contains examples with Simulink blocks, with Simulink blocks and a Stateflow chart, and using External Mode and the ROS Parameter Server (see the **paramServer** subfolder).
+
+In addition, the **distributed** subfolder contains examples of the perception, control, and visualization components broken into multiple tasks and separate models. The models can communicate with each other with a standard ROS message or with a custom message which can be found in the **custom_robot_msgs** folder. To get the latter example working, you will need to use the ROS Toolbox Interface for ROS Custom Messages add-on and follow the steps shown at https://www.mathworks.com/help/ros/ug/ros-custom-message-support.html

demoFiles/Algorithms/controlParams.m

@@ -0,0 +1,23 @@
+function params = controlParams
+% Creates object tracking controller parameters (Webcam and TurtleBot)
+% Copyright 2017 The MathWorks, Inc.
+
+    params.Ts = 0.1;          % Sample time
+
+    params.bufSize = 5;        % Filter buffer size
+    params.maxDisp = 300;      % Max object displacement [pixels]
+    params.minSize = 50;       % Min object size [pixels]
+    params.maxSize = 500;      % Max object size [pixels]
+    params.maxCounts = 5;      % Max outlier counts before stopping
+
+    params.linVelGain = 2e-3;  % Linear control gain
+    params.angVelGain = 4e-3;  % Angular control gain
+    params.maxLinVel = 0.2;   % Max linear speed
+    params.maxAngVel = 0.75;   % Max angular speed
+
+    params.posDeadZone = 30;   % Steering control marker position dead zone [pixels] 
+    params.targetSize = 180;   % Linear speed control target blob size [pixels]
+    params.sizeDeadZone = 30;  % Linear speed control size dead zone [pixels]
+    params.speedRedSize = 100; % Minimum pixel value before turning speed is ramped down
+
+end

demoFiles/Algorithms/controlParamsGazebo.m

@@ -0,0 +1,23 @@
+function params = controlParamsGazebo
+% Creates object tracking controller parameters (Gazebo)
+% Copyright 2017 The MathWorks, Inc.
+
+    params.Ts = 0.1;          % Sample time
+
+    params.bufSize = 5;        % Filter buffer size
+    params.maxDisp = 300;      % Max object displacement [pixels]
+    params.minSize = 50;       % Min object size [pixels]
+    params.maxSize = 500;      % Max object size [pixels]
+    params.maxCounts = 5;      % Max outlier counts before stopping
+
+    params.linVelGain = 2e-3;  % Linear control gain
+    params.angVelGain = 5e-4;  % Angular control gain
+    params.maxLinVel = 0.2;   % Max linear speed
+    params.maxAngVel = 0.75;   % Max angular speed
+
+    params.posDeadZone = 30;   % Steering control marker position dead zone [pixels] 
+    params.targetSize = 180;   % Linear speed control target blob size [pixels]
+    params.sizeDeadZone = 30;  % Linear speed control size dead zone [pixels]
+    params.speedRedSize = 100; % Minimum pixel value before turning speed is ramped down
+
+end

demoFiles/Algorithms/detectCircle.m

@@ -0,0 +1,59 @@
+function [centerX,centerY,blobSize] = detectCircle(img,scale)
+% Detects circle location and size in image
+% Copyright 2017 The MathWorks, Inc.
+
+    % Initialize variables
+    centerX = 0;
+    centerY = 0;
+    blobSize = 0;
+
+    % Resize the image
+    img = imresize(img,scale,'Antialiasing',false);
+
+    % Create mask based on chosen histogram thresholds
+    % For this example, we convert to L*a*b* space and then threshold
+    % Thresholds for blue marker
+    LMin = 0;
+    LMax = 70;
+    aMin = -100;
+    aMax = 100;
+    bMin = -100;
+    bMax = -15;
+    labImg = rgb2lab(img);
+    imgBW = (labImg(:,:,1)>=LMin)&(labImg(:,:,1)<=LMax)& ...
+            (labImg(:,:,2)>=aMin)&(labImg(:,:,2)<=aMax)& ...
+            (labImg(:,:,3)>=bMin)&(labImg(:,:,3)<=bMax);
+
+    % Detect blobs
+    persistent detector
+    if isempty(detector)
+        detector = vision.BlobAnalysis( ...
+                    'BoundingBoxOutputPort',false, ...
+                    'AreaOutputPort',false, ...
+                    'MajorAxisLengthOutputPort', true, ...
+                    'MinimumBlobArea',300, ...
+                    'MaximumCount', 10);
+    end
+    [centroids,majorAxes] = detector(imgBW);
+
+    % Estimate the blob location and size, if any are large enough
+    if ~isempty(majorAxes)
+
+        % Find max blob major axis
+        [blobSize,maxIdx] = max(majorAxes);
+        blobSize = double(blobSize(1));
+
+        % Find location of largest blob
+        maxLoc = centroids(maxIdx,:);
+        centerX = double(maxLoc(1));
+        centerY = double(maxLoc(2));
+
+    end
+
+    % Rescale outputs
+    centerX = centerX/scale;
+    centerY = centerY/scale;
+    blobSize = blobSize/scale;
+
+end
+

demoFiles/Algorithms/trackCircle.m

@@ -0,0 +1,62 @@
+function [v,w] = trackCircle(x,blobSize,imgWidth,params)
+% Tracks location and distance of detected circle by assigning 
+% linear and angular velocities
+% Copyright 2017 The MathWorks, Inc.
+
+    %% Initialize persistent variables
+    persistent xBuffer xPrev sizeBuffer outlierCount
+    if isempty(xBuffer); xBuffer = zeros(1,params.bufSize); end
+    if isempty(sizeBuffer); sizeBuffer = zeros(1,params.bufSize); end
+    if isempty(xPrev); xPrev = x; end
+    if isempty(outlierCount); outlierCount = 0; end
+
+    %% Input processing
+    % Count outliers
+    if (abs(x-xPrev)<params.maxDisp) && (blobSize>params.minSize) && (blobSize<params.maxSize)      
+        outlierCount = 0;
+    else
+        outlierCount = min(outlierCount+1,1000); % Increment with saturation
+    end
+    % Update and average the measurement buffers
+    xBuffer = [xBuffer(2:params.bufSize) x];
+    sizeBuffer = [sizeBuffer(2:params.bufSize) blobSize];
+    xFilt = mean(xBuffer);
+    sizeFilt = mean(sizeBuffer);
+    xPrev = x;
+
+    %% Angular velocity control: Keep the marker centered
+    w = 0;
+    if outlierCount < params.maxCounts        
+        posError = xFilt - imgWidth/2;
+        if abs(posError) > params.posDeadZone
+            speedReduction = max(sizeFilt/params.speedRedSize,1);
+            w = -params.angVelGain*posError*speedReduction;
+        end
+        if w > params.maxAngVel
+            w = params.maxAngVel;
+        elseif w < -params.maxAngVel
+            w = -params.maxAngVel;
+        end
+    end
+
+    %% Linear velocity control: Keep the marker at a certain distance
+    v = 0;
+    if outlierCount < params.maxCounts      
+        sizeError = params.targetSize - sizeFilt;
+        if abs(sizeError) > params.sizeDeadZone
+            v = params.linVelGain*sizeError;
+        end
+        if v > params.maxLinVel
+            v = params.maxLinVel;
+        elseif v < -params.maxLinVel
+            v = -params.maxLinVel;
+        end
+    end
+
+    %% Scan autonomously for a while if the outlier count has been exceeded
+    if outlierCount > 50 && outlierCount < 500
+       w = params.maxAngVel/2; 
+    end
+
+end
+

demoFiles/MATLAB/gazeboExampleMATLAB.m

@@ -0,0 +1,42 @@
+%% Object Detection Example: Gazebo
+% Copyright 2017 The MathWorks, Inc.
+
+%% SETUP
+% Connect to ROS master
+rosshutdown;
+gazeboIp = '192.168.119.133';
+rosinit(gazeboIp);
+% Create ROS subscribers and publishers
+imgSub = rossubscriber('/camera/rgb/image_raw');
+receive(imgSub,10); % Wait to receive first message
+[velPub,velMsg] = rospublisher('/mobile_base/commands/velocity');
+% Create video player for visualization
+vidPlayer = vision.DeployableVideoPlayer;
+% Load control parameters
+params = controlParamsGazebo;
+
+%% LOOP
+while(1) 
+    %% SENSE
+    % Grab images
+    img = readImage(imgSub.LatestMessage);
+
+    %% PROCESS
+    % Object detection algorithm
+    resizeScale = 0.5;
+    [centerX,centerY,circleSize] = detectCircle(img,resizeScale);
+    % Object tracking algorithm
+    [v,w] = trackCircle(centerX,circleSize,size(img,2),params);
+
+    %% CONTROL
+    % Package ROS message and send to the robot
+    velMsg.Linear.X = v;
+    velMsg.Angular.Z = w;
+    send(velPub,velMsg);
+
+    %% VISUALIZE
+    % Annotate image and update the video player
+    img = insertShape(img,'Circle',[centerX centerY circleSize/2],'LineWidth',2);
+    step(vidPlayer,img);
+
+end

demoFiles/MATLAB/timer/MyRobot.m

@@ -0,0 +1,81 @@
+classdef MyRobot < handle
+    % MYROBOT Contains Robot Information
+    % Copyright 2017 The MathWorks, Inc.
+
+    properties
+        % ROS publishers and subscribers
+        ImgSub = [];
+        VelPub = [];
+        VelMsg = [];
+
+        % Perception algorithm
+        CircleX = 0;
+        CircleY = 0;
+        CircleSize = 0;
+        ImgWidth = 0;
+        ImgResizeScale = 1;
+
+        % Control algorithm
+        Params = [];
+
+        % Visualization
+        VidPlayer = [];
+    end
+
+    methods
+
+        %% CONSTRUCTOR
+        function obj = MyRobot(ip,subName,pubName,paramFcn)
+            % Connect to ROS master
+            rosshutdown;
+            rosinit(ip);
+
+            % Create subscriber
+            obj.ImgSub = rossubscriber(subName);
+            msg = receive(obj.ImgSub,10);
+
+            % Create publisher
+            [obj.VelPub,obj.VelMsg] = rospublisher(pubName);
+
+            % Create parameters
+            obj.Params = feval(paramFcn);    
+
+            % Start visualization
+            obj.VidPlayer = vision.DeployableVideoPlayer;
+            step(obj.VidPlayer,readImage(msg)); % Display first image
+
+        end       
+
+        %% PERCEPTION ALGORITHM
+        % Receives latest image data, runs detectCircle function, and
+        % visualizes the results
+        function perceptionFcn(obj,~,~)           
+            % Read image from ROS subscriber
+            img = readImage(obj.ImgSub.LatestMessage);
+            obj.ImgWidth = size(img,2);
+
+            % Run perception algorithm
+            [obj.CircleX,obj.CircleY,obj.CircleSize] = ... 
+                detectCircle(img,obj.ImgResizeScale);
+
+            % Insert detected shape and visualize results
+            img = insertShape(img,'Circle',[obj.CircleX obj.CircleY obj.CircleSize/2], ...
+                              'LineWidth',2);
+            step(obj.VidPlayer,img);           
+        end
+
+        %% CONTROL ALGORITHM
+        % Runs trackCircle function and publishes ROS velocity commands
+        function controlFcn(obj,~,~)            
+             % Object tracking algorithm
+            [v,w] = trackCircle(obj.CircleX,obj.CircleSize,obj.ImgWidth,obj.Params);
+
+            % Package ROS message and send to the robot
+            obj.VelMsg.Linear.X = v;
+            obj.VelMsg.Angular.Z = w;
+            send(obj.VelPub,obj.VelMsg);           
+        end       
+    end
+
+end
+

demoFiles/MATLAB/timer/gazeboExampleTimer.m

@@ -0,0 +1,20 @@
+%% Object Detection Example: Gazebo (with timer and handle class)
+% Copyright 2017 The MathWorks, Inc.
+
+%% SETUP
+% Create robot class containing all ROS entities and parameters
+gazeboIp = '192.168.119.133';
+subName = '/camera/rgb/image_raw';
+pubName = '/mobile_base/commands/velocity';
+paramFcn = 'controlParamsGazebo';
+robot = MyRobot(gazeboIp,subName,pubName,paramFcn);
+
+%% TIMER
+robot.Params.Ts = 0.2; % 1/0.2 = 5 Hz
+robotTimer = timer('TimerFcn',{@robotTimerFcn,robot}, ...
+                'Period',robot.Params.Ts, ...
+                'ExecutionMode','fixedRate');
+start(robotTimer);
+
+%% CLEANUP
+% stop(robotTimer);

demoFiles/MATLAB/timer/robotTimerFcn.m

@@ -0,0 +1,11 @@
+function robotTimerFcn(obj,event,robot)
+% Timer callback function -- runs at every iteration of the timer
+% Copyright 2017 The MathWorks, Inc.
+
+    % Run perception algorithm
+    perceptionFcn(robot); 
+
+    % Run control algorithm
+    controlFcn(robot);
+
+end