Populating ros messages is slow

I noticed that populating ROS messages with values is quite slow. Filling 16 fields of a message takes in average around 2.2 ms. While this is not that much on its own, it quickly adds up in a control loop. The message in question is a control command which has to be sent at around 10 Hz to at least 10 different robots. That means at least a fifth of the control interval is spent on "creating" the messages. Together with the issue that rostime("now") is slow too (https://www.mathworks.com/matlabcentral/answers/658878-rostime-call-is-slow?s_tid=srchtitle), this leaves almost no time for the actual computation of the control commands.

I wonder if I am doing something fundamentally wrong or if the Matlab ROS API is simply that slow?

Script to time population of ROS message:

cmdFullStateMsg = rosmessage("crazyflie_driver/FullState");

pos = ones(3, 1);
vel = ones(3, 1);
acc = ones(3, 1);
yaw = 1;
omega = ones(3, 1);
quat = eul2quat([0, 0, yaw], 'xyz');

tic
for i = 1:1000  
    cmdFullStateMsg.Pose.Position.X    = pos(1);
    cmdFullStateMsg.Pose.Position.Y    = pos(2);
    cmdFullStateMsg.Pose.Position.Z    = pos(3);
    cmdFullStateMsg.Twist.Linear.X     = vel(1);
    cmdFullStateMsg.Twist.Linear.Y     = vel(2);
    cmdFullStateMsg.Twist.Linear.Z     = vel(3);
    cmdFullStateMsg.Acc.X              = acc(1);
    cmdFullStateMsg.Acc.Y              = acc(2);
    cmdFullStateMsg.Acc.Z              = acc(3);
    cmdFullStateMsg.Pose.Orientation.W = quat(1);
    cmdFullStateMsg.Pose.Orientation.X = quat(2);
    cmdFullStateMsg.Pose.Orientation.Y = quat(3);
    cmdFullStateMsg.Pose.Orientation.Z = quat(4);
    cmdFullStateMsg.Twist.Angular.X    = omega(1);
    cmdFullStateMsg.Twist.Angular.Y    = omega(2);
    cmdFullStateMsg.Twist.Angular.Z    = omega(3);   
end
cmd_full_state_msg_time = toc / 1000

Thanks for any help!

Answer 1

[Cam Salzberger]

Hello Mauro,

Cause of Performance Issues

Accessing Nested Messages (pre-R2020b)

Based on your other question, I assume you are using R2019a. All releases up through R2020a use rosjava under the hood, and the messages contain Java message objects. This means that as you use fields of deeply-nested messages, for either setting or getting data, MATLAB has to do several conversions of message objects.

For example, when you call :

cmdFullStateMsg.Pose.Position.X = pos(1)

then MATLAB:

• Accesses the geometry_msgs/Pose Java message object from the "Pose" property
• Converts that to the MATLAB robotics.ros.msggen.geometry_msgs.Pose object
• Accesses geometry_msgs/Point Java message object from the "Position" property
• Converts that to the MATLAB robotics.ros.msggen.geometry_msgs.Point object
• Accesses the "X" property of the underlying geometry_msgs/Point Java message and assigns it to the provided value
• Assigns the appropriate properties of the various objects all the way up the nested tree

Multiple Calls to rosmessage

Additionally, any call to rosmessage requires the construction of the object and its underlying Java message. Limiting the number of calls is advantageous when trying to improve performance.

Solutions

Disclaimer

The performance improvement is just what I'm seeing through casual measurement (tic-toc) on my machine in particular. You may not see the same improvement depending on your machine, resources, environment, code structure or other factors.

Extract Nested Messages Before Setting Values

When setting multiple fields in a single message that is deeply nested, it can help a lot to pull out the message manually and set the fields. Since mesage objects in MATLAB are handles, you don't need to reassign it to the nested field either.

Testing with nav_msgs/Odometry (which is similar, if not quite the same as your message), I compared running this:

m = rosmessage("nav_msgs/Odometry");
m.Pose.Pose.Position.X = 1;
m.Pose.Pose.Position.Y = 2;
m.Pose.Pose.Position.Z = 3;
m.Twist.Twist.Linear.X = 4;
m.Twist.Twist.Linear.Y = 5;
m.Twist.Twist.Linear.Z = 6;
m.Header.Stamp.Sec = 7;
m.Header.Stamp.Nsec = 8;
m.Header.FrameId = '';
m.Pose.Pose.Orientation.W = 1;
m.Pose.Pose.Orientation.X = 2;
m.Pose.Pose.Orientation.Y = 3;
m.Pose.Pose.Orientation.Z = 4;
m.Twist.Twist.Angular.X = 5;
m.Twist.Twist.Angular.Y = 6;
m.Twist.Twist.Angular.Z = 7;

to this:

m = rosmessage("nav_msgs/Odometry");
pose = m.Pose.Pose;
twist = m.Twist.Twist;
posePos = pose.Position;
poseOri = pose.Orientation;
twistLin = twist.Linear;
twistAng = twist.Angular;
head = m.Header;
posePos.X = 1;
posePos.Y = 2;
posePos.Z = 3;
twistLin.X = 4;
twistLin.Y = 5;
twistLin.Z = 6;
head.Stamp.Sec = 7;
head.Stamp.Nsec = 8;
head.FrameId = '';
poseOri.W = 1;
poseOri.X = 2;
poseOri.Y = 3;
poseOri.Z = 4;
twistAng.X = 5;
twistAng.Y = 6;
twistAng.Z = 7;

in a loop of 1000 iterations, and got about a 2X improvement with the second code. It's not ground-breaking, but it's significant enough to be worth doing.

Reuse Message Objects

A large part of this time is spent running rosmessage. If you only are using this message for immediate sending (and aren't trying to store data for later or something), and you are always overwriting the same values, you can just reuse the same message object again and again. Call rosmessage outside of the control loop and make it available to the inside in some way. Set the values on the inside and send the message. The values will remain the same when the next iteration comes up, so they'll need to all be overwritten again, but if you're doing that anyway it's not an additional cost. Testing with this method, showed an 5X improvement over the original code.

If you're going for every bit of performance possible, you can even extract all of the inner fields you're setting initially (after calling rosmessage), then pass those variables to the control loop to set. This eeked into the 6X improvement range over the original code, but it was a pretty minimal difference.

Use R2020b+

Additionally, if you combine these tips with updating to R2020b (if that is possible for you), I'm seeing around a 3X-5X improvement in control loop runtime between the two versions running the same code. This is because R2020b no longer uses a Java back-end, and so message fields are being retrieved and set directly in the MATLAB objects. Definitely create the rosmessage outside of the control still, as that nets the biggest improvement in performance.

-Cam

[Mauro]

Hi Cam

Thank you very much for the quick and exhaustive answer! Did you see any further improvement in R2020b when extracting nested messages before setting the values or is that problem completely gone?

If not I will simply move the code to R2020b and make sure all messages are initialized on startup. This should not be an issue since I use a class to represent the robots. So I can simply define the messages as properties.

Thanks again for the help!

[Cam Salzberger]

To test just the improvement of extracting nested fields, I ran this:

m = rosmessage("nav_msgs/Odometry");
tic
for k = 1:1000
m.Pose.Pose.Position.X = 1;
m.Pose.Pose.Position.Y = 2;
m.Pose.Pose.Position.Z = 3;
m.Twist.Twist.Linear.X = 4;
m.Twist.Twist.Linear.Y = 5;
m.Twist.Twist.Linear.Z = 6;
m.Header.Stamp.Sec = 7;
m.Header.Stamp.Nsec = 8;
m.Header.FrameId = '';
m.Pose.Pose.Orientation.W = 1;
m.Pose.Pose.Orientation.X = 2;
m.Pose.Pose.Orientation.Y = 3;
m.Pose.Pose.Orientation.Z = 4;
m.Twist.Twist.Angular.X = 5;
m.Twist.Twist.Angular.Y = 6;
m.Twist.Twist.Angular.Z = 7;
end
toc

against this:

m = rosmessage("nav_msgs/Odometry");
pose = m.Pose.Pose;
twist = m.Twist.Twist;
posePos = pose.Position;
poseOri = pose.Orientation;
twistLin = twist.Linear;
twistAng = twist.Angular;
head = m.Header;
tic
for k = 1:1000
posePos.X = 1;
posePos.Y = 2;
posePos.Z = 3;
twistLin.X = 4;
twistLin.Y = 5;
twistLin.Z = 6;
head.Stamp.Sec = 7;
head.Stamp.Nsec = 8;
head.FrameId = '';
poseOri.W = 1;
poseOri.X = 2;
poseOri.Y = 3;
poseOri.Z = 4;
twistAng.X = 5;
twistAng.Y = 6;
twistAng.Z = 7;
end
toc

in both R2019a and R2020b.

In R2019a, the latter was a little over 3X faster than the former. In R2020b, the latter was only around 1.5X faster than the former. So there is still some improvement to be had by extracting and setting the nested message (when you are setting multiple fields in the nested message), but it is not as dramatic.

Also, please check the release notes for ROS Toolbox, as there are some big changes between R2020a and R2020b. The biggest is the update to ROS Melodic for message definitions and back-end, and the requirement of using Python 2.7. It's still a good idea to update, but I want to let you know about any potential incompatibilities or roadblocks ahead of time.

-Cam

[Mauro]

Thanks a lot for the clarification. I updated to R2020b and it seems like it might just be fast enough for what we need to use it for. In any case it would be nice if in future versions of Matlab this could be sped up as it is currently somewhat of a bottleneck.

Unfortunately I encountered another problem after updating to R2020b. After following the instructions of rosgenmsg to generate the custom ROS messages and services I am not able to call any custom services. Messages on the other hand work just fine.

I already submitted a bug report, but maybe you could confirm the problem in the meantime?

Thanks,

Mauro

[Enrica Soria]

Hello Cam!

Same problem here. I can't call custom services on Matlab2020b, while I could with the previous releases. Is there a workaround, or did the way of calling custom services changed?

This is a blocking issue.

Best regards,

Enrica

[Cam Salzberger]

Yes, I am afraid that custom services not working in R2020b is a known issue. We are working on getting a fix out in an update as soon as possible. I apologize for the inconvenience.

-Cam

[Enrica Soria]

Thank you very much, Cam, for your answer!

Do you approximately know how much time it could take to get a fix?

In the meantime, would you recommend to install the Add-on ROS Toolbox interface for ROS Custom Messages? Would the built-in ROS Toolbox be faster?

Best regards,

Enrica

[Cam Salzberger]

Sorry, but I cannot promise dates for any upcoming fixes or releases. But keep an eye on the update release notes for ROS Toolbox.

The support package for custom messages applies only to R2020a and prior releases. It does not work with R2020b.

[Enrica Soria]

Hello Cam,

Ok, thank you.

Then, probably the Matlab Release Compatibility of the "ROS Toolbox interface for ROS Custom Messages" needs to exclude R2020b (see picture below). However, it makes sense since from R2020b, the ROS custom message/service generation is directly supported from the ROS Toolbox.

Best,

Enrica

[Cam Salzberger]

Thanks, Enrica, that has been fixed.

Also, R2020b Update 3 is out now, and includes the fix for custom services.