00001
00002
00003 import roslib
00004 roslib.load_manifest('pr2_etherCAT')
00005 import time
00006 import rospy, sys
00007 import std_srvs.srv
00008 from std_msgs.msg import Bool
00009
00010 halted = True
00011 def callback(msg):
00012 global halted
00013 print "halted: %s" % msg.data
00014 halted = msg.data
00015
00016
00017 rospy.init_node("test_halt")
00018 rospy.Subscriber("motor_state", Bool, callback)
00019 reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors", std_srvs.srv.Empty)
00020 halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors", std_srvs.srv.Empty)
00021 time.sleep(1)
00022 print "Entering main loop: motors %s" % "halted" if halted else "running"
00023 while 1:
00024 old_halted = halted
00025 if old_halted:
00026 print "Resetting motors"
00027 reset()
00028 else:
00029 print "Halting motors"
00030 halt()
00031 start = time.time()
00032 while old_halted == halted:
00033 time.sleep(0.1)
00034 if time.time() - start > 1:
00035 break
00036 if old_halted == halted:
00037 print "failed! old=%s new=%s" % (old_halted, halted)
00038 break
00039
00040
