|
| 1 | +#!/usr/bin/env python |
| 2 | +############################################################################### |
| 3 | +# Software License Agreement (BSD License) |
| 4 | +# |
| 5 | +# Copyright (c) 2021, Kei Okada |
| 6 | +# All rights reserved. |
| 7 | +# |
| 8 | +# Redistribution and use in source and binary forms, with or without |
| 9 | +# modification, are permitted provided that the following conditions |
| 10 | +# are met: |
| 11 | +# |
| 12 | +# * Redistributions of source code must retain the above copyright |
| 13 | +# notice, this list of conditions and the following disclaimer. |
| 14 | +# * Redistributions in binary form must reproduce the above |
| 15 | +# copyright notice, this list of conditions and the following |
| 16 | +# disclaimer in the documentation and/or other materials provided |
| 17 | +# with the distribution. |
| 18 | +# * Neither the name of Willow Garage, Inc. nor the names of its |
| 19 | +# contributors may be used to endorse or promote products derived |
| 20 | +# from this software without specific prior written permission. |
| 21 | +# |
| 22 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 23 | +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 24 | +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 25 | +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 26 | +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 27 | +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 28 | +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 29 | +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 30 | +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 31 | +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 32 | +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 33 | +# POSSIBILITY OF SUCH DAMAGE. |
| 34 | +############################################################################### |
| 35 | + |
| 36 | +""" |
| 37 | +Integration test node that checks if the specified topics are subscribed. |
| 38 | +below parameters must be set: |
| 39 | +
|
| 40 | +<test name="subscribetest" |
| 41 | + test-name="subscribetest" |
| 42 | + pkg="rostest" type="subscribetest"> |
| 43 | + <rosparam> |
| 44 | + topics: |
| 45 | + - name: a topic name |
| 46 | + timeout: timeout for the topic |
| 47 | + - name: another topic name |
| 48 | + timeout: timeout for the topic |
| 49 | + type: std_msgs/String |
| 50 | + - name: another topic name |
| 51 | + timeout: timeout for the topic |
| 52 | + negative: true |
| 53 | + </rosparam> |
| 54 | +</test> |
| 55 | +
|
| 56 | +Author: Kei OKada <[email protected]> |
| 57 | +""" |
| 58 | + |
| 59 | +import sys |
| 60 | +import time |
| 61 | +import unittest |
| 62 | + |
| 63 | +import rospy |
| 64 | +import rosservice |
| 65 | +import rosgraph |
| 66 | + |
| 67 | +PKG = 'rostest' |
| 68 | +NAME = 'subscribetest' |
| 69 | + |
| 70 | + |
| 71 | +class SubscribeTest(unittest.TestCase): |
| 72 | + def __init__(self, *args): |
| 73 | + super(self.__class__, self).__init__(*args) |
| 74 | + rospy.init_node(NAME) |
| 75 | + # scrape rosparam |
| 76 | + self.topics = {} |
| 77 | + params = rospy.get_param('~topics', []) |
| 78 | + for param in params: |
| 79 | + if 'name' not in param: |
| 80 | + self.fail("'name' field in rosparam is required but not specified.") |
| 81 | + topic = {'timeout': 10, 'type': None, 'negative': False,} |
| 82 | + topic.update(param) |
| 83 | + self.topics[topic['name']] = topic |
| 84 | + self.services = {} |
| 85 | + # check if there is at least one topic |
| 86 | + if not self.topics: |
| 87 | + self.fail('No topic or service is specified in rosparam.') |
| 88 | + |
| 89 | + def setUp(self): |
| 90 | + # warn on /use_sim_time is true |
| 91 | + use_sim_time = rospy.get_param('/use_sim_time', False) |
| 92 | + self.t_start = time.time() |
| 93 | + while not rospy.is_shutdown() and \ |
| 94 | + use_sim_time and (rospy.Time.now() == rospy.Time(0)): |
| 95 | + rospy.logwarn_throttle( |
| 96 | + 1, '/use_sim_time is specified and rostime is 0, /clock is published?') |
| 97 | + if time.time() - t_start > 10: |
| 98 | + self.fail('Timed out (10s) of /clock publication.') |
| 99 | + # must use time.sleep because /clock isn't yet published, so rospy.sleep hangs. |
| 100 | + time.sleep(0.1) |
| 101 | + |
| 102 | + def test_subscribe_topics(self): |
| 103 | + """Test topics are subscribed""" |
| 104 | + def topic_type(t, topic_types): |
| 105 | + matches = [t_type for t_name, t_type in topic_types if t_name == t] |
| 106 | + if matches: |
| 107 | + return matches[0] |
| 108 | + return 'unknown type' |
| 109 | + |
| 110 | + if self.topics: |
| 111 | + t_start = self.t_start |
| 112 | + t_name_set = set(self.topics.keys()) |
| 113 | + t_timeout_max = max(t['timeout'] for t in self.topics.values()) |
| 114 | + finished_topics = [] |
| 115 | + while not rospy.is_shutdown(): |
| 116 | + t_now = time.time() |
| 117 | + t_elapsed = t_now - t_start |
| 118 | + if not t_name_set: |
| 119 | + break |
| 120 | + if t_elapsed > t_timeout_max: |
| 121 | + break |
| 122 | + master = rosgraph.Master('/rostopic') |
| 123 | + topic_types = master.getTopicTypes() |
| 124 | + |
| 125 | + pubs, subs, servs = master.getSystemState() |
| 126 | + for t_name, _ in subs: |
| 127 | + t_type = topic_type(t_name, topic_types) |
| 128 | + if t_name in t_name_set: |
| 129 | + t_name_set.remove(t_name) |
| 130 | + topic = self.topics[t_name] |
| 131 | + assert t_elapsed < topic['timeout'], \ |
| 132 | + 'Topic [%s] is subscribed before timeout [%s] secs' % (topic['name'], topic['timeout']) |
| 133 | + assert not topic['negative'], \ |
| 134 | + 'Topic [%s] is not subscribed' % (topic['name']) |
| 135 | + if topic['type'] is not None: |
| 136 | + assert t_type == topic['type'], \ |
| 137 | + 'Topic type of [%s] is [%s]' % (topic['name'], topic['type']) |
| 138 | + time.sleep(0.05) |
| 139 | + |
| 140 | + for t_name in t_name_set: |
| 141 | + topic = self.topics[t_name] |
| 142 | + assert topic['negative'], \ |
| 143 | + 'Topic [%s] is not subscribed' % (topic['name']) |
| 144 | + |
| 145 | + |
| 146 | +if __name__ == '__main__': |
| 147 | + import rostest |
| 148 | + rostest.run(PKG, NAME, SubscribeTest, sys.argv) |
0 commit comments