sr_gui_cyberglove_calibrator: cyberglove

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2011 Shadow Robot Company Ltd.
00004 #
00005 # This program is free software: you can redistribute it and/or modify it
00006 # under the terms of the GNU General Public License as published by the Free
00007 # Software Foundation, either version 2 of the License, or (at your option)
00008 # any later version.
00009 #
00010 # This program is distributed in the hope that it will be useful, but WITHOUT
00011 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00012 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00013 # more details.
00014 #
00015 # You should have received a copy of the GNU General Public License along
00016 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 #
00018 
00019 # import roslib; roslib.load_manifest('sr_control_gui')
00020 import os
00021 import rospy
00022 import rospkg
00023 
00024 import traceback
00025 import sys
00026 
00027 from qt_gui.plugin import Plugin
00028 from python_qt_binding import loadUi
00029 import QtCore
00030 from QtCore import Qt, QEvent, QObject
00031 import QtGui
00032 from QtGui import *
00033 
00034 # from cyberglove_generic_plugin import CybergloveGenericPlugin
00035 
00036 from cyberglove_calibrer import *
00037 from cyberglove_mapper import *
00038 # from cyberglove_library import Cyberglove
00039 
00040 rootPath = os.path.join(
00041     rospkg.RosPack().get_path('sr_gui_cyberglove_calibrator'))
00042 noimage_path = rootPath + '/images/image-missing.png'
00043 
00044 
00045 class StepDescription():
00046 
00047     """
00048     Stores the description / images path for a given step.
00049     """
00050 
00051     def __init__(self):
00052         self.text = ""
00053         self.image_path = [noimage_path, noimage_path]
00054         self.current_substep = 0
00055 
00056 
00057 class StepDescriber(QtGui.QWidget):
00058 
00059     """
00060     Displays the description / images for the current step.
00061     """
00062 
00063     def __init__(self, parent):
00064         QtGui.QWidget.__init__(self, parent=parent)
00065 
00066         self.description = StepDescription
00067         self.frame = QtGui.QFrame()
00068         self.layout = QtGui.QVBoxLayout()
00069 
00070         self.text_description = QtGui.QTextEdit()
00071         self.text_description.setMaximumHeight(60)
00072         self.layout.addWidget(self.text_description)
00073 
00074         self.image_description = QtGui.QLabel()
00075         self.image_description.setMinimumSize(170, 170)
00076         self.layout.addWidget(self.image_description)
00077 
00078         self.frame.setLayout(self.layout)
00079         layout = QtGui.QHBoxLayout()
00080         layout.addWidget(self.frame)
00081         self.setLayout(layout)
00082         self.show()
00083 
00084     def set_description(self, description):
00085         self.text_description.setText(description.text)
00086         index = description.current_substep
00087         self.image_description.setPixmap(
00088             QtGui.QPixmap(description.image_path[index]))
00089         self.image_description.repaint()
00090         self.repaint()
00091 
00092 
00093 class StepSelector(QtGui.QWidget):
00094 
00095     """
00096     The user can select the step to calibrate through this widget.
00097     """
00098 
00099     def __init__(self, parent, calibrer):
00100         QtGui.QWidget.__init__(self, parent=parent)
00101 
00102         self.current_step_name = None
00103         self.current_row = 0
00104         self.steps = {}
00105         self.steps_description = {}
00106 
00107         self.frame = QtGui.QFrame()
00108         self.calibrer = calibrer
00109         self.layout = QtGui.QVBoxLayout()
00110         self.layout.setSpacing(5)
00111 
00112         self.title = QtGui.QLabel()
00113         self.title.setText("Calibration Steps - 2 substeps by steps")
00114 
00115         self.step_describer = StepDescriber(self)
00116 
00117         self.list = QtGui.QListWidget()
00118         first_item = self.refresh_list()
00119         self.connect(self.list, QtCore.SIGNAL(
00120             'itemClicked(QListWidgetItem*)'), self.step_choosed)
00121         self.list.setViewMode(QtGui.QListView.ListMode)
00122         self.list.setResizeMode(QtGui.QListView.Adjust)
00123 
00124         self.list.setCurrentRow(0)
00125         first_item = self.list.item(0)
00126         self.list.setItemSelected(first_item, True)
00127         self.step_choosed(first_item, second_substep=True)
00128 
00129         self.layout.addWidget(self.title)
00130         self.layout.addWidget(self.list)
00131 
00132         self.frame.setLayout(self.layout)
00133         layout = QtGui.QHBoxLayout()
00134         layout.addWidget(self.frame)
00135         layout.addWidget(self.step_describer)
00136         self.setLayout(layout)
00137         self.show()
00138 
00139     def step_choosed(self, item, first_time=False, second_substep=False):
00140         step_name = str(item.text())
00141         self.current_step_name = step_name
00142 
00143         if not second_substep:
00144             self.steps_description[step_name].current_substep = 0
00145 
00146         self.current_row = self.list.currentRow()
00147         index = self.steps_description[self.current_step_name].current_substep
00148         name = self.current_step_name
00149 
00150         description = self.steps[name].step_description[index]
00151         self.steps_description[self.current_step_name].text = description
00152         self.step_describer.set_description(
00153             self.steps_description[self.current_step_name])
00154 
00155     def refresh_list(self, value=0):
00156         self.list.clear()
00157         first_item = None
00158         steps = self.calibrer.calibration_steps
00159         index = 1
00160         base_image_path = rootPath + '/images/step'
00161         for step in steps:
00162             item = QtGui.QListWidgetItem(step.step_name)
00163             if first_item is None:
00164                 first_item = item
00165             self.list.addItem(item)
00166             self.steps[step.step_name] = step
00167 
00168             description = StepDescription()
00169             description.image_path = [
00170                 base_image_path + str(index) + "-a.jpeg", base_image_path + str(index) + "-b.jpeg"]
00171             self.steps_description[step.step_name] = description
00172             index = index + 1
00173         return first_item
00174 
00175     def calibrate_current_step(self):
00176         if self.steps_description[self.current_step_name].current_substep == 0:
00177             self.steps_description[self.current_step_name].current_substep = 1
00178             self.calibrer.do_step_min(self.current_row)
00179 
00180             description = self.steps[
00181                 self.current_step_name].step_description[1]
00182             self.steps_description[self.current_step_name].text = description
00183             self.step_describer.set_description(
00184                 self.steps_description[self.current_step_name])
00185 
00186         elif self.steps_description[self.current_step_name].current_substep == 1:
00187             self.calibrer.do_step_max(self.current_row)
00188             if self.current_row < len(self.steps) - 1:
00189                 self.list.setCurrentRow(self.current_row + 1)
00190                 next_item = self.list.item(self.current_row + 1)
00191                 self.step_choosed(next_item, second_substep=True)
00192 
00193 
00194 class GloveCalibratingWidget(QtGui.QWidget):
00195 
00196     """
00197     Displays which joints have been calibrated.
00198     """
00199 
00200     def __init__(self, parent, joint_names):
00201         QtGui.QWidget.__init__(self, parent=parent)
00202         self.frame = QtGui.QFrame()
00203 
00204         self.layout = QtGui.QGridLayout()
00205         self.layout.setHorizontalSpacing(5)
00206         self.layout.setVerticalSpacing(5)
00207 
00208         green = QtGui.QColor(126, 255, 0)
00209         red = QtGui.QColor(255, 36, 0)
00210         orange = QtGui.QColor(255, 138, 0)
00211         self.saved_palette = self.palette()
00212         self.green_palette = self.palette()
00213         self.green_palette.setBrush(QtGui.QPalette.Window, green)
00214         self.red_palette = self.palette()
00215         self.red_palette.setBrush(QtGui.QPalette.Window, red)
00216         self.orange_palette = self.palette()
00217         self.orange_palette.setBrush(QtGui.QPalette.Window, orange)
00218 
00219         col = 0
00220         # vectors to set the correct row in the layout for each col
00221         rows = [0, 0, 0, 0, 0, 0]
00222 
00223         self.joints_frames = {}
00224 
00225         for joint in joint_names:
00226             if "index" in joint.lower():
00227                 col = 0
00228             elif "middle" in joint.lower():
00229                 col = 1
00230             elif "ring" in joint.lower():
00231                 if "pinkie" in joint.lower():
00232                     col = 3
00233                 else:
00234                     col = 2
00235             elif "pinkie" in joint.lower():
00236                 col = 3
00237             elif "thumb" in joint.lower():
00238                 col = 4
00239             else:
00240                 col = 5
00241 
00242             row = rows[col]
00243             rows[col] = row + 1
00244 
00245             subframe = QtGui.QFrame()
00246             layout = QtGui.QHBoxLayout()
00247             name = QtGui.QLabel()
00248             name.setText(joint)
00249             layout.addWidget(name)
00250             subframe.setLayout(layout)
00251             subframe.setPalette(self.red_palette)
00252             subframe.setAutoFillBackground(True)
00253             subframe.repaint()
00254             self.joints_frames[joint] = subframe
00255             self.layout.addWidget(subframe, row, col)
00256 
00257         self.set_not_calibrated(joint_names)
00258 
00259         self.frame.setLayout(self.layout)
00260         layout = QtGui.QVBoxLayout()
00261         layout.addWidget(self.frame)
00262         self.frame.show()
00263         self.setLayout(layout)
00264         self.show()
00265 
00266     def set_not_calibrated(self, joints):
00267         for joint in joints:
00268             self.joints_frames[joint].setPalette(self.red_palette)
00269             self.frame.repaint()
00270 
00271     def set_half_calibrated(self, joints):
00272         for joint in joints:
00273             self.joints_frames[joint].setPalette(self.orange_palette)
00274             self.frame.repaint()
00275 
00276     def set_calibrated(self, joints):
00277         for joint in joints:
00278             self.joints_frames[joint].setPalette(self.green_palette)
00279             self.frame.repaint()
00280 
00281 # class CybergloveCalibratorPlugin(CybergloveGenericPlugin):
00282 
00283 
00284 class SrGuiCybergloveCalibrator(Plugin):
00285 
00286     """
00287     The plugin used to calibrate the glove.
00288     """
00289     name = "Cyberglove Calibrator"
00290 
00291     def __init__(self, context):
00292         super(SrGuiCybergloveCalibrator, self).__init__(context)
00293         self.setObjectName('SrGuiCybergloveCalibrator')
00294         self.icon_dir = os.path.join(
00295             rospkg.RosPack().get_path('sr_visualization_icons'), '/icons')
00296 
00297         ui_file = os.path.join(rospkg.RosPack().get_path(
00298             'sr_gui_cyberglove_calibrator'), 'uis', 'SrGuiCybergloveCalibrator.ui')
00299         self._widget = QWidget()
00300         loadUi(ui_file, self._widget)
00301         context.add_widget(self._widget)
00302 
00303         # self.frame = QtGui.QFrame()
00304         # self.layout = QtGui.QVBoxLayout()
00305         # self.frame.setLayout(self.layout)
00306         # self.window.setWidget(self.frame)
00307 
00308         self.calibrer = CybergloveCalibrer(description_function=None)
00309         self.joint_names = self.calibrer.cyberglove.joints.keys()
00310         self.joint_names.sort()
00311 
00312         self.layout = self._widget.layout
00313         subframe = QtGui.QFrame()
00314         sublayout = QtGui.QHBoxLayout()
00315 
00316         self.glove_calibrating_widget = GloveCalibratingWidget(
00317             self._widget, self.joint_names)
00318         self.layout.addWidget(self.glove_calibrating_widget)
00319 
00320         self.step_selector = StepSelector(self._widget, self.calibrer)
00321         sublayout.addWidget(self.step_selector)
00322 
00323         btn_frame = QtGui.QFrame()
00324         btn_layout = QtGui.QVBoxLayout()
00325         btn_layout.setSpacing(25)
00326         btn_calibrate = QtGui.QPushButton()
00327         btn_calibrate.setText("Calibrate")
00328         btn_calibrate.setToolTip("Calibrate the current selected step")
00329         btn_calibrate.setIcon(
00330             QtGui.QIcon(rootPath + '/images/icons/calibrate.png'))
00331         btn_layout.addWidget(btn_calibrate)
00332         btn_frame.connect(btn_calibrate, QtCore.SIGNAL(
00333             'clicked()'), self.calibrate_current_step)
00334         self.btn_save = QtGui.QPushButton()
00335         self.btn_save.setText("Save")
00336         self.btn_save.setToolTip("Save the current calibration")
00337         self.btn_save.setIcon(QtGui.QIcon(rootPath + '/images/icons/save.png'))
00338         self.btn_save.setDisabled(True)
00339         btn_layout.addWidget(self.btn_save)
00340         btn_frame.connect(
00341             self.btn_save, QtCore.SIGNAL('clicked()'), self.save_calib)
00342         btn_load = QtGui.QPushButton()
00343         btn_load.setText("Load")
00344         btn_load.setToolTip("Load a Glove calibration")
00345         btn_load.setIcon(QtGui.QIcon(rootPath + '/images/icons/load.png'))
00346         btn_layout.addWidget(btn_load)
00347         btn_frame.connect(
00348             btn_load, QtCore.SIGNAL('clicked()'), self.load_calib)
00349         btn_frame.setLayout(btn_layout)
00350         sublayout.addWidget(btn_frame)
00351         subframe.setLayout(sublayout)
00352         self.layout.addWidget(subframe)
00353 
00354         #  QtCore.QTimer.singleShot(0, self.window.adjustSize)
00355 
00356     def calibrate_current_step(self):
00357         self.step_selector.calibrate_current_step()
00358 
00359         for name in self.joint_names:
00360             if self.calibrer.is_step_done(name) == 0.5:
00361                 self.glove_calibrating_widget.set_half_calibrated([name])
00362             elif self.calibrer.is_step_done(name) == 1.0:
00363                 self.glove_calibrating_widget.set_calibrated([name])
00364 
00365         if self.calibrer.all_steps_done():
00366             self.btn_save.setEnabled(True)
00367 
00368     def save_calib(self):
00369         filename = QtGui.QFileDialog.getSaveFileName(
00370             self._widget, 'Save Calibration', '')
00371         if filename == "":
00372             return
00373 
00374         self.calibrer.write_calibration_file(filename)
00375 
00376         # QMessageBox returns 0 for yes
00377         if QtGui.QMessageBox.information(self._widget,
00378                                          "Load new Calibration",
00379                                          "Do you want to load the new calibration file?",
00380                                          "yes",
00381                                          "no") == 0:
00382             self.load_calib(filename)
00383 
00384     def load_calib(self, filename=""):
00385         if "" == filename:
00386             filename = QtGui.QFileDialog.getOpenFileName(
00387                 self._widget, 'Open Calibration', '')
00388             if "" == filename:
00389                 return
00390 
00391         self.calibrer.load_calib(str(filename))
00392 
00393         self.emit(QtCore.SIGNAL("messageToStatusbar(QString)"),
00394                   "New Cyberglove Calibration Loaded.")