how to receive ROS Topics in parallel workers?

Hello,

I try to run my ROS-Nodes and callback subscribed to topics in a parallel worker. Unfortunately my Callback is never launched.

Without the parallel computing the one and the same lines run fine in Matlab.

Second I try to follow Matlab activities by adding debugging lines into the code. Does anybody know how to debug the parallel workers and callbacks in a better way?

function app=myInit(app)  % 1st we init parallel computing and ROS

% Parallel Computing Toolbox
p = gcp('nocreate'); % If no pool, do not create new one.
if isempty(p)
    disp('Starting Parallel Computing Toolbox')
    app.pool.obj = parpool;
else
    disp('Parallel Computing Toolbox already running')
    app.pool.obj = gcp('nocreate');
end
app.pool.n = 0; % count number of parallel tasks


% check if ROS is already running, if not, start ROS
[status,cmdout] = system('ps -ef|grep ros');
if status == 0 
    if contains(cmdout,'/opt/ros/melodic/bin/roscore')
        disp('ROS is already running')
    else
        disp('Starting ROS...')
        [status,cmdout] = system('roscore &');
        pause(2);
    end
else
    warning(cmdout);
    error(['checking ROS "ps -ef|grep ros" status: ' num2str(status)]);
end

%initialize ROS by creating ROS master in MATLAB and starting a global node connected to master.
app.iP.localIpAddr = '192.168.213.1';
rosinit(app.iP.localIpAddr, 'NodeName', '/MatlabInit')
app.iP.rosNode = ros.Node('/Matlab');
disp(' ')
app.iP.rtl = rostopic("list");
app.iP.d = rosdevice(app.iP.localIpAddr,'user', 'pw');

app.pool.q = parallel.pool.DataQueue();
if isfield(app.pool, 'future') == 0
    app.pool.future = afterEach(app.pool.q, @disp);
end
end


function startTopic(app)  % here, we start and stop our ROS Topics

if app.Button.Value
    
    startMyRosTopics();
    pause(5)
    topicType = rostopic("type", 'myTopicType');  % this is fine
    topicEcho = rostopic("echo", '/myTopic');     % this is also fine
    
    
    app.pool.n = app.pool.n +1;
    
    disp('calling parMonitor1')
    app.pool.F(app.pool.n) = parfeval(@parMonitor1,1,app.pool.q, 'start');
    disp('parMonitor1 called')
    app.lP.parPoolFcnIdx1 = app.pool.n;
    
else
    
    app.pool.F(1, app.lP.parPoolFcnIdx1).Error.message
    disp('Error stack:')
    if sum(size(app.pool.F(1, app.lP.parPoolFcnIdx1).Error)) == 0
        disp('no Error @ parMonitor1')
    else
        for ii = 1:size(app.pool.F(1, app.lP.parPoolFcnIdx1).Error.stack,1)
            disp(['File: ' app.pool.F(1, app.lP.parPoolFcnIdx1).Error.stack(ii).file ...
                '  Fcn Name: ' app.pool.F(1, app.lP.parPoolFcnIdx1).Error.stack(ii).name ...
                '  Line Number: ' num2str(app.pool.F(1, app.lP.parPoolFcnIdx1).Error.stack(ii).line)])
        end
    end
    app.pool.F(1, app.lP.parPoolFcnIdx1).Diary
    disp(' ')
    
    disp('Fetching parFcn output...')
    app.lP.out = fetchOutputs(app.pool.F(app.lP.parPoolFcnIdx1));
    disp(app.lP.out);
    %                 app.lP.out = rmfield(app.lP.out,'PCSub1');
    %                 app.lP.out = rmfield(app.lP.out,'MonNode');
    
    app.pool.F(app.lP.parPoolFcnIdx1) = parfeval(@parMonitor1,1,app.pool.q, 'stop');
    app.lP.out = fetchOutputs(app.pool.F(app.lP.parPoolFcnIdx1));
    disp(app.lP.out);
    
