How to efficiently access data from ROS PoseArray Messages?

I am trying to analyse data from a rosbag from a turtlebot run. I have Pose_Array message which contains about 10,000 particles and I want to computer the mean of the X and Y position of these particles. When I try to access as follows:

rb = rosbag('test.bag');
tb_particle_bag = select(rb,'Topic','/particlecloud');
msg = readMessages(tb_particle_bag,1);

Then when I run the following I see...

    b=msg{1}.Poses
     b =
         10000×1 ROS Pose message array with properties:

           MessageType
           Position
           Orientation

However when I try to access the X-coordinates of all the particles I see the follwing error:

b.Position.X
Expected one output from a curly brace or dot indexing expression, but there were 10000 results.

The only work around I have found to this is to individually access each particles and read its x value in a for loop and this turns out to be extremely slow as follows:

for i = 1:tb_particle_bag.NumMessages
    msg = readMessages(tb02_gt_particle_bag,i);
    b=msg{1}.Poses;
    sum = [0 0];
    for j = 1:size(b,1)
        p=b(j).Position;
        sum = sum + [p.X p.Y];
    end
    tb_mp(i,:)=sum/size(b,1);
end

This above method takes about 2.5secs for each PoseArray message in the rosbag and makes the process excruciatingly long. Is there a more efficient way to do this?

Answer 1

[Sebastian Castro]

I think you can do this... try:

b = [msg.Poses]

- Sebastian

[Vikrant Shah]

b=[msg.Poses]
Throws an error
Struct contents reference from a non-struct array object.

And

b=[msg{1}.Poses]

has the same effect as

b=msg{1}.Poses
b = 

10000×1 ROS Pose message array with properties:

    MessageType
    Position
    Orientation

Just creates a Pose message array. Still doesn't let me access the data inside.

Answer 2

[Saurabh Gupta]

You could use arrayfun to perform the operations. I don't know whether it will be faster, but you will be able to avoid loops.

For example, for calculating the sum of all b.Position.X values, the following should work.

>> anonX = @(var)var.Position.X;
>> sumX = sum(arrayfun(anonX, b));

[Vikrant Shah]

This makes it look nicer but doesn't help with the speed. In fact it makes slower. Time per message increased from 2.5 secs to 3.5 secs.

[chef13]

Hi, did you reach to solve your problem ?

I am trying to use PoseArray messages in SIMULINK (but if it works I could also use matlab) but I have a lot of problems.

Thanks