bootloader.py

Go to the documentation of this file.

# Copyright 2011 Shadow Robot Company Ltd.
#
# This program is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by the Free
# Software Foundation version 2 of the License.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program. If not, see <http://www.gnu.org/licenses/>.

import os
import rospy
import rospkg

from qt_gui.plugin import Plugin
from python_qt_binding import loadUi
from QtCore import QEvent, QObject, Qt, QTimer, Slot, QThread, QPoint, pyqtSignal
from QtGui import QCursor, QColor
from QtWidgets import QWidget, QShortcut, QMessageBox, QFrame, QHBoxLayout, QCheckBox, QLabel, QFileDialog
from sr_utilities.hand_finder import HandFinder

from std_srvs.srv import Empty
from diagnostic_msgs.msg import DiagnosticArray

from sr_robot_msgs.srv import SimpleMotorFlasher, SimpleMotorFlasherResponse


class MotorBootloader(QThread):

    motor_finished = pyqtSignal('QPoint')
    failed = pyqtSignal('QString')

    def __init__(self, parent, nb_motors_to_program, prefix):
        QThread.__init__(self, None)
        self.parent = parent
        self.nb_motors_to_program = nb_motors_to_program
        self.prefix = prefix

    def run(self):
        """
        perform bootloading on the selected motors
        """
        bootloaded_motors = 0
        firmware_path = self.parent._widget.txt_path.text()
        for motor in self.parent.motors:
            if motor.checkbox.checkState() == Qt.Checked:
                try:
                    self.bootloader_service = rospy.ServiceProxy(self.prefix + '/SimpleMotorFlasher',
                                                                 SimpleMotorFlasher)
                    resp = self.bootloader_service(firmware_path.encode('ascii', 'ignore'), motor.motor_index)
                except rospy.ServiceException, e:
                    self.failed.emit("Service did not process request: %s" % str(e))
                    return

                if resp == SimpleMotorFlasherResponse.FAIL:
                    self.failed.emit("Bootloading motor {} failed".format(bootloaded_motors))
                bootloaded_motors += 1
                self.motor_finished.emit(QPoint(bootloaded_motors, 0.0))


class Motor(QFrame):

    def __init__(self, parent, motor_name, motor_index):
        QFrame.__init__(self, parent)

        self.motor_name = motor_name
        self.motor_index = motor_index

        self.layout = QHBoxLayout()

        self.checkbox = QCheckBox(
            motor_name + " [" + str(motor_index) + "]", self)
        self.layout.addWidget(self.checkbox)

        self.revision_label = QLabel("")
        self.revision_label.setToolTip("Svn Revision")
        self.layout.addWidget(self.revision_label)

        self.setLayout(self.layout)


