%% Fixed-rate Publishing of ROS Image Data
% This example shows how to regularly publish and receive image messages using ROS and the rosrate function. The rosrate function creates a Rate object to regularly access the /camera/rgb/image_raw topic on the ROS network using a subscriber. The rgb image is converted to a grayscale using rgb2gray and republished at a regular interval. Parameters such as the IP address and topic names vary with your robot and setup.
% Connect to ROS network. Setup subscriber, publisher, and data message.

ipaddress = '192.168.203.129'; % Replace with your network address
rosinit(ipaddress)
sub = rossubscriber('/camera/rgb/image_raw');
pub = rospublisher('/camera/gray/image_gray','sensor_msgs/Image');
msgGray = rosmessage('sensor_msgs/Image');
msgGray.Encoding = 'mono8';

%
% Receive the first image message. Extract image and convert to a grayscale image. Display grayscale image and publish the message.

msgImg = receive(sub);

img = readImage(msgImg);
grayImg = rgb2gray(img);
imshow(grayImg)

writeImage(msgGray,grayImg)
send(pub,msgGray)

%
% Create ROS Rate object to execute at 10 Hz. Set a loop time and the OverrunAction for handling

desiredRate = 10;
loopTime = 5;
overrunAction = 'slip';
rate = rosrate(desiredRate);
r.OverrunAction = overrunAction;

%
% Begin loop to receive, process and send messages every 0.1 seconds (10 Hz). Reset the Rate object before beginning.

reset(rate)

for i = 1:desiredRate*loopTime
    
    msgImg = receive(sub);

    img = readImage(msgImg);
    grayImg = rgb2gray(img);
    writeImage(msgGray,grayImg)

    send(pub,msgGray)
    
    waitfor(rate);
end

%
% View the statistics for the code execution. AveragePeriod and StandardDeviation show how well the code maintained the desiredRate. OverRuns occur when data processing takes longer than the desired period length.

statistics(rate)

%
% Shut down ROS node

rosshutdown

%
% Copyright 2019 The MathWorks, Inc.