l1.5-safety-system/safety_user_plugin/scripts/qt_wrapper.py

#!/usr/bin/env python
# coding=utf-8

import os
import rospy
import rospkg

import math
import datetime

from enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS

from python_qt_binding.QtGui import QPixmap
from python_qt_binding.QtGui import QTextCursor
from python_qt_binding.QtWidgets import QMessageBox

from polish import PolishTexts as PT
from english import EnglishTexts as ET

class QtWrapper:

    def __init__(self,widget):
        self.widget = widget
        self.displayed_robots_id_list = []

        lang = rospy.get_param('lang')

        if lang == 'eng':
            self.language = ET
            self.log_info('Started english version of plugin')
        elif lang == 'pol':
            self.language = PT
            self.log_info('Polska wersja pluginu uruchomiona')
        else:
            raise ValueError('language parameter has value different than "eng" or "pol"')

        self.clear_robot_info()
        
        ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_user_plugin'))

        self.distance_pixmap = QPixmap('{0}/Distance.png'.format(ui_directory_path))
        self.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path))
        self.linear_pixmap = QPixmap('{0}/Linear.png'.format(ui_directory_path))

        self.widget.distanceImage.setPixmap(self.distance_pixmap.scaled(66,46))
        self.widget.angularImage.setPixmap(self.angular_pixmap)
        self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20,48))

        self.ok_pixmap = QPixmap('{0}/Ok.jpg'.format(ui_directory_path))
        self.cancel_pixmap = QPixmap('{0}/Cancel.jpg'.format(ui_directory_path))

    def disconnect_signals(self):
        self.widget.clutchButton.clicked.disconnect()
        self.widget.stopButton.clicked.disconnect()
        self.widget.choiceButton.clicked.disconnect()

    def connect_robot_signals(self):
        self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback)
        self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked)
        self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked)

    def connect_signals(self):
        self.widget.clutchButton.clicked.connect(self.handle_not_selected_error)
        self.widget.stopButton.clicked.connect(self.handle_not_selected_error)
        self.widget.choiceButton.clicked.connect(self.handle_openButton_clicked)


    def handle_emitted_signal(self,method_name):
        exec('self.{0}()'.format(method_name))

    def handle_emitted_signal_with_list_argument(self,method_name,argument):
        if method_name == 'update_selected_robot_info':
            self.update_selected_robot_info(argument[0])

        else:
            method_with_argument = 'self.{0}({1})'.format(method_name,argument[0])
            exec(method_with_argument)
    

    def handle_stopButton_clicked(self):
        if self.robot_state == SS.RUNNING:
            self.handle_stopButton_clicked_callback()
        else:
            reply = QMessageBox.warning(self.widget,self.language.warning_text_1,self.language.warning_text_2,QMessageBox.Yes,QMessageBox.No)
            
            if reply == QMessageBox.Yes:
                self.handle_stopButton_clicked_callback()


    def get_selected_robot_id(self):
        current_text = self.widget.robotsList.currentText()

        if 'PIONIER' in current_text:
            return int(current_text[7:])
        else:
            return None

    def display_clutch_on(self):
        # self.widget.clutchLabel.setPixmap(self.ok_pixmap.scaled(40,40))
        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
        self.widget.clutchButton.setText(self.language.disengage_clutch_text)

    def display_clutch_off(self):
        # self.widget.clutchLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
        self.widget.clutchButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
        self.widget.clutchButton.setText(self.language.engage_clutch_text)

    def display_state_on(self):
        self.widget.stateLabel.setPixmap(self.ok_pixmap.scaled(40,40))
        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
        self.widget.stopButton.setText(self.language.stop_robot_text)
        self.robot_state = SS.RUNNING

    def display_state_off(self):
        self.widget.stateLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
        self.widget.stopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
        self.widget.stopButton.setText(self.language.unlock_robot_text)
        self.robot_state = SS.STOPPED

    def handle_not_selected_error(self):
        self.log_error(self.language.selection_error_text)

    def handle_openButton_clicked(self):
        selected_robot_id = self.get_selected_robot_id()

        if selected_robot_id != None:
            self.handle_openButton_clicked_callback(selected_robot_id)
        else:
            self.log_error(self.language.selection_error_text)

    def handle_closeButton_clicked(self):
        self.handle_closeButton_clicked_callback()

    def set_robot_selection_callback(self,callback):
        self.handle_openButton_clicked_callback = callback

    def set_robot_release_callback(self,callback):
        self.handle_closeButton_clicked_callback = callback

    def set_user_stop_state_updated_callback(self,callback):
        self.handle_stopButton_clicked_callback = callback

    def set_clutch_switched_callback(self,callback):
        self.handle_clutchButton_clicked_callback = callback

    def select_robot(self,robot_id):
        self.disconnect_signals()
        self.connect_robot_signals()
        self.log_info(self.language.robot_selected_text.format(robot_id)) 
        self.log_info(self.language.stopped_and_engaged_text)

        self.widget.choiceButton.setText(self.language.release_robot_text)

    def release_robot(self):
        self.disconnect_signals()
        self.connect_signals()
        self.log_info(self.language.robot_released_text)
        self.clear_robot_info()

    def update_robots_list_gui(self,robots_id_list):
        robots_id_list.sort()
        id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list)

        robots_to_add = []
        robots_to_remove = []

        for robot_id in robots_id_list:
            if robot_id not in self.displayed_robots_id_list:
                robots_to_add.append(robot_id)

        for robot_id in self.displayed_robots_id_list:
            if robot_id not in robots_id_list:
                robots_to_remove.append(robot_id)

        for robot_id in robots_to_remove:
            self.remove_robot_from_list(robot_id)

        for robot_id in robots_to_add:
            self.add_robot_to_list(robot_id)


    def remove_robot_from_list(self,robot_id):
        count = self.widget.robotsList.count()

        for i in range(count):
            if str(robot_id) in self.widget.robotsList.itemText(i):
                self.widget.robotsList.removeItem(i)
                self.displayed_robots_id_list.remove(robot_id)
                return
            

    def add_robot_to_list(self,robot_id):
        self.widget.robotsList.addItem('PIONIER{0}'.format(robot_id))
        self.displayed_robots_id_list.append(robot_id)


    def update_selected_robot_info(self,robot_info):
        linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)

        self.widget.idLabel.setText('PIONIER{0}'.format(robot_info.robot_id))
        self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage))
        self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
        self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2]))

        # self.log_info(str(robot_info.clutch == CS.ENGAGED))
        
        if robot_info.clutch == CS.ENGAGED:
            self.display_clutch_on()
        else:
            self.display_clutch_off()

        if robot_info.state == SS.RUNNING:
            self.display_state_on()
        else:
            self.display_state_off()

        if robot_info.obstacle_detected == True:
            self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
        else:
            self.widget.obstaclestopLabel.setPixmap(self.ok_pixmap.scaled(40,40))

    def master_stopped(self):
        self.widget.masterstopLabel.setPixmap(self.cancel_pixmap.scaled(40,40))
        self.log_info(self.language.master_stop_clicked_text)

    def master_started(self):
        self.widget.masterstopLabel.setPixmap(self.ok_pixmap.scaled(40,40))
        self.log_info(self.language.master_stop_released_text)

    def user_stopped(self):
        self.log_info(self.language.robot_stopped_text)
        self.display_state_off()

    def user_started(self):
        self.log_info(self.language.robot_started_text)
        self.display_state_on()

    def disengage_clutch(self):
        self.log_info(self.language.clutch_disengaged_text)
        self.display_clutch_off()

    def engage_clutch(self):
        self.log_info(self.language.clutch_engaged_text)
        self.display_clutch_on()
        
    def master_is_stopped_notify(self):
        self.log_error(self.language.cannot_start_masterstop_text)

    def obstacle_is_detected_notify(self):
        self.log_error(self.language.cannot_start_obstacle_text)

    def robot_selected_by_another_group_notify(self):
        self.log_error(self.language.cannot_select_robot_text)

    def update_restrictions_gui(self,restrictions):
        self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
        self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
        self.widget.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity))       

    def log_info(self,info_text):
        time = datetime.datetime.now().strftime('[%H:%M:%S]')

        cursor = self.widget.logsBrowser.textCursor()
        cursor.movePosition(QTextCursor.End)
        self.widget.logsBrowser.setTextCursor(cursor)
        self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
        self.scroll_to_bottom()
        # self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_text)

    def log_warning(self,warning_text):
        time = datetime.datetime.now().strftime('[%H:%M:%S]')
        
        cursor = self.widget.logsBrowser.textCursor()
        cursor.movePosition(QTextCursor.End)
        self.widget.logsBrowser.setTextCursor(cursor)
        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
        self.widget.logsBrowser.insertHtml('<font color="orange"> {0} {1}</font><br>'.format(time,warning_text))
        self.scroll_to_bottom()
        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text)

    def log_error(self,error_text):
        time = datetime.datetime.now().strftime('[%H:%M:%S]')

        cursor = self.widget.logsBrowser.textCursor()
        cursor.movePosition(QTextCursor.End)
        self.widget.logsBrowser.setTextCursor(cursor)
        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
        self.widget.logsBrowser.insertHtml('<font color="red"> {0} {1}</font><br>'.format(time,error_text))
        self.scroll_to_bottom()
        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)

    def connection_to_robot_lost(self,lost_robot_id):
        self.clear_robot_info()
        self.disconnect_signals()
        self.connect_signals()
        self.log_info(self.language.connection_to_robot_lost_text.format(lost_robot_id))

    def connection_to_master_lost(self):
        self.log_error(self.language.connection_to_master_lost_text)

    def obstacle_detected(self):
        self.log_error(self.language.obstacle_detected_text)
        # TODO Zmiana

    def obstacle_removed(self):
        self.log_info(self.language.obstacle_removed_text)

    def scroll_to_bottom(self):
        scrollbar = self.widget.logsBrowser.verticalScrollBar()
        scrollbar.setValue(scrollbar.maximum())

    def clear_robot_info(self):
        self.widget.idLabel.setText('-')
        self.widget.batteryLabel.setText('-')
        self.widget.linearLabel.setText('-')
        self.widget.angularLabel.setText('-')
        self.widget.stateLabel.setText('-')
        self.widget.masterstopLabel.setText('-')
        self.widget.obstaclestopLabel.setText('-')

        self.widget.choiceButton.setText(self.language.select_text)

        self.widget.stopButton.setText('-')
        self.widget.stopButton.setStyleSheet("")

        self.widget.clutchButton.setText('-')
        self.widget.clutchButton.setStyleSheet("")

        self.widget.angularText.setText(self.language.angular_text)
        self.widget.batteryText.setText(self.language.battery_text)
        self.widget.idText.setText(self.language.id_text)
        self.widget.linearText.setText(self.language.linear_text)
        self.widget.masterStopText.setText(self.language.masterstop_text)
        self.widget.obstacleText.setText(self.language.obstacle_text)
        self.widget.robotText.setText(self.language.robot_text)
        self.widget.historyLabel.setText(self.language.history_text)
        self.widget.restrictionsLabel.setText(self.language.restrictions_text)