Main Content

Publish Variable-Length Nested ROS Messages in MATLAB

This example shows how to work with complex ROS messages in MATLAB, such as messages with nested submessages and variable-length arrays.

Some ROS message types have nested submessages that are of different message types. Such nested ROS messages can be arrays whose length (number of elements) cannot be predetermined. Typical examples of such message types include:

  • geometry_msgs/PoseArray : This message type contains an array of poses of type geometry_msgs/Pose. It is typically used to send a bunch of waypoints to the robot in a specific time step.

  • nav_msgs/Path : This message type contains an array of poses of type geometry_msgs/PoseStamped. It is typically used for the output of motion planners that send a path for the robot to follow. The path is represented as a sequence of poses, each with its own header and timestamp.

In this example, you send pose arrays of different lengths over a single topic that publishes messages of type geometry_msgs/PoseArray.

Load and View Waypoints

Load the source data, which contains waypoints of different lengths that need to be published on a single topic, for the robot to follow. The MAT file wayPointSets.mat loads two sets of waypoints. These can be used to specify the pose array message. The waypoints are in the form of XYZ coordinates.

load wayPointSets.mat;

Visualize the two sets of waypoints using the plot3 function. Note that the two sets contain different numbers of waypoints.

figure
plot3(wayPointSet1(:,1),wayPointSet1(:,2),wayPointSet1(:,3),'*-')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 1')

Figure contains an axes. The axes with title Waypoint Set 1 contains an object of type line.

figure
plot3(wayPointSet2(:,1),wayPointSet2(:,2),wayPointSet2(:,3),'*-r')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 2')

Figure contains an axes. The axes with title Waypoint Set 2 contains an object of type line.

Initialize and Configure ROS Network

Use rosinit to create a ROS master in MATLAB and start a global node that is connected to the master.

rosinit
Launching ROS Core...
Done in 1.0273 seconds.
Initializing ROS master on http://192.168.0.10:51908.
Initializing global node /matlab_global_node_71848 with NodeURI http://bat1002909glnxa64:34241/

Use rospublisher to create a ROS publisher for sending messages of type geometry_msgs/PoseArray. Specify the name of the topic as /waypoints. Add a ROS subscriber that subscribes to the published topic using rossubscriber.

pub = rospublisher('/waypoints','geometry_msgs/PoseArray');
sub = rossubscriber('/waypoints');

Use rosmessage to create an empty message based on the topic published by the publisher, pub.

poseArrayMsg = rosmessage(pub);

Populate Message and Publish

Specify the workspace variable corresponding to the waypoint set that you want to publish. Then, populate the pose array with geometry_msgs/Pose messages. Assign the XYZ position fields of the individual pose message elements from the waypoint set data. Continue adding new individual pose message elements until the the pose array message contains all of the waypoint set data.

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet1;

% Populate the pose array message
for i = 1:size(wayPointsToPublish,1)
    poseMsg = rosmessage('geometry_msgs/Pose');
    poseMsg.Position.X = wayPointsToPublish(i,1);
    poseMsg.Position.Y = wayPointsToPublish(i,2);
    poseMsg.Position.Z = wayPointsToPublish(i,3);
    poseArrayMsg.Poses(i) = poseMsg;  
end

Use the send function to publish the pose array message to the topic /waypoints, using the ROS publisher object, pub.

send(pub,poseArrayMsg);
pause(0.5)

View the pose array message data, as received by the subscriber, using the LatestMessage property of the Subscriber object. Use horzcat to concatenate the position information extracted from the received message into a structure array for the purposes of visualization. Use plot3 to visualize the waypoints as received by the subscriber. Note that the visualization matches that of the corresponding source waypoint data set.

receivedPoseArrayMsg1 = sub.LatestMessage;
waypointPositions1 = horzcat(receivedPoseArrayMsg1.Poses.Position);

figure
plot3([waypointPositions1.X],[waypointPositions1.Y],[waypointPositions1.Z],'*-')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 1 Received Through ROS Topic')

Figure contains an axes. The axes with title Waypoint Set 1 Received Through ROS Topic contains an object of type line.

Now publish the second waypoint using the same procedure. Populate the pose array message with the new set of waypoint information.

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet2;

% Populate the Pose Array Message
for i = 1:size(wayPointsToPublish,1)
    poseMsg = rosmessage('geometry_msgs/Pose');
    poseMsg.Position.X = wayPointsToPublish(i,1);
    poseMsg.Position.Y = wayPointsToPublish(i,2);
    poseMsg.Position.Z = wayPointsToPublish(i,3);
    poseArrayMsg.Poses(i) = poseMsg;  
end

Use the send function to publish the new pose array message to the same topic via the same ROS publisher object, pub.

send(pub,poseArrayMsg);
pause(0.5)

Visualize the pose array message data received by the subscriber by following the same procedure as before.

receivedPoseArrayMsg2 = sub.LatestMessage;
waypointPositions2 = vertcat(receivedPoseArrayMsg2.Poses.Position);

figure
plot3([waypointPositions2.X],[waypointPositions2.Y],[waypointPositions2.Z],'*-r')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 2 Received Through ROS Topic')

Figure contains an axes. The axes with title Waypoint Set 2 Received Through ROS Topic contains an object of type line.

The visualization matches that of the corresponding source waypoint data set, indicating the successful broadcast of two sets of pose arrays with different lengths over a single topic. Use rosshutdown to shut down the ROS network in MATLAB. Doing so, shuts down the ROS master initialized by rosinit and deletes the global node. Using rosshutdown is the recommended procedure once you are done working with the ROS network.

rosshutdown
Shutting down global node /matlab_global_node_71848 with NodeURI http://bat1002909glnxa64:34241/
Shutting down ROS master on http://192.168.0.10:51908.

If the waypoint set data has orientation information, you can populate it in the quaternion orientation fields of the individual pose message elements before publishing. To publish messages of type nav_msgs/Path, use the same procedure, but specify the individual pose message elements as geometry_msgs/PoseStamped type. To publish messages of any other type, specify the appropriate nested message type as individual array elements, and ensure that the source data set contains the required information you want to publish.

See Also

Work with ROS Messages in Simulink

ROS Custom Message Support