sr_gui_muscle_driver_bootloader: bootloader.py Source File

Go to the documentation of this file.
00001 # Copyright 2011 Shadow Robot Company Ltd.
00002 #
00003 # This program is free software: you can redistribute it and/or modify it
00004 # under the terms of the GNU General Public License as published by the Free
00005 # Software Foundation, either version 2 of the License, or (at your option)
00006 # any later version.
00007 #
00008 # This program is distributed in the hope that it will be useful, but WITHOUT
00009 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00010 # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
00011 # more details.
00012 #
00013 # You should have received a copy of the GNU General Public License along
00014 # with this program.  If not, see <http://www.gnu.org/licenses/>.
00015 
00016 import os, rospy, rospkg
00017 
00018 from qt_gui.plugin import Plugin
00019 from python_qt_binding import loadUi
00020 from QtCore import QEvent, QObject, Qt, QTimer, Slot, QThread, SIGNAL, QPoint
00021 from QtGui import QWidget, QShortcut, QMessageBox, QFrame, QHBoxLayout, QCheckBox, QLabel, QCursor, QColor, QFileDialog
00022 
00023 from std_srvs.srv import Empty
00024 from diagnostic_msgs.msg import DiagnosticArray
00025 
00026 from sr_robot_msgs.srv import SimpleMotorFlasher, SimpleMotorFlasherResponse
00027 
00028 class MotorBootloader(QThread):
00029     def __init__(self, parent, nb_motors_to_program):
00030         QThread.__init__(self, None)
00031         self.parent = parent
00032         self.nb_motors_to_program = nb_motors_to_program
00033 
00034     def run(self):
00035         bootloaded_motors = 0
00036         firmware_path = self.parent._widget.txt_path.text()
00037         for motor in self.parent.motors:
00038             if motor.checkbox.checkState() == Qt.Checked:
00039                 try:
00040                     self.bootloader_service = rospy.ServiceProxy('SimpleMotorFlasher', SimpleMotorFlasher)
00041                     resp = self.bootloader_service( firmware_path.encode('ascii', 'ignore'), motor.motor_index )
00042                 except rospy.ServiceException, e:
00043                     self.emit( SIGNAL("failed(QString)"),
00044                                "Service did not process request: %s"%str(e) )
00045                     return
00046 
00047                 if resp == SimpleMotorFlasherResponse.FAIL:
00048                     self.emit( SIGNAL("failed(QString)"),
00049                                "Bootloading motor "++"failed" )
00050                 bootloaded_motors += 1
00051                 self.emit( SIGNAL("motor_finished(QPoint)"), QPoint( bootloaded_motors, 0.0 ) )
00052 
00053 class Motor(QFrame):
00054     def __init__(self, parent, motor_name, motor_index):
00055         QFrame.__init__(self, parent)
00056 
00057         self.motor_name = motor_name
00058         self.motor_index = motor_index
00059 
00060         self.layout = QHBoxLayout()
00061 
00062         self.checkbox = QCheckBox(motor_name + " [" + str(motor_index) +"]", self)
00063         self.layout.addWidget(self.checkbox)
00064 
00065         self.revision_label = QLabel( "" )
00066         self.revision_label.setToolTip("Svn Revision")
00067         self.layout.addWidget(self.revision_label)
00068 
00069         self.setLayout(self.layout)
00070 
00071 
00072 class SrGuiBootloader(Plugin):
00073     """
00074     A GUI plugin for bootloading the muscle drivers on the etherCAT muscle shadow hand
00075     """
00076     def __init__(self, context):
00077         super(SrGuiBootloader, self).__init__(context)
00078         self.setObjectName('SrGuiBootloader')
00079 
00080         self._publisher = None
00081         self._widget = QWidget()
00082 
00083         ui_file = os.path.join(rospkg.RosPack().get_path('sr_gui_bootloader'), 'uis', 'SrBootloader.ui')
00084         loadUi(ui_file, self._widget)
00085         self._widget.setObjectName('SrMotorResetterUi')
00086         context.add_widget(self._widget)
00087 
00088         # motors_frame is defined in the ui file with a grid layout
00089         self.motors = []
00090         self.motors_frame = self._widget.motors_frame
00091         self.populate_motors()
00092         self.progress_bar = self._widget.motors_progress_bar
00093         self.progress_bar.hide()
00094 
00095         self.server_revision = 0
00096         self.diag_sub = rospy.Subscriber("diagnostics", DiagnosticArray, self.diagnostics_callback)
00097 
00098         # Bind button clicks
00099         self._widget.btn_select_bootloader.pressed.connect(self.on_select_bootloader_pressed)
00100         self._widget.btn_select_all.pressed.connect(self.on_select_all_pressed)
00101         self._widget.btn_select_none.pressed.connect(self.on_select_none_pressed)
00102         self._widget.btn_bootload.pressed.connect(self.on_bootload_pressed)
00103 
00104     def on_select_bootloader_pressed(self):
00105         """
00106         Select a hex file to bootload. Hex files must exist in the released_firmaware folder
00107         """
00108         path_to_bootloader = "~"
00109         try:
00110             path_to_bootloader = os.path.join(rospkg.RosPack().get_path('sr_external_dependencies'), 'compiled_firmware', 'released_firmware')
00111         except:
00112             rospy.logwarn("couldn't find the sr_edc_controller_configuration package")
00113 
00114         filter_files = "*.hex"
00115         filename, _ = QFileDialog.getOpenFileName(self._widget.motors_frame, self._widget.tr('Select hex file to bootload'),
00116                                                   self._widget.tr(path_to_bootloader),
00117                                                   self._widget.tr(filter_files))
00118         if filename == "":
00119             return
00120 
00121         self._widget.txt_path.setText( filename )
00122         self._widget.btn_bootload.setEnabled( True )
00123 
00124 
00125     def populate_motors(self):
00126         """
00127         Find motors according to joint_to_motor_mapping mapping that must exist on the parameter server
00128         and add to the list of Motor objects etherCAT hand node must be running
00129         """
00130         row = 0
00131         col = 0
00132         for motor_index in range(0, 4):
00133             if motor_index == 0:
00134                 row = 0
00135                 col = 0
00136             elif motor_index == 1:
00137                 row = 0
00138                 col = 1
00139             elif motor_index == 2:
00140                 row = 1
00141                 col = 0
00142             elif motor_index == 3:
00143                 row = 1
00144                 col = 1
00145                  
00146             muscle_driver_name = "Muscle driver " + str(motor_index)
00147             motor = Motor(self.motors_frame, muscle_driver_name, motor_index)
00148             self.motors_frame.layout().addWidget(motor, row, col)
00149             self.motors.append( motor )
00150 
00151 
00152     def diagnostics_callback(self, msg):
00153         for status in msg.status:
00154             for motor in self.motors:
00155                 if motor.motor_name in status.name:
00156                     for key_values in status.values:
00157                         if "Firmware svn revision" in key_values.key:
00158                             server_current_modified = key_values.value.split(" / ")
00159 
00160                             if server_current_modified[0] > self.server_revision:
00161                                 self.server_revision = int( server_current_modified[0].strip() )
00162 
00163                             palette = motor.revision_label.palette();
00164                             palette.setColor(motor.revision_label.foregroundRole(), Qt.green)
00165                             if server_current_modified[0].strip() != server_current_modified[1].strip():
00166                                 palette.setColor(motor.revision_label.foregroundRole(), QColor(255, 170, 23) )
00167                                 motor.revision_label.setPalette(palette);
00168 
00169                             if "True" in server_current_modified[2]:
00170                                 palette.setColor(motor.revision_label.foregroundRole(), Qt.red)
00171                                 motor.revision_label.setText( "svn: "+ server_current_modified[1] + " [M]" )
00172                                 motor.revision_label.setPalette(palette);
00173                             else:
00174                                 motor.revision_label.setText( " svn: " + server_current_modified[1] )
00175                                 motor.revision_label.setPalette(palette);
00176 
00177     def on_select_all_pressed(self):
00178         """
00179         Select all motors
00180         """
00181         for motor in self.motors:
00182             motor.checkbox.setCheckState( Qt.Checked )
00183 
00184     def on_select_none_pressed(self):
00185         """
00186         Unselect all motors
00187         """
00188         for motor in self.motors:
00189             motor.checkbox.setCheckState( Qt.Unchecked )
00190 
00191     def on_bootload_pressed(self):
00192         """
00193         Start programming motors
00194         """
00195         self.progress_bar.reset()
00196         nb_motors_to_program = 0
00197         for motor in self.motors:
00198             if motor.checkbox.checkState() == Qt.Checked:
00199                 nb_motors_to_program += 1
00200         if nb_motors_to_program == 0:
00201             QMessageBox.warning(self._widget, "Warning", "No motors selected for resetting.")
00202             return
00203         self.progress_bar.setMaximum(nb_motors_to_program)
00204 
00205         self.motor_bootloader = MotorBootloader(self, nb_motors_to_program)
00206         self._widget.connect(self.motor_bootloader, SIGNAL("finished()"), self.finished_programming_motors)
00207         self._widget.connect(self.motor_bootloader, SIGNAL("motor_finished(QPoint)"), self.one_motor_finished)
00208         self._widget.connect(self.motor_bootloader, SIGNAL("failed(QString)"), self.failed_programming_motors)
00209 
00210         self._widget.setCursor(Qt.WaitCursor)
00211         self.motors_frame.setEnabled(False)
00212         self._widget.btn_select_all.setEnabled(False)
00213         self._widget.btn_select_none.setEnabled(False)
00214         self.progress_bar.show()
00215         self._widget.btn_bootload.hide()
00216 
00217         self.motor_bootloader.start()
00218 
00219     def one_motor_finished(self, point):
00220         self.progress_bar.setValue( int(point.x()) )
00221 
00222     def finished_programming_motors(self):
00223         """
00224         Programming of motors completed
00225         """
00226         self.motors_frame.setEnabled(True)
00227         self._widget.btn_select_all.setEnabled(True)
00228         self._widget.btn_select_none.setEnabled(True)
00229         self._widget.setCursor(Qt.ArrowCursor)
00230         self.progress_bar.hide()
00231         self._widget.btn_bootload.show()
00232 
00233     def failed_programming_motors(self, message):
00234         QMessageBox.warning(self._widget.motors_frame, "Warning", message)
00235 
00236     def _unregisterPublisher(self):
00237         if self._publisher is not None:
00238             self._publisher.unregister()
00239             self._publisher = None
00240 
00241     def shutdown_plugin(self):
00242         self._unregisterPublisher()
00243 
00244     def save_settings(self, global_settings, perspective_settings):
00245         pass
00246 
00247     def restore_settings(self, global_settings, perspective_settings):
00248         pass
00249