244 lines
10 KiB
Python
244 lines
10 KiB
Python
|
#!/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 master_restrictions import Restrictions
|
||
|
|
|
||
|
|
from python_qt_binding.QtGui import QPixmap
|
||
|
|
from python_qt_binding.QtGui import QTextCursor
|
||
|
|
from python_qt_binding.QtGui import QStandardItem
|
||
|
|
from python_qt_binding.QtGui import QStandardItemModel
|
||
|
|
from python_qt_binding.QtGui import QBrush
|
||
|
|
from python_qt_binding.QtWidgets import QMessageBox
|
||
|
|
from python_qt_binding.QtWidgets import QHeaderView
|
||
|
|
from python_qt_binding.QtWidgets import QAbstractItemView
|
||
|
|
from python_qt_binding.QtCore import Qt
|
||
|
|
|
||
|
|
|
||
|
|
class QtWrapper:
|
||
|
|
|
||
|
|
def __init__(self,widget):
|
||
|
|
self.widget = widget
|
||
|
|
self.displayed_robots_id_list = []
|
||
|
|
|
||
|
|
self.master_stop_state = None
|
||
|
|
self.init_robots_list()
|
||
|
|
|
||
|
|
# self.disable_sliders_tracking()
|
||
|
|
|
||
|
|
ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_master_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)
|
||
|
|
self.widget.angularImage.setPixmap(self.angular_pixmap)
|
||
|
|
self.widget.linearImage.setPixmap(self.linear_pixmap)
|
||
|
|
|
||
|
|
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(75)
|
||
|
|
self.widget.angularSlider.setValue(100)
|
||
|
|
self.widget.linearSlider.setValue(100)
|
||
|
|
|
||
|
|
def init_robots_list(self):
|
||
|
|
self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList)
|
||
|
|
self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' )
|
||
|
|
self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' )
|
||
|
|
self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' )
|
||
|
|
self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' )
|
||
|
|
self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' )
|
||
|
|
self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' )
|
||
|
|
|
||
|
|
self.widget.robotsList.setModel(self.stdItemModel)
|
||
|
|
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch)
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch)
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch)
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch)
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch)
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, QHeaderView.Stretch)
|
||
|
|
|
||
|
|
self.widget.robotsList.horizontalHeader().setSectionsMovable(False)
|
||
|
|
# self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows)
|
||
|
|
self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection)
|
||
|
|
|
||
|
|
|
||
|
|
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_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 odblokować 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_robot_info(self,robot_info):
|
||
|
|
items_to_add = []
|
||
|
|
items_to_add.append(QStandardItem('PIONIER{}'.format(robot_info.robot_id)))
|
||
|
|
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage)))
|
||
|
|
|
||
|
|
if robot_info.state == SS.RUNNING:
|
||
|
|
item = QStandardItem('Działa')
|
||
|
|
item.setBackground(QBrush(Qt.green))
|
||
|
|
items_to_add.append(item)
|
||
|
|
elif robot_info.state == SS.STOPPED:
|
||
|
|
item = QStandardItem('Zatrzymany')
|
||
|
|
item.setBackground(QBrush(Qt.red))
|
||
|
|
items_to_add.append(item)
|
||
|
|
else:
|
||
|
|
raise ValueError('Undefined value of robot_info.robot_state')
|
||
|
|
|
||
|
|
if robot_info.clutch == CS.ENGAGED:
|
||
|
|
item = QStandardItem('Sprzęgnięte')
|
||
|
|
item.setBackground(QBrush(Qt.green))
|
||
|
|
items_to_add.append(item)
|
||
|
|
elif robot_info.clutch == CS.DISENGAGED:
|
||
|
|
item = QStandardItem('Rozprzęgnięte')
|
||
|
|
item.setBackground(QBrush(Qt.red))
|
||
|
|
items_to_add.append(item)
|
||
|
|
else:
|
||
|
|
raise ValueError('Undefined value of robot_info.clutch_state')
|
||
|
|
|
||
|
|
linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)
|
||
|
|
items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel)))
|
||
|
|
|
||
|
|
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2])))
|
||
|
|
|
||
|
|
|
||
|
|
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_info.robot_id),Qt.MatchExactly)
|
||
|
|
if len(items_list) > 0:
|
||
|
|
for item in items_list:
|
||
|
|
row_number = item.row()
|
||
|
|
else:
|
||
|
|
row_number = self.stdItemModel.rowCount()
|
||
|
|
|
||
|
|
for i,item in enumerate(items_to_add):
|
||
|
|
item.setTextAlignment(Qt.AlignCenter)
|
||
|
|
item.setEditable(0)
|
||
|
|
self.stdItemModel.setItem(row_number,i,item)
|
||
|
|
|
||
|
|
def master_stopped(self):
|
||
|
|
self.master_stop_state = SS.STOPPED
|
||
|
|
self.display_master_stop_off()
|
||
|
|
self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty')
|
||
|
|
|
||
|
|
def master_started(self):
|
||
|
|
self.master_stop_state = SS.RUNNING
|
||
|
|
self.display_master_stop_on()
|
||
|
|
self.log_info('Przycisk masterSTOP odciśnięty')
|
||
|
|
|
||
|
|
|
||
|
|
def add_robot(self,robot_id):
|
||
|
|
|
||
|
|
if robot_id not in self.displayed_robots_id_list:
|
||
|
|
new_robot_info=QStandardItem('PIONIER{}'.format(robot_id))
|
||
|
|
new_robot_info.setTextAlignment(Qt.AlignCenter)
|
||
|
|
new_robot_info.setEditable(0)
|
||
|
|
# new_robot_info.setBackground(QBrush(Qt.white))
|
||
|
|
self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info)
|
||
|
|
|
||
|
|
self.displayed_robots_id_list.append(robot_id)
|
||
|
|
self.log_info('PIONIER{0} połączony'.format(robot_id))
|
||
|
|
else:
|
||
|
|
raise RuntimeError('Adding robot, which is already in master GUI')
|
||
|
|
|
||
|
|
def remove_robot(self,robot_id):
|
||
|
|
items_list = self.stdItemModel.findItems('PIONIER{0}'.format(robot_id),Qt.MatchExactly)
|
||
|
|
|
||
|
|
if len(items_list) > 0:
|
||
|
|
for item in items_list:
|
||
|
|
row_number = item.row()
|
||
|
|
self.stdItemModel.removeRow(row_number)
|
||
|
|
|
||
|
|
self.log_info('PIONIER{0} rozłączony'.format(robot_id))
|
||
|
|
self.displayed_robots_id_list.remove(robot_id)
|
||
|
|
|
||
|
|
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.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text))
|
||
|
|
self.scroll_to_bottom()
|
||
|
|
|
||
|
|
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()
|
||
|
|
|
||
|
|
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()
|
||
|
|
|
||
|
|
def scroll_to_bottom(self):
|
||
|
|
scrollbar = self.widget.logsBrowser.verticalScrollBar()
|
||
|
|
scrollbar.setValue(scrollbar.maximum())
|