%% Work with Basic ROS 2 Messages
% ROS messages are the primary container for exchanging data in ROS 2. Publishers and subscribers exchange data using messages on specified topics to carry data between nodes. For more information on sending and receiving messages, see Exchange Data with ROS 2 Publishers and Subscribers.
% To identify its data structure, each message has a message type. For example, sensor data from a laser scanner is typically sent in a message of type sensor_msgs/LaserScan. Each message type identifies the data elements that are contained in a message. Every message type name is a combination of a package name, followed by a forward slash /, and a type name:
%
% MATLAB® supports many ROS 2 message types that are commonly encountered in robotics applications. This example examines some of the ways to create, inspect, and populate ROS 2 messages in MATLAB.
% Prerequisites: Get Started with ROS 2, Connect to a ROS 2 Network
%
%% Find Message Types
% Use exampleHelperROS2CreateSampleNetwork to populate the ROS 2 network with three nodes and setup sample publishers and subscribers on specific topics.

exampleHelperROS2CreateSampleNetwork

% Use ros2 topic list -t to find the available topics and their associated message type.

ros2 topic list -t

% To find out more about the topic message type, use ros2message to create an empty message of the same type. ros2message supports tab completion for the message type. To quickly complete message type names, type the first few characters of the name you want to complete, and then press the Tab key.

scanData = ros2message("sensor_msgs/LaserScan")

% The created message, scanData, has many fields associated with data that you typically received from a laser scanner. For example, the minimum sensing distance is stored in the range_min property and the maximum sensing distance in range_max property.
% You can now delete the created message.

clear scanData

% To see a complete list of all message types available for topics and services, use ros2 msg list.
%
%% Explore Message Structure and Get Message Data
% ROS 2 messages are represented as structures and the message data is stored in fields. MATLAB provides convenient ways to find and explore the contents of messages.
% Use ros2 msg show to view the definition of the message type.

ros2 msg show geometry_msgs/Twist

% If you subscribe to the /pose topic, you can receive and examine the messages that are sent.

controlNode = ros2node("/base_station");
poseSub = ros2subscriber(controlNode,"/pose","geometry_msgs/Twist")

% Use receive to acquire data from the subscriber. Once a new message is received, the function returns it and stores it in the posedata variable. Specify a timeout of 10 seconds for receiving messages.

poseData = receive(poseSub,10)

% The message has a type of geometry_msgs/Twist. There are two other fields in the message: linear and angular. You can see the values of these message fields by accessing them directly.

poseData.linear
poseData.angular

% You can see that each of the values of these message fields is actually a message in itself. geometry_msgs/Twist is a composite message made up of two geometry_msgs/Vector3 messages.
% Data access for these nested messages works exactly the same as accessing the data in other messages. Access the x component of the linear message using this command:

xPose = poseData.linear.x

%
%% Set Message Data
% You can also set message property values. Create a message with type geometry_msgs/Twist.

twist = ros2message("geometry_msgs/Twist")

% The numeric properties of this message are initialized to 0 by default. You can modify any of the properties of this message. Set the linear.y field to 5.

twist.linear.y = 5;

% You can view the message data to make sure that your change took effect.

twist.linear

% Once a message is populated with your data, you can use it with publishers and subscribers.
%
%% Copy Messages
% ROS 2 messages are structures. They can be copied directly to make a new message. The copy and the original messages each have their own data.
% Make a new empty message to convey temperature data, then make a copy for modification.

tempMsgBlank = ros2message("sensor_msgs/Temperature");
tempMsgCopy = tempMsgBlank

% Modify the temperature property of tempMsg and notice that the contents of tempMsgBlank remain unchanged.

tempMsgCopy.temperature = 100
tempMsgBlank

% It may be useful to keep a blank message structure around, and only set the specific fields when there is data for it before sending the message.

thermometerNode = ros2node("/thermometer");
tempPub = ros2publisher(thermometerNode,"/temperature","sensor_msgs/Temperature");
tempMsgs(10) = tempMsgBlank;    % Pre-allocate message structure array
for iMeasure = 1:10
    % Copy blank message fields
    tempMsgs(iMeasure) = tempMsgBlank;
    
    % Record sample temperature
    tempMsgs(iMeasure).temperature = 20+randn*3;
    
    % Only calculate the variation once sufficient data observed
    if iMeasure >= 5
        tempMsgs(iMeasure).variance = var([tempMsgs(1:iMeasure).temperature]);
    end
    
    % Pass the data to subscribers
    send(tempPub,tempMsgs(iMeasure))
end
errorbar([tempMsgs.temperature],[tempMsgs.variance])

%
%% Save and Load Messages
% You can save messages and store the contents for later use.
% Get a new message from the subscriber.

poseData = receive(poseSub,10)

% Save the pose data to a MAT file using the save function.

save("poseFile.mat","poseData")

% Before loading the file back into the workspace, clear the poseData variable.

clear poseData

% Now you can load the message data by calling the load function. This loads the poseData from above into the messageData structure. poseData is a data field of the struct.

messageData = load("poseFile.mat")

% Examine messageData.poseData to see the message contents.

messageData.poseData

% You can now delete the MAT file.

delete("poseFile.mat")

%% Disconnect From ROS 2 Network
% Remove the sample nodes, publishers, and subscribers from the ROS 2 network.

exampleHelperROS2ShutDownSampleNetwork

%
%% Next Steps
% Exchange Data with ROS 2 Publishers and Subscribers
% ROS 2 Custom Message Support
% Copyright 2019 The MathWorks, Inc.
%
%