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

How can I make consecutive waitForMessage wait for the consecutive messages?

asked 2020-12-29 04:18:55 -0500

Rufus gravatar image

updated 2020-12-29 20:50:28 -0500

To my surprise, the following simple program

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

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

    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;
}

Prints 3 0s when I run a single rostopic pub /topic std_msgs/Int32 "data: 0".

One workaround is to add a ros::Duration(1).sleep() between the waits but that doesn't work for my situation.

How can I have waitForMessage actually wait for 3 publishes, provided that I don't know the interval between publishes?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-12-29 04:41:54 -0500

gvdhoorn gravatar image

updated 2020-12-29 04:43:22 -0500

To my surprise, the following simple program: [..] Prints 3 0s when I run a single rostopic pub /topic std_msgs/Int32 "data: 0".

The behaviour you observe is actually as it should be, and the cause is not waitForMessage(..) doing something "wrong", but your expectations are incorrect.

The following is the output of the command you ran:

$ rostopic pub /topic std_msgs/Int32 "data: 0"
publishing and latching message. Press ctrl-C to terminate

Notice the "and latching message" part of what rostopic pub prints.

Latched publishers (wiki/roscpp/Overview/Publishers and Subscribers - Publisher Options) will republish the last published message upon subscription by new subscribers, even if those subscribers were not online when the initial message was published.

So in your case, rostopic pub publishes "the 0" once when it is started, and then subsequently again for every ros::Subscriber created by ros::topic::waitForMessage(..) (as that function creates a new subsciber every time it is run).

Receiving the same message with every waitForMessage(..) seems to be as it should.

One workaround is to add a ros::Duration(1).sleep() between the waits [..]

That would surprise me actually, as that's not how latched publishers work.

How can I have waitForMessage actually wait for 3 publishes, provided that I don't know the interval between publishes?

I wouldn't know, as that's not what that function is for. How would it distinguish between "the same 0" and another message, which happens to also carry a 0?

As a final comment: please note that waitForMessage(..) is not intended to be used to periodically "sample" a message stream, or to implement some sort of rate-limiter (ie: calling waitForMessage(..) repeatedly in a while loop with a ros::Rate or something similar).

It is a fairly costly operation which is typically only used to get some initial state from a topic, or when really only a single message is needed for some reason.

In almost all other cases it would be better to use something like message_filters and/or regular subscribers.

edit flag offensive delete link more

Comments

Further testing shows the sleep workaround doesn't actually work.

Rufus gravatar image Rufus  ( 2020-12-29 23:21:29 -0500 )edit

Curiously, it appears waitForMessage does not work as expected for non-latching messages, even if waitForMessage occurs before the publishing of the non-latching message, I've asked that as a separate question

Rufus gravatar image Rufus  ( 2020-12-29 23:37:31 -0500 )edit

Curiously, it appears waitForMessage does not work as expected for non-latching messages, even if waitForMessage occurs before the publishing of the non-latching message

waitForMessage(..) works fine for "non latched messages".

Perhaps you are expecting things to work in a way they don't.

gvdhoorn gravatar image gvdhoorn  ( 2020-12-30 03:08:19 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2020-12-29 04:18:55 -0500

Seen: 566 times

Last updated: Dec 29 '20