Simple Matlab Subscriber/Publisher

edit

So ill get right to the point:

I have a ROS network running and a matlab script successfully connecting to it. The matlab node defines publishers and a subscriber with a callback function, all of which is shown at the bottom.

What I cant figure out is publishing the dis, theta, phi msgs on every new /motionCommand . The callback function does get called every-time theres a new message and prints the "!!!here!!!" line, however thats the only code that runs... do I just put the publishers in the callback function? Can I make the publishers global?

What is the best way of publishing a topic upon receiving a topic in a ROS Matlab Node?

Thank you in advance for any advice!

inversekin.m:

rosinit
%N = robotics.ros.Node("motionPlanner")
%node = robotics.ros.Node('/test');

%global params to be set in callback function


%Create publishersn
disPub = rospublisher('/valvControlMsg', 'std_msgs/Float64');
pause(2);
thetaPub = rospublisher('thetaControlMsg', 'std_msgs/Float64');
pause(2);
phiPub = rospublisher('/phiControlMsg', 'std_msgs/Float64');
pause(2);
sub = rossubscriber('/motionCommand', ...
"geometry_msgs/Point", @solvDH);
pause(1);
global dis;
global theta;
global phi;
pause(2);

%Create msgs to publish
disMsg = rosmessage(disPub);
thetaMsg = rosmessage(thetaPub);
phiMsg = rosmessage(phiPub);

%Populate msgs to Publish
disMsg.Data = dis;
thetaMsg.Data = theta;
phiMsg.Data = phi;

%Publish
send(disPub, disMsg);
send(thetaPub, thetaMsg);
send(phiPub, phiMsg);

solvDH.m (some of these lines appear commented out but are not)

function solvDH(~, newPos)
disp('!!!!!!!!!!!!!!!!!!HERE!!!!!!!!!!!!!! ');


global dis;
global theta;
global phi;


%newPos = receive(sub, 10);
x = newPos.X;
y = newPos.Y;
z = newPos.Z;

%inches to meters
x = x / 39.37; y = y / 39.37; z = z / 39.37;
pos = [x y z];

robot = importrobot('appleRobot.urdf');
homeConfig = robot.homeConfiguration;

pose = eye(4);
pose(1:3, 4) = pos';

ik = robotics.InverseKinematics('RigidBodyTree', robot);
weights = [0.25 0.25 0.25 1 1 1];

[configSoln,solnInfo] = ik('end', pose, weights, homeConfig);

dis = configSoln(1).JointPosition;
theta = configSoln(2).JointPosition;
phi = configSoln(3).JointPosition;

end