Go to the documentation of this file.00001 from __future__ import division
00002
00003 import os
00004 import rospy
00005 import roslib
00006 roslib.load_manifest('rqt_bipedRobinDriver')
00007
00008 from rqt_gui_py.plugin import Plugin
00009 from python_qt_binding import loadUi
00010 from python_qt_binding.QtCore import Qt, QTimer, Slot
00011 from python_qt_binding.QtGui import QKeySequence, QShortcut, QWidget
00012
00013 from geometry_msgs.msg import Twist
00014 from std_srvs.srv import Empty, EmptyRequest
00015 from bipedRobin_msgs.srv import SetModeService
00016 from bipedRobin_msgs.msg import BipedRobinGlobalBehaviour
00017
00018 class BipedRobinServicePlugin(Plugin):
00019 def __init__(self, context):
00020 super(BipedRobinServicePlugin, self).__init__(context)
00021 self.setObjectName('BipedRobinService')
00022
00023 self._widget = QWidget()
00024 ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BipedRobinService.ui')
00025 loadUi(ui_file, self._widget)
00026 self._widget.setObjectName('BipedRobinServiceUi')
00027 if context.serial_number() > 1:
00028 self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00029 context.add_widget(self._widget)
00030
00031 self._widget.stop_push_button.pressed.connect(self._on_stop_pressed)
00032 self._widget.ack_push_button.pressed.connect(self._on_ack_pressed)
00033 self._widget.reference_push_button.pressed.connect(self._on_reference_pressed)
00034 self._widget.resetInternalState_push_button.pressed.connect(self._on_reset_pressed)
00035 self._widget.mode0_push_button.pressed.connect(self._on_mode0_pressed)
00036 self._widget.mode1_push_button.pressed.connect(self._on_mode1_pressed)
00037 self._widget.mode2_push_button.pressed.connect(self._on_mode2_pressed)
00038 self._widget.mode3_push_button.pressed.connect(self._on_mode3_pressed)
00039
00040 rospy.wait_for_service('reference_srv')
00041 self._reference_srv = rospy.ServiceProxy('reference_srv', Empty)
00042 rospy.wait_for_service('acknowledge_srv')
00043 self._acknowledge_srv = rospy.ServiceProxy('acknowledge_srv', Empty)
00044 rospy.wait_for_service('stop_srv')
00045 self._stop_srv = rospy.ServiceProxy('stop_srv', Empty)
00046 rospy.wait_for_service('resetState_srv')
00047 self._reset_srv = rospy.ServiceProxy('resetState_srv', Empty)
00048 rospy.wait_for_service('stop_srv')
00049 self._setMode_srv = rospy.ServiceProxy('setMode_srv', SetModeService)
00050
00051 self._globalRobotBehaviour_sub = rospy.Subscriber('bipedRobinGlobalBehaviour', BipedRobinGlobalBehaviour, self._message_callback)
00052 self._globalRobotBehaviour = BipedRobinGlobalBehaviour()
00053 self._globalRobotBehaviour.areJointErrors = 0
00054 self._globalRobotBehaviour.areJointsReferenced = 0
00055 self._globalRobotBehaviour.areJointTowErrorsSmall = 0
00056 self._globalRobotBehaviour.areJointsReadyToDrive = 0
00057 self._globalRobotBehaviour.areForceSensorsOK = 0
00058 self._globalRobotBehaviour.isAirborne = 0
00059 self._globalRobotBehaviour.areJointsReadyToDriveAirborne = 0
00060 self._globalRobotBehaviour.isReadyToWalk = 0
00061 self._globalRobotBehaviour.currentMotionMode = 0
00062
00063
00064 self._update_timer = QTimer(self)
00065 self._update_timer.timeout.connect(self._update_timeout)
00066 self._update_timer.start(100)
00067
00068 def _message_callback(self, data):
00069 self._globalRobotBehaviour.areJointErrors = data.areJointErrors
00070 self._globalRobotBehaviour.areJointsReferenced = data.areJointsReferenced
00071 self._globalRobotBehaviour.areJointTowErrorsSmall = data.areJointTowErrorsSmall
00072 self._globalRobotBehaviour.areJointsReadyToDrive = data.areJointsReadyToDrive
00073 self._globalRobotBehaviour.areForceSensorsOK = data.areForceSensorsOK
00074 self._globalRobotBehaviour.isAirborne = data.isAirborne
00075 self._globalRobotBehaviour.areJointsReadyToDriveAirborne = data.areJointsReadyToDriveAirborne
00076 self._globalRobotBehaviour.isReadyToWalk = data.isReadyToWalk
00077 self._globalRobotBehaviour.currentMotionMode = data.currentMotionMode
00078
00079 def _update_timeout(self):
00080 if self._globalRobotBehaviour.currentMotionMode == 0:
00081 self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: blue}')
00082 else:
00083 self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: grey}')
00084
00085 if self._globalRobotBehaviour.currentMotionMode == 1:
00086 self._widget.mode1_push_button.setStyleSheet('QPushButton {background-color: blue}')
00087 else:
00088 self._widget.mode1_push_button.setStyleSheet('QPushButton {background-color: grey}')
00089
00090 if self._globalRobotBehaviour.currentMotionMode == 2:
00091 self._widget.mode2_push_button.setStyleSheet('QPushButton {background-color: blue}')
00092 else:
00093 self._widget.mode2_push_button.setStyleSheet('QPushButton {background-color: grey}')
00094
00095 if self._globalRobotBehaviour.currentMotionMode == 3:
00096 self._widget.mode3_push_button.setStyleSheet('QPushButton {background-color: yellow}')
00097 else:
00098 self._widget.mode3_push_button.setStyleSheet('QPushButton {background-color: grey}')
00099
00100 if self._globalRobotBehaviour.areJointErrors:
00101 self._widget.ack_push_button.setStyleSheet('QPushButton {background-color: red}')
00102 else:
00103 self._widget.ack_push_button.setStyleSheet('QPushButton {background-color: green}')
00104
00105 if self._globalRobotBehaviour.areJointsReferenced:
00106 self._widget.reference_push_button.setStyleSheet('QPushButton {background-color: green}')
00107 else:
00108 self._widget.reference_push_button.setStyleSheet('QPushButton {background-color: red}')
00109
00110 if self._globalRobotBehaviour.areJointTowErrorsSmall:
00111 self._widget.resetInternalState_push_button.setStyleSheet('QPushButton {background-color: green}')
00112 else:
00113 self._widget.resetInternalState_push_button.setStyleSheet('QPushButton {background-color: red}')
00114 if self._globalRobotBehaviour.currentMotionMode != 0:
00115 self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: red}')
00116
00117 def _on_stop_pressed(self):
00118 self._stop_srv(EmptyRequest())
00119
00120 def _on_ack_pressed(self):
00121 self._acknowledge_srv(EmptyRequest())
00122
00123 def _on_reference_pressed(self):
00124 self._reference_srv(EmptyRequest())
00125
00126 def _on_reset_pressed(self):
00127 self._reset_srv(EmptyRequest())
00128
00129 def _on_mode0_pressed(self):
00130 self._setMode_srv(0)
00131 self._widget.mode0_push_button.setStyleSheet('QPushButton {color: green}')
00132 self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
00133 self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
00134 self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
00135
00136 def _on_mode1_pressed(self):
00137 self._setMode_srv(1)
00138 self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
00139 self._widget.mode1_push_button.setStyleSheet('QPushButton {color: green}')
00140 self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
00141 self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
00142
00143 def _on_mode2_pressed(self):
00144 self._setMode_srv(2)
00145 self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
00146 self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
00147 self._widget.mode2_push_button.setStyleSheet('QPushButton {color: green}')
00148 self._widget.mode3_push_button.setStyleSheet('QPushButton {color: black}')
00149
00150 def _on_mode3_pressed(self):
00151 self._setMode_srv(3)
00152 self._widget.mode0_push_button.setStyleSheet('QPushButton {color: black}')
00153 self._widget.mode1_push_button.setStyleSheet('QPushButton {color: black}')
00154 self._widget.mode2_push_button.setStyleSheet('QPushButton {color: black}')
00155 self._widget.mode3_push_button.setStyleSheet('QPushButton {color: green}')
00156
00157
00158 def shutdown_plugin(self):
00159 self._globalRobotBehaviour_sub.unregister()
00160 self._update_timer.stop()
00161
00162
00163 def save_settings(self, plugin_settings, instance_settings):
00164 pass
00165
00166 def restore_settings(self, plugin_settings, instance_settings):
00167 pass