Skip to content

Commit

Permalink
[rostest] Check /clock publication neatly in publishtest (ros#973)
Browse files Browse the repository at this point in the history
* Check /clock publication neatly in publishtest

- Use time.sleep because rospy.sleep(0.1) hangs if /clock is not published
- Add timeout for clock publication

* Add comment explaining use of time.sleep.

* Use logwarn_throttle not to flood the console
  • Loading branch information
wkentaro authored and sputnick1124 committed Jul 30, 2017
1 parent 9676734 commit 3c11f47
Showing 1 changed file with 10 additions and 4 deletions.
14 changes: 10 additions & 4 deletions tools/rostest/nodes/publishtest
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ Author: Kentaro Wada <www.kentaro.wada@gmail.com>
"""

import sys
import time
import unittest

from nose.tools import assert_true
Expand Down Expand Up @@ -107,10 +108,15 @@ class PublishTest(unittest.TestCase):
def test_publish(self):
"""Test topics are published and messages come"""
use_sim_time = rospy.get_param('/use_sim_time', False)
while use_sim_time and (rospy.Time.now() == rospy.Time(0)):
rospy.logwarn('/use_sim_time is specified and rostime is 0, '
'/clock is published?')
rospy.sleep(0.1)
t_start = time.time()
while not rospy.is_shutdown() and \
use_sim_time and (rospy.Time.now() == rospy.Time(0)):
rospy.logwarn_throttle(
1, '/use_sim_time is specified and rostime is 0, /clock is published?')
if time.time() - t_start > 10:
self.fail('Timed out (10s) of /clock publication.')
# must use time.sleep because /clock isn't yet published, so rospy.sleep hangs.
time.sleep(0.1)
# subscribe topics
checkers = []
for topic in self.topics:
Expand Down

0 comments on commit 3c11f47

Please sign in to comment.