bipedRobinNavigation.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_bipedRobinNavigation')
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 std_srvs.srv import Empty, EmptyRequest
00014 from bipedRobin_msgs.msg import BipedRobinGlobalBehaviour
00015 
00016 class BipedRobinNavigationPlugin(Plugin):
00017     def __init__(self, context):
00018         super(BipedRobinNavigationPlugin, self).__init__(context)
00019         self.setObjectName('BipedRobinNavigation')
00020 
00021         self._widget = QWidget()
00022         ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BipedRobinNavigation.ui')
00023         loadUi(ui_file, self._widget)
00024         self._widget.setObjectName('BipedRobinNavigationUi')
00025         if context.serial_number() > 1:
00026             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
00027         context.add_widget(self._widget)
00028 
00029         self._widget.localize_button.pressed.connect(self._on_localize_pressed)
00030         self._widget.calcMap_button.pressed.connect(self._on_calcMap_pressed)
00031         self._widget.staticNav_button.pressed.connect(self._on_staticNav_pressed)
00032         self._widget.dynNav_button.pressed.connect(self._on_dynNav_pressed)
00033         self._widget.walk_button.pressed.connect(self._on_walk_pressed)
00034         self._widget.replan_button.pressed.connect(self._on_replan_pressed)
00035         
00036         rospy.wait_for_service('createMap_srv')
00037         self._createMap_srv = rospy.ServiceProxy('createMap_srv', Empty)
00038         rospy.wait_for_service('localize_srv')
00039         self._localize_srv = rospy.ServiceProxy('localize_srv', Empty)
00040         rospy.wait_for_service('staticPlanning_srv')
00041         self._staticNav_srv = rospy.ServiceProxy('staticPlanning_srv', Empty)
00042         rospy.wait_for_service('dynamicPlanning_srv')
00043         self._dynNav_srv = rospy.ServiceProxy('dynamicPlanning_srv', Empty)
00044         rospy.wait_for_service('walk_srv')
00045         self._walk_srv = rospy.ServiceProxy('walk_srv', Empty)
00046         rospy.wait_for_service('replan_srv')
00047         self._replan_srv = rospy.ServiceProxy('replan_srv', Empty)
00048         
00049         self._globalRobotBehaviour_sub = rospy.Subscriber('bipedRobinGlobalBehaviour', BipedRobinGlobalBehaviour, self._message_callback)
00050         self._globalRobotBehaviour = BipedRobinGlobalBehaviour()
00051         self._globalRobotBehaviour.areJointErrors = 0
00052         self._globalRobotBehaviour.areJointsReferenced = 0
00053         self._globalRobotBehaviour.areJointTowErrorsSmall = 0
00054         self._globalRobotBehaviour.areJointsReadyToDrive = 0
00055         self._globalRobotBehaviour.areForceSensorsOK = 0
00056         self._globalRobotBehaviour.isAirborne = 0
00057         self._globalRobotBehaviour.areJointsReadyToDriveAirborne = 0 
00058         self._globalRobotBehaviour.isReadyToWalk = 0
00059         self._globalRobotBehaviour.currentMotionMode = 0
00060         
00061         # init and start update timer with 100ms
00062         self._update_timer = QTimer(self)
00063         self._update_timer.timeout.connect(self._update_timeout)
00064         self._update_timer.start(100)
00065 
00066     def _message_callback(self, data):
00067         self._globalRobotBehaviour.areJointErrors = data.areJointErrors
00068         self._globalRobotBehaviour.areJointsReferenced = data.areJointsReferenced
00069         self._globalRobotBehaviour.areJointTowErrorsSmall = data.areJointTowErrorsSmall
00070         self._globalRobotBehaviour.areJointsReadyToDrive = data.areJointsReadyToDrive
00071         self._globalRobotBehaviour.areForceSensorsOK = data.areForceSensorsOK
00072         self._globalRobotBehaviour.isAirborne = data.isAirborne
00073         self._globalRobotBehaviour.areJointsReadyToDriveAirborne = data.areJointsReadyToDriveAirborne
00074         self._globalRobotBehaviour.isReadyToWalk = data.isReadyToWalk
00075         self._globalRobotBehaviour.currentMotionMode = data.currentMotionMode
00076 
00077     def _update_timeout(self):
00078     #    if self._globalRobotBehaviour.currentMotionMode == 0:
00079     #        self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: blue}')
00080     #    else:
00081     #        self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: grey}')
00082         pass
00083 
00084     def _on_localize_pressed(self):
00085         self._localize_srv(EmptyRequest())
00086 
00087     def _on_calcMap_pressed(self):
00088         self._createMap_srv(EmptyRequest())
00089 
00090     def _on_staticNav_pressed(self):
00091         self._staticNav_srv(EmptyRequest())
00092 
00093     def _on_dynNav_pressed(self):
00094         self._dynNav_srv(EmptyRequest())
00095 
00096     def _on_walk_pressed(self):
00097         self._walk_srv(EmptyRequest())
00098         
00099     def _on_replan_pressed(self):
00100         self._replan_srv(EmptyRequest())
00101 
00102     def shutdown_plugin(self):
00103         self._globalRobotBehaviour_sub.unregister()
00104         self._update_timer.stop()
00105         
00106 
00107     def save_settings(self, plugin_settings, instance_settings):
00108         pass
00109 
00110     def restore_settings(self, plugin_settings, instance_settings):
00111         pass


rqt_bipedRobinNavigation
Author(s): Johannes Mayr
autogenerated on Fri Nov 15 2013 11:12:16