rospy.Publisher.publish() should wait for the publisher to be ready

[ghost]

This issue is a result of this question on answers.ros.org: http://answers.ros.org/question/84336/latchtrue-in-rospypublisher-appears-to-have-no-effect

If a message is published too soon after a publisher is created, the message is lost, even if the publisher is latched. This behavior should be changed in one of the following ways, in order of decreasing preference:

1) Publisher.publish() should wait for the publisher to be registered with the master, then publish the message.

2) A wait_until_ready(timeout) method should be added to Publisher. If timeout is nonzero and the publisher does not become ready before the timeout is exceeded, wait_until_ready() returns False. Otherwise, it returns True.

3) If the publisher is not ready, Publisher.publish() should display a warning.

[tfoote]

Please provide a test case where this fails.

#!/usr/bin/env python                                                           
import rospy
from std_msgs.msg import Float64

rospy.init_node("mynode")
pub = rospy.Publisher("command", Float64, latch=True)
pub.publish(45.0)
rospy.spin()

My tests show this working with rostopic echo whether started before or after the latched publisher.

[ghost]

I did some more testing, and I've found that the problem isn't that no subscriber will receive a message if it's published too soon after the latched publisher is created. The problem is that the gazebo_ros_control Gazebo plug-in will not receive (or will ignore) a message on one of its "command" topics if the message is published too soon after the latched publisher is created. It looks as if this is actually a problem with gazebo_ros_control (or a related package), not ros_comm.

To reproduce the problem:

1) Go to a directory in your ROS path.

2) git clone https://github.com/wunderkammer-laboratory/ackermann_vehicle.git

3) roslaunch ackermann_vehicle_gazebo pub_problem.launch

Note that the shock absorbers will be inactive, i.e. the vehicle's chassis will be below the wheel's axles. If the shocks were active, the chassis would be above the axles. Now, run the following:

rostopic echo /ackermann_vehicle/left_front_shock_ctrlr/command

This will be displayed:

data: 0.042

This shows that the shock absorber command (0.042) is indeed being published. However, the shocks are activated only if there is a significant delay between creating the publisher and publishing the command. For a demonstration, run this:

roslaunch ackermann_vehicle_gazebo ackermann_vehicle.launch

In this example, the shock absorber commands are published every second, activating the shocks.

[dirk-thomas]

What you describe sounds like a common misuse of pub/sub - assuming that all messages in the start-up phase will be delivered.

But if the message which is responsible to active the shock absorbers is published repeatedly it does not make sense that they stay inactive (even if the first couple of messages would be dropped). Usually that is only an issue if you send configuration messages once in the beginning (which are not received by the intended subscribers) and neither send the message again nor use latching.

But I don't know anything about your scenario at all so I can't tell you which topic is the problematic one. You should either ask on answers.ros.org to get better feedback where the issue actually is or file a ticket against gazebo_ros_control if you are pretty sure that that package is responsible.

Anyway you might consider closing this ticket with a reference to either the question or the new issue since it is really unlikely that it is a ros_comm issue.

[ghost]

What you describe sounds like a common misuse of pub/sub - assuming that all messages in the start-up phase will be delivered.

In my previous comment, I wasn't clear that I'm using latched publishers. I have since updated the comment. Tully's comment above implies that if a message is published by a latched publisher, that message should definitely be received by subscribers. Tully, am I understanding your comment correctly?

Are you saying that losing some messages (perhaps just the first few) published to a latched topic is correct behavior? If so, that explains everything. The answers on this subject I've found at answers.ros.org basically say, "Use a latched topic and everything will be fine."

But if the message which is responsible to active the shock absorbers is published repeatedly it does not make sense that they stay inactive

The shocks are inactive only in the pub_problem.launch case, in which a single message is published for each latched publisher (one per shock), immediately after the publisher is created. In the ackermann_vehicle.launch case, messages are published every second, therefore the shocks are activated even if some messages are lost.

[dirk-thomas]

Latching only provides the last message published to new subscriber when they establish the connection. All older messages are gone.

