l1.5-safety-system/safety_master_plugin/scripts/qt_master_wrapper.py

#!/usr/bin/env python
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 master_restrictions import Restrictions

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


class QtWrapper:

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

        self.master_stop_state = None

        # self.disable_sliders_tracking()

        # self.ok_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Ok.jpg')
        # self.cancel_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Cancel.jpg')

    def set_sliders_to_initial_values(self):
        self.widget.distanceSlider.setMaximum(200)
        self.widget.angularSlider.setMaximum(200)
        self.widget.linearSlider.setMaximum(200)

        self.widget.distanceSlider.setValue(50)
        self.widget.angularSlider.setValue(100)
        self.widget.linearSlider.setValue(100)


    def disable_sliders_tracking(self):
        self.widget.distanceSlider.setTracking(False)
        self.widget.angularSlider.setTracking(False)
        self.widget.linearSlider.setTracking(False)


    def set_master_stop_state_updated_callback(self,callback_function):
        self.master_stop_state_updated_callback = callback_function

    def set_restrictions_updated_callback(self,callback_function):
        self.restrictions_updated_callback = callback_function

    def handle_restrictions_update(self):
        distance = (float(self.widget.distanceSlider.value())/100)
        angular_velocity = (float(self.widget.angularSlider.value())/100)
        linear_velocity = (float(self.widget.linearSlider.value())/100)

        self.widget.distanceLabel.setText("{:.2f}".format(distance))
        self.widget.angularLabel.setText("{:.2f}".format(angular_velocity))
        self.widget.linearLabel.setText("{:.2f}".format(linear_velocity))

        restrictions = Restrictions(distance,angular_velocity,linear_velocity)

        self.restrictions_updated_callback(restrictions)


    def connect_signals(self):
        self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update)
        self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update)
        self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update)
        self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked)

        self.set_sliders_to_initial_values()

    # 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_masterstopButton_clicked(self):
        if self.master_stop_state == SS.RUNNING:
            self.master_stop_state_updated_callback()
        else:
            reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokowac wszystkie roboty?",QMessageBox.Yes,QMessageBox.No)
            
            if reply == QMessageBox.Yes:
                self.master_stop_state_updated_callback()


    def display_master_stop_on(self):
        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
        self.widget.masterstopButton.setText('Zatrzymaj roboty')

    def display_master_stop_off(self):
        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
        self.widget.masterstopButton.setText('Odblokuj roboty')


    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'+str(robot_id))
    #     self.displayed_robots_id_list.append(robot_id)


    def master_stopped(self):
        self.master_stop_state = SS.STOPPED
        self.display_master_stop_off()
        self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty')

    def master_started(self):
        self.master_stop_state = SS.RUNNING
        self.display_master_stop_on()
        self.log_info('Przycisk masterSTOP odcisniety')


    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">' + str(time) + '. ' + info_text + '</font><br>')
        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">' + str(time) + '. ' + warning_text + '</font><br> ')
        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">' + str(time) + '. ' + error_text + '</font><br>')
        self.scroll_to_bottom()
        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)

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