MATLAB Answers

Simulink/ROS: Publishing JointTrajectory Messages

26 views (last 30 days)
Hi,
I currently have a robot arm model simulated in Gazebo that uses ROS for controls. I have verified that the communication pipeline works using a small ROS publisher I wrote in Python. Now I want to write a Simulink interface to control the robot.
The robot uses trajectory_msgs/JointTrajectory type messages to control how the robotic arm moves. I have been following this tutorial to try to make a publisher.
I have been following the nested message tutorial (Task 3), since the JointTrajectory type message is fairly complex and consists of multiple arrays. I created a Matlab function block in Simulink to properly format and prepare my message.
Unfortunately I ran into two problems.
1. I am having issues publishing string arrays, specifically for the trajectory_msgs/JointTrajectory/joint_names field. I have tried various methods such as:
msg.JointNames(1) = 'joint_base_1'
or
coder.extrinsic('rosString');
msg.JointNames(1) = rosString('joint_base_1');
where rosString is a short function that does the following:
function msg = rosString(string)
msg = rosmessage('std_msgs/String');
msg.Data = string;
or even
n1 = transpose(uint8('joint_base_1'));
n1 = vertcat(n1, zeros(128-size(n1,1),1)); % Pad the rest of the uint8 array with 0s
msg.JointNames(1).Data= n1;
Does anyone know a way to properly transmit string arrays?
2. After ignoring the joint_names field, I tried just publishing the goal position and velocities. My code for creating the message is as follows:
msg.Points(1).Positions(1) = p1;
msg.Points(1).Velocities(1) = v1;
...
After creating the message, I also made a Matlab Function Block Parser that seems to correctly retrieve the values. However, after publishing the message, I echo'ed the topic I published the message to and saw the following message being repeated:
rostopic echo /my_robot/joint_trajectory_controller/commands
---
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
points: []
---
...
It seems like my message is not correctly parsed and I am having trouble figuring out the underlying cause.
Thank you for anyone who can provide any hints in debugging.

Accepted Answer

Sebastian Castro
Sebastian Castro on 15 Jun 2017
Edited: Sebastian Castro on 15 Jun 2017
I can only guess what's going on...
Are you planning on sticking to desktop simulations from MATLAB or will you be generating code from the model? It's fairlytough to work with string messages with the ROS-Simulink interface, so if you're sticking to desktop simulations I'd suggest writing a MATLAB Function and enveloping all the ROS interface code as interpreted.
For example, here is a 3-joint, 2-point example:
function sendMsg
coder.extrinsic('sendTrajMsgs'); % Will run the sendTrajMsgs function interpreted
sendTrajMsgs([1 2 3],[4 5 6]);
end
The following implementation of sendTrajMsgs.m worked fine for me:
function sendTrajMsgs( pos1, pos2 )
% Create the publisher only on the first function call
persistent pub
if isempty(pub)
pub = rospublisher('/my_joints','trajectory_msgs/JointTrajectory');
end
% Create an empty message with the joint names
msg = rosmessage('trajectory_msgs/JointTrajectory');
msg.JointNames = {'joint1','joint2','joint3'};
% Fill in the message positions
p1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
p1.Positions = pos1;
msg.Points(1) = p1;
p2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
p2.Positions = pos2;
msg.Points(2) = p2;
% Send messages
send(pub,msg)
end
If you have to use the Simulink blocks, you'll notice that you have to treat the strings as you would in C code. The JointNames field of the bus will actually have an array of string buses, each of which has a 128-element uint8 (or char ) array and a supplementary "information" bus that tells you the length of that string -- in other words, how far along the 128 char vector you have to read before it's just empty space.
Going this route, I would follow the tutorial you linked to. What would help is grabbing an "Empty Message" block from the ROS library, filling in the trajectory_msgs/JointTrajectory message type, and then going to Edit > Bus Editor in Simulink. You'll be able to view the structure of all the bus messages that were created for this message.
This should help you add all the necessary "info" metadata to the string messages such as
strLength = length(str);
msg.Name(idx).Data(1:strLength) = uint8(str);
msg.Name(idx).Data_SL_Info.CurrentLength = uint32(strLength);
Hope this helps.
- Sebastian
  6 Comments
Sebastian Castro
Sebastian Castro on 19 Jun 2017
Awesome. Thanks for the updates, and glad you got it fixed.
Did you end up going interpreted MATLAB or Simulink route?

Sign in to comment.

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by