Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('wouse')
00004 import rospy
00005 from std_msgs.msg import Header, Bool, String
00006 from pr2_power_board.srv import PowerBoardCommand2, PowerBoardCommand2Request
00007 from std_srvs.srv import Empty, EmptyRequest
00008 from sound_play.libsoundplay import SoundClient
00009
00010 from wouse.srv import WouseRunStop
00011
00012 CIRCUITS=[0,1,2]
00013 DEAD_MAN_CONFIGURATION=False
00014
00015 class RunStop(object):
00016 """Provide utility functions for starting/stopping PR2."""
00017 def __init__(self):
00018 """Establish service connections for motors, power board."""
00019 self.init_successful = True
00020 try:
00021 rospy.wait_for_service('pr2_etherCAT/halt_motors', 5)
00022 self.halt_motors_client=rospy.ServiceProxy('pr2_etherCAT/halt_motors',Empty)
00023 rospy.loginfo("Found halt motors service")
00024 except:
00025 rospy.logerr("Cannot find halt motors service")
00026 self.init_successful = False
00027
00028 try:
00029 rospy.wait_for_service('pr2_etherCAT/reset_motors',5)
00030 self.reset_motors_client=rospy.ServiceProxy('pr2_etherCAT/reset_motors',Empty)
00031 rospy.loginfo("Found reset motors service")
00032 except:
00033 rospy.logerr("Cannot find halt motors service")
00034 self.init_successful = False
00035
00036 try:
00037 rospy.wait_for_service('power_board/control2',5)
00038 self.power_board_client=rospy.ServiceProxy('power_board/control2',PowerBoardCommand2)
00039 rospy.loginfo("Found power_board/control2 service")
00040 except:
00041 rospy.logerr("Cannot find power_board/control2 service")
00042 self.init_successful = False
00043
00044 def stop(self):
00045 """Halt motors, place power board into standboy. Stops robot."""
00046 self.halt_motors_client(EmptyRequest())
00047 success = [False, False, False]
00048 for circuit in CIRCUITS:
00049 success[circuit] = self.standby_power(circuit)
00050 if success[0] and success[1] and success[2]:
00051 return True
00052 else:
00053 return False
00054
00055 def start(self):
00056 """Reset power board, reset motors. Un-does 'run_stop'."""
00057 success = [False, False, False]
00058 for circuit in CIRCUITS:
00059 success[circuit] = self.reset_power(circuit)
00060 if success[0] and success[1] and success[2]:
00061 rospy.sleep(2.0)
00062 self.reset_motors_client(EmptyRequest())
00063 return True
00064 else:
00065 return False
00066
00067 def standby_power(self, circuit):
00068 """Place PR2 power board into standby"""
00069 stdby_cmd = PowerBoardCommand2Request()
00070 stdby_cmd.circuit = circuit
00071 stdby_cmd.command = "stop"
00072 return self.power_board_client(stdby_cmd)
00073
00074 def reset_power(self,circuit):
00075 """Reset PR2 power board to active from standby"""
00076 reset_cmd = PowerBoardCommand2Request()
00077 reset_cmd.circuit = circuit
00078 reset_cmd.command = "start"
00079 return self.power_board_client(reset_cmd)
00080
00081
00082 class RunStopServer(object):
00083 def __init__(self):
00084 """Provide dead-man-switch like server for handling wouse run-stops."""
00085 rospy.Service("wouse_run_stop", WouseRunStop, self.service_cb)
00086 self.run_stop = RunStop()
00087 if DEAD_MAN_CONFIGURATION:
00088 self.sound_client = SoundClient()
00089 self.timeout = rospy.Duration(rospy.get_param('wouse_timeout', 1.5))
00090 self.tone_period = rospy.Duration(10)
00091 self.last_active_time = rospy.Time.now()
00092 self.last_sound = rospy.Time.now()
00093 rospy.Timer(self.timeout, self.check_receiving)
00094
00095 def check_receiving(self, event):
00096 """After timeout, check to ensure that activity is seen from wouse."""
00097 silence = rospy.Time.now() - self.last_active_time
00098
00099 if silence < self.timeout:
00100
00101 return
00102
00103
00104 if (silence > self.timeout and (rospy.Time.now() - self.last_sound) > self.tone_period):
00105 rospy.logwarn("RunStopServer has not heard from wouse recently.")
00106 self.sound_client.play(3)
00107 self.last_sound = rospy.Time.now()
00108
00109 def service_cb(self, req):
00110 """Handle service requests to start/stop run-stop. Used to reset."""
00111
00112 self.last_active_time = req.time
00113
00114 if req.stop:
00115 return self.run_stop.stop()
00116 elif req.start:
00117 return self.run_stop.start()
00118 else:
00119 return True
00120
00121 if __name__=='__main__':
00122 rospy.init_node('run_stop_server')
00123 rss = RunStopServer()
00124 rospy.spin()