Simulink/ROS: Publishing JointTrajectory Messages

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.

Answer 1

[Sebastian Castro]

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

[Sebastian Castro]

So... I just tried the Simulink approach and I may know what you need to fix to make it all work in Simulink and for code generation!

In addition to assigning the points struct, you need to tell your bus the number of points up front in the Points_SL_Info field!

msg.Points_SL_Info.CurrentLength = numPts;

That did it for me. I've attached my model in a ZIP file. Just run rosinit beforehand.

Sebastian

[Scott Chow]

Thank you for the quick response Sebastian.

Wrapping the entire publisher inside a Matlab Function module is a great idea. I was stuck on using the provided publisher block that I didn't realize I could just use Matlab code by itself.

I gave your encapsulate Matlab in a Simulink Block solution a shot and got really close. Echoing the topic now gives very promising messages.

---
header: 
  seq: 22082
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
joint_names: ['joint_1', 'joint_2', 'joint_3']
points: 
  - 
    positions: [-2.0, 0.0, 0.0]
    velocities: [0.0, 0.0, 0.0]
    accelerations: [0.0, 0.0, 0.0]
    effort: [0.0, 0.0, 0.0]
    time_from_start: 
      secs: -2
      nsecs:         0
---

However I keep getting an error that the joint names do not match:

[ERROR] [1497559269.897254704, 351.936000000]: Cannot create trajectory from message. It does not contain the expected joints.

However looking at the echo of the command topic (which Matlab is publishing to) and the state topic (which Gazebo publishes to), the joint_names array look identical for both. Perhaps there might be some difference in how the joint names are being encoded?

Some potential issues I am thinking through are:

• Do I need to provide the string metadata as well, even though I am doing it through purely Matlab?
• Another issue may be because I am running Matlab on Windows 10 and Gazebo/Ros is running on Ubuntu 16.04. Perhaps Unix vs Windows line endings might be a problem?

Any thoughts?

Again, thank you for your help.

[Sebastian Castro]

No, you shouldn't need the string metadata with the MATLAB approach.

As far as the error message... how about trying to change the row/column order of the joint names and positions? I never really checked whether they should be row or column vectors...

- Sebastian

[Scott Chow]

Thank you for your help! It turns out I had commented out one of the joints in the JointTrajectoryController and that was causing the mismatch.

Also, just for reference, one issue that I ran into with the Matlab solution was when I restarted the simulator, the pub variable may be instantiated, but referencing a deleted publisher. I fixed this issue by changing the check to not only check if pub is empty, but also check if it is valid. I also modified it so that the program restarts ROS in this case. The following is my modified check:

function sendTrajMsgs( pos1, pos2 )
    % Create the publisher only on the first function call 
    % or if pub is no longer a valid publisher
    persistent pub
    if isempty(pub) | ~isvalid(pub)
        % Restart ROS
        rosshutdown
        rosinit
        pub = rospublisher('/my_joints','trajectory_msgs/JointTrajectory');
    end

Hopefully this helps out people in the future.

[Scott Chow]

Just a final update, I have realized that it is not consistent to put the calls to rosshutdown and rosinit in my function call. There are constantly errors and it makes Simulink hang while waiting for rosinit to complete.

I suggest handling rosinit and rosshutdown separately, either in a script run beforehand or just manually running them.

[Sebastian Castro]

Awesome. Thanks for the updates, and glad you got it fixed.

Did you end up going interpreted MATLAB or Simulink route?