Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest('openni_launch')
00003
00004 import rospy
00005 from sensor_msgs.msg import PointCloud2
00006 import os
00007 import subprocess
00008 import shutil
00009 import shlex
00010 import tempfile
00011 import signal
00012 import time
00013
00014 def callback(data, args):
00015 args[0] = True
00016
00017 rospy.init_node('test_sub')
00018
00019 attempt = 0
00020 successes = 0
00021 failures = 0
00022 consecutive_failures = 0
00023 while not rospy.is_shutdown() and consecutive_failures < 5:
00024
00025
00026 received = [False]
00027 ns = "depth_reg_%d" % attempt
00028 topic = "/camera/%s/points" % ns
00029 attempt = attempt + 1
00030 sub = rospy.Subscriber(topic, PointCloud2, callback, callback_args=received)
00031
00032 command = "roslaunch openni_launch openni.launch depth_registered:=%s" % ns
00033
00034 tmp = open(os.devnull, 'w')
00035 p = subprocess.Popen(shlex.split(command), stdout=tmp, stderr=subprocess.STDOUT)
00036 start = time.time()
00037 end = start + 15.0
00038 while (not rospy.is_shutdown()) and (time.time() < end) and (not received[0]):
00039 time.sleep(0.1)
00040 duration = time.time() - start
00041
00042 sub.unregister()
00043 p.send_signal(signal.SIGINT)
00044 p.wait()
00045
00046 if not rospy.is_shutdown():
00047 if received[0]:
00048 rospy.loginfo("Succeeded in %.1fs", duration)
00049 successes = successes + 1
00050 consecutive_failures = 0
00051 status = "success"
00052 else:
00053 rospy.logerr("Failed!")
00054 failures = failures + 1
00055 consecutive_failures = consecutive_failures + 1
00056 status = "fail"
00057 if 0:
00058
00059 with open("%s_%05d.log" % (status, attempt), 'w') as f:
00060 tmp.seek(0)
00061 shutil.copyfileobj(tmp, f)
00062
00063 try:
00064 core_name = "core_%d" % attempt
00065 os.rename("core", core_name)
00066 print "Core dump %s" % core_name
00067 except OSError:
00068 pass
00069
00070 print "Successes: %d" % successes
00071 print "Failures: %d" % failures