pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

252
thirdparty/ros/ros_comm/tools/rostest/nodes/hztest vendored Executable file
View File

@@ -0,0 +1,252 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# 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.
#
# Revision $Id$
## Integration test node that subscribes to any topic and verifies
## the publishing rate to be within a specified bounds. The following
## parameters must be set:
##
## * ~/hz: expected hz
## * ~/hzerror: errors bound for hz
## * ~/test_duration: time (in secs) to run test
##
from __future__ import print_function
import sys
import threading
import time
import unittest
import rospy
import rostest
NAME = 'hztest'
from threading import Thread
class HzTest(unittest.TestCase):
def __init__(self, *args):
super(HzTest, self).__init__(*args)
rospy.init_node(NAME)
self.lock = threading.Lock()
self.message_received = False
def setUp(self):
self.errors = []
# Count of all messages received
self.msg_count = 0
# Time of first message received
self.msg_t0 = -1.0
# Time of last message received
self.msg_tn = -1.0
## performs two tests of a node, first with /rostime off, then with /rostime on
def test_hz(self):
# Fetch parameters
try:
# expected publishing rate
hz = float(rospy.get_param('~hz'))
# length of test
test_duration = float(rospy.get_param('~test_duration'))
# topic to test
topic = rospy.get_param('~topic')
# time to wait before
wait_time = rospy.get_param('~wait_time', 20.)
except KeyError as e:
self.fail('hztest not initialized properly. Parameter [%s] not set. debug[%s] debug[%s]'%(str(e), rospy.get_caller_id(), rospy.resolve_name(e.args[0])))
# We only require hzerror if hz is non-zero
hzerror = 0.0
if hz != 0.0:
try:
# margin of error allowed
hzerror = float(rospy.get_param('~hzerror'))
except KeyError as e:
self.fail('hztest not initialized properly. Parameter [%s] not set. debug[%s] debug[%s]'%(str(e), rospy.get_caller_id(), rospy.resolve_name(e.args[0])))
# We optionally check each inter-message interval
try:
self.check_intervals = bool(rospy.get_param('~check_intervals'))
except KeyError:
self.check_intervals = False
# We optionally measure wall clock time
try:
self.wall_clock = bool(rospy.get_param('~wall_clock'))
except KeyError:
self.wall_clock = False
print("""Hz: %s
Hz Error: %s
Topic: %s
Test Duration: %s"""%(hz, hzerror, topic, test_duration))
self._test_hz(hz, hzerror, topic, test_duration, wait_time)
def _test_hz(self, hz, hzerror, topic, test_duration, wait_time):
self.assert_(hz >= 0.0, "bad parameter (hz)")
self.assert_(hzerror >= 0.0, "bad parameter (hzerror)")
self.assert_(test_duration > 0.0, "bad parameter (test_duration)")
self.assert_(len(topic), "bad parameter (topic)")
if hz == 0:
self.min_rate = 0.0
self.max_rate = 0.0
self.min_interval = 0.0
self.max_interval = 0.0
else:
self.min_rate = hz - hzerror
self.max_rate = hz + hzerror
self.min_interval = 1.0 / self.max_rate
if self.min_rate <= 0.0:
self.max_interval = 0.0
else:
self.max_interval = 1.0 / self.min_rate
# Start actual test
sub = rospy.Subscriber(topic, rospy.AnyMsg, self.callback)
self.assert_(not self.errors, "bad initialization state (errors)")
print("Waiting for messages")
# we have to wait until the first message is received before measuring the rate
# as time can advance too much before publisher is up
# - give the test 20 seconds to start, may parameterize this in the future
wallclock_timeout_t = time.time() + wait_time
while not self.message_received and time.time() < wallclock_timeout_t:
time.sleep(0.1)
if hz > 0.:
self.assert_(self.message_received, "no messages before timeout")
else:
self.failIf(self.message_received, "message received")
print("Starting rate measurement")
if self.wall_clock:
timeout_t = time.time() + test_duration
while time.time() < timeout_t:
time.sleep(0.1)
else:
timeout_t = rospy.get_time() + test_duration
while rospy.get_time() < timeout_t:
rospy.sleep(0.1)
print("Done waiting, validating results")
sub.unregister()
# Check that we got at least one message
if hz > 0:
self.assert_(self.msg_count > 0, "no messages received")
else:
self.assertEquals(0, self.msg_count)
# Check whether inter-message intervals were violated (if we were
# checking them)
self.assert_(not self.errors, '\n'.join(self.errors))
# If we have a non-zero rate target, make sure that we hit it on
# average
if hz > 0.0:
self.assert_(self.msg_t0 >= 0.0, "no first message received")
self.assert_(self.msg_tn >= 0.0, "no last message received")
dt = self.msg_tn - self.msg_t0
self.assert_(dt > 0.0, "only one message received")
rate = ( self.msg_count - 1) / dt
self.assert_(rate >= self.min_rate,
"average rate (%.3fHz) exceeded minimum (%.3fHz)" %
(rate, self.min_rate))
self.assert_(rate <= self.max_rate,
"average rate (%.3fHz) exceeded maximum (%.3fHz)" %
(rate, self.max_rate))
def callback(self, msg):
# flag that message has been received
self.message_received = True
try:
self.lock.acquire()
if self.wall_clock:
curr = time.time()
else:
curr_rostime = rospy.get_rostime()
#print "CURR ROSTIME", curr_rostime.to_sec()
if curr_rostime.is_zero():
return
curr = curr_rostime.to_sec()
if self.msg_t0 <= 0.0 or self.msg_t0 > curr:
self.msg_t0 = curr
self.msg_count = 1
last = 0
else:
self.msg_count += 1
last = self.msg_tn
self.msg_tn = curr
# If we're instructed to check each inter-message interval, do
# so
if self.check_intervals and last > 0:
interval = curr - last
if interval < self.min_interval:
print("CURR", str(curr), file=sys.stderr)
print("LAST", str(last), file=sys.stderr)
print("msg_count", str(self.msg_count), file=sys.stderr)
print("msg_tn", str(self.msg_tn), file=sys.stderr)
self.errors.append(
'min_interval exceeded: %s [actual] vs. %s [min]'%\
(interval, self.min_interval))
# If max_interval is <= 0.0, then we have no max
elif self.max_interval > 0.0 and interval > self.max_interval:
self.errors.append(
'max_interval exceeded: %s [actual] vs. %s [max]'%\
(interval, self.max_interval))
finally:
self.lock.release()
if __name__ == '__main__':
# A dirty hack to work around an apparent race condition at startup
# that causes some hztests to fail. Most evident in the tests of
# rosstage.
time.sleep(0.75)
try:
rostest.run('rostest', NAME, HzTest, sys.argv)
except KeyboardInterrupt:
pass
print("exiting")

