Joint_states Publisher.

Hello, I am using the Robotics Toolbox on Matlab to work on the Baxter robot. While I subscribe to a topic like /robot/joint_states and create a publisher for the same, instead of receiving the stream of joint angle sets I just receive one set of joint angles. The commands I am using are the following:

 joint_state_sub = rossubscriber('/robot/joint_states');
 state_pub = receive(joint_state_sub,10);
Also in the publisher I am unable to understand the value 10, as even if I change it to 100 or any other value  I still get the same result.

I want to be able to record the joint angle for an entire motion. Can someone help to let me know what I am doing wrong. Just to let you know: I have installed the toolbox last night and have been at it till now.

Answer 1

[Sebastian Castro]

The receive function waits up to N (10, 100, etc.) seconds to receive the NEXT incoming message in the stream -- it is not meant to receive the entire stream.

There are plenty of ways to receive the whole data set by repeatedly calling receive (Waits for the next message) or using the LatestMessage property (picks the last received message, can get the same message multiple times in a row if no new ones come in).

msg = receive(joint_state_sub,10);
msg = joint_state_sub.LatestMessage;

One way you could do it is in a for-loop. For example, suppose you want to receive the next 5000 messages that come in:

% Preallocate buffer of 5000 elements -- assuming there are 3 joints in this robot, hence 3 columns.
angles = zeros(5000,3);

% Loop through and receive
for idx = 1:5000
  state_msg = receive(joint_state_sub,10);
  angles(idx,:) = state_msg.Position;
end

By the way, it might be useful to also pick out the time stamps for each message if you want to plot these. To do this, you can use the Header portion of the message:

t_sec = state_msg.Header.Time.Stamp.Sec;
t_nsec = state_msg.Header.Time.Stamp.Nsec;
...
times(idx) = double(t_sec) + 1e-9*double(t_nsec)

- Sebastian