video_teleop.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # License: BSD
00004 #   https://raw.github.com/robotics-in-concert/rocon_qt_gui/license/LICENSE
00005 #
00006 ##############################################################################
00007 # Imports
00008 ##############################################################################
00009 
00010 from __future__ import division
00011 import os
00012 import math
00013 
00014 from python_qt_binding import loadUi
00015 from python_qt_binding.QtCore import Qt, pyqtSlot
00016 from python_qt_binding.QtGui import QWidget
00017 
00018 from qt_gui.plugin import Plugin
00019 import rospkg
00020 import rospy
00021 
00022 from rocon_qt_library.interfaces import VideoTeleopInterface
00023 from rocon_qt_library.views import QCameraView, QVirtualJoystickView
00024 
00025 ##############################################################################
00026 # Teleop
00027 ##############################################################################
00028 
00029 class QVideoTeleop(QWidget):
00030 
00031     degrees_to_radians = 3.141592 / 180
00032 
00033     def __init__(self, parent=None):
00034         super(QVideoTeleop, self).__init__()
00035         self.maximum_linear_velocity = rospy.get_param('~maximum_linear_velocity', 2.0)                         
00036         self.maximum_angular_velocity = rospy.get_param('~maximum_angular_velocity', 90 * QVideoTeleop.degrees_to_radians)
00037         self._teleop_interface = None
00038 
00039         self._load_ui()
00040         self._init_events()
00041         #self._init_callbacks()
00042 
00043     def _load_ui(self):
00044         rospack = rospkg.RosPack()
00045         ui_file = os.path.join(rospack.get_path('rocon_qt_library'), 'ui', 'video_teleop.ui')
00046         loadUi(ui_file, self, {'QCameraView': QCameraView, 'QVirtualJoystickView': QVirtualJoystickView})
00047 
00048 
00049     def _init_events(self):
00050         # virtual joystick signals
00051         self.virtual_joystick_view.joystick_feedback().connect(self.on_joystick_feedback)
00052         self.virtual_joystick_view.mouse_released().connect(self.on_mouse_released)
00053         self.on_compressed_image_received = self.camera_view.on_compressed_image_received
00054 
00055         #keyboard control
00056         for k in self.children():
00057             try:
00058                 k.keyPressEvent = self.on_key_press
00059                 k.keyReleaseEvent = self.on_key_release
00060             except:
00061                 pass
00062 
00063     def reset(self):
00064         self.camera_view.load_default_image()
00065 
00066         if self._teleop_interface:
00067             self._teleop_interface.shutdown()
00068             self._teleop_interface = None
00069     
00070     def init_teleop_interface(self, cmd_vel_topic_name, compressed_image_topic_name):
00071         self._teleop_interface = VideoTeleopInterface(image_received_slot=self.on_compressed_image_received, cmd_vel_topic_name=cmd_vel_topic_name, compressed_image_topic_name=compressed_image_topic_name)
00072 
00073     def shutdown_plugin(self):
00074         if self._teleop_interface:
00075             self._teleop_interface.shutdown()
00076 
00077     def on_key_release(self, e):
00078         if e.isAutoRepeat():
00079             return
00080         else:
00081             if self._teleop_interface:
00082                 self._teleop_interface.cmd_vel = (0.0, 0.0)
00083 
00084     def on_key_press(self, e):
00085         '''
00086         Currently a very crude implementation - no smoothing, no acceleration, just min, max.
00087         Need to replace this with something like our non-gui keyboard teleop in python.
00088         '''
00089         linear_command = 0
00090         angular_command = 0
00091         if Qt.Key_Up == e.key():
00092             linear_command = self.maximum_linear_velocity
00093         elif Qt.Key_Down == e.key():
00094             linear_command = -self.maximum_linear_velocity
00095         elif Qt.Key_Right == e.key():
00096             angular_command = -self.maximum_angular_velocity
00097         elif Qt.Key_Left == e.key():
00098             angular_command = self.maximum_angular_velocity
00099         if self._teleop_interface:
00100             self._teleop_interface.cmd_vel = (linear_command, angular_command)
00101 
00102     @pyqtSlot(float, float)
00103     def on_joystick_feedback(self, x, y):
00104         '''
00105         Takes a normalised double pair coming in with which we can apply to our
00106         velocity bounds.
00107 
00108         We also use a narrow dead zone along each x and y axis.
00109 
00110         :param double x: normalised (-1.0, 1.0) left to right position
00111         :param double y: normalised (-1.0, 1.0) bottom to top position
00112         '''
00113         dead_zone_radius = 0.05
00114         if math.fabs(x) < dead_zone_radius:
00115             angular_command = 0.0
00116         else:
00117             angular_command = -x * self.maximum_angular_velocity
00118         if  math.fabs(y) < dead_zone_radius:
00119             linear_command = 0.0
00120         else:
00121             linear_command = y * self.maximum_linear_velocity
00122         
00123         if self._teleop_interface:
00124             self._teleop_interface.cmd_vel = (linear_command, angular_command)
00125 
00126     @pyqtSlot()
00127     def on_mouse_released(self):
00128         if self._teleop_interface:
00129             self._teleop_interface.cmd_vel = (0.0, 0.0)


rocon_qt_library
Author(s): Donguk Lee
autogenerated on Fri Feb 12 2016 02:50:13