110
thirdparty/ros/ros_comm/tools/rostest/nodes/paramtest vendored Executable file
View File

@@ -0,0 +1,110 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# 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.
# Original copied from hztest node
# https://github.com/ros/ros_comm/blob/24e45419bdd4b0d588321e3b376650c7a51bf11c/tools/rostest/nodes/hztest
# Integration test node that checks if a designated parameter is already
# registered at the Parameter Server. Following parameters must be set:
#
# * ~/param_name_target: expected parameter name
# * ~/test_duration: time (in secs) to run test
#
from __future__ import print_function
import sys
import threading
import time
import unittest
import rospy
import rostest
CLASSNAME = 'paramtest'
class ParamTest(unittest.TestCase):
def __init__(self, *args):
super(ParamTest, self).__init__(*args)
rospy.init_node(CLASSNAME)
self.lock = threading.Lock()
self.parameter_obtained = False
def setUp(self):
self.errors = []
def test_param(self):
# performs two tests of a node, first with /rostime off,
# then with /rostime on
# Fetch parameters
try:
# Getting the attributes of the test.
testattr_paramname_target = rospy.get_param("~param_name_target")
paramvalue_expected = rospy.get_param("~param_value_expected", None) # This is the expected param value.
# length of test
testattr_duration = float(rospy.get_param("~test_duration", 5))
# time to wait before
wait_time = rospy.get_param("~wait_time", 20)
except KeyError as e:
self.fail("ParamTest not initialized properly. Parameter [%s] not set. Caller ID: [%s] Resolved name: [%s]"%(str(e), rospy.get_caller_id(), rospy.resolve_name(e.args[0])))
print("Parameter: %s Test Duration: %s" % (testattr_paramname_target, testattr_duration))
self._test_param(testattr_paramname_target, testattr_duration, wait_time, paramvalue_expected)
def _test_param(self, testattr_paramname_target, testattr_duration, wait_time, paramvalue_expected=None):
self.assert_(testattr_duration > 0.0, "bad parameter (test_duration)")
self.assert_(len(testattr_paramname_target), "bad parameter (testattr_paramname_target)")
print("Waiting for parameters")
wallclock_timeout_t = time.time() + wait_time
param_obtained = None
while not param_obtained and time.time() < wallclock_timeout_t:
try:
param_obtained = rospy.get_param(testattr_paramname_target)
except KeyError as e:
print('Designated parameter [%s] is not registered yet, will wait. Caller ID: [%s] Resolved name: [%s]'%(testattr_paramname_target, rospy.get_caller_id(), rospy.resolve_name(e.args[0])))
time.sleep(0.1)
if paramvalue_expected:
self.assertEqual(paramvalue_expected, param_obtained)
else:
self.assertIsNotNone(param_obtained)
if __name__ == '__main__':
try:
rostest.run('rostest', CLASSNAME, ParamTest, sys.argv)
except KeyboardInterrupt:
pass
print("{} exiting".format(CLASSNAME))

155
thirdparty/ros/ros_comm/tools/rostest/nodes/publishtest vendored Executable file
View File

@@ -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)