How to efficiently access data from ROS PoseArray Messages?

9 views (last 30 days)
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?

Answers (2)

Sebastian Castro
Sebastian Castro on 21 Jul 2017
I think you can do this... try:
b = [msg.Poses]
- Sebastian
  1 Comment
Vikrant Shah
Vikrant Shah on 21 Jul 2017
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.

Sign in to comment.


Saurabh Gupta
Saurabh Gupta on 21 Jul 2017
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));
  2 Comments
Vikrant Shah
Vikrant Shah on 21 Jul 2017
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
chef13 on 6 Mar 2018
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

Sign in to comment.

Community Treasure Hunt

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

Start Hunting!