How to publish ROS message inside a parfor loop?

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!!!

Answer 1

[Edric Ellis]

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

[Dingqiao Zhu]

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]

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]

How did you manage to run rosinit for every worker?