%% Manage Quality of Service Policies in ROS 2
% Quality of Service (QoS) policy options allow for changing the behavior of communication within a ROS 2 network. QoS policies are modified for specific communication objects, such as publishers and subscribers, and change the way that messages are handled in the object and transported between them. For any messages to pass between two communication objects, their QoS policies must be compatible.
% The available Quality of Service policies in ROS 2 are:
% History - Message queue mode
% Depth - Message queue size
% Reliability - Delivery guarantee of messages
% Durability - Persistence of messages
% For more information, see About Quality of Service Settings.
%
%% History and Depth
% The history and depth QoS policies determine the behavior of communication objects when messages are being made available faster than they can be processed. This is primarily a concern for subscribers that are receiving messages while still processing the previous message. Messages are placed into a processing queue, which can affect publishers as well.  History has the options of:
% "keeplast" - The message processing queue has a maximum size equal to the Depth value. If the queue is full, the oldest messages are dropped to make room for newer ones.
% "keepall" - The message processing queue attempts to keep all messages received in the queue until processed.
% Under either history setting, the queue size is subject to hardware resource limits. If the subscriber calls a calllback when new messages are received, the queue size is also limited by the maximum recursion limit.
% In situations where it is important to process all messages, increating the Depth value or using History,"keepall" is recommended.
% This example shows how to setup a publisher and subcriber for sending and receiving point cloud messages. The publisher Depth is 20 and the subsriber history is set to "keepall". The subscriber uses a call back to plot the time stamp for each message to show the timing of processing each message. The intial messages take longer to process, but all the messages are eventually processed from the queue.

% Create a publisher to provide sensor data
robotNode = ros2node("/simple_robot");
lidarPub = ros2publisher(robotNode,"/laser_scan","sensor_msgs/PointCloud2",...
    "History","keeplast","Depth",20);

% Create a subscriber representing localization, requiring all scan data
hFig = figure;
hAxesLidar = axes("Parent",hFig);
title("Message Timeline (Keep All)")
localizationSub = ros2subscriber(robotNode,"/laser_scan",...
    @(msg)exampleHelperROS2PlotTimestamps(msg,hAxesLidar),...
    "History","keepall");

% Send messages, simulating an extremely fast sensor
load robotPoseLidarData.mat lidarScans
for iMsg = 1:numel(lidarScans)
    send(lidarPub,lidarScans(iMsg))
end

% Allow messages to arrive, then remove the localization subscriber
pause(3)
clear localizationSub

% In situations where messages being dropped is less important, and only the most up-to-date information really matters, a smaller queue is recommended to improve performance and ensure the most recent information is being used. This example shows quicker processing of the first messages and still gets all the messages. Depending on your resources however, you may see messages get dropped.

% Create a subscriber representing user interface display
hFig = figure;
hAxesLidar2 = axes("Parent",hFig);
title("Message Timeline (Keep Last 1)")
scanDisplaySub = ros2subscriber(robotNode,"/laser_scan",...
    @(msg)exampleHelperROS2PlotTimestamps(msg,hAxesLidar2),...
    "History","keeplast","Depth",1);
for iMsg = 1:numel(lidarScans)
    send(lidarPub,lidarScans(iMsg))
end

% Allow messages to arrive, then remove the subscriber and publisher
pause(3)
clear lidarPub scanDisplaySub

%
%% Reliability
% The reliability QoS policy determines whether to guarantee delivery of messages, and has the options:
% "reliable" - The publisher continuously sends the message to the subscriber until the subscriber confirms receipt of the message.
% "besteffort" - The publisher sends the message only once, and does not confirm that the subscriber receives it.
%% Reliable
% A "reliable" connection is useful when all of the data must be processed, and any dropped messages may impact the result. This example publishes Odometry messages and uses a subscriber callback to plot the position. Because for the "reliable" setting, all the positions are plotted in the figure.

% Create a publisher for odometry data
odomPub = ros2publisher(robotNode,"/odom","nav_msgs/Odometry",...
    "Reliability","reliable");

% Create a subscriber for localization
hFig = figure;
hAxesReliable = axes("Parent",hFig);
title("Robot Position (Reliable Connection)")
xlabel("X (m)")
ylabel("Y (m)")
odomPlotSub = ros2subscriber(robotNode,"/odom",...
    @(msg)exampleHelperROS2PlotOdom(msg,hAxesReliable,"ok"),...
    "Reliability","reliable");

% Send messages, simulating an extremely fast sensor
load robotPoseLidarData.mat odomData
for iMsg = 1:numel(odomData)
    send(odomPub,odomData(iMsg))
end

pause(3)    % Allow messages to arrive and be plotted

% Temporarily prevent reliable subscriber from reacting to new messages
odomPlotSub.NewMessageFcn = [];

