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

10 次查看（过去 30 天）
Omar Ramadan
Omar Ramadan 2023-5-7
回答： Prabeen Sahu 2023-5-10
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 :)

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

回答（2 个）

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.
  2 个评论
Omar Ramadan
Omar Ramadan 2023-5-8
编辑：Omar Ramadan 2023-5-8
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
Omar Ramadan 2023-5-8
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.

请先登录，再进行评论。

Prabeen Sahu
Prabeen Sahu 2023-5-10
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

类别

在 Help Center 和 File Exchange 中查找有关 Publishers and Subscribers 的更多信息

标签

产品


版本

R2023a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by