rospy.wait_for_message does not get the message even though new messages are being published
I'm running a script where I use the rospy.wait_for_message in a while loop to wait for a message of a topic, that is publishing at certain event.
On the first event the while loop runs as expected, sometimes even on the second. I can see by running rostopic echo /event_topic
that the messages are being published after the code stopped inside the while loop at the instruction rospy.wait_for_message("/event_topic", Bool, timeout=None)
, but the instruction is never executed. The program just stays at this line, waiting for messages, that are being published.
When I reset the script the first one or two events are again processed as expected and that the process halts. I cannot find any errors nor warnings printed in the terminal.
This is my script:
import rospy
import rospkg
from std_msgs.msg import Bool
from std_msgs.msg import Float64
from std_msgs.msg import Float32MultiArray
def mynode():
rospy.init_node('mynode')
publisher = rospy.Publisher('/command', Float64, queue_size=1)
time.sleep(.5)
while not rospy.is_shutdown():
rospy.wait_for_message("/event_topic", Bool, timeout=None)
data = rospy.wait_for_message("/data_topic", Float32MultiArray, timeout=None)
msg = data.data[0]
publisher.publish(msg)
time.sleep(.2)
rospy.spin()
if __name__ == '__main__':
try:
mynode()
except rospy.ROSInterruptException:
pass
It also seems, that when I subscribe to the /event_topic
by running rostopic echo
in the terminal, the next event is being processed by the rospy.wait_for_message
, but following, again, are not.
EDIT
The problem is somehow connected to the multimaster-FKIE package. I run two simulations on two ROS masters and use the multimaster-FKIE package to allow communication between them. The node shown above is supposed to read messages from topics started in one ROS master and publish messages into the topics started in the other ROS master.
When I run the node on the ROS core where the /event_topic
was started, it works as expected. If I run the node on the ROS master where the /data_topic
was started, the problem occurs. But I don't understand why. The topics are supposed to be shared among the ROS masters.