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