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