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');ticfor i = 1:1000cmdFullStateMsg.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);endcmd_full_state_msg_time = toc / 1000
Thanks for any help!