How to publish ROS message inside a parfor loop?

Question

Dingqiao Zhu 2017-2-12

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/324560-how-to-publish-ros-message-inside-a-parfor-loop

评论： Flavia Causa 2018-3-28

I'm processing real-time data inside a parfor loop. I want to publish the result as soon as I get it.

segmentMsg{1} = rosmessage('std_msgs/Float64');
segmentMsg{2} = rosmessage('std_msgs/Float64');
segmentPub{1} = rospublisher('/topic_segments_1', 'std_msgs/Float64');
segmentPub{2} = rospublisher('/topic_segments_2', 'std_msgs/Float64');
parfor i=1:2
      %%Processing...
      % if get some result, assign it to segmentMsg, publish it immediately 
      send(segmentPub{i},segmentMsg{i});
  end

If I use for loop instead of parfor loop, my code works well. But when I publish ROS message inside parfor loop, an error named,

Invalid or deleted object

throwed. It means that publisher is deleted. I don't know why this problem happened. Is it because when enter parfor loop certain data type will be cleared? I've checked the type of ROS publisher, it's not a handle. It may be an object.

>> class(segmentPub{1})
ans =
robotics.ros.Publisher

Then I find an article: Use Objects and Handles in parfor-Loops It says that, if you are passing objects into or out of a parfor-loop, the objects must properly facilitate being saved and loaded. For more information , see Save and Load Process for Objects And then I run the following code in command window to test if save/load can help me:

segmentMsg = rosmessage('std_msgs/Float64');
segmentPub = rospublisher('/topic_segments_1', 'std_msgs/Float64');
save('segmentPub.mat','segmentPub');

Warning: robotics.ros.Publisher object cannot be saved to a MAT-file. > In robotics.ros.internal.mixin.Unsaveable/saveobj (line 28)

clear segmentPub; % clear the variable, so that we can test if save/load work well
load('segmentPub.mat')

Warning: Loaded robotics.ros.Publisher object is invalid. > In robotics.ros.internal.mixin.Unsaveable/loadobj (line 46)

segmentMsg.Data = 111.1;
send(segmentPub,segmentMsg);

Invalid or deleted object. Error in robotics.ros.Publisher/send (line 203) if ~strcmp(sendType, obj.MessageType)

Still throwing the same problem... I double-click the variable "segmentPub", and found that it only contained its properties, the corresponding values is missed. I wonder, if there any way to publish ROS message inside a parfor loop? Thanks!!!

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Edric Ellis 2017-2-13

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/324560-how-to-publish-ros-message-inside-a-parfor-loop#answer_254535

在 MATLAB Online 中打开

I think what you need to do here is ensure you call rosmessage and rospublisher directly on the workers. One way to do that is to use a parallel.pool.Constant to store those objects. You can build the Constant using a function handle, and that way, the function gets executed on the workers. Here's what I think you need to do (I have no way to test unfortunately).

segmentMsgConstant = parallel.pool.Constant(@() rosmessage('std_msgs/Float64'));
segmentPubConstant = parallel.pool.Constant(@() rospublisher('/topic_segments_1', 'std_msgs/Float64'));
parfor i = 1:2
    ...
    send(segmentPubConstant.Value, segmentMsgConstant.Value);
end

3 个评论
显示 1更早的评论隐藏 1更早的评论

Dingqiao Zhu 2017-2-16

在 MATLAB Online 中打开

Thank you a lot! Your answer offered me a new method to solve the problem. I've tested your code and got a runnable version:

delete(gcp);
parpool(2);
rosinit;
% Set ROS Node and ROS Publisher, be used outside parfor-loop. (for reference)
segmentNode = robotics.ros.Node('/segment_node1');
segmentPub = robotics.ros.Publisher(segmentNode,'/topic_segments_1', 'std_msgs/Float64');
% Subscribe to /topic_segments_1, be used outside parfor-loop. (for reference)
sub = rossubscriber('/topic_segments_1','std_msgs/Float64');
% Set ROS Node and ROS Publisher, be used inside parfor-loop.
segmentNodeConstant{1} = parallel.pool.Constant(@() robotics.ros.Node('/segment_node2'));
segmentPubConstant{1} = parallel.pool.Constant(@() robotics.ros.Publisher(segmentNodeConstant{1}.Value,'/topic_segments_1', 'std_msgs/Float64'));
% Subscribe to /topic_segments_1, be used inside parfor-loop.
subNodeConstant = parallel.pool.Constant(@() robotics.ros.Node('/segment_node3'));
subConstant = parallel.pool.Constant(@() robotics.ros.Subscriber(subNodeConstant.Value, '/topic_segments_1','std_msgs/Float64'));
% Set ROS message
segmentMsg{1} = rosmessage('std_msgs/Float64');
segmentMsg{1}.Data = 111;
% Send message outside parfor-loop
send(segmentPub,segmentMsg{1});
% Check if the message is received
sub.LatestMessage
parfor i=1:1
    subConstant.Value.LatestMessage
end
segmentMsg{1}.Data = 222;
% Send message inside parfor-loop
parfor i=1:1
    fprintf(segmentPubConstant{i}.Value.TopicName);fprintf('\n');
    fprintf(segmentPubConstant{i}.Value.MessageType);fprintf('\n');
    fprintf(class(segmentPubConstant{i}.Value));fprintf('\n');
    send(segmentPubConstant{i}.Value,segmentMsg{i});
end
% Check if the message is received
sub.LatestMessage
parfor i=1:1
    subConstant.Value.LatestMessage
end

The result is, message published outside parfor-loop can be received by both subscriber outside or inside( use parallel.pool.Constant) parfor-loop. However, message published inside parfor-loop can't be received by neither subscirber inside nor outside parfor-loop. I've checked that, parallel.pool.Constant can store ROS publisher's type, properties and values. It should be able to work. I don't know why it can't be received...

Dingqiao Zhu 2017-3-14

I finally find that if I want to publish ROS message in workers, I have to run rosinit in every worker so that they can connect to the ROS network. By this way, workers can publish ROS message to the ROS network.

Flavia Causa 2018-3-28