sr_gui_motor_resetter: motor_resetter.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Disabling E1002 check since it complains about super for no reason -
00004 # inheriting from QObject
00005 #
00006 
00007 # Copyright 2011 Shadow Robot Company Ltd.
00008 #
00009 # This program is free software: you can redistribute it and/or modify it
00010 # under the terms of the GNU General Public License as published by the Free
00011 # Software Foundation, either version 2 of the License, or (at your option)
00012 # any later version.
00013 #
00014 # This program is distributed in the hope that it will be useful, but WITHOUT
00015 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00017 # more details.
00018 #
00019 # You should have received a copy of the GNU General Public License along
00020 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 
00022 import os
00023 import rospkg
00024 import rospy
00025 
00026 from qt_gui.plugin import Plugin
00027 from python_qt_binding import loadUi
00028 
00029 from QtCore import Qt, QThread, SIGNAL, QPoint
00030 from QtGui import QWidget, QMessageBox, QFrame, \
00031     QHBoxLayout, QCheckBox, QLabel, QColor
00032 
00033 from std_srvs.srv import Empty
00034 from diagnostic_msgs.msg import DiagnosticArray
00035 from sr_utilities.hand_finder import HandFinder
00036 
00037 
00038 class MotorFlasher(QThread):
00039 
00040     def __init__(self, parent, nb_motors_to_program, prefix):
00041         QThread.__init__(self, None)
00042         self.parent = parent
00043         self.nb_motors_to_program = nb_motors_to_program
00044         self.prefix = prefix
00045 
00046     def run(self):
00047         programmed_motors = 0
00048         for motor in self.parent.motors:
00049             if motor.checkbox.checkState() == Qt.Checked:
00050                 try:
00051                     print(
00052                         "resetting: realtime_loop/" + self.prefix +
00053                         "reset_motor_" + motor.motor_name)
00054                     self.flasher_service = rospy.ServiceProxy(
00055                         'realtime_loop/' + self.prefix + 'reset_motor_' +
00056                         motor.motor_name, Empty)
00057                     self.flasher_service()
00058                 except rospy.ServiceException, e:
00059                     self.emit(SIGNAL("failed(QString)"),
00060                               "Service did not process request: %s" % str(e))
00061                     return
00062 
00063                 programmed_motors += 1
00064                 self.emit(SIGNAL("motor_finished(QPoint)"),
00065                           QPoint(programmed_motors, 0.0))
00066 
00067 
00068 class Motor(QFrame):
00069 
00070     def __init__(self, parent, motor_name, motor_index):
00071         QFrame.__init__(self, parent)
00072 
00073         self.motor_name = motor_name
00074         self.motor_index = motor_index
00075 
00076         self.layout = QHBoxLayout()
00077 
00078         self.checkbox = QCheckBox(motor_name + " [" + str(motor_index) + "]",
00079                                   self)
00080         self.layout.addWidget(self.checkbox)
00081 
00082         self.revision_label = QLabel("")
00083         self.revision_label.setToolTip("Svn Revision")
00084         self.layout.addWidget(self.revision_label)
00085 
00086         self.setLayout(self.layout)
00087 
00088 
00089 class SrGuiMotorResetter(Plugin):
00090 
00091     """
00092     A gui plugin for resetting motors on the shadow hand.
00093     """
00094 
00095     def __init__(self, context):
00096         super(SrGuiMotorResetter, self).__init__(context)
00097         self.setObjectName('SrGuiMotorResetter')
00098 
00099         self._publisher = None
00100         self._widget = QWidget()
00101 
00102         ui_file = os.path.join(
00103             rospkg.RosPack().get_path('sr_gui_motor_resetter'), 'uis',
00104             'SrGuiMotorResetter.ui')
00105         loadUi(ui_file, self._widget)
00106         self._widget.setObjectName('SrMotorResetterUi')
00107         context.add_widget(self._widget)
00108 
00109         # setting the prefixes
00110         self.diag_sub = rospy.Subscriber("diagnostics", DiagnosticArray,
00111                                          self.diagnostics_callback)
00112 
00113         self._hand_finder = HandFinder()
00114         hand_parameters = self._hand_finder.get_hand_parameters()
00115         self._prefix = ""
00116         for hand in hand_parameters.mapping:
00117             self._widget.select_prefix.addItem(hand_parameters.mapping[hand])
00118         if not hand_parameters.mapping:
00119             rospy.logerr("No hand detected")
00120             QMessageBox.warning(
00121                 self._widget, "warning", "No hand is detected")
00122         else:
00123             self._widget.select_prefix.setCurrentIndex(0)
00124             self._prefix = hand_parameters.mapping.values()[0] + "/"
00125 
00126         self._widget.select_prefix.currentIndexChanged['QString'].connect(
00127             self.prefix_selected)
00128         # motors_frame is defined in the ui file with a grid layout
00129         self.motors = []
00130         self.motors_frame = self._widget.motors_frame
00131         self.populate_motors()
00132         self.progress_bar = self._widget.motors_progress_bar
00133         self.progress_bar.hide()
00134 
00135         self.server_revision = 0
00136 
00137         # Bind button clicks
00138         self._widget.btn_select_all.pressed.connect(self.on_select_all_pressed)
00139         self._widget.btn_select_none.pressed.connect(
00140             self.on_select_none_pressed)
00141         self._widget.btn_reset_motors.pressed.connect(
00142             self.on_reset_motors_pressed)
00143 
00144     def populate_motors(self):
00145 
00146         for i in reversed(range(self.motors_frame.layout().count())):
00147             self.motors_frame.layout().itemAt(i).widget().setParent(None)
00148         self.motors = []
00149 
00150         if rospy.has_param(self._prefix + "joint_to_motor_mapping"):
00151             joint_to_motor_mapping = rospy.get_param(
00152                 self._prefix + "joint_to_motor_mapping")
00153         else:
00154             QMessageBox.warning(
00155                 self.motors_frame, "Warning", "Couldn't find the " +
00156                 self._prefix + "joint_to_motor_mapping parameter."
00157                                " Make sure the etherCAT Hand node is running")
00158             return
00159 
00160         joint_names = [
00161             ["FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4"],
00162             ["MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4"],
00163             ["RFJ0", "RFJ1", "RFJ2", "RFJ3", "RFJ4"],
00164             ["LFJ0", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5"],
00165             ["THJ1", "THJ2", "THJ3", "THJ4", "THJ5"],
00166             ["WRJ1", "WRJ2"]
00167         ]
00168 
00169         row = 0
00170         col = 0
00171         index_jtm_mapping = 0
00172         for finger in joint_names:
00173             col = 0
00174             for joint_name in finger:
00175                 motor_index = joint_to_motor_mapping[index_jtm_mapping]
00176                 if motor_index != -1:
00177                     motor = Motor(self.motors_frame, joint_name, motor_index)
00178                     self.motors_frame.layout().addWidget(motor, row, col)
00179                     self.motors.append(motor)
00180                     col += 1
00181                 index_jtm_mapping += 1
00182             row += 1
00183 
00184     def diagnostics_callback(self, msg):
00185         for status in msg.status:
00186             for motor in self.motors:
00187                 if motor.motor_name in status.name and self._prefix.replace(
00188                         "/", "") in status.name:
00189                     for key_values in status.values:
00190                         if "Firmware svn revision" in key_values.key:
00191                             server_current_modified = key_values.value.split(
00192                                 " / ")
00193 
00194                             if server_current_modified[0] \
00195                                     > self.server_revision:
00196                                 self.server_revision = int(
00197                                     server_current_modified[0].strip())
00198 
00199                             palette = motor.revision_label.palette()
00200                             palette.setColor(
00201                                 motor.revision_label.foregroundRole(),
00202                                 Qt.green)
00203                             if server_current_modified[0].strip() != \
00204                                     server_current_modified[1].strip():
00205                                 palette.setColor(
00206                                     motor.revision_label.foregroundRole(),
00207                                     QColor(255, 170, 23))
00208                                 motor.revision_label.setPalette(palette)
00209 
00210                             if "True" in server_current_modified[2]:
00211                                 palette.setColor(
00212                                     motor.revision_label.foregroundRole(),
00213                                     Qt.red)
00214                                 motor.revision_label.setText(
00215                                     "svn: " + server_current_modified[
00216                                         1] + " [M]")
00217                                 motor.revision_label.setPalette(palette)
00218                             else:
00219                                 motor.revision_label.setText(
00220                                     " svn: " + server_current_modified[1])
00221                                 motor.revision_label.setPalette(palette)
00222 
00223     def on_select_all_pressed(self):
00224         for motor in self.motors:
00225             motor.checkbox.setCheckState(Qt.Checked)
00226 
00227     def on_select_none_pressed(self):
00228         for motor in self.motors:
00229             motor.checkbox.setCheckState(Qt.Unchecked)
00230 
00231     def on_reset_motors_pressed(self):
00232         print("Reset motors pressed")
00233         self.progress_bar.reset()
00234         nb_motors_to_program = 0
00235         for motor in self.motors:
00236             if motor.checkbox.checkState() == Qt.Checked:
00237                 nb_motors_to_program += 1
00238         if nb_motors_to_program == 0:
00239             QMessageBox.warning(self._widget, "Warning",
00240                                 "No motors selected for resetting.")
00241             return
00242         self.progress_bar.setMaximum(nb_motors_to_program)
00243 
00244         self.motor_flasher = MotorFlasher(self, nb_motors_to_program,
00245                                           self._prefix)
00246         self._widget.connect(self.motor_flasher, SIGNAL("finished()"),
00247                              self.finished_programming_motors)
00248         self._widget.connect(self.motor_flasher,
00249                              SIGNAL("motor_finished(QPoint)"),
00250                              self.one_motor_finished)
00251         self._widget.connect(self.motor_flasher, SIGNAL("failed(QString)"),
00252                              self.failed_programming_motors)
00253 
00254         self._widget.setCursor(Qt.WaitCursor)
00255         self.motors_frame.setEnabled(False)
00256         self._widget.btn_select_all.setEnabled(False)
00257         self._widget.btn_select_none.setEnabled(False)
00258         self.progress_bar.show()
00259         self._widget.btn_reset_motors.hide()
00260 
00261         self.motor_flasher.start()
00262 
00263     def one_motor_finished(self, point):
00264         self.progress_bar.setValue(int(point.x()))
00265 
00266     def finished_programming_motors(self):
00267         self.motors_frame.setEnabled(True)
00268         self._widget.btn_select_all.setEnabled(True)
00269         self._widget.btn_select_none.setEnabled(True)
00270         self._widget.setCursor(Qt.ArrowCursor)
00271         self.progress_bar.hide()
00272         self._widget.btn_reset_motors.show()
00273 
00274     def failed_programming_motors(self, message):
00275         QMessageBox.warning(self.motors_frame, "Warning", message)
00276 
00277     def _unregisterPublisher(self):
00278         if self._publisher is not None:
00279             self._publisher.unregister()
00280             self._publisher = None
00281 
00282     def shutdown_plugin(self):
00283         self._unregisterPublisher()
00284 
00285     def save_settings(self, global_settings, perspective_settings):
00286         pass
00287 
00288     def restore_settings(self, global_settings, perspective_settings):
00289         pass
00290 
00291     def prefix_selected(self, prefix):
00292         self._prefix = str(prefix) + "/"
00293         self.populate_motors()