def diag_agg_cb(self, msg):
with self._mutex:
for stat in msg.status:
if stat.name.find(MULTI_NAME) > 0:
self._multi_items[get_header_name(stat.name)] = DiagnosticItem(stat)
def test_multiple_match(self):
while not rospy.is_shutdown():
sleep(1.0)
if rospy.get_time() - self._starttime > DURATION:
break
self.assert_(not rospy.is_shutdown(), "Rospy shutdown!")
with self._mutex:
self.assert_(self._multi_items.has_key(HEADER1), "Didn't have item under %s. Items: %s" % (HEADER1, self._multi_items))
self.assert_(self._multi_items[HEADER1].name == MULTI_NAME, "Item name under %s didn't match %s" % (HEADER1, MULTI_NAME))
self.assert_(self._multi_items.has_key(HEADER2), "Didn't have item under %s" % HEADER2)
self.assert_(self._multi_items[HEADER2].name == MULTI_NAME, "Item name under %s didn't match %s" % (HEADER2, MULTI_NAME))
if __name__ == '__main__':
if False:
suite = unittest.TestSuite()
suite.addTest(TestMultipleMatch('test_multiple_match'))
unittest.TextTestRunner(verbosity = 2).run(suite)
else:
rostest.run(PKG, sys.argv[0], TestMultipleMatch, sys.argv)
assert_false(math.isnan(box.pose.position.x),
'pose.position.x is nan')
assert_false(math.isnan(box.pose.position.y),
'pose.position.y is nan')
assert_false(math.isnan(box.pose.position.z),
'pose.position.z is nan')
assert_false(math.isnan(box.pose.orientation.x),
'pose.orientation.x is nan')
assert_false(math.isnan(box.pose.orientation.y),
'pose.orientation.y is nan')
assert_false(math.isnan(box.pose.orientation.z),
'pose.orientation.z is nan')
assert_false(math.isnan(box.pose.orientation.w),
'pose.orientation.w is nan')
assert_false(math.isnan(box.dimensions.x),
'dimensions.x is nan')
assert_false(math.isnan(box.dimensions.y),
'dimensions.y is nan')
assert_false(math.isnan(box.dimensions.z),
'dimensions.z is nan')
assert_true(box.dimensions.x != 0, 'dimensions.x is 0')
assert_true(box.dimensions.y != 0, 'dimensions.y is 0')
assert_true(box.dimensions.z != 0, 'dimensions.z is 0')
assert_true(
not is_boxes_empty,
'Bboxes array is always empty in %d trials' % self.check_times)
if __name__ == '__main__':
rostest.run(PKG, NAME, TestClusterPointIndicesDecomposerBbox, sys.argv)
rospy.spin()
class TestServiceOrder(unittest.TestCase):
def _test(self, name, srv, req):
rospy.wait_for_service(name, WAIT_TIMEOUT)
s = rospy.ServiceProxy(name, srv)
resp = s.call(req)
self.assert_(resp is not None)
return resp
def test_before(self):
resp = self._test(SERVICE_BEFORE, EmptyReqSrv, EmptyReqSrvRequest())
self.assertEquals(FAKE_SECRET, resp.fake_secret,
"fake_secret fields is not set as expected")
def test_after(self):
resp = self._test(SERVICE_AFTER, EmptyReqSrv, EmptyReqSrvRequest())
self.assertEquals(FAKE_SECRET, resp.fake_secret,
"fake_secret fields is not set as expected")
if __name__ == '__main__':
if '--before' in sys.argv:
service_before()
elif '--after' in sys.argv:
service_after()
else:
rostest.run(PKG, 'rospy_service_decl_order', TestServiceOrder,
sys.argv)
_, _, srv = state
# filter out rosout services
#[['/rosout/set_logger_level', ['/rosout']], ['/rosout/get_loggers', ['/rosout']]]
srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')]
self.failIf(srv, srv)
print("Creating service ", SERVICE)
service = rospy.Service(SERVICE, EmptySrv, callback)
# we currently cannot interrogate a node's services, have to rely on master
# verify that master has service
state = master.getSystemState()
_, _, srv = state
srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')]
self.assertEquals(srv, [[rospy.resolve_name(SERVICE), [rospy.get_caller_id()]]])
# begin actual test by unsubscribing
service.shutdown()
time.sleep(1.0) # give API 1 second to sync with master
state = master.getSystemState()
_, _, srv = state
srv = [s for s in srv if not s[0].startswith('/rosout/') and not s[0].endswith('/get_loggers') and not s[0].endswith('/set_logger_level')]
self.failIf(srv, srv)
if __name__ == '__main__':
rospy.init_node('test_dereg', disable_rostime=True)
rostest.run(PKG, 'rospy_deregister', TestDeregister, sys.argv)
# Test that rosout is outputting fatal messages as expected
def test_rosout_fatal(self):
self.assertIsNone(self.callback_data, 'invalid test fixture')
log_msg = MSG_PREFIX + 'logging test fatal message'
rospy.logfatal(log_msg)
# wait for log messages to be received
timeout_time = time.time() + TIMEOUT
while not self.callback_data and time.time() < timeout_time:
time.sleep(0.1)
print(self.callback_data)
# checking number of messages received on /rosout is expected
self.assertIsNotNone(self.callback_data,
'did not receive expected message')
# checking contents of message
self.assertEquals(rospy.FATAL, self.callback_data.level)
self.assertEquals(SUBTOPIC, self.callback_data.name)
self.assertEquals(log_msg, self.callback_data.msg)
self.assertEquals(NAME + '.py', self.callback_data.file)
self.assertEquals('TestRosout.test_rosout_fatal',
self.callback_data.function)
self.assertEquals([SUBTOPIC], self.callback_data.topics)
if __name__ == '__main__':
rospy.init_node(NAME, log_level=rospy.DEBUG)
rostest.run(PKG, NAME, TestRosout, sys.argv)
# msg_types should be unaffected by the filter
self.assertEquals(len(msg_types), 2)
self.assertTrue("std_msgs/Int32" in msg_types)
self.assertTrue("std_msgs/String" in msg_types)
# topics should be empty
self.assertEquals(len(topics), 0)
def _print_bag_records(self, fn):
with open(fn) as f:
f.seek(0, os.SEEK_END)
size = f.tell()
f.seek(0)
version_line = f.readline().rstrip()
print(version_line)
while f.tell() < size:
header = bag._read_header(f)
op = bag._read_uint8_field(header, 'op')
data = bag._read_record_data(f)
print(bag._OP_CODES.get(op, op))
if __name__ == '__main__':
import rostest
PKG = 'rosbag'
rostest.run(PKG, 'TestRosbag', TestRosbag, sys.argv)
tmpdqi = quaternion_from_rotation_matrix(linalg.inv(tmpdri))
delta = quaternion_multiply(tmpdqi, odom_q_delta)
delta_euler = euler_from_quaternion(delta)
delta_yaw = delta_euler[2]
print "delta error:", euler_from_quaternion(delta)
print "------------------------"
# check odom error (odom error from ground truth)
odom_error = abs(self.odom_x - self.p3d_x - self.odom_xi + self.p3d_xi ) \
+ abs(self.odom_y - self.p3d_y - self.odom_yi + self.p3d_yi ) \
+ abs(delta_yaw)
# check total error (difference between ground truth and target)
current_euler_angles = euler_from_quaternion(self.p3d_q)
current_yaw = current_euler_angles[2]
nav_error = abs(self.p3d_x - TARGET_X) \
+ abs(self.p3d_y - TARGET_Y) \
+ abs(current_yaw - TARGET_T)
print "nav error:" + str(nav_error) + " tol:" + str(
TARGET_TOL) + " odom error:" + str(odom_error)
if nav_error < TARGET_TOL and self.bumped == False:
self.success = True
self.assert_(self.success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], NavStackTest, sys.argv) #, text_mode=True)
"Duration should be (%.2f) and was instead (%.2f)"%
(1325.0, testlog.duration()))
testlog.reset_all()
start_time = time.time()
testlog.record(1.0)
time.sleep(0.5)
testlog.record(2.0)
time.sleep(0.5)
testlog.record(3.0)
end_time = time.time()
self.assertTrue(math.fabs(end_time - testlog.max_time()) < 0.1,
"Max time should be (%.2f) and was instead (%.2f)"%
(end_time, testlog.max_time()))
self.assertTrue(math.fabs(end_time - testlog.max_time()) < 0.1,
"Min time should be (%.2f) and was instead (%.2f)"%
(end_time, testlog.min_time()))
self.assertTrue(math.fabs((end_time - start_time) - testlog.duration()) < 0.15,
"Duration should be (%.2f) and was instead (%.2f)"%
((end_time - start_time), testlog.duration()))
if __name__ == '__main__':
try:
rostest.run('network_monitor_udp', 'blahblah', MetricLogTest)
except KeyboardInterrupt, e:
pass
sub_agg = rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self.diag_agg_cb)
def diag_agg_cb(self, msg):
with self._mutex:
for stat in msg.status:
if stat.name.find(self._ns) > 0:
self._item = stat
break
def test_fail_init(self):
while not rospy.is_shutdown():
sleep(1.0)
if rospy.get_time() - self._starttime > DURATION:
break
self.assert_(not rospy.is_shutdown(), "Rospy shutdown!")
with self._mutex:
self.assert_(self._ns, "Namespace is none. Option --ns not given")
self.assert_(self._item, "No item with name %s found in diag_agg" % self._ns)
self.assert_(self._item.level == 3, "Item failed to initialize, but was not stale. Level: %d" % self._item.level)
if __name__ == '__main__':
if False:
suite = unittest.TestSuite()
suite.addTest(TestFailInit('test_fail_init'))
unittest.TextTestRunner(verbosity = 2).run(suite)
else:
rostest.run(PKG, sys.argv[0], TestFailInit, sys.argv)
"""
import imp, sys, os
# set path to hrpsys to get import rpy, see <hrpsys>/test/test-jointangle.py for using HrpsysConfigurator
try:
imp.find_module('hrpsys') # catkin installed
sys.path.append(imp.find_module('hrpsys')[1])
except: # rosbuild installed
import rospkg
rp = rospkg.RosPack()
sys.path.append(rp.get_path('hrpsys')+'/lib/python2.7/dist-packages/hrpsys')
sys.path.append(rp.get_path('openrtm_aist_python')+'/lib/python2.7/dist-packages')
sys.path.append(os.path.dirname(os.path.abspath(__file__))+'/../share/hrpsys/samples/PA10/') # set path to PA10
#os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:{0}:{1}/NameService'.format('localhost','2809')
import PA10
PA10.rtm.initCORBA()
PA10.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s] :
import unittest, rostest
rostest.run('hrpsys', 'pa10_sample', unittest.TestCase, sys.argv)
if os.path.exists(os.path.join(openhrp3_path, "bin")):
PKG_CONFIG_PATH = "PKG_CONFIG_PATH=%s/lib/pkgconfig:$PKG_CONFIG_PATH" % (openhrp3_path)
cmd = "%s pkg-config openhrp3.1 --variable=idl_dir" % (PKG_CONFIG_PATH)
os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample")
h.loadPattern(
os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample"), 1
)
self.assertEqual(h.waitInterpolation(), None)
def test_set_joint_angles(self):
h = SampleRobot()
h.findComps()
self.assertTrue(h.setJointAngles(h.getJointAngles(), 1))
self.assertEqual(h.waitInterpolation(), None)
import random
a = [(360 * random.random() - 180) for i in xrange(len(h.getJointAngles()))]
self.assertTrue(h.setJointAngles(a, 2))
self.assertEqual(h.waitInterpolation(), None)
a = [0 for i in xrange(len(h.getJointAngles()))]
self.assertTrue(h.setJointAngles(a, 2))
self.assertEqual(h.waitInterpolation(), None)
# unittest.main()
if __name__ == "__main__":
rostest.run(PKG, NAME, TestJointAngle, sys.argv)
class HardwareTest(unittest.TestCase):
def __init__(self, *args):
super(HardwareTest, self).__init__(*args)
rospy.init_node('test_hardware_test')
torso_joint_states = []
self.message_received = False
self.sss = simple_script_server()
def test_play(self):
dialog_client(0, 'Listen up for the Sound' )
rospy.set_param("script_server/sound/language","de")
handle = self.sss.play("grasp_tutorial_01")
self.assertEqual(handle.get_state(), 3)
self.assertTrue(dialog_client(1, 'Did you hear File?'))
def test_say(self):
dialog_client(0, 'Listen up for the Sound' )
handle = self.sss.say(["Hello"])
self.assertEqual(handle.get_state(), 3)
self.assertTrue(dialog_client(1, 'Did you hear Hello?'))
if __name__ == '__main__':
try:
rostest.run('rostest', 'test_hardware_test', HardwareTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
(-0.454364, 0.004407, 0.442485)),
((-0.187758, -0.878333, -0.437030, 0.047787),
( 0.475119, 0.474862, 0.425745)),
(( 0.055407, 0.425327, -0.664491, 0.611946),
( 0.138564, -0.309946, 0.355384)),
((-0.502542, 0.403851, 0.619655, 0.447642),
(-0.206100, 0.433057, 0.910329)),
(( 0.087219, 0.274837, 0.401357, 0.869350),
( 0.097688, -0.199488, 0.848627))]
for joint, result in zip(joints, results):
ja = kdl.JntArray(7)
for i in range(7):
ja[i] = joint[i]
expected_result = kdl.Frame(kdl.Rotation.Quaternion(*result[0]),
kdl.Vector(*result[1]))
actual_result = kdl.Frame()
err = self.robot.kin_fwd.JntToCart(ja, actual_result)
self.assertEqual(err, 0)
self.assertTrue(kdl.Equal(expected_result, actual_result, 0.00001))
if __name__ == '__main__':
rostest.run('robot_test', 'robot_test',
TestRobot, sys.argv)
error_msg = 'Could not initialize sdh'
self.fail(error_msg)
#aks user
self.assertTrue(dialog_client(0, self.user_message))
# init component
sub = rospy.Subscriber(self.topic, schunk_sdh.msg.TactileSensor , self.cb_state)
abort_time = time.time() + self.user_time
while (not self.message_received) and time.time() < abort_time:
time.sleep(0.1)
if not self.message_received:
self.fail('No valid state message received within %s seconds' % (self.user_time))
# callback functions
def cb_state(self, msg):
# add all values of the 6 sensor matrices to a list
for i in range(len(msg.tactile_matrix)):
self.actual_values = self.actual_values + list((msg.tactile_matrix[i].tactile_array))
#check for desired values
#print 'values: %s' % str(self.actual_values)
if ((max(self.actual_values)) > self.min_tactile_value):
self.message_received = True
if __name__ == '__main__':
try:
rostest.run('rostest', 'tactile_test', UnitTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
while (pin_state):
if messages >= maximum_messages:
self.fail(
"Could not read desired state after {} messages.".format(
maximum_messages))
io_state = rospy.wait_for_message(
'/ur_hardware_interface/io_states', IOStates)
pin_state = io_state.digital_out_states[pin].state
messages += 1
self.assertEqual(pin_state, 0)
self.service_client(SetIORequest.FUN_SET_DIGITAL_OUT, pin, 1)
messages = 0
pin_state = False
while (not pin_state):
if messages >= maximum_messages:
self.fail(
"Could not read desired state after {} messages.".format(
maximum_messages))
io_state = rospy.wait_for_message(
'/ur_hardware_interface/io_states', IOStates)
pin_state = io_state.digital_out_states[pin].state
messages += 1
self.assertEqual(pin_state, 1)
if __name__ == '__main__':
import rostest
rostest.run(PKG, NAME, IOTest, sys.argv)
time.sleep(0.1)
# listenerpublisher is supposed to repeat our messages back onto /listenerpublisher,
# make sure we got it
self.assert_(self.callback_data is not None, "no callback data from listenerpublisher")
print "Got ", self.callback_data.time
errorstr = "callback msg field [%s] from listenerpublisher does not match"
self.assertEquals(2.0, self.callback_data.time, errorstr % "time")
self.assertEquals(2, len(self.callback_data.joint_states))
self.assertEquals(2, len(self.callback_data.actuator_states))
self.assertEquals("name0", self.callback_data.joint_states[0].name)
self.assertEquals("name0", self.callback_data.actuator_states[0].name)
self.assertEquals("name1", self.callback_data.joint_states[1].name)
self.assertEquals("name1", self.callback_data.actuator_states[1].name)
self.assertEquals(0, self.callback_data.actuator_states[0].encoder_count)
self.assertEquals(0, self.callback_data.actuator_states[0].calibration_reading)
self.assertEquals(0, self.callback_data.actuator_states[0].last_calibration_high_transition)
self.assertEquals(0, self.callback_data.actuator_states[0].num_encoder_errors)
self.assertEquals(1, self.callback_data.actuator_states[1].encoder_count)
self.assertEquals(1, self.callback_data.actuator_states[1].calibration_reading)
self.assertEquals(1, self.callback_data.actuator_states[1].last_calibration_high_transition)
self.assertEquals(1, self.callback_data.actuator_states[1].num_encoder_errors)
if __name__ == "__main__":
rospy.ready(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
# + " y:" + str(self.odom_y - self.p3d_y) \
# + " e:" + str(error.x) + "," + str(error.y) + "," + str(error.z) \
# + " t_odom:" + str(self.odom_e.x) + "," + str(self.odom_e.y) + "," + str(self.odom_e.z) \
# + " t_p3d:" + str(self.p3d_e.x) + "," + str(self.p3d_e.y) + "," + str(self.p3d_e.z)
# check total error
total_error = abs(self.odom_x -
self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(
error.x) + abs(error.y) + abs(error.z)
# print "total error:" + str(total_error) + " tol:" + str(TARGET_TOL)
total_dist = math.sqrt(self.p3d_x * self.p3d_x +
self.p3d_y * self.p3d_y +
self.p3d_t * self.p3d_t)
if total_error / total_dist < TARGET_TOL:
self.success = True
if not self.success:
rospy.logerr(
"Testing pr2 base odometry control against simulated ground truth with target (vx,vy) = (0.5,0.5), but odom data does not match gound truth from simulation. Total deviation in position is %f percent over a distance of %f meters."
% (total_error / total_dist, total_dist))
else:
rospy.loginfo(
"Testing pr2 base odometry control against simulated ground truth with target (vx,vy) = (0.5,0.5), total deviation in position is %f percent over a distance of %f meters."
% (total_error / total_dist, total_dist))
self.assert_(self.success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], XYW_GT, sys.argv) #, text_mode=True)
#!/usr/bin/env python
import imp, sys, os, time
# set path to hrpsys to use HrpsysConfigurator
from subprocess import check_output
sys.path.append(os.path.join(check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip(),'share/hrpsys/samples/SampleRobot/')) # set path to SampleRobot
import samplerobot_sequence_player
import unittest, rostest
class TestSampleRobotSequencePlayer(unittest.TestCase):
def test_demo (self):
samplerobot_sequence_player.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s] :
rostest.run('hrpsys', 'samplerobot_sequence_player', TestSampleRobotSequencePlayer, sys.argv)
self.fail(error_msg)
else:
print >> sys.stderr, "Object in/on '", pos_name, "'"
def calc_dist(self, des_pos, act_pos):
# function to calculate distance between actual and desired object position
distance = sqrt((des_pos.x - act_pos.x) ** 2 + (des_pos.y - act_pos.y) ** 2 + (des_pos.z - act_pos.z) ** 2)
return distance
def trans_into_arm_7_link(self, coord_world):
# function to transform given coordinates into arm_7_link coordinates
coord_arm_7_link = PointStamped()
coord_arm_7_link.header.stamp = rospy.Time.now()
coord_arm_7_link.header.frame_id = "/map"
coord_arm_7_link.point = coord_world
self.sss.sleep(2) # wait for transform to be calculated
if not self.sss.parse:
coord_arm_7_link = self.listener.transformPoint("/arm_7_link", coord_arm_7_link)
# transform grasp point to sdh center
# coord_arm_7_link.point.z = coord_arm_7_link.point.z - 0.2
return coord_arm_7_link
if __name__ == "__main__":
try:
rostest.run("rostest", "grasp_test", UnitTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
PKG = 'hrpsys'
NAME = 'test-colcheck'
import sys
print "running test with PYTHONPATH=%s" % ";".join(sys.path)
import imp ## for rosbuild
try:
imp.find_module(PKG)
except:
import roslib; roslib.load_manifest(PKG)
from hrpsys import hrpsys_config
import socket
from hrpsys import rtm
import unittest
class TestHrpsysColcheck(unittest.TestCase):
def test_dummy(self):
pass
if __name__ == '__main__':
try:
import rostest
rostest.run(PKG, NAME, TestHrpsysColcheck, sys.argv)
except ImportError:
unittest.main()
def setUp(self):
self.grasped_object_sub = rospy.Subscriber(
'/art/pr2/left_arm/grasped_object', String, self.state_cb)
def test_grasping(self):
left_gr = rospy.wait_for_message('/art/pr2/left_arm/grasped_object',
String)
rgiht_gr = rospy.wait_for_message('/art/pr2/left_arm/grasped_object',
String)
self.assertEquals(left_gr.data, "", "test_left_arm_grasped_object")
self.assertEquals(right_gr.data, "", "test_right_arm_grasped_object")
left_client = actionlib.SimpleActionClient(
'/art/pr2/left_arm/pp', art_msgs.msg.pickplaceAction)
left_client.wait_for_server()
right_client = actionlib.SimpleActionClient(
'/art/pr2/right_arm/pp', art_msgs.msg.pickplaceAction)
right_client.wait_for_server()
# TODO simulate some object and try to grasp / place it
if __name__ == '__main__':
rospy.init_node('test_grasping_node')
rostest.run('art_pr2_grasping', 'test_grasping', TestGrasping, sys.argv)
rtm.nsport = nsport
rtm.initCORBA()
ms = rtm.findRTCmanager()
rh = rtm.findRTC("RobotHardware0")
self.assertTrue(ms and rh)
except Exception as e:
print "{0}, RTCmanager={1}, RTC(RobotHardware0)={2}".format(str(e),ms,rh)
self.fail()
pass
def test_gethostname(self):
self.check_initCORBA(socket.gethostname())
def test_localhost(self):
self.check_initCORBA('localhost')
def test_127_0_0_1(self):
self.check_initCORBA('127.0.0.1')
def test_None(self):
self.check_initCORBA(None)
@unittest.expectedFailure
def test_X_unknown(self):
self.check_initCORBA('unknown')
@unittest.expectedFailure
def test_X_123_45_67_89(self):
self.check_initCORBA('123.45.67.89')
#unittest.main()
if __name__ == '__main__':
rostest.run(PKG, NAME, TestHrpsysHostname, sys.argv)
for name, value in params.items():
if not re.match('^topic_\d$', name):
continue
self.topics.append(value)
id = name.replace('topic_', '')
self.timeouts.append(rospy.get_param('~timeout_{}'.format(id)))
self.negatives.append(
rospy.get_param('~negative_{}'.format(id), False))
if not self.topics:
rospy.logerr('No topic is specified.')
sys.exit(1)
def test_published(self):
"""Test topics are published and messages come"""
checkers = []
for topic_name, timeout in zip(self.topics, self.timeouts):
checker = TopicPublishedChecker(topic_name, timeout)
checkers.append(checker)
for negative, checker in zip(self.negatives, checkers):
if negative:
msg = '{} is published'.format(checker.topic_name)
assert_false(checker.check(), msg)
else:
msg = '{} is not published'.format(checker.topic_name)
assert_true(checker.check(), msg)
if __name__ == '__main__':
import rostest
rostest.run(PKG, NAME, TestTopicPublished, sys.argv)
print command_traj
# get last point out of trajectory
traj_endpoint = command_traj[len(command_traj) - 1]
print traj_endpoint
actual_pos = self.actual_pos # fix current position configuration for later evaluation
# checking if target position is really reached
print "actual_pos = ", actual_pos
print "traj_endpoint = ", traj_endpoint
for i in range(len(traj_endpoint)):
self.assert_(((math.fabs(traj_endpoint[i] - actual_pos[i])) < self.error_range), "Target position out of error_range")
self.assertTrue(dialog_client(1, 'Did %s move to %s ?' % (self.component, target)))
# callback functions
def cb_state(self, msg):
self.actual_pos = msg.actual.positions
self.message_received = True
if __name__ == '__main__':
try:
rostest.run('rostest', 'component_test', UnitTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
if self.reached_target_base:
print " base duration: " + str(time.time() -
self.duration_start_base)
if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
if time.time() - self.duration_start_base > TARGET_DURATION:
self.base_success = True
else:
# failed to maintain target vw, reset duration
self.base_success = False
self.reached_target_base = False
else:
if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
print 'success base'
self.reached_target_base = True
self.duration_start_base = time.time()
def test_arm(self):
print "LNK\n"
rospy.Subscriber("/base_pose_ground_truth", Odometry,
self.baseP3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_TIMEOUT
while not rospy.is_shutdown(
) and not self.base_success and time.time() < timeout_t:
time.sleep(1.0)
self.assert_(self.base_success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], PoseTest, sys.argv) #, text_mode=True)
head_goal.target.header.frame_id = 'base_link'
head_goal.target.point = Point(10, 0, 0.55)
projector_pub = rospy.Publisher(
"/projector_wg6802418_controller/projector", Int32, latch=True)
projector_pub.publish(Int32(0))
print " subscribe stereo left image from ROS "
rospy.Subscriber(
"/narrow_stereo/left/image_raw", image_msg, self.imageInput
) # this is a camera mounted on PR2 head (left stereo camera)
rospy.Subscriber(
"/narrow_stereo/left/camera_info", camerainfo_msg,
self.camerainfoInput
) # this is a camera mounted on PR2 head (left stereo camera)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown(
) and not self.success and time.time() < timeout_t:
head_goal_status = send_head_goal(head_goal)
time.sleep(1.0)
self.assert_(self.success)
if __name__ == '__main__':
#while (os.path.isfile(FRAME_DIR + "/" + FRAME_TARGET)):
# print "Old test case still alive."
# time.sleep(1)
print " starting test "
rostest.run(PKG, sys.argv[0], TestCameras, sys.argv)
$ roslaunch hrpsys samplerobot.launch CONF_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.500.el.conf
$ rosrun hrpsys samplerobot-soft-error-limiter.py
"""
import imp, sys, os
# set path to hrpsys to use HrpsysConfigurator
try:
imp.find_module("hrpsys") # catkin installed
sys.path.append(imp.find_module("hrpsys")[1])
except: # rosbuild installed
import roslib
roslib.load_manifest("hrpsys")
sys.path.append(
os.path.dirname(os.path.abspath(__file__)) + "/../share/hrpsys/samples/SampleRobot/"
) # set path to SampleRobot
import samplerobot_soft_error_limiter
if __name__ == "__main__":
samplerobot_soft_error_limiter.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s]:
import unittest, rostest
rostest.run("hrpsys", "samplerobot_soft_error_limiter", unittest.TestCase, sys.argv)
import unittest, os, sys
import subprocess
from subprocess import call, check_output, Popen, PIPE, STDOUT
class TestOpenrtmTools(unittest.TestCase):
def test_rtshell_setup(self):
pkg_path = check_output("rospack find openrtm_tools",
shell=True).rstrip()
script = os.path.join(pkg_path, "scripts/rtshell-setup.sh")
self.assertTrue(os.path.exists(script))
try:
check_output(
"bash -c 'source %s; RTCTREE_NAMESERVERS=localhost:2809 rtls'"
% (script),
shell=True,
stderr=subprocess.STDOUT)
except subprocess.CalledProcessError, (e):
self.assertTrue(
False,
'subprocess.CalledProcessError: cmd:%s returncode:%s output:%s'
% (e.cmd, e.returncode, e.output))
self.assertTrue(True) # ok rtls works
#unittest.main()
if __name__ == '__main__':
import rostest
rostest.run(PKG, NAME, TestOpenrtmTools, sys.argv)
msg_types, topics = bag.get_type_and_topic_info("/none")
# msg_types should be unaffected by the filter
self.assertEquals(len(msg_types), 2)
self.assertTrue("std_msgs/Int32" in msg_types)
self.assertTrue("std_msgs/String" in msg_types)
# topics should be empty
self.assertEquals(len(topics), 0)
def _print_bag_records(self, fn):
with open(fn) as f:
f.seek(0, os.SEEK_END)
size = f.tell()
f.seek(0)
version_line = f.readline().rstrip()
print(version_line)
while f.tell() < size:
header = bag._read_header(f)
op = bag._read_uint8_field(header, 'op')
data = bag._read_record_data(f)
print(bag._OP_CODES.get(op, op))
if __name__ == '__main__':
import rostest
PKG='rosbag'
rostest.run(PKG, 'TestRosbag', TestRosbag, sys.argv)
if (d < TARGET_RAD and abs(dz) < CUP_HEIGHT):
self.hits = self.hits + 1
if (self.runs > 10 and self.runs < 50):
print "Got to goal too quickly! (",self.runs,")"
self.success = False
self.fail = True
if (self.hits > MIN_HITS):
if (self.runs > MIN_RUNS):
self.success = True
def test_slide(self):
print "LINK\n"
rospy.Subscriber("/base_pose_ground_truth", Odometry, self.positionInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
time.sleep(0.1)
time.sleep(2.0)
self.assert_(self.success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], TestSlide, sys.argv) #, text_mode=True)
import sys
import unittest
import rostest
import rospkg
import roslaunch.arg_dump
class TestRoslaunchRosArgs(unittest.TestCase):
def setUp(self):
self.vals = set()
self.msgs = {}
def test_roslaunch(self):
rospack = rospkg.RosPack()
filename = os.path.join(rospack.get_path('test_roslaunch'), 'test', 'ros_args.launch')
args = roslaunch.arg_dump.get_args([filename])
expected_args = {
'foo': ("I pity the foo'.", 'true'),
'bar': ("Someone walks into this.", None,),
'baz': (None, 'false'),
'nop': (None, None)}
self.assertEqual(args, expected_args)
if __name__ == '__main__':
rostest.run(PKG, NAME, TestRoslaunchRosArgs, sys.argv)
#!/usr/bin/env python
import imp, sys, os, time
# set path to hrpsys to use HrpsysConfigurator
from subprocess import check_output
sys.path.append(
os.path.join(
check_output(['pkg-config', 'hrpsys-base',
'--variable=prefix']).rstrip(),
'share/hrpsys/samples/SampleRobot/')) # set path to SampleRobot
import samplerobot_stabilizer
if __name__ == '__main__':
samplerobot_stabilizer.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s]:
import unittest, rostest
rostest.run('hrpsys', 'samplerobot_stabilizer', unittest.TestCase,
sys.argv)
# make sure we got it
self.assert_(len(self.callback_data),
"no callback data from %s" % PUBNODE)
for d in self.callback_data:
errorstr = "callback msg field [%%s] from %s does not match" % PUBNODE
self.assertAlmostEqual(3.14, d.time, 2, errorstr % "time")
self.assertEquals(250, len(d.joint_states))
self.assertEquals(260, len(d.actuator_states))
for f in d.joint_states:
self.assertEquals("jointstate", f.name)
self.assertAlmostEqual(1.0, f.position, 1)
self.assertAlmostEqual(1.0, f.velocity, 1)
self.assertAlmostEqual(1.0, f.applied_effort, 1)
self.assertAlmostEqual(1.0, f.commanded_effort, 1)
for i in xrange(0, len(d.actuator_states)):
f = d.actuator_states[i]
self.assertEquals("actuatorstate", f.name)
self.assertEquals(1.0, f.motor_voltage)
self.assertEquals(i, f.encoder_count)
self.assertEquals(0, f.calibration_reading)
self.assertEquals(i + 1, f.last_calibration_low_transition)
self.assertEquals(i + 2, f.last_calibration_high_transition)
self.assertEquals(0, f.num_encoder_errors)
if __name__ == '__main__':
rospy.ready(NAME)
rostest.run(PKG, NAME, TestMechanismState, sys.argv)
def test_result(self):
self.standup_msgs = 0
self.result_ready = False
self.result = False
self.pub = rospy.Subscriber("/atlas/atlas_state", AtlasState,
self.callback)
while not self.result_ready:
time.sleep(0.1)
pass
self.assertTrue(
self.result,
"Robot is not stand up (orientation not close to (0,0,0,1)")
if __name__ == '__main__':
rospy.init_node('orientation_checker', anonymous=True)
# Wait until /clock is being published; this can take an unpredictable
# amount of time when we're downloading models.
while rospy.Time.now().to_sec() == 0.0:
print('Waiting for Gazebo to start...')
time.sleep(1.0)
time.sleep(10.0)
rostest.run('drcsim_gazebo', 'orientation_checker', OrientationChecker,
sys.argv)
self.assertEquals(ConstantsMultiplexResponse.CONFIRM_X,
resp.select_confirm)
self.assertEquals(Req.BYTE_X, resp.ret_byte)
self.assertEquals(Req.INT32_X, resp.ret_int32)
self.assertEquals(Req.UINT32_X, resp.ret_uint32)
self.assert_(math.fabs(Req.FLOAT32_X - resp.ret_float32) < 0.001)
resp = self._test(name, Cls,
ConstantsMultiplexRequest(Req.SELECT_Y))
self.assertEquals(ConstantsMultiplexResponse.CONFIRM_Y,
resp.select_confirm)
self.assertEquals(Req.BYTE_Y, resp.ret_byte)
self.assertEquals(Req.INT32_Y, resp.ret_int32)
self.assertEquals(Req.UINT32_Y, resp.ret_uint32)
self.assert_(math.fabs(Req.FLOAT32_Y - resp.ret_float32) < 0.001)
resp = self._test(name, Cls,
ConstantsMultiplexRequest(Req.SELECT_Z))
self.assertEquals(ConstantsMultiplexResponse.CONFIRM_Z,
resp.select_confirm)
self.assertEquals(Req.BYTE_Z, resp.ret_byte)
self.assertEquals(Req.INT32_Z, resp.ret_int32)
self.assertEquals(Req.UINT32_Z, resp.ret_uint32)
self.assert_(math.fabs(Req.FLOAT32_Z - resp.ret_float32) < 0.001)
if __name__ == '__main__':
if '--service' in sys.argv:
services()
else:
rostest.run(PKG, 'rospy_basic_services', TestBasicServicesClient, sys.argv)
class X_GT(BaseTest):
def __init__(self, *args):
super(X_GT, self).__init__(*args)
def test_base(self):
self.init_ros(NAME)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
# do not start commanding base until p3d and odom are initialized
if self.p3d_initialized == True and self.odom_initialized == True:
self.pub.publish(Twist(Vector3(TARGET_VX, TARGET_VY, 0), Vector3(0, 0, TARGET_VW)))
time.sleep(0.1)
# self.debug_pos()
# display what the odom error is
print " error " + " x: " + str(self.odom_x - self.p3d_x) + " y: " + str(
self.odom_y - self.p3d_y
) + " t: " + str(self.odom_t - self.p3d_t)
# check total error
total_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(self.odom_t - self.p3d_t)
if total_error < TARGET_TOL:
self.success = True
self.assert_(self.success)
if __name__ == "__main__":
rostest.run(PKG, sys.argv[0], X_GT, sys.argv) # , text_mode=True)
- 1]
# Start evaluation
timeout_t = traj_endpoint.time_from_start.to_sec(
) * 0.5 # movement should already be finished, but let wait with an additional buffer of 50% times the desired time
rospy.sleep(timeout_t)
print "Done waiting, validating results"
actual_pos = self.actual_pos # fix current position configuration for later evaluation
# checking if target position is realy reached
for i in range(len(traj_endpoint.positions)):
self.assert_(
((traj_endpoint.positions[i] - actual_pos[i]) < error_range),
"Target position out of error_range")
# callback functions
def cb_state(self, msg):
self.message_received = True
self.actual_pos = msg.actual.positions
def cb_command(self, msg):
self.command_traj = msg
if __name__ == '__main__':
try:
rostest.run('rostest', 'component_test', UnitTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
def test_monitor(self):
while not rospy.is_shutdown():
sleep(0.1)
self.publish_mech_stats()
if rospy.get_time() - self._start_time > DURATION:
break
rospy.loginfo('Sent some normal data')
with self._mutex:
self.assert_(self._message, "No data from test monitor")
self.assert_(self._ok, "Not OK after sending only good data. Message: %s" % self._message)
for i in range(0, 10): # 10 consecutive errors
self.publish_mech_stats(self._flex_error, self._roll_error)
sleep(0.1)
for i in range(0, 10): # Some OK values
self.publish_mech_stats()
sleep(0.1)
with self._mutex:
self.assert_(not rospy.is_shutdown(), "Rospy shutdown")
self.assert_(self._message, "No data from test monitor")
self.assert_(self._level == 2, "Transmission listener didn't report error. Level: %d. Message: %s" % (self._level, self._message))
self.assert_(not self._ok, "Transmission listener didn't call halt motors!")
if __name__ == '__main__':
print 'SYS ARGS:', sys.argv
rostest.run(PKG, sys.argv[0], TransListenerErrorTest, sys.argv)
assert toggle_a04_led_bottom_state == 'ON'
rospy.loginfo("Case 1 Done.")
# Perform successful switch to UP state
manipulateSafeToggle(1, 0, 10, 2)
manipulateSafeToggle(1, 1, deg2rad(40), 1)
wait(4)
assert toggle_a04_state == 'UP'
assert toggle_a04_led_top_state == 'ON'
assert toggle_a04_led_bottom_state == 'OFF'
rospy.loginfo("Case 2 Done.")
# Go to the center position
manipulateSafeToggle(1, 0, 10, 2)
manipulateSafeToggle(1, 1, deg2rad(-26), 1)
wait(4)
assert toggle_a04_state == 'CENTER'
assert toggle_a04_led_top_state == 'OFF'
assert toggle_a04_led_bottom_state == 'OFF'
rospy.loginfo("Case 3 Done.")
except rospy.ServiceException, e:
print "Service call failed: %s" % e
time.sleep(3.0)
if __name__ == '__main__':
print "Waiting for test to start at time "
rostest.run(PKG, 'test_toggle_a04', ToggleA04Test)
def test_import_waitinput(self):
# https://github.com/start-jsk/rtmros_hironx/blob/groovy-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py
from hrpsys.waitInput import waitInputConfirm, waitInputSelect
self.assertTrue(True)
def test_findcomp(self):
global h
h.findComps()
def setUp(self):
global h
parser = argparse.ArgumentParser(description='hrpsys command line interpreters')
parser.add_argument('--host', help='corba name server hostname')
parser.add_argument('--port', help='corba name server port number')
args, unknown = parser.parse_known_args()
if args.host:
rtm.nshost = args.host
if args.port:
rtm.nsport = args.port
h = SampleHrpsysConfigurator()
if __name__ == '__main__':
try:
import rostest
rostest.run(PKG, NAME, TestHrpsysConfig, sys.argv)
except ImportError:
unittest.main()
#!/usr/bin/env python
import imp, sys, os, time
# set path to hrpsys to use HrpsysConfigurator
from subprocess import check_output
sys.path.append(os.path.join(check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip(),'share/hrpsys/samples/SampleRobot/')) # set path to SampleRobot
import samplerobot_remove_force_offset
if __name__ == '__main__':
samplerobot_remove_force_offset.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s] :
import unittest, rostest
rostest.run('hrpsys', 'samplerobot_remove_force_offset', unittest.TestCase, sys.argv)
#!/usr/bin/env python
import imp, sys, os, time
# set path to hrpsys to use HrpsysConfigurator
from subprocess import check_output
sys.path.append(
os.path.join(
check_output(["pkg-config", "hrpsys-base", "--variable=prefix"]).rstrip(), "share/hrpsys/samples/SampleRobot/"
)
) # set path to SampleRobot
import samplerobot_auto_balancer
import unittest, rostest
class TestSampleRobotAutoBalancer(unittest.TestCase):
def test_demo(self):
samplerobot_auto_balancer.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s]:
rostest.run("hrpsys", "samplerobot_auto_balancer", TestSampleRobotAutoBalancer, sys.argv)
# filter out rosout services
#[['/rosout/set_logger_level', ['/rosout']], ['/rosout/get_loggers', ['/rosout']]]
srv = [s for s in srv if not s[0].startswith('/rosout/')]
self.failIf(srv, srv)
print "Creating service ", SERVICE
service = rospy.Service(SERVICE, EmptySrv, callback)
# we currently cannot interrogate a node's services, have to rely on master
# verify that master has service
state = master.getSystemState()
_, _, srv = state
srv = [s for s in srv if not s[0].startswith('/rosout/')]
self.assertEquals(
srv, [[rospy.resolve_name(SERVICE), [rospy.get_caller_id()]]])
# begin actual test by unsubscribing
service.shutdown()
time.sleep(1.0) # give API 1 second to sync with master
state = master.getSystemState()
_, _, srv = state
srv = [s for s in srv if not s[0].startswith('/rosout/')]
self.failIf(srv, srv)
if __name__ == '__main__':
rospy.init_node('test_dereg', disable_rostime=True)
rostest.run(PKG, 'rospy_deregister', TestDeregister, sys.argv)
self.assert_(
rostest.is_subscriber(rospy.resolve_name(PUBTOPIC),
rospy.resolve_name(LPNODE)),
"%s is not up" % LPNODE)
print "Publishing to ", PUBTOPIC
pub = rospy.Publisher(PUBTOPIC, MSG)
rospy.Subscriber(LPTOPIC, MSG, self._test_subscriber_first_callback)
# publish about 10 messages for fun
import random
val = random.randint(0, 109812312)
msg = "hi [%s]" % val
for i in xrange(0, 10):
pub.publish(MSG(msg))
time.sleep(0.1)
# listenerpublisher is supposed to repeat our messages back onto /listenerpublisher,
# make sure we got it
self.assert_(self.callback_data is not None,
"no callback data from listenerpublisher")
self.assertEquals(
msg, self.callback_data.data,
"callback data from listenerpublisher does not match")
if __name__ == '__main__':
rospy.init_node(NAME)
rostest.run(PKG, NAME, TestPubSubOrder, sys.argv)
print "initialized successfully"
def loadPattern(basename, tm=1.0):
seq_svc.loadPattern(basename, tm)
seq_svc.waitInterpolation()
if __name__ == '__main__':
initCORBA()
init()
from subprocess import check_output
openhrp3_path = check_output(['rospack','find','openhrp3']).rstrip() # for rosbuild
PKG_CONFIG_PATH=""
if os.path.exists(os.path.join(openhrp3_path, "bin")) :
PKG_CONFIG_PATH='PKG_CONFIG_PATH=%s/lib/pkgconfig:$PKG_CONFIG_PATH'%(openhrp3_path)
cmd = "%s pkg-config openhrp3.1 --variable=idl_dir"%(PKG_CONFIG_PATH)
os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample")
loadPattern(os.path.join(check_output(cmd, shell=True).rstrip(), "../sample/controller/SampleController/etc/Sample"))
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s] :
import unittest, rostest
rostest.run('hrpsys', 'samplerobot_walk', unittest.TestCase, sys.argv)
rospy.loginfo("sending goal")
client.send_goal(goal)
rospy.loginfo("Waiting for result")
client.wait_for_result()
#client.wait_for_result(rospy.Duration.from_sec(5.0))
rospy.loginfo(client.get_result())
#
# sub = rospy.Subscriber('kuri_msgs/ObjectsMap', ObjectsMap, self.callback)
# while not self.objectsReceived:
# rospy.loginfo("Waiting for objects")
# rospy.sleep(1)
# assert(self.objectsReceived)
#def callback(self, msg):
# rospy.loginfo("Objects Received (%s)", len(msg.objects))
# if len(msg.objects)>=4:
# self.objectsReceived = True
if __name__ == '__main__':
time.sleep(0.75)
try:
rostest.run('test_path_planning', testName, PathPlanGenerated,
sys.argv)
except KeyboardInterrupt:
pass
print("exiting")
def txpower(self):
txpower = re.findall('Tx-Power=\s*(\S+)\s*dBm', self.out)
if not txpower:
raise IOError(
-1,
"Could not find tx power in iwconfig output: %s" % (self.out))
return int(txpower[0])
def ap(self):
if self.is_master():
raise TypeError(
"Interface is in master mode, can't be associated to an AP")
ap = re.findall('Access Point: \s*(\w\w:\w\w:\w\w:\w\w:\w\w:\w\w)',
self.out)
if ap:
return ap[0]
else:
return None
def associated(self):
return self.ap() is not None
if __name__ == '__main__':
try:
rostest.run('network_control_tests', 'hostapd_test', HostapdTest)
except KeyboardInterrupt, e:
pass
if global_localization == 1:
#print 'Waiting for service global_localization'
rospy.wait_for_service('global_localization')
global_localization = rospy.ServiceProxy('global_localization', Empty)
resp = global_localization()
rospy.init_node('test', anonymous=True)
while(rospy.rostime.get_time() == 0.0):
#print 'Waiting for initial time publication'
time.sleep(0.1)
start_time = rospy.rostime.get_time()
# TODO: This should be replace by a pytf listener
rospy.Subscriber('/tf', tfMessage, self.tf_cb)
while (rospy.rostime.get_time() - start_time) < target_time:
#print 'Waiting for end time %.6f (current: %.6f)'%(target_time,(rospy.rostime.get_time() - start_time))
time.sleep(0.1)
(a_curr, a_diff) = self.compute_angle_diff()
print 'Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr)
print 'Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a)
print 'Diff:\t %16.6f %16.6f %16.6f' % (abs(self.tf.translation.x-self.target_x),abs(self.tf.translation.y - self.target_y), a_diff)
self.assertNotEquals(self.tf, None)
self.assertTrue(abs(self.tf.translation.x - self.target_x) <= tolerance_d)
self.assertTrue(abs(self.tf.translation.y - self.target_y) <= tolerance_d)
self.assertTrue(a_diff <= tolerance_a)
if __name__ == '__main__':
rostest.run('amcl', 'amcl_localization',
TestBasicLocalization, sys.argv)
wrl_s.inertia, dae_s.inertia), inertia_ok)
self.print_ok(
" trans {0:50}\n {1:50}".format(
wrl_s.transformMatrix, dae_s.transformMatrix),
transformMatrix_ok)
self.print_ok(
" shape {0:24} {1:24}".format(wrl_s.shapeIndices,
dae_s.shapeIndices),
shapeIndices_ok)
if not ret:
print "===========\n== ERROR == {0} and {1} differs\n===========".format(
wrl_file, dae_file)
return ret
class TestModelLoader(TestModelLoaderBase):
""" Make class for testing by inheriting TestModelLoaderBase """
def test_sample_models(self):
self.checkModels("sample.wrl", "sample.dae")
def test_pa10(self):
self.checkModels("PA10/pa10.main.wrl", "PA10/pa10.main.dae")
#def test_3dof_arm(self):
# self.checkModels("sample3dof.wrl","sample3dof.dae")
if __name__ == '__main__':
rostest.run(PKG, NAME, TestModelLoader, sys.argv)
#!/usr/bin/env python
import imp, sys, os, time
# set path to hrpsys to use HrpsysConfigurator
from subprocess import check_output
sys.path.append(
os.path.join(
check_output(['pkg-config', 'hrpsys-base',
'--variable=prefix']).rstrip(),
'share/hrpsys/samples/SampleRobot/')) # set path to SampleRobot
import samplerobot_emergency_stopper
import unittest, rostest
class TestSampleRobotEmergencyStopper(unittest.TestCase):
def test_demo(self):
samplerobot_emergency_stopper.demo()
## IGNORE ME: this code used for rostest
if [s for s in sys.argv if "--gtest_output=xml:" in s]:
rostest.run('hrpsys', 'samplerobot_emergency_stopper',
TestSampleRobotEmergencyStopper, sys.argv)
duration_list = [6.0, 6.5]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending scaled goal without time restrictions")
self.client.send_goal(goal)
self.client.wait_for_result()
self.assertEqual(self.client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
# Now do the same again, but with a goal time constraint
rospy.loginfo("Sending scaled goal with time restrictions")
goal.goal_time_tolerance = rospy.Duration(0.01)
self.client.send_goal(goal)
self.client.wait_for_result()
self.assertEqual(self.client.get_result().error_code,
FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED)
rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")
if __name__ == '__main__':
import rostest
rostest.run(PKG, NAME, TrajectoryTest, sys.argv)
env['ROS_NAMESPACE'] = 'foo'
uri1 = Popen([cmd, 'uri', 'add_two_ints'],
stdout=PIPE).communicate()[0]
uri2 = Popen([cmd, 'uri', 'add_two_ints'], env=env,
stdout=PIPE).communicate()[0]
self.assert_(uri2.startswith('rosrpc://'))
self.assertNotEquals(uri1, uri2)
# test_call_wait
def task1():
output = Popen([cmd, 'call', '--wait', 'wait_two_ints', '1', '2'],
stdout=PIPE).communicate()[0]
self.assertEquals('sum: 3', output.strip())
timeout_t = time.time() + 5.
t1 = TestTask(task1)
t1.start()
rospy.init_node('test_call_wait')
rospy.Service("wait_two_ints", test_rosmaster.srv.AddTwoInts,
lambda x: x.a + x.b)
while not t1.done and time.time() < timeout_t:
time.sleep(0.5)
self.assert_(t1.success)
PKG = 'test_rosservice'
NAME = 'test_rosservice_command_line_online'
if __name__ == '__main__':
rostest.run(PKG, NAME, TestRosserviceOnline, sys.argv)
def test_record(self):
dialog_client(0, 'Prepare to Record')
sub = rospy.Subscriber("/audio_in", AudioData, self.callback)
pub = rospy.Publisher("/audio_out", AudioData)
rospy.sleep(4.0)
dialog_client(0, 'Done recording, listen up')
r = rospy.Rate(len(self.record) / 4.0)
for Data in self.record:
pub.publish(Data)
r.sleep()
self.assertTrue(dialog_client(1, 'Did you hear your record?'))
def callback(self, Data):
self.record.append(Data)
if __name__ == '__main__':
# Copied from hztest: 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', 'sound_test', HardwareTest, sys.argv)
except KeyboardInterrupt, e:
pass
print "exiting"
def test_off_force_sensor(self):
while self.off_lfsensor == None:
time.sleep(1)
rospy.logwarn("wait for off_lfsensor...")
self.assertEqual(self.off_lfsensor.header.frame_id, "lfsensor")
def test_ref_force_sensor(self):
while self.ref_lfsensor == None:
time.sleep(1)
rospy.logwarn("wait for ref_lfsensor...")
self.assertEqual(self.ref_lfsensor.header.frame_id, "lfsensor")
def test_hcf_init(self):
hcf = samplerobot_hrpsys_config.SampleRobotHrpsysConfigurator()
model_url = rospkg.RosPack().get_path(
"openhrp3") + "/share/OpenHRP-3.1/sample/model/sample1.wrl"
if os.path.exists(model_url):
try:
hcf.init("SampleRobot(Robot)0", model_url)
assert (True)
except AttributeError as e:
print >> sys.stderr, "[test-samplerobot-hcf.py] catch exception", e
assert (False)
#unittest.main()
if __name__ == '__main__':
import rostest
rostest.run(PKG, NAME, TestSampleRobotHcf, sys.argv)
self.feedback_interval_too_large = True
self.feedback_interval_too_large_value = curr_time - self.last_feedback_time
elif curr_time - self.last_feedback_time < self.update_interval * 0.5:
self.feedback_interval_too_small = True
self.feedback_interval_too_small_value = curr_time - self.last_feedback_time
self.last_feedback_time = curr_time
def test_stop_test(self):
test = self.srcnode.create_test("finite_duration_test",
sink_ip="127.0.0.1",
sink_port=12345,
duration=5.0,
bw=1e6)
test.start()
time.sleep(1.0)
test.stop()
time.sleep(1.0)
self.assertTrue(test.done, "Test should have been stopped")
self.assertTrue(
test.overall_bandwidth > 0.6e6,
"Expected overall bandwidth to be ~1Mbit/s, instead it was %.2fMbit/s"
% (test.overall_bandwidth / 1e6))
if __name__ == '__main__':
try:
rostest.run('network_monitor_udp', 'blahblah', BasicTest)
except KeyboardInterrupt, e:
pass
self.print_ok(" lvlim {0:24} {1:24}".format(wrl_l.lvlimit, dae_l.lvlimit), lvlimit_ok)
self.print_ok(" uvlim {0:24} {1:24}".format(wrl_l.uvlimit, dae_l.uvlimit), uvlimit_ok)
for (wrl_s, dae_s) in zip(wrl_l.segments, dae_l.segments):
# name, mass, centerOfMass, inertia, transformMatrix, shapeIndices
name_ok = wrl_s.name == dae_s.name
mass_ok = equal(wrl_s.mass, dae_s.mass)
centerOfMass_ok = equal(wrl_s.centerOfMass, dae_s.centerOfMass)
inertia_ok = equal(wrl_s.inertia, dae_s.inertia)
transformMatrix_ok = equal(wrl_s.transformMatrix, dae_s.transformMatrix)
shapeIndices_ok = equal(wrl_s.shapeIndices, dae_s.shapeIndices)
ret = all([ret, name_ok, mass_ok, centerOfMass_ok,inertia_ok, transformMatrix_ok, shapeIndices_ok])
self.print_ok(" name {0:24s} {1:24s} ".format(wrl_s.name, dae_s.name), name_ok)
self.print_ok(" mass {0:<24} {1:<24}".format(wrl_s.mass, dae_s.mass), mass_ok)
self.print_ok(" CoM {0:24} {1:24}".format(wrl_s.centerOfMass, dae_s.centerOfMass), centerOfMass_ok)
self.print_ok(" iner {0:50}\n {1:50}".format(wrl_s.inertia, dae_s.inertia), inertia_ok)
self.print_ok(" trans {0:50}\n {1:50}".format(wrl_s.transformMatrix, dae_s.transformMatrix), transformMatrix_ok)
self.print_ok(" shape {0:24} {1:24}".format(wrl_s.shapeIndices, dae_s.shapeIndices), shapeIndices_ok)
if not ret:
print "===========\n== ERROR == {0} and {1} differs\n===========".format(wrl_file, dae_file)
return ret
def test_sample_models(self):
self.checkModels("sample.wrl","sample.dae")
def test_pa10(self):
self.checkModels("PA10/pa10.main.wrl","PA10/pa10.dae")
if __name__ == '__main__':
rostest.run(PKG, NAME, TestModelLoader, sys.argv)
os.kill(popen.pid, signal.SIGKILL)
# test with dictionary
t = '/pub2/chatter'
key = len(topics)+1
rospy.Subscriber(t, std_msgs.msg.String, self.callback, key)
args = [cmd, 'pub', t, 'std_msgs/String', "{data: %s}"%s]
popen = Popen(args, stdout=PIPE, stderr=PIPE, close_fds=True)
# - give rostopic pub 5 seconds to send us a message
all = set(range(0, key+2))
timeout_t = time.time() + 5.
while time.time() < timeout_t and self.vals != all:
time.sleep(0.1)
# - check published value
try:
msg = self.msgs[key]
except KeyError:
self.fail("no message received on "+str(key))
self.assertEquals(s, msg.data)
os.kill(popen.pid, signal.SIGKILL)
PKG = 'test_rostopic'
NAME = 'test_rostopic_command_line_online'
if __name__ == '__main__':
rostest.run(PKG, NAME, TestRostopicOnline, sys.argv)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_vw:
if error < TARGET_TOL:
if time.time() - self.duration_start > TARGET_DURATION:
self.success = True
else:
# failed to maintain target vw, reset duration
self.success = False
self.reached_target_vw = False
else:
if error < TARGET_TOL:
self.reached_target_vw = True
self.duration_start = time.time()
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", BaseVel)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 60.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
pub.publish(BaseVel(0.0,0.0,TARGET_VW))
time.sleep(0.1)
self.assert_(self.success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], BaseTest, sys.argv) #, text_mode=True)
