bipedRobinService.py
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         # init and start update timer with 100ms
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


rqt_bipedRobinDriver
Author(s): Johannes Mayr
autogenerated on Fri Nov 15 2013 11:10:41