Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import rospy
00019 from cob_phidgets.msg import AnalogSensor
00020 from cob_phidgets.msg import DigitalSensor
00021 from cob_msgs.msg import PowerState
00022 from cob_msgs.msg import EmergencyStopState
00023
00024 class EmState():
00025 ST_EM_FREE = 0
00026 ST_EM_ACTIVE = 1
00027 ST_EM_CONFIRMED = 2
00028
00029 class EMStatePhidget():
00030 def __init__(self):
00031 self.last_update = None
00032 self.em_caused_by_button = False
00033
00034 self.last_rear_em_state = False
00035 self.last_front_em_state = False
00036
00037 self.em_stop_status = EmState.ST_EM_ACTIVE
00038 self.em_msg = EmergencyStopState()
00039
00040 self.sub_digital_sensors = rospy.Subscriber("digital_sensors", DigitalSensor, self.phidget_cb)
00041 self.pub_em_state = rospy.Publisher('em_stop_state', EmergencyStopState, queue_size=1)
00042
00043 def phidget_cb(self, msg):
00044 front_em_active = False
00045 rear_em_active = False
00046 got_message = False
00047
00048 for i in range(0, len(msg.uri)):
00049 if msg.uri[i] == "em_stop_laser_rear":
00050 rear_em_active = not msg.state[i]
00051 got_message = True
00052 if msg.uri[i] == "em_stop_laser_front":
00053 front_em_active = not msg.state[i]
00054 got_message = True
00055
00056 if got_message:
00057 self.last_update = rospy.Time.now()
00058
00059 if (front_em_active and rear_em_active) and (not self.last_front_em_state and not self.last_rear_em_state):
00060 self.em_msg.emergency_button_stop = True
00061 self.em_caused_by_button = True
00062 elif (not front_em_active and not rear_em_active) and (self.last_front_em_state and self.last_rear_em_state):
00063 self.em_msg.emergency_button_stop = False
00064 self.em_caused_by_button = False
00065 elif (front_em_active is not rear_em_active) and self.em_caused_by_button :
00066 self.em_msg.emergency_button_stop = False
00067 self.em_caused_by_button = False
00068 self.em_msg.scanner_stop = (front_em_active or rear_em_active)
00069 else:
00070 self.em_msg.scanner_stop = (front_em_active or rear_em_active);
00071
00072 em_signal = self.em_msg.scanner_stop or self.em_msg.emergency_button_stop
00073
00074 if self.em_stop_status == EmState.ST_EM_FREE:
00075 if em_signal:
00076 rospy.logdebug("Emergency stop was issued")
00077 self.em_stop_status = EmergencyStopState.EMSTOP
00078
00079 elif self.em_stop_status == EmState.ST_EM_ACTIVE:
00080 if not em_signal:
00081 rospy.logdebug("Emergency stop was confirmed")
00082 self.em_stop_status = EmergencyStopState.EMCONFIRMED
00083 elif self.em_stop_status == EmState.ST_EM_CONFIRMED:
00084 if em_signal:
00085 rospy.logdebug("Emergency stop issued")
00086 self.em_stop_status = EmergencyStopState.EMSTOP
00087 else:
00088 rospy.logdebug("Emergency stop released")
00089 self.em_stop_status = EmergencyStopState.EMFREE
00090
00091 self.em_msg.emergency_state = self.em_stop_status
00092
00093 self.last_rear_em_state = rear_em_active
00094 self.last_front_em_state = front_em_active
00095
00096 def publish(self):
00097 if self.last_update is not None:
00098 self.pub_em_state.publish(self.em_msg)
00099
00100
00101 if __name__ == "__main__":
00102 rospy.init_node("em_state_phidget")
00103 emsp = EMStatePhidget()
00104 rospy.loginfo("em_state_phidget running")
00105 rate = rospy.Rate(10)
00106 while not rospy.is_shutdown():
00107 emsp.publish()
00108 rate.sleep()