end
end

function out = parMonitor1(q, task)   
% This function already runs in the parallel worker. 
% It subscribes to the ROS-Topic and registeres the callback
persistent n
persistent nodeAndSub

if isempty(n)
    n = 0;
end
n = n+1;
send(q, sprintf('parMonitor1 launched %d. times. Task: %s',n, task));

switch task
    case 'start'
        try
            rosinit('192.168.213.1')
        catch e
            disp(e)
        end
        
        if contains(e.message, 'The global ROS node is already initialized and connected to the master')
            send(q, sprintf('ROS was already runnning @ parMonitor1'))
        else
            send(q, sprintf('starting ROS'))
            pause(10)
        end
        
        send(q, sprintf('parMonitor1 rosinit done'));
        
        nodeAndSub.MonNode = ros.Node('MonNode','http://192.168.213.1:11311');
        send(q, sprintf('ros.Node created'))
        disp(nodeAndSub.MonNode)
        pause(5);
               
        try
            send(q, sprintf('Subscribing to /myTopic  in parMonitor1()'));
            nodeAndSub.PCSub1 = ros.Subscriber(nodeAndSub.MonNode,'/myTopic',@Callback1);
            send(q, sprintf('/myTopic subscribed'));
        catch e
            send(q, sprintf('Error at ros.Subscriber in parMonitor1()'));
            send(q, sprintf('The identifier was:\n%s',e.identifier));
            send(q, sprintf('There was an error! The message was:\n%s',e.message));
        end
        
        send(q, sprintf('ros.Subscriber called'))
    case 'stop'
        nodeAndSub.PCSub1 = [];
        nodeAndSub.MonNode = [];
    otherwise
        send(q, sprintf('Error: parMonitor1 do not know what to do here'))
end
out = task;
send(q, sprintf('parMonitor1 done'))
end

function Callback1(~, message)  
% This Callback should be executed periodically in a parallel worker.
persistent nn

here = 'hier';
save('CallbackWasHere.mat','here')

if isempty(nn)
    nn = 0;
end
nn = nn+1;

debug = ['Callback1 was running n:' mnum2str(nn) ' times'];
filename = ['Callback1_' num2str(nn) '.mat'];
save(filename, 'debug','message')

end

I'd assume, the Callback1 function should run.

However, the files "CallbackWasHere.mat" and "Callback1_*.mat" were never created.

Does anybody know why?

Why is Callback1 never executed?

This is my workspace output:

Parallel Computing Toolbox already running
ROS is already running
The value of the ROS_IP environment variable, 192.168.213.1, will be used to set the advertised address for the ROS node.
Initializing global node /MatlabInit with NodeURI http://192.168.213.1:40537/
The value of the ROS_MASTER_URI environment variable, http://localhost:11311, will be used to connect to the ROS master.
The value of the ROS_IP environment variable, 192.168.213.1, will be used to set the advertised address for the ROS node.

calling parMonitor1
parMonitor1 called
parMonitor1 launched 3. times. Task: start
ROS was already runnning @ parMonitor1
parMonitor1 rosinit done
The value of the ROS_IP environment variable, 192.168.213.1, will be used to set the advertised address for the ROS node.
ros.Node created
Subscribing to /myTopic  in parMonitor1()
/myTopic subscribed
ros.Subscriber called
parMonitor1 done

Error stack:
no Error @ parMonitor1

ans =

    '  ROSException with properties:
     
         identifier: 'ros:mlros:node:GlobalNodeRunningError'
            message: 'The global ROS node is already initialized and connected to the master at URI "http://192.168.213.1:11311". If you want to restart the global node, call "rosshutdown" first.'
              cause: {}
              stack: [2x1 struct]
         Correction: []
     
     The value of the ROS_IP environment variable, 192.168.213.1, will be used to set the advertised address for the ROS node.
       Node with properties:
     
                Name: '/MonNode'
           MasterURI: 'http://192.168.213.1:11311'
             NodeURI: 'http://192.168.213.1:33741/'
         CurrentTime: [1x1 Time]
     
     '

 
