bipedRobinNavigation.py

Go to the documentation of this file.

from __future__ import division

import os
import rospy
import roslib
roslib.load_manifest('rqt_bipedRobinNavigation')

from rqt_gui_py.plugin import Plugin
from python_qt_binding import loadUi
from python_qt_binding.QtCore import Qt, QTimer, Slot
from python_qt_binding.QtGui import QKeySequence, QShortcut, QWidget

from std_srvs.srv import Empty, EmptyRequest
from bipedRobin_msgs.msg import BipedRobinGlobalBehaviour

class BipedRobinNavigationPlugin(Plugin):
    def __init__(self, context):
        super(BipedRobinNavigationPlugin, self).__init__(context)
        self.setObjectName('BipedRobinNavigation')

        self._widget = QWidget()
        ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BipedRobinNavigation.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('BipedRobinNavigationUi')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

        self._widget.localize_button.pressed.connect(self._on_localize_pressed)
        self._widget.calcMap_button.pressed.connect(self._on_calcMap_pressed)
        self._widget.staticNav_button.pressed.connect(self._on_staticNav_pressed)
        self._widget.dynNav_button.pressed.connect(self._on_dynNav_pressed)
        self._widget.walk_button.pressed.connect(self._on_walk_pressed)
        self._widget.replan_button.pressed.connect(self._on_replan_pressed)
        
        rospy.wait_for_service('createMap_srv')
        self._createMap_srv = rospy.ServiceProxy('createMap_srv', Empty)
        rospy.wait_for_service('localize_srv')
        self._localize_srv = rospy.ServiceProxy('localize_srv', Empty)
        rospy.wait_for_service('staticPlanning_srv')
        self._staticNav_srv = rospy.ServiceProxy('staticPlanning_srv', Empty)
        rospy.wait_for_service('dynamicPlanning_srv')
        self._dynNav_srv = rospy.ServiceProxy('dynamicPlanning_srv', Empty)
        rospy.wait_for_service('walk_srv')
        self._walk_srv = rospy.ServiceProxy('walk_srv', Empty)
        rospy.wait_for_service('replan_srv')
        self._replan_srv = rospy.ServiceProxy('replan_srv', Empty)
        
        self._globalRobotBehaviour_sub = rospy.Subscriber('bipedRobinGlobalBehaviour', BipedRobinGlobalBehaviour, self._message_callback)
        self._globalRobotBehaviour = BipedRobinGlobalBehaviour()
        self._globalRobotBehaviour.areJointErrors = 0
        self._globalRobotBehaviour.areJointsReferenced = 0
        self._globalRobotBehaviour.areJointTowErrorsSmall = 0
        self._globalRobotBehaviour.areJointsReadyToDrive = 0
        self._globalRobotBehaviour.areForceSensorsOK = 0
        self._globalRobotBehaviour.isAirborne = 0
        self._globalRobotBehaviour.areJointsReadyToDriveAirborne = 0 
        self._globalRobotBehaviour.isReadyToWalk = 0
        self._globalRobotBehaviour.currentMotionMode = 0
        
        # init and start update timer with 100ms
        self._update_timer = QTimer(self)
        self._update_timer.timeout.connect(self._update_timeout)
        self._update_timer.start(100)

    def _message_callback(self, data):
        self._globalRobotBehaviour.areJointErrors = data.areJointErrors
        self._globalRobotBehaviour.areJointsReferenced = data.areJointsReferenced
        self._globalRobotBehaviour.areJointTowErrorsSmall = data.areJointTowErrorsSmall
        self._globalRobotBehaviour.areJointsReadyToDrive = data.areJointsReadyToDrive
        self._globalRobotBehaviour.areForceSensorsOK = data.areForceSensorsOK
        self._globalRobotBehaviour.isAirborne = data.isAirborne
        self._globalRobotBehaviour.areJointsReadyToDriveAirborne = data.areJointsReadyToDriveAirborne
        self._globalRobotBehaviour.isReadyToWalk = data.isReadyToWalk
        self._globalRobotBehaviour.currentMotionMode = data.currentMotionMode

    def _update_timeout(self):
    #    if self._globalRobotBehaviour.currentMotionMode == 0:
    #        self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: blue}')
    #    else:
    #        self._widget.mode0_push_button.setStyleSheet('QPushButton {background-color: grey}')
        pass

    def _on_localize_pressed(self):
        self._localize_srv(EmptyRequest())

    def _on_calcMap_pressed(self):
        self._createMap_srv(EmptyRequest())

    def _on_staticNav_pressed(self):
        self._staticNav_srv(EmptyRequest())

    def _on_dynNav_pressed(self):
        self._dynNav_srv(EmptyRequest())

    def _on_walk_pressed(self):
        self._walk_srv(EmptyRequest())
        
    def _on_replan_pressed(self):
        self._replan_srv(EmptyRequest())

    def shutdown_plugin(self):
        self._globalRobotBehaviour_sub.unregister()
        self._update_timer.stop()
        

    def save_settings(self, plugin_settings, instance_settings):
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        pass