sr_gui_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
00017 import rospy, rospkg
00018 
00019 from qt_gui.plugin import Plugin
00020 from python_qt_binding import loadUi
00021 from QtCore import QEvent, QObject, Qt, QTimer, Slot, QThread, SIGNAL, QPoint
00022 from QtGui import QWidget, QShortcut, QMessageBox, QFrame, QHBoxLayout, QCheckBox, QLabel, QCursor, QColor, QFileDialog
00023 
00024 from std_srvs.srv import Empty
00025 from diagnostic_msgs.msg import DiagnosticArray
00026 
00027 from sr_robot_msgs.srv import SimpleMotorFlasher, SimpleMotorFlasherResponse
00028 
00029 class MotorBootloader(QThread):
00030     def __init__(self, parent, nb_motors_to_program):
00031         QThread.__init__(self, None)
00032         self.parent = parent
00033         self.nb_motors_to_program = nb_motors_to_program
00034 
00035     def run(self):
00036         """
00037         perform bootloading on the selected motors
00038         """
00039         bootloaded_motors = 0
00040         firmware_path = self.parent._widget.txt_path.text()
00041         for motor in self.parent.motors:
00042             if motor.checkbox.checkState() == Qt.Checked:
00043                 try:
00044                     self.bootloader_service = rospy.ServiceProxy('SimpleMotorFlasher', SimpleMotorFlasher)
00045                     resp = self.bootloader_service( firmware_path.encode('ascii', 'ignore'), motor.motor_index )
00046                 except rospy.ServiceException, e:
00047                     self.emit( SIGNAL("failed(QString)"),
00048                                "Service did not process request: %s"%str(e) )
00049                     return
00050 
00051                 if resp == SimpleMotorFlasherResponse.FAIL:
00052                     self.emit( SIGNAL("failed(QString)"),
00053                                "Bootloading motor {} failed".format(bootloaded_motors) )
00054                 bootloaded_motors += 1
00055                 self.emit( SIGNAL("motor_finished(QPoint)"), QPoint( bootloaded_motors, 0.0 ) )
00056 
00057 class Motor(QFrame):
00058     def __init__(self, parent, motor_name, motor_index):
00059         QFrame.__init__(self, parent)
00060 
00061         self.motor_name = motor_name
00062         self.motor_index = motor_index
00063 
00064         self.layout = QHBoxLayout()
00065 
00066         self.checkbox = QCheckBox(motor_name + " [" + str(motor_index) +"]", self)
00067         self.layout.addWidget(self.checkbox)
00068 
00069         self.revision_label = QLabel( "" )
00070         self.revision_label.setToolTip("Svn Revision")
00071         self.layout.addWidget(self.revision_label)
00072 
00073         self.setLayout(self.layout)
00074 
00075 
00076 class SrGuiBootloader(Plugin):
00077     """
00078     A GUI plugin for bootloading the motors on the shadow etherCAT hand.
00079     """
00080     def __init__(self, context):
00081         super(SrGuiBootloader, self).__init__(context)
00082         self.setObjectName('SrGuiBootloader')
00083 
00084         self._publisher = None
00085         self._widget = QWidget()
00086 
00087         ui_file = os.path.join(rospkg.RosPack().get_path('sr_gui_bootloader'), 'uis', 'SrBootloader.ui')
00088         loadUi(ui_file, self._widget)
00089         self._widget.setObjectName('SrMotorResetterUi')
00090         context.add_widget(self._widget)
00091 
00092         # motors_frame is defined in the ui file with a grid layout
00093         self.motors = []
00094         self.motors_frame = self._widget.motors_frame
00095         self.populate_motors()
00096         self.progress_bar = self._widget.motors_progress_bar
00097         self.progress_bar.hide()
00098 
00099         self.server_revision = 0
00100         self.diag_sub = rospy.Subscriber("/diagnostics", DiagnosticArray, self.diagnostics_callback)
00101 
00102         # Bind button clicks
00103         self._widget.btn_select_bootloader.pressed.connect(self.on_select_bootloader_pressed)
00104         self._widget.btn_select_all.pressed.connect(self.on_select_all_pressed)
00105         self._widget.btn_select_none.pressed.connect(self.on_select_none_pressed)
00106         self._widget.btn_bootload.pressed.connect(self.on_bootload_pressed)
00107 
00108     def on_select_bootloader_pressed(self):
00109         """
00110         Select a hex file to bootload. Hex files must exist in the released_firmaware folder
00111         """
00112         path_to_bootloader = "~"
00113         try:
00114             rp = rospkg.RosPack()
00115             path_to_bootloader = os.path.join(rospkg.RosPack().get_path('sr_external_dependencies'), '/compiled_firmware/released_firmware/')
00116         except:
00117             rospy.logwarn("Couldn't find the sr_edc_controller_configuration package")
00118 
00119         filter_files = "*.hex"
00120         filename, _ = QFileDialog.getOpenFileName(self._widget.motors_frame, self._widget.tr('Select hex file to bootload'),
00121                                                   self._widget.tr(path_to_bootloader),
00122                                                   self._widget.tr(filter_files))
00123         if filename == "":
00124             return
00125 
00126         self._widget.txt_path.setText( filename )
00127         self._widget.btn_bootload.setEnabled( True )
00128 
00129 
00130     def populate_motors(self):
00131         """
00132         Find motors according to joint_to_motor_mapping mapping that must exist on the parameter server
00133         and add to the list of Motor objects etherCAT hand node must be running
00134         """
00135         if rospy.has_param("joint_to_motor_mapping"):
00136             joint_to_motor_mapping = rospy.get_param("joint_to_motor_mapping")
00137         else:
00138             QMessageBox.warning(self.motors_frame, "Warning", "Couldn't find the joint_to_motor_mapping parameter. Make sure the etherCAT Hand node is running")
00139             self.close_plugin()
00140             return
00141 
00142         joint_names = [
00143                 ["FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4"],
00144                 ["MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4"],
00145                 ["RFJ0", "RFJ1", "RFJ2", "RFJ3", "RFJ4"],
00146                 ["LFJ0", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5"],
00147                 ["THJ1", "THJ2", "THJ3", "THJ4","THJ5"],
00148                 ["WRJ1", "WRJ2"]
00149             ]
00150 
00151         row = 0
00152         col = 0
00153         index_jtm_mapping = 0
00154         for finger in joint_names:
00155             col = 0
00156             for joint_name in finger:
00157                 motor_index = joint_to_motor_mapping[index_jtm_mapping]
00158                 if motor_index != -1:
00159                     motor = Motor(self.motors_frame, joint_name, motor_index)
00160                     self.motors_frame.layout().addWidget(motor, row, col)
00161                     self.motors.append( motor )
00162                     col += 1
00163                 index_jtm_mapping += 1
00164             row += 1
00165 
00166     def diagnostics_callback(self, msg):
00167         for status in msg.status:
00168             for motor in self.motors:
00169                 if motor.motor_name in status.name:
00170                     for key_values in status.values:
00171                         if "Firmware svn revision" in key_values.key:
00172                             server_current_modified = key_values.value.split(" / ")
00173 
00174                             if server_current_modified[0] > self.server_revision:
00175                                 self.server_revision = int( server_current_modified[0].strip() )
00176 
00177                             palette = motor.revision_label.palette();
00178                             palette.setColor(motor.revision_label.foregroundRole(), Qt.green)
00179                             if server_current_modified[0].strip() != server_current_modified[1].strip():
00180                                 palette.setColor(motor.revision_label.foregroundRole(), QColor(255, 170, 23) )
00181                                 motor.revision_label.setPalette(palette);
00182 
00183                             if "True" in server_current_modified[2]:
00184                                 palette.setColor(motor.revision_label.foregroundRole(), Qt.red)
00185                                 motor.revision_label.setText( "svn: "+ server_current_modified[1] + " [M]" )
00186                                 motor.revision_label.setPalette(palette);
00187                             else:
00188                                 motor.revision_label.setText( " svn: " + server_current_modified[1] )
00189                                 motor.revision_label.setPalette(palette);
00190 
00191     def on_select_all_pressed(self):
00192         """
00193         Select all motors
00194         """
00195         for motor in self.motors:
00196             motor.checkbox.setCheckState( Qt.Checked )
00197 
00198     def on_select_none_pressed(self):
00199         """
00200         Unselect all motors
00201         """
00202         for motor in self.motors:
00203             motor.checkbox.setCheckState( Qt.Unchecked )
00204 
00205     def on_bootload_pressed(self):
00206         """
00207         Start programming motors
00208         """
00209         self.progress_bar.reset()
00210         nb_motors_to_program = 0
00211         for motor in self.motors:
00212             if motor.checkbox.checkState() == Qt.Checked:
00213                 nb_motors_to_program += 1
00214         if nb_motors_to_program == 0:
00215             QMessageBox.warning(self._widget, "Warning", "No motors selected for resetting.")
00216             return
00217         self.progress_bar.setMaximum(nb_motors_to_program)
00218 
00219         self.motor_bootloader = MotorBootloader(self, nb_motors_to_program)
00220         self._widget.connect(self.motor_bootloader, SIGNAL("finished()"), self.finished_programming_motors)
00221         self._widget.connect(self.motor_bootloader, SIGNAL("motor_finished(QPoint)"), self.one_motor_finished)
00222         self._widget.connect(self.motor_bootloader, SIGNAL("failed(QString)"), self.failed_programming_motors)
00223 
00224         self._widget.setCursor(Qt.WaitCursor)
00225         self.motors_frame.setEnabled(False)
00226         self._widget.btn_select_all.setEnabled(False)
00227         self._widget.btn_select_none.setEnabled(False)
00228         self.progress_bar.show()
00229         self._widget.btn_bootload.hide()
00230 
00231         self.motor_bootloader.start()
00232 
00233     def one_motor_finished(self, point):
00234         self.progress_bar.setValue( int(point.x()) )
00235 
00236     def finished_programming_motors(self):
00237         """
00238         Programming of motors completed
00239         """
00240         self.motors_frame.setEnabled(True)
00241         self._widget.btn_select_all.setEnabled(True)
00242         self._widget.btn_select_none.setEnabled(True)
00243         self._widget.setCursor(Qt.ArrowCursor)
00244         self.progress_bar.hide()
00245         self._widget.btn_bootload.show()
00246 
00247     def failed_programming_motors(self, message):
00248         QMessageBox.warning(self._widget.motors_frame, "Warning", message)
00249 
00250     def _unregisterPublisher(self):
00251         if self._publisher is not None:
00252             self._publisher.unregister()
00253             self._publisher = None
00254 
00255     def shutdown_plugin(self):
00256         self._unregisterPublisher()
00257 
00258     def save_settings(self, global_settings, perspective_settings):
00259         pass
00260 
00261     def restore_settings(self, global_settings, perspective_settings):
00262         pass
00263