From 5075aa718b5a6133ace4b9ac0a7f501733cb337a Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 15 Oct 2019 13:52:14 -0400 Subject: [PATCH] Updated README with video and blog links. --- README.md | 124 ++++++++------ demoFiles/Algorithms/controlParams.m | 44 ++--- demoFiles/Algorithms/controlParamsGazebo.m | 44 ++--- demoFiles/Algorithms/trackCircle.m | 124 +++++++------- demoFiles/MATLAB/gazeboExampleMATLAB.m | 82 ++++----- demoFiles/MATLAB/timer/MyRobot.m | 162 +++++++++--------- demoFiles/MATLAB/timer/gazeboExampleTimer.m | 38 ++-- demoFiles/MATLAB/timer/robotTimerFcn.m | 20 +-- .../MATLAB/timer/turtlebotExampleTimer.m | 38 ++-- demoFiles/MATLAB/turtleBotExampleMATLAB.m | 82 ++++----- demoFiles/MATLAB/webcamExampleMATLAB.m | 70 ++++---- .../msg/circle_detection.msg | 8 +- .../distributed/custom_robot_msgs/package.xml | 32 ++-- demoFiles/loadIpAddresses.m | 14 +- demoFiles/startupDemo.m | 22 +-- templateFiles/MATLAB/RobotTemplateClass.m | 154 ++++++++--------- templateFiles/MATLAB/async/asyncTemplate.m | 60 +++---- templateFiles/MATLAB/async/mySubCallback.m | 36 ++-- templateFiles/MATLAB/loopTemplate.m | 122 ++++++------- templateFiles/MATLAB/myControlAlgorithm.m | 16 +- templateFiles/MATLAB/myPerceptionAlgorithm.m | 16 +- templateFiles/MATLAB/rateTemplate.m | 138 +++++++-------- templateFiles/MATLAB/timer/myTimerFcn.m | 48 +++--- templateFiles/MATLAB/timer/timerTemplate.m | 66 +++---- .../Simulink/architecture/myPerceptionFcn.m | 20 +-- .../Simulink/architecture/myPreloadScript.m | 32 ++-- templateFiles/startupTemplates.m | 22 +-- 27 files changed, 822 insertions(+), 812 deletions(-) diff --git a/README.md b/README.md index af420ce..716ec76 100644 --- a/README.md +++ b/README.md @@ -1,57 +1,67 @@ -# 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 +# Getting Started with MATLAB, Simulink, and ROS +Copyright 2017-2019 The MathWorks, Inc. + +## Getting Started +This repository contains resources for getting started with MATLAB and Simulink +and the Robot Operating System (ROS). This functionality is provided by ROS Toolbox. + +To learn more, refer to our [blog post](https://blogs.mathworks.com/racing-lounge/2017/11/08/matlab-simulink-ros/) and the following videos. + +* [Getting Started with MATLAB and ROS](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-getting-started-with-matlab-and-ros-1508263034047.html) +* [Getting Started with Simulink and ROS](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-getting-started-with-simulink-and-ros-1509397202143.html) +* [Deploying Standalone ROS Nodes from Simulink](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-deploying-algorithms-to-ros-1510659362460.html) +* [Distributed Computing with MATLAB, Simulink, and ROS](https://www.mathworks.com/videos/matlab-and-simulink-robotics-arena-designing-distributed-systems-with-ros-1514584072926.html) + +## 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 diff --git a/demoFiles/Algorithms/controlParams.m b/demoFiles/Algorithms/controlParams.m index 6b2241b..59ba773 100644 --- a/demoFiles/Algorithms/controlParams.m +++ b/demoFiles/Algorithms/controlParams.m @@ -1,23 +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 - +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 \ No newline at end of file diff --git a/demoFiles/Algorithms/controlParamsGazebo.m b/demoFiles/Algorithms/controlParamsGazebo.m index d84f213..dc3acda 100644 --- a/demoFiles/Algorithms/controlParamsGazebo.m +++ b/demoFiles/Algorithms/controlParamsGazebo.m @@ -1,23 +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 - +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 \ No newline at end of file diff --git a/demoFiles/Algorithms/trackCircle.m b/demoFiles/Algorithms/trackCircle.m index a93b1e4..ea6bb46 100644 --- a/demoFiles/Algorithms/trackCircle.m +++ b/demoFiles/Algorithms/trackCircle.m @@ -1,62 +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.minSize) && (blobSize 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 - +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.minSize) && (blobSize 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 + diff --git a/demoFiles/MATLAB/gazeboExampleMATLAB.m b/demoFiles/MATLAB/gazeboExampleMATLAB.m index 1bce371..86f53fb 100644 --- a/demoFiles/MATLAB/gazeboExampleMATLAB.m +++ b/demoFiles/MATLAB/gazeboExampleMATLAB.m @@ -1,42 +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); - +%% 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 \ No newline at end of file diff --git a/demoFiles/MATLAB/timer/MyRobot.m b/demoFiles/MATLAB/timer/MyRobot.m index c6270e6..a975eef 100644 --- a/demoFiles/MATLAB/timer/MyRobot.m +++ b/demoFiles/MATLAB/timer/MyRobot.m @@ -1,81 +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 - +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 + diff --git a/demoFiles/MATLAB/timer/gazeboExampleTimer.m b/demoFiles/MATLAB/timer/gazeboExampleTimer.m index d7c1a33..da2c230 100644 --- a/demoFiles/MATLAB/timer/gazeboExampleTimer.m +++ b/demoFiles/MATLAB/timer/gazeboExampleTimer.m @@ -1,20 +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 +%% 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); \ No newline at end of file diff --git a/demoFiles/MATLAB/timer/robotTimerFcn.m b/demoFiles/MATLAB/timer/robotTimerFcn.m index c83f60e..71e30f4 100644 --- a/demoFiles/MATLAB/timer/robotTimerFcn.m +++ b/demoFiles/MATLAB/timer/robotTimerFcn.m @@ -1,11 +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); - +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 \ No newline at end of file diff --git a/demoFiles/MATLAB/timer/turtlebotExampleTimer.m b/demoFiles/MATLAB/timer/turtlebotExampleTimer.m index 97bbb57..43b29e4 100644 --- a/demoFiles/MATLAB/timer/turtlebotExampleTimer.m +++ b/demoFiles/MATLAB/timer/turtlebotExampleTimer.m @@ -1,20 +1,20 @@ -%% Object Detection Example: TurtleBot (with timer and handle class) -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Create robot class containing all ROS entities and parameters -turtlebotIp = '172.31.28.38'; -subName = '/camera/rgb/image_rect_color'; -pubName = '/mobile_base/commands/velocity'; -paramFcn = 'controlParams'; -robot = MyRobot(turtlebotIp,subName,pubName,paramFcn); - -%% TIMERS -robot.Params.Ts = 0.2; % 1/0.2 = 5 Hz -robotTimer = timer('TimerFcn',{@robotTimerFcn,robot}, ... - 'Period',robot.Params.Ts, ... - 'ExecutionMode','fixedRate'); -start(robotTimer); - -%% CLEANUP +%% Object Detection Example: TurtleBot (with timer and handle class) +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Create robot class containing all ROS entities and parameters +turtlebotIp = '172.31.28.38'; +subName = '/camera/rgb/image_rect_color'; +pubName = '/mobile_base/commands/velocity'; +paramFcn = 'controlParams'; +robot = MyRobot(turtlebotIp,subName,pubName,paramFcn); + +%% TIMERS +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); \ No newline at end of file diff --git a/demoFiles/MATLAB/turtleBotExampleMATLAB.m b/demoFiles/MATLAB/turtleBotExampleMATLAB.m index 80728b9..4e67438 100644 --- a/demoFiles/MATLAB/turtleBotExampleMATLAB.m +++ b/demoFiles/MATLAB/turtleBotExampleMATLAB.m @@ -1,42 +1,42 @@ -%% Object Detection Example: TurtleBot -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Connect to ROS master -rosshutdown; -turtlebotIp = '172.31.28.38'; -rosinit(turtlebotIp); -% Create ROS subscribers and publishers -imgSub = rossubscriber('/camera/rgb/image_rect_color'); -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 = controlParams; - -%% 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); - +%% Object Detection Example: TurtleBot +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Connect to ROS master +rosshutdown; +turtlebotIp = '172.31.28.38'; +rosinit(turtlebotIp); +% Create ROS subscribers and publishers +imgSub = rossubscriber('/camera/rgb/image_rect_color'); +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 = controlParams; + +%% 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 \ No newline at end of file diff --git a/demoFiles/MATLAB/webcamExampleMATLAB.m b/demoFiles/MATLAB/webcamExampleMATLAB.m index a013d26..75c2e6d 100644 --- a/demoFiles/MATLAB/webcamExampleMATLAB.m +++ b/demoFiles/MATLAB/webcamExampleMATLAB.m @@ -1,36 +1,36 @@ -%% Object Detection Example: Webcam -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Create a video device to acquire data from a camera -if ~exist('myCam','var') - myCam = imaq.VideoDevice('winvideo',1,'RGB24_640x480','ReturnedDataType','uint8'); -end -% Create video player for visualization -vidPlayer = vision.DeployableVideoPlayer; -% Load control parameters -params = controlParams; - -%% LOOP -while(1) - %% SENSE - % Grab images - img = step(myCam); - - %% 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 - % Display velocity results - fprintf('Linear Velocity: %f, Angular Velocity: %f\n',v,w); - - %% VISUALIZE - % Annotate image and update the video player - img = insertShape(img,'Circle',[centerX centerY circleSize/2],'LineWidth',2); - step(vidPlayer,img); - +%% Object Detection Example: Webcam +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Create a video device to acquire data from a camera +if ~exist('myCam','var') + myCam = imaq.VideoDevice('winvideo',1,'RGB24_640x480','ReturnedDataType','uint8'); +end +% Create video player for visualization +vidPlayer = vision.DeployableVideoPlayer; +% Load control parameters +params = controlParams; + +%% LOOP +while(1) + %% SENSE + % Grab images + img = step(myCam); + + %% 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 + % Display velocity results + fprintf('Linear Velocity: %f, Angular Velocity: %f\n',v,w); + + %% VISUALIZE + % Annotate image and update the video player + img = insertShape(img,'Circle',[centerX centerY circleSize/2],'LineWidth',2); + step(vidPlayer,img); + end \ No newline at end of file diff --git a/demoFiles/Simulink/distributed/custom_robot_msgs/msg/circle_detection.msg b/demoFiles/Simulink/distributed/custom_robot_msgs/msg/circle_detection.msg index d864203..022bd87 100644 --- a/demoFiles/Simulink/distributed/custom_robot_msgs/msg/circle_detection.msg +++ b/demoFiles/Simulink/distributed/custom_robot_msgs/msg/circle_detection.msg @@ -1,5 +1,5 @@ -#Contains results of object detection algorithm -int16 circleX -int16 circleY -int16 blobSize +#Contains results of object detection algorithm +int16 circleX +int16 circleY +int16 blobSize int16 imgWidth \ No newline at end of file diff --git a/demoFiles/Simulink/distributed/custom_robot_msgs/package.xml b/demoFiles/Simulink/distributed/custom_robot_msgs/package.xml index b5b17fb..50345d0 100644 --- a/demoFiles/Simulink/distributed/custom_robot_msgs/package.xml +++ b/demoFiles/Simulink/distributed/custom_robot_msgs/package.xml @@ -1,17 +1,17 @@ - - custom_robot_msgs - 1.0.42 - Custom messages for robotics demo - MathWorks Student Competitions Team - BSD - - catkin - - message_generation - std_msgs - roscpp - message_runtime - std_msgs - roscpp - + + custom_robot_msgs + 1.0.42 + Custom messages for robotics demo + MathWorks Student Competitions Team + BSD + + catkin + + message_generation + std_msgs + roscpp + message_runtime + std_msgs + roscpp + \ No newline at end of file diff --git a/demoFiles/loadIpAddresses.m b/demoFiles/loadIpAddresses.m index f5adf5a..8b7c8e6 100644 --- a/demoFiles/loadIpAddresses.m +++ b/demoFiles/loadIpAddresses.m @@ -1,8 +1,8 @@ -%% Modify the IP addresses of your ROS enabled robots/simulators here -% Copyright 2017 The MathWorks, Inc. - -% Gazebo IP address -gazeboIp = '192.168.119.133'; - -% Turtlebot IP address +%% Modify the IP addresses of your ROS enabled robots/simulators here +% Copyright 2017 The MathWorks, Inc. + +% Gazebo IP address +gazeboIp = '192.168.119.133'; + +% Turtlebot IP address turtlebotIp = '172.31.28.38'; \ No newline at end of file diff --git a/demoFiles/startupDemo.m b/demoFiles/startupDemo.m index bc3ab26..116b751 100644 --- a/demoFiles/startupDemo.m +++ b/demoFiles/startupDemo.m @@ -1,12 +1,12 @@ -% Copyright 2017 The MathWorks, Inc. - -% Clean up -close all; -clear; -clc; - -% Set up the path -addpath(pwd,genpath('MATLAB'),genpath('Simulink'),genpath('Algorithms')); - -% Set up simulation cache and code generation folders +% Copyright 2017 The MathWorks, Inc. + +% Clean up +close all; +clear; +clc; + +% Set up the path +addpath(pwd,genpath('MATLAB'),genpath('Simulink'),genpath('Algorithms')); + +% Set up simulation cache and code generation folders Simulink.fileGenControl('set','CacheFolder','work','CodeGenFolder','work'); \ No newline at end of file diff --git a/templateFiles/MATLAB/RobotTemplateClass.m b/templateFiles/MATLAB/RobotTemplateClass.m index a59bdce..a58bfb3 100644 --- a/templateFiles/MATLAB/RobotTemplateClass.m +++ b/templateFiles/MATLAB/RobotTemplateClass.m @@ -1,77 +1,77 @@ -classdef RobotTemplateClass < handle - % ROBOTTEMPLATECLASS Contains Robot Information - % Copyright 2017 The MathWorks, Inc. - - properties - % ROS publishers and subscribers - MySub = []; - MyPub = []; - PubMsg = []; - - % Perception algorithm - PerceptionParam = 1; - - % Control algorithm - ControlInputs = 0; - ControlOutputs = 0; - ControlParams = struct('gain',1); - - % Visualization - MyFig = []; - MyAxes = []; - CurrentTime = 0; - end - - methods - %% CONSTRUCTOR - function obj = RobotTemplateClass(ipAddr) - % Connect to ROS master at the IP address specified - rosshutdown; - rosinit(ipAddr); - - % Create subscribers - obj.MySub = ... - rossubscriber('/my_sub_topic','geometry_msgs/Point'); - - % Create publisher - [obj.MyPub,obj.PubMsg] = ... - rospublisher('/my_pub_topic','geometry_msgs/Point'); - - % Start visualization - obj.MyFig = figure; - obj.MyAxes = axes(obj.MyFig); - title(obj.MyAxes,'Control Output') - xlabel(obj.MyAxes,'Time [s]') - hold(obj.MyAxes,'on'); - end - - %% PERCEPTION ALGORITHM - % Receives sensor data, runs perception algorithm, and visualizes - function perceptionFcn(obj,receivedMsg) - if isempty(receivedMsg) - receivedData = rand; % If message is empty, assign random number - else - receivedData = receivedMsg.Z; - end - - % Perception algorithm - obj.ControlInputs = ... - myPerceptionAlgorithm(receivedData,obj.PerceptionParam); - end - - %% CONTROL ALGORITHM - % Runs control algorithm and publishes ROS commands - function controlFcn(obj) - % Control algorithm - obj.ControlOutputs = ... - myControlAlgorithm(obj.ControlInputs,obj.ControlParams); - - % Package ROS message and send to the robot - obj.PubMsg.X = obj.ControlOutputs; - send(obj.MyPub,obj.PubMsg); - - end - end - -end - +classdef RobotTemplateClass < handle + % ROBOTTEMPLATECLASS Contains Robot Information + % Copyright 2017 The MathWorks, Inc. + + properties + % ROS publishers and subscribers + MySub = []; + MyPub = []; + PubMsg = []; + + % Perception algorithm + PerceptionParam = 1; + + % Control algorithm + ControlInputs = 0; + ControlOutputs = 0; + ControlParams = struct('gain',1); + + % Visualization + MyFig = []; + MyAxes = []; + CurrentTime = 0; + end + + methods + %% CONSTRUCTOR + function obj = RobotTemplateClass(ipAddr) + % Connect to ROS master at the IP address specified + rosshutdown; + rosinit(ipAddr); + + % Create subscribers + obj.MySub = ... + rossubscriber('/my_sub_topic','geometry_msgs/Point'); + + % Create publisher + [obj.MyPub,obj.PubMsg] = ... + rospublisher('/my_pub_topic','geometry_msgs/Point'); + + % Start visualization + obj.MyFig = figure; + obj.MyAxes = axes(obj.MyFig); + title(obj.MyAxes,'Control Output') + xlabel(obj.MyAxes,'Time [s]') + hold(obj.MyAxes,'on'); + end + + %% PERCEPTION ALGORITHM + % Receives sensor data, runs perception algorithm, and visualizes + function perceptionFcn(obj,receivedMsg) + if isempty(receivedMsg) + receivedData = rand; % If message is empty, assign random number + else + receivedData = receivedMsg.Z; + end + + % Perception algorithm + obj.ControlInputs = ... + myPerceptionAlgorithm(receivedData,obj.PerceptionParam); + end + + %% CONTROL ALGORITHM + % Runs control algorithm and publishes ROS commands + function controlFcn(obj) + % Control algorithm + obj.ControlOutputs = ... + myControlAlgorithm(obj.ControlInputs,obj.ControlParams); + + % Package ROS message and send to the robot + obj.PubMsg.X = obj.ControlOutputs; + send(obj.MyPub,obj.PubMsg); + + end + end + +end + diff --git a/templateFiles/MATLAB/async/asyncTemplate.m b/templateFiles/MATLAB/async/asyncTemplate.m index d012e12..eed9adc 100644 --- a/templateFiles/MATLAB/async/asyncTemplate.m +++ b/templateFiles/MATLAB/async/asyncTemplate.m @@ -1,31 +1,31 @@ -%% MATLAB Template: Asynchronous Execution -% Algorithms are run immediately on receiving a new sensor message -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Create object containing all ROS entities, parameters, and algorithms -ipAddr = ''; -robot = RobotTemplateClass(ipAddr); -% Modify parameters in the robot object -robot.PerceptionParam = 2; -robot.ControlParams.gain = 0.25; - -% Modify ROS subscriber to process sensor data asynchronously -% Define subscriber callback function, passing in the additional robot object -subCallbackFcn = {@mySubCallback,robot}; -% Using a callback, the algorithm can be run when a new message is received. -robot.MySub = ... - rossubscriber('/my_sub_topic','geometry_msgs/Point',subCallbackFcn); - -%% TEST -% Send random messages to the subscriber -tic; -currentTime = 0; -[testPub,testMsg] = rospublisher('/my_sub_topic'); -while(currentTime < 10) - testMsg.Z = rand; % Create random message - send(testPub,testMsg); % Send message - pause(rand^2); % Pause at a random time - % (Squaring exaggerates the time spread) - currentTime = toc; +%% MATLAB Template: Asynchronous Execution +% Algorithms are run immediately on receiving a new sensor message +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Create object containing all ROS entities, parameters, and algorithms +ipAddr = ''; +robot = RobotTemplateClass(ipAddr); +% Modify parameters in the robot object +robot.PerceptionParam = 2; +robot.ControlParams.gain = 0.25; + +% Modify ROS subscriber to process sensor data asynchronously +% Define subscriber callback function, passing in the additional robot object +subCallbackFcn = {@mySubCallback,robot}; +% Using a callback, the algorithm can be run when a new message is received. +robot.MySub = ... + rossubscriber('/my_sub_topic','geometry_msgs/Point',subCallbackFcn); + +%% TEST +% Send random messages to the subscriber +tic; +currentTime = 0; +[testPub,testMsg] = rospublisher('/my_sub_topic'); +while(currentTime < 10) + testMsg.Z = rand; % Create random message + send(testPub,testMsg); % Send message + pause(rand^2); % Pause at a random time + % (Squaring exaggerates the time spread) + currentTime = toc; end \ No newline at end of file diff --git a/templateFiles/MATLAB/async/mySubCallback.m b/templateFiles/MATLAB/async/mySubCallback.m index ddb7cff..0d29a79 100644 --- a/templateFiles/MATLAB/async/mySubCallback.m +++ b/templateFiles/MATLAB/async/mySubCallback.m @@ -1,18 +1,18 @@ -function mySubCallback(~,msg,robot) -% Example subscriber callback function -% Copyright 2017 The MathWorks, Inc. - - % Run perception algorithm - perceptionFcn(robot,msg); - - % Run control algorithm - controlFcn(robot); - - % (Optional) Visualize results - robot.CurrentTime = toc; - plot(robot.MyAxes,robot.CurrentTime,robot.ControlOutputs, ... - 'bo','MarkerSize',5); - drawnow - -end - +function mySubCallback(~,msg,robot) +% Example subscriber callback function +% Copyright 2017 The MathWorks, Inc. + + % Run perception algorithm + perceptionFcn(robot,msg); + + % Run control algorithm + controlFcn(robot); + + % (Optional) Visualize results + robot.CurrentTime = toc; + plot(robot.MyAxes,robot.CurrentTime,robot.ControlOutputs, ... + 'bo','MarkerSize',5); + drawnow + +end + diff --git a/templateFiles/MATLAB/loopTemplate.m b/templateFiles/MATLAB/loopTemplate.m index 31a94ae..2144340 100644 --- a/templateFiles/MATLAB/loopTemplate.m +++ b/templateFiles/MATLAB/loopTemplate.m @@ -1,62 +1,62 @@ -%% MATLAB Template: Simple Loop -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Start or connect to ROS master -rosshutdown; -ipAddr = ''; -rosinit(ipAddr) - -% Create ROS subscribers -mySub = rossubscriber('/my_sub_topic','geometry_msgs/Point'); - -% Create ROS publishers -[myPub,pubMsg] = rospublisher('/my_pub_topic','geometry_msgs/Point'); - -% Start visualization -close all -myViz = axes; -title(myViz,'Control Output') -xlabel(myViz,'Time [s]') -hold(myViz,'on') - -% Load parameters -perceptionParam = 2; % Individual parameter -controlParams.gain = 0.25; % Parameter structure - -%% CONTROL LOOP -% Start while-loop, which runs indefinitely and as quickly as possible. -tic -currentTime = 0; -while(currentTime < 10) - %% 1: SENSE - % Get latest data from ROS subscribers - receivedMsg = mySub.LatestMessage; - if isempty(receivedMsg) - receivedData = rand; % If message is empty, assign random number - else - receivedData = receivedMsg.Z; - end - - %% 2: PROCESS - % Run perception and control algorithms, which use received data and - % control parameters to produce some output. - ctrlInput = myPerceptionAlgorithm(receivedData,perceptionParam); - ctrlOutput = myControlAlgorithm(ctrlInput,controlParams); - - %% 3: CONTROL - % Package and send control outputs as ROS messages - pubMsg.X = ctrlOutput; - send(myPub,pubMsg); - - %% 4: VISUALIZE - % (Optional) Visualize data as the algorithm is running - currentTime = toc; - plot(myViz,currentTime,ctrlOutput,'bo','MarkerSize',5) - drawnow - - % (Optional) Pause execution to add delay between iterations - pause(0.1) - -% End while-loop +%% MATLAB Template: Simple Loop +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Start or connect to ROS master +rosshutdown; +ipAddr = ''; +rosinit(ipAddr) + +% Create ROS subscribers +mySub = rossubscriber('/my_sub_topic','geometry_msgs/Point'); + +% Create ROS publishers +[myPub,pubMsg] = rospublisher('/my_pub_topic','geometry_msgs/Point'); + +% Start visualization +close all +myViz = axes; +title(myViz,'Control Output') +xlabel(myViz,'Time [s]') +hold(myViz,'on') + +% Load parameters +perceptionParam = 2; % Individual parameter +controlParams.gain = 0.25; % Parameter structure + +%% CONTROL LOOP +% Start while-loop, which runs indefinitely and as quickly as possible. +tic +currentTime = 0; +while(currentTime < 10) + %% 1: SENSE + % Get latest data from ROS subscribers + receivedMsg = mySub.LatestMessage; + if isempty(receivedMsg) + receivedData = rand; % If message is empty, assign random number + else + receivedData = receivedMsg.Z; + end + + %% 2: PROCESS + % Run perception and control algorithms, which use received data and + % control parameters to produce some output. + ctrlInput = myPerceptionAlgorithm(receivedData,perceptionParam); + ctrlOutput = myControlAlgorithm(ctrlInput,controlParams); + + %% 3: CONTROL + % Package and send control outputs as ROS messages + pubMsg.X = ctrlOutput; + send(myPub,pubMsg); + + %% 4: VISUALIZE + % (Optional) Visualize data as the algorithm is running + currentTime = toc; + plot(myViz,currentTime,ctrlOutput,'bo','MarkerSize',5) + drawnow + + % (Optional) Pause execution to add delay between iterations + pause(0.1) + +% End while-loop end \ No newline at end of file diff --git a/templateFiles/MATLAB/myControlAlgorithm.m b/templateFiles/MATLAB/myControlAlgorithm.m index 1feb9e2..e8c37ce 100644 --- a/templateFiles/MATLAB/myControlAlgorithm.m +++ b/templateFiles/MATLAB/myControlAlgorithm.m @@ -1,8 +1,8 @@ -function ctrlOutput = myControlAlgorithm(ctrlInput,params) -% Placeholder control algorithm -% Copyright 2017 The MathWorks, Inc. - - ctrlOutput = ctrlInput * params.gain; - -end - +function ctrlOutput = myControlAlgorithm(ctrlInput,params) +% Placeholder control algorithm +% Copyright 2017 The MathWorks, Inc. + + ctrlOutput = ctrlInput * params.gain; + +end + diff --git a/templateFiles/MATLAB/myPerceptionAlgorithm.m b/templateFiles/MATLAB/myPerceptionAlgorithm.m index c14e48b..afcf748 100644 --- a/templateFiles/MATLAB/myPerceptionAlgorithm.m +++ b/templateFiles/MATLAB/myPerceptionAlgorithm.m @@ -1,8 +1,8 @@ -function perceptionOutput = myPerceptionAlgorithm(recvData,param) -% Placeholder perception algorithm -% Copyright 2017 The MathWorks, Inc. - - perceptionOutput = recvData / param; - -end - +function perceptionOutput = myPerceptionAlgorithm(recvData,param) +% Placeholder perception algorithm +% Copyright 2017 The MathWorks, Inc. + + perceptionOutput = recvData / param; + +end + diff --git a/templateFiles/MATLAB/rateTemplate.m b/templateFiles/MATLAB/rateTemplate.m index 175b96d..667a06c 100644 --- a/templateFiles/MATLAB/rateTemplate.m +++ b/templateFiles/MATLAB/rateTemplate.m @@ -1,69 +1,69 @@ -%% MATLAB Template: Simple Rate -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Start or connect to ROS master -rosshutdown; -ipAddr = ''; -rosinit(ipAddr) - -% Create ROS subscribers -mySub = rossubscriber('/my_sub_topic','geometry_msgs/Point'); - -% Create ROS publishers -[myPub,pubMsg] = rospublisher('/my_pub_topic','geometry_msgs/Point'); - -% Start visualization -close all -myViz = axes; -title(myViz,'Control Output') -xlabel(myViz,'Time [s]') -hold(myViz,'on') - -% Load parameters -perceptionParam = 2; % Individual parameter -controlParams.gain = 0.25; % Parameter structure - -%% CONTROL LOOP -% Start while-loop, which runs indefinitely at a given rate, or as quickly -% as possible if the rate cannot be satisfied. - -% Uses wall clock time -myRate = robotics.Rate(2); - -% Rate uses global ROS node time source. -% Useful if connecting to a ROS enabled simulator like Gazebo or V-REP -% myRate = rosrate(2); - -while(myRate.TotalElapsedTime < 10) - %% 1: SENSE - % Get latest data from ROS subscribers - receivedMsg = mySub.LatestMessage; - if isempty(receivedMsg) - receivedData = rand; % If message is empty, assign random number - else - receivedData = receivedMsg.Z; - end - - %% 2: PROCESS - % Run perception and control algorithms, which use received data and - % control parameters to produce some output. - ctrlInput = myPerceptionAlgorithm(receivedData,perceptionParam); - ctrlOutput = myControlAlgorithm(ctrlInput,controlParams); - - %% 3: CONTROL - % Package and send control outputs as ROS messages - pubMsg.X = ctrlOutput; - send(myPub,pubMsg); - - %% 4: VISUALIZE - % (Optional) Visualize data as the algorithm is running. - currentTime = myRate.TotalElapsedTime; - plot(myViz,currentTime,ctrlOutput,'bo','MarkerSize',5) - drawnow - - %% Block execution based on the rate defined. - waitfor(myRate); - -% End while-loop -end +%% MATLAB Template: Simple Rate +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Start or connect to ROS master +rosshutdown; +ipAddr = ''; +rosinit(ipAddr) + +% Create ROS subscribers +mySub = rossubscriber('/my_sub_topic','geometry_msgs/Point'); + +% Create ROS publishers +[myPub,pubMsg] = rospublisher('/my_pub_topic','geometry_msgs/Point'); + +% Start visualization +close all +myViz = axes; +title(myViz,'Control Output') +xlabel(myViz,'Time [s]') +hold(myViz,'on') + +% Load parameters +perceptionParam = 2; % Individual parameter +controlParams.gain = 0.25; % Parameter structure + +%% CONTROL LOOP +% Start while-loop, which runs indefinitely at a given rate, or as quickly +% as possible if the rate cannot be satisfied. + +% Uses wall clock time +myRate = robotics.Rate(2); + +% Rate uses global ROS node time source. +% Useful if connecting to a ROS enabled simulator like Gazebo or V-REP +% myRate = rosrate(2); + +while(myRate.TotalElapsedTime < 10) + %% 1: SENSE + % Get latest data from ROS subscribers + receivedMsg = mySub.LatestMessage; + if isempty(receivedMsg) + receivedData = rand; % If message is empty, assign random number + else + receivedData = receivedMsg.Z; + end + + %% 2: PROCESS + % Run perception and control algorithms, which use received data and + % control parameters to produce some output. + ctrlInput = myPerceptionAlgorithm(receivedData,perceptionParam); + ctrlOutput = myControlAlgorithm(ctrlInput,controlParams); + + %% 3: CONTROL + % Package and send control outputs as ROS messages + pubMsg.X = ctrlOutput; + send(myPub,pubMsg); + + %% 4: VISUALIZE + % (Optional) Visualize data as the algorithm is running. + currentTime = myRate.TotalElapsedTime; + plot(myViz,currentTime,ctrlOutput,'bo','MarkerSize',5) + drawnow + + %% Block execution based on the rate defined. + waitfor(myRate); + +% End while-loop +end diff --git a/templateFiles/MATLAB/timer/myTimerFcn.m b/templateFiles/MATLAB/timer/myTimerFcn.m index 9f3f3ef..84226a3 100644 --- a/templateFiles/MATLAB/timer/myTimerFcn.m +++ b/templateFiles/MATLAB/timer/myTimerFcn.m @@ -1,24 +1,24 @@ -function myTimerFcn(obj,event,robot) -% Timer callback function -- runs at every iteration of the timer -% Copyright 2017 The MathWorks, Inc. - - %% SENSE - % Get data from ROS subscribers - msg = robot.MySub.LatestMessage; - - %% PROCESS + CONTROL - % Run perception algorithm - perceptionFcn(robot,msg); - - % Run control algorithm - controlFcn(robot); - - %% VISUALIZE - % (Optional) Plot results - robot.CurrentTime = toc; - plot(robot.MyAxes,robot.CurrentTime,robot.ControlOutputs, ... - 'bo','MarkerSize',5); - drawnow - -end - +function myTimerFcn(obj,event,robot) +% Timer callback function -- runs at every iteration of the timer +% Copyright 2017 The MathWorks, Inc. + + %% SENSE + % Get data from ROS subscribers + msg = robot.MySub.LatestMessage; + + %% PROCESS + CONTROL + % Run perception algorithm + perceptionFcn(robot,msg); + + % Run control algorithm + controlFcn(robot); + + %% VISUALIZE + % (Optional) Plot results + robot.CurrentTime = toc; + plot(robot.MyAxes,robot.CurrentTime,robot.ControlOutputs, ... + 'bo','MarkerSize',5); + drawnow + +end + diff --git a/templateFiles/MATLAB/timer/timerTemplate.m b/templateFiles/MATLAB/timer/timerTemplate.m index fb6574f..4af1f7f 100644 --- a/templateFiles/MATLAB/timer/timerTemplate.m +++ b/templateFiles/MATLAB/timer/timerTemplate.m @@ -1,34 +1,34 @@ -%% MATLAB Template: Timer and Handle Class -% Timers are useful for scheduling tasks, for example, algorithm execution, -% in the background without blocking the MATLAB session. -% Storing all robot data and functionality in a handle class allows you to -% easily use and modify a single object. Multiple actions, such as timers -% or user input, can access the object functionality at the same time. -% Copyright 2017 The MathWorks, Inc. - -%% SETUP -% Create object containing all ROS entities, parameters, and algorithms -ipAddr = ''; -robot = RobotTemplateClass(ipAddr); -% Modify parameters in the robot object -obj.PerceptionParam = 2; -obj.ControlParams.gain = 0.25; - -%% TIMERS -% Create a timer that executes at a fixed rate. -% Note that you can create multiple timers for a multirate system. -% Each timer function requires at least two arguments. The syntax -% {@fcnName,robot} allows you to pass the robot class as a third argument. -sampleTime = 0.5; -myTimer = timer('TimerFcn',{@myTimerFcn,robot}, ... - 'Period',sampleTime, ... - 'ExecutionMode','fixedRate'); -% Begin keeping track of time and start timer -tic; -start(myTimer); - -%% CLEANUP -% To stop the algorithms, use the command -% stop(myTimer); -% If there are any unstopped timers, you can enter +%% MATLAB Template: Timer and Handle Class +% Timers are useful for scheduling tasks, for example, algorithm execution, +% in the background without blocking the MATLAB session. +% Storing all robot data and functionality in a handle class allows you to +% easily use and modify a single object. Multiple actions, such as timers +% or user input, can access the object functionality at the same time. +% Copyright 2017 The MathWorks, Inc. + +%% SETUP +% Create object containing all ROS entities, parameters, and algorithms +ipAddr = ''; +robot = RobotTemplateClass(ipAddr); +% Modify parameters in the robot object +obj.PerceptionParam = 2; +obj.ControlParams.gain = 0.25; + +%% TIMERS +% Create a timer that executes at a fixed rate. +% Note that you can create multiple timers for a multirate system. +% Each timer function requires at least two arguments. The syntax +% {@fcnName,robot} allows you to pass the robot class as a third argument. +sampleTime = 0.5; +myTimer = timer('TimerFcn',{@myTimerFcn,robot}, ... + 'Period',sampleTime, ... + 'ExecutionMode','fixedRate'); +% Begin keeping track of time and start timer +tic; +start(myTimer); + +%% CLEANUP +% To stop the algorithms, use the command +% stop(myTimer); +% If there are any unstopped timers, you can enter % delete(timerfindall); \ No newline at end of file diff --git a/templateFiles/Simulink/architecture/myPerceptionFcn.m b/templateFiles/Simulink/architecture/myPerceptionFcn.m index d1fb163..8459eb7 100644 --- a/templateFiles/Simulink/architecture/myPerceptionFcn.m +++ b/templateFiles/Simulink/architecture/myPerceptionFcn.m @@ -1,10 +1,10 @@ -function perceptionOutput = myPerceptionFcn(perceptionInput,param) -% Placeholder perception algorithm -% Copyright 2017 The MathWorks, Inc. - - perceptionOutput = perceptionInput / param; - -end - - - +function perceptionOutput = myPerceptionFcn(perceptionInput,param) +% Placeholder perception algorithm +% Copyright 2017 The MathWorks, Inc. + + perceptionOutput = perceptionInput / param; + +end + + + diff --git a/templateFiles/Simulink/architecture/myPreloadScript.m b/templateFiles/Simulink/architecture/myPreloadScript.m index 0471491..a4d7145 100644 --- a/templateFiles/Simulink/architecture/myPreloadScript.m +++ b/templateFiles/Simulink/architecture/myPreloadScript.m @@ -1,17 +1,17 @@ -%% Simulink Architecture Template: Sample Preload Script -% Performs Simulink model setup and is called in model's pre-load function -% Copyright 2017 The MathWorks, Inc. - -%% ROS setup -rosshutdown; -ipAddr = ''; -rosinit(ipAddr) - -%% Sample times -sampleTimeSlow = 1; -sampleTimeFast = 0.2; - -%% Control parameters -perceptionParam = 0.25; -ctrlParam1 = 2; +%% Simulink Architecture Template: Sample Preload Script +% Performs Simulink model setup and is called in model's pre-load function +% Copyright 2017 The MathWorks, Inc. + +%% ROS setup +rosshutdown; +ipAddr = ''; +rosinit(ipAddr) + +%% Sample times +sampleTimeSlow = 1; +sampleTimeFast = 0.2; + +%% Control parameters +perceptionParam = 0.25; +ctrlParam1 = 2; ctrlParam2 = 3.5; \ No newline at end of file diff --git a/templateFiles/startupTemplates.m b/templateFiles/startupTemplates.m index 1ce6deb..74ba87e 100644 --- a/templateFiles/startupTemplates.m +++ b/templateFiles/startupTemplates.m @@ -1,12 +1,12 @@ -% Copyright 2017 The MathWorks, Inc. - -% Clean up -close all; -clear; -clc; - -% Set up the path -addpath(pwd,genpath('MATLAB'),genpath('Simulink')); - -% Set up simulation cache and code generation folders +% Copyright 2017 The MathWorks, Inc. + +% Clean up +close all; +clear; +clc; + +% Set up the path +addpath(pwd,genpath('MATLAB'),genpath('Simulink')); + +% Set up simulation cache and code generation folders Simulink.fileGenControl('set','CacheFolder','work','CodeGenFolder','work'); \ No newline at end of file