ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Why does waitForMessage not work for a single non-latching message?

asked 2020-12-29 23:28:52 -0500

Rufus gravatar image

updated 2020-12-29 23:46:33 -0500

I need a way to get the first message that was published after this line of code. I am thinking waitForMessage seems like a logical choice. However, it does not seem to be behaving as I expected.

Given the following simple program that waits for messages from a topic a couple of times.

#include "ros/ros.h"
#include "std_msgs/Int32.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listener");

    ros::NodeHandle nh;
    std_msgs::Int32::ConstPtr ret;
    ret = ros::topic::waitForMessage<std_msgs::Int32>("/topic");
    ROS_INFO("%d", ret->data);
    ret = ros::topic::waitForMessage<std_msgs::Int32>("/topic");
    ROS_INFO("%d", ret->data);
    ret = ros::topic::waitForMessage<std_msgs::Int32>("/topic");
    ROS_INFO("%d", ret->data);

    ros::spin();

    return 0;
}

I publish with the following non-latching publish

#!/usr/bin/env python  
import rospy
import std_msgs

if __name__ == "__main__":
        rospy.init_node('publisher')
        pub = rospy.Publisher('/topic', std_msgs.msg.Int32, queue_size=1, latch=False)
        pub.publish(0)
        rospy.spin()

rostopic echo /topic shows that the message is published correctly. However, the listener program is still stuck at waitForMessage even though the listener program was started before the publishing node. Why is that the case?

Further testing with

#!/usr/bin/env python  
import rospy
import std_msgs

if __name__ == "__main__":
    i = 0
    while not rospy.is_shutdown():
        pub = rospy.Publisher('/topic', std_msgs.msg.Int32, queue_size=1)
        rospy.init_node('publisher')
        pub.publish(i)
        i += 1
        rospy.sleep(10)

shows that it's always only the first message that is ignored. The subsequent messages are correctly caught by waitForMessage

edit retag flag offensive close merge delete

Comments

Seeing your question (and #q368589), perhaps it would be good if you could clarify why you "need a way to get the first message that was published after this line of code". What is it you're trying to achieve?

Publish-subscribe being a completely asynchronous pattern of communication, implementing anything which requires strict ordering of events in different entities is typically a bit more complex than normal. So things may need a different approach.

gvdhoorn gravatar image gvdhoorn  ( 2020-12-30 03:53:48 -0500 )edit

Thanks for the patience and detailed responses, I am currently running tests in simulation where I rearrange some 200+ objects in gazebo and check if the sensor detects them. This is repeated many times for many different configurations. Since I am simulating multiple lidars, getting each sensor measurement takes an incredibly long time. As such, to speed up my testing, I only get the first sensor reading immediately after rearrangement (which I was thinking to accomplish with waitForMessage). I am currently tracking down an issue where the sensor measurements don't seem to be updated afte rearrangement (question on gazebosim here) which led me to the questions on waitForMessage.

Rufus gravatar image Rufus  ( 2020-12-30 21:07:52 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-12-30 03:51:25 -0500

gvdhoorn gravatar image

updated 2020-12-30 03:55:28 -0500

tl;dr: creating publisher->subscriber connections takes time. You are not giving waitForMessage(..) sufficient time, hence it doesn't see any messages on your topic. Result: first message is "lost" and your program then "hangs".


Longer explanation:

Why does waitForMessage not work for a single non-latching message?

From what you show, I don't believe this is caused by waitForMessage(..) not working, but again (as in #q368589) has to do with your expectations.

First, to get it out of the way: there is no central buffer or message queue maintained between publishers and subscribers. Publishers and subscribers have their own internal buffer, but that is not shared with any other nodes, and only exists as long as a node is running.

Consequence of this: messages published before a subscriber is online (or: has a connection with a publisher) will disappear.

Second: waitForMessage(..) does approximately the following (from here and here):

  1. create a NodeHandle (if needed)
  2. subscribe to the topic you specify with the message type you specify
  3. register a callback, which sets a bool to true as soon it has received a message
  4. returns the message received, or times-out

Note: approximately, as the actual implementation is a bit more complicated (uses separate ros::CallbackQueues fi).

From this it should be clear waitForMessage(..) essentially does the same thing as any code which subscribes to a topic.

And it also has the exact same limitations and requirements as any other code subscribing to a topic.

As to your observations:

I publish with the following non-latching publish

#!/usr/bin/env python  
import rospy
import std_msgs

if __name__ == "__main__":
        rospy.init_node('publisher')
        pub = rospy.Publisher('/topic', std_msgs.msg.Int32, queue_size=1, latch=False)
        pub.publish(0)
        rospy.spin()

rostopic echo /topic shows that the message is published correctly. However, the listener program is still stuck at waitForMessage even though the listener program was started before the publishing node. Why is that the case?

There are many, many Q&As here on ROS Answers about this (keywords: "subscriber first message lost"), but summarising: the cause here is most likely that setting up publisher<->subscriber connections takes time. Even though your listener program was started earlier, the code creating the ros::Subscriber in waitForMessage(..) simply didn't have sufficient time to subscribe to /topic before you published the 0.

So waitForMessage(..) subscribes, but then never sees a message (as you only publish a single one), causing it to "get stuck".

Further testing with

#!/usr/bin/env python  
import rospy
import std_msgs

if __name__ == "__main__":
    i = 0
    while not rospy.is_shutdown():
        pub = rospy.Publisher('/topic', std_msgs.msg.Int32, queue_size=1)
        rospy.init_node('publisher')
        pub.publish(i)
        i += 1
        rospy.sleep(10)

shows that it's always only the first message that is ignored. The subsequent messages are correctly caught by waitForMessage

This should now be less of a surprise: while this code goes against best-practices (you should never call rospy.init_node(..) nor rospy.Publisher(..) in a loop), there is simply more time for waitForMessage(..) to create the necessary ... (more)

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-12-29 23:28:52 -0500

Seen: 894 times

Last updated: Dec 30 '20