#!/usr/bin/python import sys import os import rospy from threading import Thread import time import signal import requests #--------------------------------------------- def exit_properly_runtime_test(): f = open("result.txt", "w") f.write("0") f.close() rospy.signal_shutdown("SUCCESS TEST") os.kill(os.getpid(), signal.SIGUSR1) sys.exit(0) class ExitCommand(Exception): pass def signal_handler(signal, frame): sys.exit(1) #timeout countdown def start_countdown(seconds): def timeout_countdown(seconds): time.sleep(seconds) rospy.logfatal("COUNTDOWN ERROR RUNTIME TEST") os.kill(os.getpid(), signal.SIGUSR1) sys.exit(1) t= Thread(target = lambda: timeout_countdown(seconds)) t.start() #--------------------------------------------- # launch countdown signal.signal(signal.SIGUSR1, signal_handler) import gazebo_msgs.msg import intera_core_msgs.msg rospy.init_node("test_node") start_countdown(320) rospy.logwarn("[Test Node] waiting from gazebo message, first time can be slow (gazebo has to download models)...") rospy.wait_for_message("/gazebo/link_states", gazebo_msgs.msg.LinkStates,timeout=200) # wait until box is picked up def on_boxes_state_message(msg): #rospy.loginfo("received from test node: " + str(msg)) str_msg = "" exit_flag = False for i, pose in enumerate(msg.pose): name = msg.name[i] if "block" in name: str_msg += str(name) + ".y: "+ str(pose.position.y) +"\n" if pose.position.y >0.75: exit_flag = True rospy.logwarn("success, cube properly sent to tray. Ending!") break rospy.loginfo_throttle(1.0, "[Test Node] "+ str_msg) if exit_flag: exit_properly_runtime_test() sub = rospy.Subscriber("/gazebo/link_states", gazebo_msgs.msg.LinkStates, on_boxes_state_message) rospy.logwarn("[Test Node] waiting sorting demo...") rospy.wait_for_message("/robot/limb/right/joint_command", intera_core_msgs.msg.JointCommand, timeout=60) rospy.logwarn("[Test Node] Calling start sorting...") rospy.sleep(10) res = requests.get('http://localhost:5000/start') rospy.logwarn(str(res)) rospy.spin()