v01

thirdparty/ros/ros_comm/tools/rostest/nodes/publishtest

@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+###############################################################################
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2016, Kentaro Wada.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+###############################################################################
+
+"""
+Integration test node that subscribes to any topic and verifies
+there is at least one message publishing of the topic.
+below parameters must be set:
+
+<test name="publishtest"
+      test-name="publishtest"
+      pkg="rostest" type="publishtest">
+  <rosparam>
+    topics:
+      - name: a topic name
+        timeout: timeout for the topic
+      - name: another topic name
+        timeout: timeout for the topic
+  </rosparam>
+</test>
+
+Author: Kentaro Wada <www.kentaro.wada@gmail.com>
+"""
+
+import sys
+import time
+import unittest
+
+from nose.tools import assert_true
+
+import rospy
+import rostopic
+
+
+PKG = 'rostest'
+NAME = 'publishtest'
+
+
+class PublishChecker(object):
+    def __init__(self, topic_name, timeout, negative):
+        self.topic_name = topic_name
+        self.negative = negative
+        self.deadline = rospy.Time.now() + rospy.Duration(timeout)
+        msg_class, _, _ = rostopic.get_topic_class(
+            rospy.resolve_name(topic_name), blocking=True)
+        self.msg = None
+        self.sub = rospy.Subscriber(topic_name, msg_class, self._callback)
+
+    def _callback(self, msg):
+        self.msg = msg
+
+    def assert_published(self):
+        if self.msg:
+            return not self.negative
+        if rospy.Time.now() > self.deadline:
+            return self.negative
+        return None
+
+
+class PublishTest(unittest.TestCase):
+    def __init__(self, *args):
+        super(self.__class__, self).__init__(*args)
+        rospy.init_node(NAME)
+        # scrape rosparam
+        self.topics = []
+        params = rospy.get_param('~topics', [])
+        for param in params:
+            if 'name' not in param:
+                self.fail("'name' field in rosparam is required but not specified.")
+            topic = {'timeout': 10, 'negative': False}
+            topic.update(param)
+            self.topics.append(topic)
+        # check if there is at least one topic
+        if not self.topics:
+            self.fail('No topic is specified in rosparam.')
+
+    def test_publish(self):
+        """Test topics are published and messages come"""
+        use_sim_time = rospy.get_param('/use_sim_time', False)
+        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:
+            topic_name = topic['name']
+            timeout = topic['timeout']
+            negative = topic['negative']
+            print('Waiting [%s] for [%d] seconds with negative [%s]'
+                  % (topic_name, timeout, negative))
+            checkers.append(
+                PublishChecker(topic_name, timeout, negative))
+        deadline = max(checker.deadline for checker in checkers)
+        # assert
+        finished_topics = []
+        while not rospy.is_shutdown():
+            if len(self.topics) == len(finished_topics):
+                break
+            for checker in checkers:
+                if checker.topic_name in finished_topics:
+                    continue  # skip topic testing has finished
+                ret = checker.assert_published()
+                if ret is None:
+                    continue  # skip if there is no test result
+                finished_topics.append(checker.topic_name)
+                if checker.negative:
+                    assert_true(
+                        ret, 'Topic [%s] is published' % (checker.topic_name))
+                else:
+                    assert_true(
+                        ret,
+                        'Topic [%s] is not published' % (checker.topic_name))
+            rospy.sleep(0.01)
+
+
+if __name__ == '__main__':
+    import rostest
+    rostest.run(PKG, NAME, PublishTest, sys.argv)