Can you please describe exactly which topic in your case is latched and when / how often you publish on it? Are you still referring to /ackermann_vehicle/left_front_shock_ctrlr/command? Can you reproduce it by just starting a single node and then running rostopic?

[ghost]

I'm closing this issue. I took a look at the ros_controllers code, and it's receiving the messages correctly (btw, all the publishers I've been discussing are latched). The problem is caused by JointPositionController. The joint position from any message received before the first call to update() is overwritten by the joint's current position. Sorry for the false alarm.

[subodh-malgonde]

I have to add a small delay before publishing a geometry_msgs/PoseStamped message. The following code does not work:

import rospy
from geometry_msgs.msg import PoseStamped

rospy.init_node("mynode")

goal_publisher = rospy.Publisher("move_base_simple/goal", PoseStamped, queue_size=5)

goal = PoseStamped()

goal.header.seq = 1
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "map"

goal.pose.position.x = 1.0
goal.pose.position.y = 2.0
goal.pose.position.z = 0.0

goal.pose.orientation.x = 0.0
goal.pose.orientation.y = 0.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 1.0

goal_publisher.publish(goal)

rospy.spin()

Adding a small delay makes it work:

rospy.sleep(1)
goal_publisher.publish(goal)

[dirk-thomas]

@subodh-malgonde please do not comment on a ticket which is closed for 5 years. Your comment won't get any attention. Also your exact question had been answered above.

[ledmonster]

I uses following magic to get publisher. This waits until the number of subscribers assigned to publisher would be same as number of subscribers registered to ros master.

def get_publisher(self, topic_path, msg_type, **kwargs):
    pub = rospy.Publisher(topic_path, msg_type, **kwargs)
    num_subs = len(_get_subscribers(topic_path))
    for i in range(10):
        num_cons = pub.get_num_connections()
        if num_cons == num_subs:
            return pub
        time.sleep(0.1)
    raise RuntimeError("failed to get publisher")

def _get_subscribers(self, topic_path):
    ros_master = rosgraph.Master('/rostopic')
    topic_path = rosgraph.names.script_resolve_name('rostopic', topic_path)
    state = ros_master.getSystemState()
    subs = []
    for sub in state[1]:
        if sub[0] == topic_path:
            subs.extend(sub[1])
    return subs

[Maverobot]

Inspired by the python solution of @ledmonster , I implemented its C++ counterpart. I post it here in case it is helpful for someone in the future.

/**
 * Checks if a stable connection has been established from the given publisher to its
 * subscribers.
 */
bool isReady(const ros::Publisher& pub) {
  XmlRpc::XmlRpcValue req, res, payload;
  req[0] = ros::this_node::getName();
  // For more information, check http://wiki.ros.org/ROS/Master_API
  if (!ros::master::execute("getSystemState", req, res, payload, false)) {
    return false;
  }
  const size_t kSubIndex = 1;
  const auto& subscribers = payload[kSubIndex];
  for (size_t j = 0; j < subscribers.size(); j++) {
    const auto& subscriber = subscribers[j];
    // Finds the topic which this node publishes to
    std::ostringstream topic;
    const size_t kTopicNameIndex = 0;
    const size_t kSubscribersIndex = 1;
    subscriber[kTopicNameIndex].write(topic);
    if (topic.str() == pub.getTopic()) {
      auto real_num_subscribers = subscriber[kSubscribersIndex].size();
      if (pub.getNumSubscribers() == real_num_subscribers) {
        return true;
      } else {
        return false;
      }
    }
  }

 // The publisher is ready to publish since the topic does not exist yet
  return true;
}

[pallgeuer]

Wrapped into a neat class that subclasses rospy.Publisher:

# Connection-aware ROS publisher class
class ROSPublisher(rospy.Publisher):

    def wait_connected(self, poll_rate=10):
        rate = rospy.Rate(poll_rate)
        master = rosgraph.Master(caller_id=rospy.get_name())
        while self.get_num_connections() != sum(len(sub[1]) for sub in master.getSystemState()[1] if sub[0] == self.resolved_name) and not rospy.is_shutdown():
            rate.sleep()