Fetching parFcn output...
start
parMonitor1 launched 4. times. Task: stop
parMonitor1 done
stop

Can anybody help, please?

How to debug inside parallel workers and callback functions?

How to check if they must be called and executed or not?

Is there any registry in Matlab where to check if the Callback is subscribed correctly?

Checking "rostopic bw /myTopic" in the linux shell, we see that the topic is present with an update rate of 1 Hz.

Thank you

[Jagadeesh Konakalla]

Hi Andreas,

I have replied to your previous post here.

https://www.mathworks.com/matlabcentral/answers/846365-custom-ros-messages-in-parfeval-function?s_tid=srchtitle

Can you please let me know if the workarounds provided in the post helped you or not ?

With regards,

Jagadeesh K.

[Andreas Schwager]

Dear Jagadeesh,

thanks fro your reply.

However, I followed your recommendation as close than possible.

If you compare the code at my previous question and above, there are significant differnces:

The code in the other chat uses network intropsection API only. Reading ROS message inside a parfeval function did not run.

Here, first a worker is created (parMonitor1) and inside this worker a Node and Subscription are created using:

nodeAndSub.MonNode = ros.Node('MonNode','http://192.168.213.1:11311');
nodeAndSub.PCSub1  = ros.Subscriber(nodeAndSub.MonNode,'/myTopic',@Callback1);

The objects of ros.Node and ros.Subscriber are used. There are no casual API's and introspection API's in parFunction, any more.

I expect if the creation of the new node is done inside the parallel worker, all callbacks by the subscriber should also be done in parallel.

However, the "Callback1" is never executed. Why?

Would you mind to check the code lines above, one more time, please?

Thanks a lot for your support

Andreas

[Jagadeesh Konakalla]

Hi Andreas,

I will try your code on my end and let you know.

Thanks,

Jagadeesh K.

[Jagadeesh Konakalla]

Hi Andreas,

Hi Andreas,

Here is what i have done on my end and i can see the mat files created with the code that you shared.

• In the callback function, i have updated the mnu2str to num2str. Updated the type. >>debug = ['Callback1 was running n:' mnum2str(nn) ' times'];
• Started the local master by running Core = ros.Core(11311) from MATLAB command line.
• Updated the IP address everywhere in the code to my local IP.[This is for testing]
• Split the myInit and startTopic into seperate scripts.
• First run the myInit script from MATLAB command line. (app = myInit)
• Then created a publihser to the /mytopic from command line and sent the sample data.

    app.Button.Value = true

• Commented startMyRosTopics function call in startTopic as the code is not provided.

    %startMyRosTopics();
    %pause(5)
    %topicType = rostopic("type", 'myTopicType');  % this is fine
    %topicEcho = rostopic("echo", '/myTopic');     % this is also fine

• startTopic(app)
• I can see that mat files are created and mat files contains the received message.

On your end, i expect atleast CallbackWasHere.mat to be created. Let me know how you tested it on your end.

Which version of MATLAB that you are using ?

I would recommend you to use the MathWorks Tehnical Support for further assitance.

Thanks,

Jagadeesh K

[Andreas Schwager]

Dear Jagadeesh,

thanks for your help!

Do you know this chat?

https://www.mathworks.com/matlabcentral/answers/119559-why-is-the-ros-subscriber-callback-in-matlab-not-triggered-when-messages-are-published-from-an-exter

This is the solution!

anyway, thanks a lot!

Best Regards

[Jagadeesh Konakalla]

Hi Andreas,

Good to know that your problem is resolved. But you mentioned that everything works without parallel toolbox.

Thanks,

Jagadeesh K.

Answer 1

[Jagadeesh Konakalla]

https://www.mathworks.com/matlabcentral/answers/119559-why-is-the-ros-subscriber-callback-in-matlab-not-triggered-when-messages-are-published-from-an-exter