emergency_stop_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest('cob_monitoring')
00005 import rospy
00006 
00007 from cob_relayboard.msg import *
00008 
00009 from simple_script_server import *
00010 sss = simple_script_server()
00011 
00012 ##################
00013 ### TODO: add diagnostics for em_stop (probably better to be implemented in relayboard) --> then create a diagnostics_monitor.py with sets leds and sound from diagnostics information (for arm, base, torso, ...)
00014 ### which color and flashing code assign to diagnostics?
00015 ##################
00016 
00017 class emergency_stop_monitor():
00018         def __init__(self):
00019                 rospy.Subscriber("/emergency_stop_state", EmergencyStopState, self.emergency_callback)
00020                 self.em_status = EmergencyStopState()
00021                 self.first_time = True
00022 
00023         ## Emergency stop monitoring
00024         def emergency_callback(self,msg):
00025                 # skip first message to avoid speach output on startup
00026                 if self.first_time:
00027                         self.first_time = False
00028                         self.em_status = msg
00029                         return
00030         
00031                 if self.em_status.emergency_state != msg.emergency_state:
00032                         self.em_status = msg
00033                         rospy.loginfo("Emergency change to "+ str(self.em_status.emergency_state))
00034                 
00035                         if self.em_status.emergency_state == 0: # ready
00036                                 sss.set_light("green")
00037                         elif self.em_status.emergency_state == 1: # em stop
00038                                 sss.set_light("red")
00039                                 if self.em_status.scanner_stop and not self.em_status.emergency_button_stop:
00040                                         sss.say(["laser emergency stop issued"])
00041                                 elif not self.em_status.scanner_stop and self.em_status.emergency_button_stop:
00042                                         sss.say(["emergency stop button pressed"])
00043                                 else:
00044                                         sss.say(["emergency stop issued"])
00045                         elif self.em_status.emergency_state == 2: # release
00046                                 sss.set_light("yellow")
00047                                 sss.say(["emergency stop released"])
00048 
00049 if __name__ == "__main__":
00050         rospy.init_node("emergency_stop_monitor")
00051         emergency_stop_monitor()
00052         rospy.spin()


cob_monitoring
Author(s): Florian Weisshardt
autogenerated on Sun Oct 5 2014 23:09:25