pose_view_widget.py
Go to the documentation of this file.
00001 # Copyright (c) 2011, Dorian Scholz, TU Darmstadt
00002 # All rights reserved.
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions
00006 # are met:
00007 #
00008 #   * Redistributions of source code must retain the above copyright
00009 #     notice, this list of conditions and the following disclaimer.
00010 #   * Redistributions in binary form must reproduce the above
00011 #     copyright notice, this list of conditions and the following
00012 #     disclaimer in the documentation and/or other materials provided
00013 #     with the distribution.
00014 #   * Neither the name of the TU Darmstadt nor the names of its
00015 #     contributors may be used to endorse or promote products derived
00016 #     from this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 
00031 from __future__ import division
00032 import os
00033 import rospkg
00034 
00035 from python_qt_binding import loadUi
00036 from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot
00037 from python_qt_binding.QtGui import QAction, QMenu, QWidget
00038 
00039 import rospy
00040 from rostopic import get_topic_class
00041 from tf.transformations import quaternion_matrix, quaternion_about_axis
00042 
00043 from OpenGL.GL import glBegin, glColor3f, glEnd, glLineWidth, glMultMatrixf, glTranslatef, glVertex3f, GL_LINES, GL_QUADS
00044 from .gl_widget import GLWidget
00045 
00046 
00047 # main class inherits from the ui window class
00048 class PoseViewWidget(QWidget):
00049 
00050     def __init__(self, plugin):
00051         super(PoseViewWidget, self).__init__()
00052         rp = rospkg.RosPack()
00053         ui_file = os.path.join(rp.get_path('rqt_pose_view'), 'resource', 'PoseViewWidget.ui')
00054         loadUi(ui_file, self)
00055         self._plugin = plugin
00056 
00057         self._position = (0.0, 0.0, 0.0)
00058         self._orientation = quaternion_about_axis(0.0, (1.0, 0.0, 0.0))
00059         self._topic_name = None
00060         self._subscriber = None
00061 
00062         # create GL view
00063         self._gl_view = GLWidget()
00064         self._gl_view.setAcceptDrops(True)
00065 
00066         # backup and replace original paint method
00067         self._gl_view.paintGL_original = self._gl_view.paintGL
00068         self._gl_view.paintGL = self._gl_view_paintGL
00069 
00070         # backup and replace original mouse release method
00071         self._gl_view.mouseReleaseEvent_original = self._gl_view.mouseReleaseEvent
00072         self._gl_view.mouseReleaseEvent = self._gl_view_mouseReleaseEvent
00073 
00074         # add GL view to widget layout
00075         self.layout().addWidget(self._gl_view)
00076 
00077         # init and start update timer with 40ms (25fps)
00078         self._update_timer = QTimer(self)
00079         self._update_timer.timeout.connect(self.update_timeout)
00080         self._update_timer.start(40)
00081 
00082     def save_settings(self, plugin_settings, instance_settings):
00083         view_matrix_string = repr(self._gl_view.get_view_matrix())
00084         instance_settings.set_value('view_matrix', view_matrix_string)
00085 
00086     def restore_settings(self, plugin_settings, instance_settings):
00087         view_matrix_string = instance_settings.value('view_matrix')
00088         try:
00089             view_matrix = eval(view_matrix_string)
00090         except Exception:
00091             view_matrix = None
00092 
00093         if view_matrix is not None:
00094             self._gl_view.set_view_matrix(view_matrix)
00095         else:
00096             self._set_default_view()
00097 
00098     def _set_default_view(self):
00099         self._gl_view.makeCurrent()
00100         self._gl_view.reset_view()
00101         self._gl_view.rotate((0, 0, 1), 45)
00102         self._gl_view.rotate((1, 0, 0), -45)
00103         self._gl_view.translate((0, -3, -15))
00104 
00105     def message_callback(self, message):
00106         self._position = (message.position.x, message.position.y, message.position.z)
00107         self._orientation = (message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w)
00108 
00109     def update_timeout(self):
00110         self._gl_view.makeCurrent()
00111         self._gl_view.updateGL()
00112 
00113     def _gl_view_paintGL(self):
00114         self._gl_view.paintGL_original()
00115         self._paintGLGrid()
00116         self._paintGLCoorsystem()
00117         self._paintGLBox()
00118 
00119     def _paintGLBox(self):
00120         self._position = (2.0, 2.0, 2.0)  # Set fixed translation for now
00121         glTranslatef(*self._position)     # Translate Box
00122 
00123         matrix = quaternion_matrix(self._orientation)  # convert quaternion to translation matrix
00124         glMultMatrixf(matrix)             # Rotate Box
00125 
00126         glBegin(GL_QUADS)                 # Start Drawing The Box
00127 
00128         glColor3f(0.0, 1.0, 0.0)
00129         glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Top)
00130         glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Top)
00131         glVertex3f(-1.0, 1.0, 1.0)        # Bottom Left Of The Quad (Top)
00132         glVertex3f(1.0, 1.0, 1.0)         # Bottom Right Of The Quad (Top)
00133 
00134         glColor3f(0.5, 1.0, 0.5)
00135         glVertex3f(1.0, -1.0, 1.0)        # Top Right Of The Quad (Bottom)
00136         glVertex3f(-1.0, -1.0, 1.0)       # Top Left Of The Quad (Bottom)
00137         glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Bottom)
00138         glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Bottom)
00139 
00140         glColor3f(0.0, 0.0, 1.0)
00141         glVertex3f(1.0, 1.0, 1.0)         # Top Right Of The Quad (Front)
00142         glVertex3f(-1.0, 1.0, 1.0)        # Top Left Of The Quad (Front)
00143         glVertex3f(-1.0, -1.0, 1.0)       # Bottom Left Of The Quad (Front)
00144         glVertex3f(1.0, -1.0, 1.0)        # Bottom Right Of The Quad (Front)
00145 
00146         glColor3f(0.5, 0.5, 1.0)
00147         glVertex3f(1.0, -1.0, -1.0)       # Bottom Left Of The Quad (Back)
00148         glVertex3f(-1.0, -1.0, -1.0)      # Bottom Right Of The Quad (Back)
00149         glVertex3f(-1.0, 1.0, -1.0)       # Top Right Of The Quad (Back)
00150         glVertex3f(1.0, 1.0, -1.0)        # Top Left Of The Quad (Back)
00151 
00152         glColor3f(1.0, 0.5, 0.5)
00153         glVertex3f(-1.0, 1.0, 1.0)        # Top Right Of The Quad (Left)
00154         glVertex3f(-1.0, 1.0, -1.0)       # Top Left Of The Quad (Left)
00155         glVertex3f(-1.0, -1.0, -1.0)      # Bottom Left Of The Quad (Left)
00156         glVertex3f(-1.0, -1.0, 1.0)       # Bottom Right Of The Quad (Left)
00157 
00158         glColor3f(1.0, 0.0, 0.0)
00159         glVertex3f(1.0, 1.0, -1.0)        # Top Right Of The Quad (Right)
00160         glVertex3f(1.0, 1.0, 1.0)         # Top Left Of The Quad (Right)
00161         glVertex3f(1.0, -1.0, 1.0)        # Bottom Left Of The Quad (Right)
00162         glVertex3f(1.0, -1.0, -1.0)       # Bottom Right Of The Quad (Right)
00163         glEnd()                           # Done Drawing The Quad
00164 
00165     def _paintGLGrid(self):
00166         resolutionMillimeters = 1
00167         griddedAreaSize = 100
00168 
00169         glLineWidth(1.0)
00170 
00171         glBegin(GL_LINES)
00172 
00173         glColor3f(1.0, 1.0, 1.0)
00174 
00175         glVertex3f(griddedAreaSize, 0, 0)
00176         glVertex3f(-griddedAreaSize, 0, 0)
00177         glVertex3f(0, griddedAreaSize, 0)
00178         glVertex3f(0, -griddedAreaSize, 0)
00179 
00180         numOfLines = int(griddedAreaSize / resolutionMillimeters)
00181 
00182         for i in range(numOfLines):
00183             glVertex3f(resolutionMillimeters * i, -griddedAreaSize, 0)
00184             glVertex3f(resolutionMillimeters * i, griddedAreaSize, 0)
00185             glVertex3f(griddedAreaSize, resolutionMillimeters * i, 0)
00186             glVertex3f(-griddedAreaSize, resolutionMillimeters * i, 0)
00187 
00188             glVertex3f(resolutionMillimeters * (-i), -griddedAreaSize, 0)
00189             glVertex3f(resolutionMillimeters * (-i), griddedAreaSize, 0)
00190             glVertex3f(griddedAreaSize, resolutionMillimeters * (-i), 0)
00191             glVertex3f(-griddedAreaSize, resolutionMillimeters * (-i), 0)
00192 
00193         glEnd()
00194 
00195     def _paintGLCoorsystem(self):
00196         glLineWidth(10.0)
00197 
00198         glBegin(GL_LINES)
00199 
00200         glColor3f(1.0, 0.0, 0.0)
00201         glVertex3f(0.0, 0.0, 0.0)
00202         glVertex3f(1.0, 0.0, 0.0)
00203 
00204         glColor3f(0.0, 1.0, 0.0)
00205         glVertex3f(0.0, 0.0, 0.0)
00206         glVertex3f(0.0, 1.0, 0.0)
00207 
00208         glColor3f(0.0, 0.0, 1.0)
00209         glVertex3f(0.0, 0.0, 0.0)
00210         glVertex3f(0.0, 0.0, 1.0)
00211 
00212         glEnd()
00213 
00214     def _gl_view_mouseReleaseEvent(self, event):
00215         if event.button() == Qt.RightButton:
00216             menu = QMenu(self._gl_view)
00217             action = QAction(self._gl_view.tr("Reset view"), self._gl_view)
00218             menu.addAction(action)
00219             action.triggered.connect(self._set_default_view)
00220             menu.exec_(self._gl_view.mapToGlobal(event.pos()))
00221 
00222     @Slot('QDragEnterEvent*')
00223     def dragEnterEvent(self, event):
00224         if not event.mimeData().hasText():
00225             if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
00226                 qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
00227                 return
00228             item = event.source().selectedItems()[0]
00229             ros_topic_name = item.data(0, Qt.UserRole)
00230             if ros_topic_name is None:
00231                 qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
00232                 return
00233         # TODO: do some checks for valid topic here
00234         event.acceptProposedAction()
00235 
00236     @Slot('QDropEvent*')
00237     def dropEvent(self, event):
00238         if event.mimeData().hasText():
00239             topic_name = str(event.mimeData().text())
00240         else:
00241             droped_item = event.source().selectedItems()[0]
00242             topic_name = str(droped_item.data(0, Qt.UserRole))
00243 
00244         self.unregister_topic()
00245         self.subscribe_topic(topic_name)
00246 
00247     def unregister_topic(self):
00248         if self._subscriber:
00249             self._subscriber.unregister()
00250 
00251     def subscribe_topic(self, topic_name):
00252         msg_class, self._topic_name, _ = get_topic_class(topic_name)
00253         self._subscriber = rospy.Subscriber(self._topic_name, msg_class, self.message_callback)
00254 
00255     def shutdown_plugin(self):
00256         self.unregister_topic()


rqt_pose_view
Author(s): Dorian Scholz
autogenerated on Mon Oct 6 2014 07:16:20