emergency_stop.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2015 Airbus
00004 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00005 #
00006 # Licensed under the Apache License, Version 2.0 (the "License");
00007 # you may not use this file except in compliance with the License.
00008 # You may obtain a copy of the License at
00009 #
00010 #   http://www.apache.org/licenses/LICENSE-2.0
00011 #
00012 # Unless required by applicable law or agreed to in writing, software
00013 # distributed under the License is distributed on an "AS IS" BASIS,
00014 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015 # See the License for the specific language governing permissions and
00016 # limitations under the License.
00017 
00018 import rospy
00019 import os
00020 import sys
00021 import threading
00022 
00023 from actionlib_msgs.msg import GoalID
00024 from std_msgs.msg import Bool
00025 
00026 from python_qt_binding.QtGui import *
00027 from python_qt_binding.QtCore import *
00028 from python_qt_binding import loadUi
00029 
00030 from airbus_cobot_gui.alarm import Alarm
00031 from airbus_cobot_gui.res import R
00032 
00033 ## @class EmergencyStopState
00034 class EmergencyStopState:
00035     LOCKED   = True
00036     UNLOCKED = False
00037 
00038 ## @class EmergencyStopButton
00039 class EmergencyStopButton(QPushButton):
00040 
00041     EMERGENCY_STOP_TOPIC_NAME = rospy.get_param('emergency_stop_topic_name','/emergency_stop/state')
00042     
00043     def __init__(self, context):
00044         """! The constructor."""
00045         QPushButton.__init__(self)
00046         
00047         self._context = context
00048         self._context.addLanguageEventListner(self.onTranslate)
00049         self._context.addCloseEventListner(self.onDestroy)
00050         
00051         self.setCheckable(True)
00052         self.setFocusPolicy(Qt.NoFocus)
00053         self.setStyleSheet(R.values.styles.transparent_background)
00054         
00055         self.setIcon(R.getIconById("icon_pause"))
00056         self.setIconSize(QSize(80,80))
00057         
00058         self._button_state  = EmergencyStopState.UNLOCKED
00059         self._keep_running = True
00060         
00061         self.connect(self,SIGNAL('clicked(bool)'),self._trigger_button)
00062         
00063         self._estop_pub = rospy.Publisher(self.EMERGENCY_STOP_TOPIC_NAME,
00064                                           Bool, latch=True, queue_size=1)
00065         
00066         self._preempt_move_base_pub = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1)
00067         
00068         self._estop_pub_thread = threading.Thread(name='emergency_stop_publisher_loop',
00069                                                   target=self._emergency_stop_publisher_loop)
00070         self._estop_pub_thread.start()
00071         
00072     def _trigger_button(self, checked):
00073         """Called when user click on ermergency stop button.
00074         @param checked: Button status (True/False).
00075         @type checked: bool.
00076         """
00077         self._context.resquestEmergencyStop(checked)
00078         
00079         self._button_state = checked
00080         
00081         if checked == EmergencyStopState.LOCKED:
00082             lng = self._context.getLanguage()
00083             self._context.sendAlarm(Alarm.CRITICAL, R.values.strings.emergency_stop(lng))
00084             self.setIcon(R.getIconById("icon_play"))
00085         else:
00086             self.setIcon(R.getIconById("icon_pause"))
00087         
00088     def _emergency_stop_publisher_loop(self):
00089         """Loop to publish the emergency stop status."""
00090         
00091         r = rospy.Rate(10) # 10hz
00092         
00093         while not rospy.is_shutdown() and self._keep_running:
00094             
00095             if self._button_state == EmergencyStopState.UNLOCKED:
00096                 self._estop_pub.publish(Bool(True))
00097             else:
00098                 self._preempt_move_base_pub.publish(GoalID())
00099                 self._estop_pub.publish(Bool(False))
00100             
00101             r.sleep()
00102             
00103     def onTranslate(self, lng):
00104         pass
00105     
00106     def onDestroy(self):
00107         """Called when appli closes."""
00108         self._keep_running = False
00109         
00110 ##Unittest
00111 if __name__ == "__main__":
00112     
00113     from airbus_cobot_gui.context import Context
00114     
00115     rospy.init_node('unittest_emergency_stop_2')
00116     
00117     a = QApplication(sys.argv)
00118     utt_appli = QMainWindow()
00119     context = Context(utt_appli)
00120     context.requestNewLanguage('fr')
00121     estop = EmergencyStopButton(context)
00122     estop.setIconSize(QSize(80,80))
00123     utt_appli.setCentralWidget(estop)
00124     utt_appli.show()
00125     a.exec_()
00126     estop.onDestroy()
00127     
00128     
00129 #End of file


airbus_cobot_gui
Author(s): Martin Matignon
autogenerated on Thu Jun 6 2019 17:59:19