%% Best Effort
% A "besteffort" connection is useful to avoid impacting performance if dropped messages are acceptable. If a publisher is set to "reliable", and a subscriber is set to "besteffort", the publisher treats that connection as only requiring "besteffort", and does not confirm delivery. Connections with "reliable" subscribers on the same topic are guaranteed delivery from the same publisher.
% This example uses a "besteffort" subscriber, but still receives all messages due to the low impact on the network.

hFig = figure;
hAxesBestEffort = axes("Parent",hFig);
title("Message Timeline (Best Effort Connection)")
odomTimingSub = ros2subscriber(robotNode,"/odom",...
    @(msg)exampleHelperROS2PlotTimestamps(msg,hAxesBestEffort),...
    "Reliability","besteffort");
for iMsg = 1:numel(odomData)
    send(odomPub,odomData(iMsg))
end

pause(3)    % Allow messages to arrive and be plotted

%
%% Compatibility
% Ensuring compatibility is an important consideration when setting reliability. A subscriber with a "reliable" option set requires a publisher that meets that standard. Any "besteffort" publishers do not connect to a "reliable" subscriber because messages are not guaranteed to be delivered. In the opposite situatiuon, a "reliable" publisher and a "besteffort" subscriber do connect, but the connection behaves as "besteffort" with no confirmation when receiving messages. This example shows a "besteffort" publisher sending messages to the "besteffort" subscriber already set up. Again, due to the low impact on the network, the "besteffort" connection is sufficient to process all the messages.

% Reactivate reliable subscriber to show no messages received
odomPlotSub.NewMessageFcn = @(msg)exampleHelperROS2PlotOdom(msg,hAxesReliable,"*r");

% Send messages from a best-effort publisher
bestEffortOdomPub = ros2publisher(robotNode,"/odom","nav_msgs/Odometry",...
    "Reliability","besteffort");
for iMsg = 1:numel(odomData)
    send(bestEffortOdomPub,odomData(iMsg))
end

% Allow messages to arrive, then remove odometry publishers and subscribers
pause(3)    % Allow messages to arrive and be plotted
clear odomPub bestEffortOdomPub odomPlotSub odomTimingSub

%
%% Durability and Depth
% The durability QoS policy controls the persistence of messages for late-joining connections, and has the options:
% "transientlocal" - For a publisher, messages that have already been sent are maintained. If a subscriber joins the network with "transientlocal" durability after that, then the publisher sends the persisted messages to the subscriber.
% "volatile" - Publishers do not persist messages after sending them, and subscribers do not request persisted messages from publishers.
% The number of messages persisted by publishers with "transientlocal" durability is also controlled by the Depth input. Subscribers only request the number of recent messages based on their individual Depth settings. Publishers can still store more messages for other subscribers to get more. For example, a full list of the robot position may be useful for visualizing its path, but a localization algorithm may only be interested in the last known location. This example illustrates that by using a localization subscriber to display the current position and a plotting subscriber to show all opositions in the queue.

% Publish robot location information
posePub = ros2publisher(robotNode,"/bot_position","geometry_msgs/Pose2D",...
    "Durability","transientlocal","Depth",100);
load robotPoseLidarData.mat robotPositions
for iMsg = 1:numel(robotPositions)
    send(posePub,robotPositions(iMsg))
    pause(0.2)     % Allow for processing time
end

% Create a localization update subscriber that only needs current position
localUpdateSub = ros2subscriber(robotNode,"/bot_position",@disp,...
    "Durability","transientlocal","Depth",1);
pause(1)    % Allow message to arrive

%         x: 0.1047
%         y: -2.3168
%     theta: -8.5194
%

% Create a visualization subscriber to show where the robot has been
hFig = figure;
hAxesMoreMsgs = axes("Parent",hFig);
title("Robot Position (Transient Local Connection)")
xlabel("X (m)")
ylabel("Y (m)")
hold on
posePlotSub = ros2subscriber(robotNode,"/bot_position",...
    @(msg)plot(hAxesMoreMsgs,msg.x,msg.y,"ok"),...
    "Durability","transientlocal","Depth",20);
pause(3)    % Allow messages to arrive and be plotted

%% Compatibility
% Similar to reliability, incompatible durability settings can prevent communication between publishers and subscribers. A subscriber with "transientlocal" durability requires a publisher with "transientlocal" durability. If a publisher is "volatile", no connection is established with "transientlocal" subscribers. If a publisher is "transientlocal" and the subscriber "volatile", then that connection is created, without sending persisting messages to the subscriber.

% Reset plotting behavior
posePlotSub.NewMessageFcn = @(msg)plot(hAxesMoreMsgs,msg.x,msg.y,"xr");

% Send messages from volatile publisher
volatilePosePub = ros2publisher(robotNode,"/bot_position",...
    "Durability","volatile");
for iMsg = 1:numel(robotPositions)
    send(volatilePosePub,robotPositions(iMsg))
    pause(0.2)     % Allow for processing time
end

% No messages are received by either "transientlocal" subscriber.

% Remove pose publishers and subscribers
clear posePub volatilePosePub localUpdateSub posePlotSub robotNode

% Copyright 2019 The MathWorks, Inc.
%