class SrGuiBootloader(Plugin):

    """
    A GUI plugin for bootloading the motors on the shadow etherCAT hand.
    """

    def __init__(self, context):
        super(SrGuiBootloader, self).__init__(context)
        self.setObjectName('SrGuiBootloader')

        self._publisher = None
        self._widget = QWidget()

        ui_file = os.path.join(rospkg.RosPack().get_path(
            'sr_gui_bootloader'), 'uis', 'SrBootloader.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('SrMotorResetterUi')
        if context is not None:
            context.add_widget(self._widget)

        # setting the prefixes
        self._hand_finder = HandFinder()
        hand_parameters = self._hand_finder.get_hand_parameters()
        self._prefix = ""
        for hand in hand_parameters.mapping:
            self._widget.select_prefix.addItem(hand_parameters.mapping[hand])
        if not hand_parameters.mapping:
            rospy.logerr("No hand detected")
            QMessageBox.warning(self._widget, "warning", "No hand is detected")
            return
        else:
            self._widget.select_prefix.setCurrentIndex(0)
            self._prefix = hand_parameters.mapping.values()[0]
        self._widget.select_prefix.currentIndexChanged['QString'].connect(self.prefix_selected)

        # motors_frame is defined in the ui file with a grid layout
        self.motors = []
        self.motors_frame = self._widget.motors_frame
        self.progress_bar = self._widget.motors_progress_bar
        self.progress_bar.hide()

        self.server_revision = 0
        self.diag_sub = rospy.Subscriber("/diagnostics", DiagnosticArray, self.diagnostics_callback)

        # Bind button clicks
        self._widget.btn_select_bootloader.pressed.connect(self.on_select_bootloader_pressed)
        self._widget.btn_select_all.pressed.connect(self.on_select_all_pressed)
        self._widget.btn_select_none.pressed.connect(self.on_select_none_pressed)
        self._widget.btn_bootload.pressed.connect(self.on_bootload_pressed)

        # select the first available hand
        self.prefix_selected(hand_parameters.mapping.values()[0])

    def on_select_bootloader_pressed(self):
        """
        Select a hex file to bootload. Hex files must exist in the released_firmaware folder
        """
        path_to_bootloader = "~"
        try:
            rp = rospkg.RosPack()
            path_to_bootloader = os.path.join(rospkg.RosPack().get_path(
                'sr_external_dependencies'), '/compiled_firmware/released_firmware/')
        except:
            rospy.logwarn(
                "Couldn't find the sr_edc_controller_configuration package")

        filter_files = "*.hex"
        filename, _ = QFileDialog.getOpenFileName(
            self._widget.motors_frame, self._widget.tr(
                'Select hex file to bootload'),
            self._widget.tr(
                path_to_bootloader),
            self._widget.tr(filter_files))
        if filename == "":
            return

        self._widget.txt_path.setText(filename)
        self._widget.btn_bootload.setEnabled(True)

    def populate_motors(self):
        """
        Find motors according to joint_to_motor_mapping mapping that must exist on the parameter server
        and add to the list of Motor objects etherCAT hand node must be running
        """
        if rospy.has_param(self._prefix + "/joint_to_motor_mapping"):
            joint_to_motor_mapping = rospy.get_param(
                self._prefix + "/joint_to_motor_mapping")
        else:
            QMessageBox.warning(self.motors_frame, "Warning",
                                "Couldn't find the " + self._prefix +
                                "/joint_to_motor_mapping parameter. Make sure the etherCAT Hand node is running")
            return

        joint_names = [
            ["FFJ0", "FFJ1", "FFJ2", "FFJ3", "FFJ4"],
            ["MFJ0", "MFJ1", "MFJ2", "MFJ3", "MFJ4"],
            ["RFJ0", "RFJ1", "RFJ2", "RFJ3", "RFJ4"],
            ["LFJ0", "LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5"],
            ["THJ1", "THJ2", "THJ3", "THJ4", "THJ5"],
            ["WRJ1", "WRJ2"]
        ]

        row = 0
        col = 0
        index_jtm_mapping = 0
        for finger in joint_names:
            col = 0
            for joint_name in finger:
                motor_index = joint_to_motor_mapping[index_jtm_mapping]
                if motor_index != -1:
                    motor = Motor(self.motors_frame, joint_name, motor_index)
                    self.motors_frame.layout().addWidget(motor, row, col)
                    self.motors.append(motor)
                    col += 1
                index_jtm_mapping += 1
            row += 1

    def diagnostics_callback(self, msg):
        for status in msg.status:
            for motor in self.motors:
                if motor.motor_name in status.name and self._prefix.replace("/", "") in status.name:
                    for key_values in status.values:
                        if key_values.key.startswith("Firmware git revision"):
                            server_current_modified = key_values.value.split(" / ")

                            if server_current_modified[0] > self.server_revision:
                                self.server_revision = int(server_current_modified[0].strip())

                            palette = motor.revision_label.palette()
                            palette.setColor(motor.revision_label.foregroundRole(), Qt.green)
                            if server_current_modified[0].strip() != server_current_modified[1].strip():
                                palette.setColor(motor.revision_label.foregroundRole(), QColor(255, 170, 23))
                                motor.revision_label.setPalette(palette)

                            if "True" in server_current_modified[2]:
                                palette.setColor(motor.revision_label.foregroundRole(), Qt.red)
                                motor.revision_label.setText("svn: " + server_current_modified[1] + " [M]")
                                motor.revision_label.setPalette(palette)
                            else:
                                motor.revision_label.setText(" svn: " + server_current_modified[1])
                                motor.revision_label.setPalette(palette)

    def on_select_all_pressed(self):
        """
        Select all motors
        """
        for motor in self.motors:
            motor.checkbox.setCheckState(Qt.Checked)

    def on_select_none_pressed(self):
        """
        Unselect all motors
        """
        for motor in self.motors:
            motor.checkbox.setCheckState(Qt.Unchecked)

    def on_bootload_pressed(self):
        """
        Start programming motors
        """
        self.progress_bar.reset()
        nb_motors_to_program = 0
        for motor in self.motors:
            if motor.checkbox.checkState() == Qt.Checked:
                nb_motors_to_program += 1
        if nb_motors_to_program == 0:
            QMessageBox.warning(self._widget, "Warning", "No motors selected for resetting.")
            return
        self.progress_bar.setMaximum(nb_motors_to_program)

        self.motor_bootloader = MotorBootloader(
            self, nb_motors_to_program, self._prefix)

        self.motor_bootloader.finished.connect(self.finished_programming_motors)
        self.motor_bootloader.motor_finished.connect(self.one_motor_finished)
        self.motor_bootloader.failed['QString'].connect(self.failed_programming_motors)

        self._widget.setCursor(Qt.WaitCursor)
        self.motors_frame.setEnabled(False)
        self._widget.btn_select_all.setEnabled(False)
        self._widget.btn_select_none.setEnabled(False)
        self.progress_bar.show()
        self._widget.btn_bootload.hide()

        self.motor_bootloader.start()

    def one_motor_finished(self, point):
        self.progress_bar.setValue(int(point.x()))

    def finished_programming_motors(self):
        """
        Programming of motors completed
        """
        self.motors_frame.setEnabled(True)
        self._widget.btn_select_all.setEnabled(True)
        self._widget.btn_select_none.setEnabled(True)
        self._widget.setCursor(Qt.ArrowCursor)
        self.progress_bar.hide()
        self._widget.btn_bootload.show()

    def failed_programming_motors(self, message):
        QMessageBox.warning(self._widget.motors_frame, "Warning", message)

    def _unregisterPublisher(self):
        if self._publisher is not None:
            self._publisher.unregister()
            self._publisher = None

    def shutdown_plugin(self):
        self._unregisterPublisher()

    def save_settings(self, global_settings, perspective_settings):
        pass

    def restore_settings(self, global_settings, perspective_settings):
        pass

    def prefix_selected(self, prefix):
        self._prefix = prefix
        self.populate_motors()

if __name__ == "__main__":
    from QtWidgets import QApplication
    import sys
    rospy.init_node("bootloader")
    app = QApplication(sys.argv)
    ctrl = SrGuiBootloader(None)
    ctrl._widget.show()
    app.exec_()