test_halt.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
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 


pr2_etherCAT
Author(s): Rob Wheeler/wheeler@willowgarage.com
autogenerated on Sat Nov 30 2013 21:17:41