270 lines
11 KiB
Python
270 lines
11 KiB
Python
#!/usr/bin/env python
|
|
# coding=utf-8
|
|
import math
|
|
import datetime
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from safety_master_plugin.clutch_state import ClutchState as CS
|
|
from safety_master_plugin.stop_state import StopState as SS
|
|
from safety_master_plugin.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_updated_callback = None
|
|
self.restrictions_updated_callback = None
|
|
|
|
self.master_stop_state = None
|
|
self.init_robots_list()
|
|
|
|
# self.disable_sliders_tracking()
|
|
|
|
ui_directory_path = "{0}/ui".format(
|
|
get_package_share_directory("safety_master_plugin")
|
|
)
|
|
|
|
self.distance_pixmap = QPixmap(f"{ui_directory_path}/Distance.png")
|
|
self.angular_pixmap = QPixmap(f"{ui_directory_path}/Angular.png")
|
|
self.linear_pixmap = QPixmap(f"{ui_directory_path}/Linear.png")
|
|
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_master_stop_button_clicked
|
|
)
|
|
|
|
self.set_sliders_to_initial_values()
|
|
|
|
def handle_master_stop_button_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: red; font: bold 20px}"
|
|
)
|
|
self.widget.masterstopButton.setText("Odblokuj roboty")
|
|
|
|
def display_master_stop_off(self):
|
|
self.widget.masterstopButton.setStyleSheet(
|
|
"QPushButton { color: black; background-color: green; font: bold 20px}"
|
|
)
|
|
self.widget.masterstopButton.setText("Zatrzymaj roboty")
|
|
|
|
def update_robot_info(self, robot_info):
|
|
items_to_add = []
|
|
items_to_add.append(QStandardItem(f"pioneer{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(
|
|
f"pioneer{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 when_robots_disabled(self):
|
|
self.master_stop_state = SS.STOPPED
|
|
self.display_master_stop_on()
|
|
self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty")
|
|
|
|
def when_robots_enabled(self):
|
|
self.master_stop_state = SS.RUNNING
|
|
self.display_master_stop_off()
|
|
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("pioneer{}".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(f"pioneer{robot_id} połączony")
|
|
else:
|
|
raise RuntimeError("Adding robot, which is already in master GUI")
|
|
|
|
def remove_robot(self, robot_id):
|
|
items_list = self.stdItemModel.findItems(f"pioneer{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(f"pioneer{robot_id} rozłączony")
|
|
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(
|
|
f'<font color="black"> {time} {info_text}</font><br>'
|
|
)
|
|
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())
|