receiving from a subscriber in ROS is taking indefinite time resulting in a fail

Hello,

I am new to working with ROS. I am trying to implement my own Franka Emika robot arm Pick and Place using this provided example https://www.mathworks.com/help/robotics/ug/pick-and-place-workflow-in-gazebo-using-ros.html.

Bulk of my code so far is word for word from this example.

My issue is that when carrying out this function

jMsg = receive(rossubscriber('/franka_gripper/joint_states'));

Script keeps going on forever until I stop it manually. When stop it i get this error

In ros.Subscriber/receive (line 456)
                util.waitUntilTrue(@() obj.MessageCount > nMessages, ...

From my understanding, this means that

1. There is no data being published for the subscriber to recieve, which I belive is not true due to the fact that i tested what response /franka_gripper/joint_states is outputting using rostopic echo, and it is clear that i am getting a constant flow of data at a high frequency.

Header      
    Stamp      
      Sec  :  6413
      Nsec :  927000000
    Seq     :  181740
    FrameId :  
  Name     :  {1}  panda_finger_joint1   {2}  panda_finger_joint2   
  Position :  [0.01344062581074294, 0.0134407826620155]
  Velocity :  [-1.977668961469445e-05, 1.969509947091252e-05]
  Effort   :  [2.712875049178911e-06, -2.796997938358815e-06]

2. Some other subscriber is interfering with this topic, of which i tested using rostopic info /franka_gripper/joint_states

Type: sensor_msgs/JointState
 
Publishers:
* /gazebo (http://vm:34813/)
 
Subscribers:
* /joint_state_publisher (http://vm:45961/)

• I tried restarting my VM, restarting Gazbo, changing buffer size, disabling all publishers and subscribers except the one I am testing which resulted in nothing but failure
• It is worth mentioning that the connection is set since i can move the robot, set config of the robot and other.
• This issue sometimes also applies to the topic /franka_state_controller/joint_states

Immediate help would be highly appreciated :)

Answer 1

[Jagadeesh Konakalla]

Hi Omar,

Which verion of MATLAB that you are using ?

Are you able to run the example provided by Mathworks without any issue ?

Above, you mentioned that ros topic echo is working. Did you run this command in MATLAB or VM ?

Are you using the VM provided by Mathowkrs ? If so, let me know from where you downloaded.

Thanks,

Jagadeesh K.

[Omar Ramadan]

Hi Jagadeesh, thanks for your hasty reply.

1. I am using version 9.14.0.2206163 (R2023a)
2. I did not run the example provided, however all of the ROS initiliazing code was copied from the provided example
3. The ROS echo was ran on Matlab, roscore is not installed on the VM
4. I am using VMware workstation 16 player (downloaded from the official website), not sure what VM does Mathwork provide.

Here is my code so far incase you want to take a look at it

(I usually manually initiate the ROS connection through the command console, hence why it is commented in the code)

File name : myMain.m

%initiate ROS connection
robot = importrobot("frankaEmikaPanda.urdf");
% rosIP = "192.168.79.129"; % IP address of ROS enabled machine  
% rosshutdown;
% rosinit(rosIP,11311);

% setInitialConfig_franka; This file was not modified, it is now
% implemented in classRobot for convience

% [gripAct,gripGoal] = rosactionclient('/franka_gripper/gripper_action');
% gripperCommand = rosmessage(gripAct); 

% frankaSLActivateGripper(1);


coordinator = classRobot(robot, 'gripper');

File name : classRobot.m

(This code will return an error at line 80 even if the ROS issues were fixed, it is intentional since it is a work in progress)

classdef classRobot <handle
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        FlowChart
        Robot
        World = {};
        Parts = {};
        Obstacles = {};
        ObstaclesVisible = {};
        DetectedParts = {};
        RobotEndEffector
        CurrentRobotJConfig
        CurrentRobotTaskConfig
        NextPart = 0;
        PartOnRobot = 0;
        HomeRobotTaskConfig 
        PlacingPose
        GraspPose
        Figure
        TimeStep
        MotionModel
        NumJoints
        NumDetectionRuns = 0;
        CollisionHelper
        ROSinfo
        DetectorModel
        jointMess
        jointPub
    end
    
    methods
        function obj = classRobot(robot, robotEndEffector)
            obj.Robot = robot;

            % Initialize ROS utilities
            disp('Initializing ROS Utilities')
            % obj.ROSinfo.jointsSub = rossubscriber('/joint_states');
            obj.ROSinfo.configClient = rossvcclient('/gazebo/set_model_configuration');
            obj.ROSinfo.gazeboJointNames = {'panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7'}; % joint names of robot model in GAZEBO
            obj.ROSinfo.controllerStateSub = rossubscriber('/franka_state_controller/joint_states');
            obj.ROSinfo.rgbImgSub = rossubscriber("/camera/rgb/image_raw");
            % obj.ROSinfo.rgbImgSub = rossubscriber("/camera/rgb/image_raw","sensor_msgs/Image","DataFormat","struct"); %To convert the pic to struct
            obj.jointMess = rosmessage("geometry_msgs/PoseStamped");
            obj.jointPub = rospublisher("/cartesian_impedance_example_controller/equilibrium_pose");
            % initialconfig = [0.4000,0,0.8000,0.0000,-1.0000,-0.0000,0.0050]

            % Initialize robot configuration in GAZEBO
            disp('Initialize robot configuration')
            gripperX = 0.4;
            gripperY = 0;
            gripperZ = 0.4;

            gripperRotationX = -pi; % radians
            gripperRotationY = -0.01; % radians
            gripperRotationZ = 0; % radians

            obj.jointMess.Pose.Position.X = gripperX;
            obj.jointMess.Pose.Position.Y = gripperY;
            obj.jointMess.Pose.Position.Z = gripperZ;
            quat = angle2quat(gripperRotationX, gripperRotationY, gripperRotationZ, "XYZ");
            obj.jointMess.Pose.Orientation.W = quat(1);
            obj.jointMess.Pose.Orientation.X = quat(2);
            obj.jointMess.Pose.Orientation.Y = quat(3);
            obj.jointMess.Pose.Orientation.Z = quat(4);
            send(obj.jointPub,obj.jointMess)
            pause(2);
            % delete(obj.jointPub); %delete jointPub because it interfers with joint_states
            
            % Unpause GAZEBO physics
            disp('Unpause GAZEBO physics')
            physicsClient = rossvcclient('gazebo/unpause_physics');
            physicsResp = call(physicsClient,'Timeout',3);
            pause(3);

            % Update robot properties
            obj.CurrentRobotJConfig = getCurrentRobotJConfig(obj);
            obj.RobotEndEffector = robotEndEffector;
            obj.CurrentRobotTaskConfig = getTransform(obj.Robot, obj.CurrentRobotJConfig, obj.RobotEndEffector);
            obj.TimeStep = 0.01; % used by Motion Planner
            obj.NumJoints = numel(obj.CurrentRobotJConfig);
        end
        
        function configResp = setCurrentRobotJConfig(obj, JConfig)            
            configReq = rosmessage(obj.ROSinfo.configClient)
            configReq.ModelName = "panda"
            configReq.UrdfParamName = "/robot_description"
            configReq.JointNames = obj.ROSinfo.gazeboJointNames
            configReq.JointPositions = JConfig
            configResp = call(obj.ROSinfo.configClient, configReq, 'Timeout', 3)
        end

        function JConfig = getCurrentRobotJConfig(obj)
            jMsg = receive(rossubscriber('/joint_states'));
            jointStruct = struct(); %add gripper to corners of array throught /frank_gripper
            for i = 1:length(obj.ROSinfo.gazeboJointNames)
                jointStruct.(obj.ROSinfo.gazeboJointNames{i}) = jMsg.Position(i);
            end
            % jMsg = receive(rossubscriber('/franka_gripper/joint_states', 'BufferSize', 100));
            % JConfig =  jMsg.Position';
            JConfig =  jointStruct;
        end
    end
end

[Omar Ramadan]

One observation that i just deducted is that when i run the script for the very first time, it works just fine until i hit any error, in my case the inputs to the getTransform function are wrong

After I rerrun the program, it gets stuck on the receive function, however i found that if i stop it, execute the receive function in the command console, it returns data.

And when i rerun the script, it starts to working again and do this loop everytime.

Answer 2

[Prabeen Sahu]

Hi Omar,

These are the VMs shipped by MathWorks:

1. Foxy and Noetic VM
2. Melodic and Dashing VM

The Pick and Place Workflow example that you mentioned, works fine with the Melodic and Dashing VM. Please run the example with MathWorks shipped VM and check if it is working.

In your code you are creating subscriber inside receive function as mentioned below.

jMsg = receive(rossubscriber('/franka_gripper/joint_states'));

This is not an efficient way of using subscriber. In your code you can create the subscriber before receive() (May be as a member of the class like obj.ROSinfo.controllerStateSub) and use the handle of subscriber in receive(subHandle).

If still you are not able to get the message you could try:

1. Using subHandle.LatestMessage as mentioned in this link, and check if you are able to get the message.
2. Use a callback to check if subscriber is getting the messages. Using a callback function is another approach you can consider(follow the same link mentioned above). By defining a callback function and setting it for the subscriber, the callback will be executed each time a new message arrives. This approach allows for more flexibility in handling messages asynchronously.

-Prabeen