368 lines
14 KiB
Python
368 lines
14 KiB
Python
#!/usr/bin/env python
|
|
# coding=utf-8
|
|
|
|
import os
|
|
|
|
import math
|
|
import datetime
|
|
|
|
from safety_user_plugin.enums.clutch_state import ClutchState as CS
|
|
from safety_user_plugin.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 safety_user_plugin.polish import PolishTexts as PT
|
|
from safety_user_plugin.english import EnglishTexts as ET
|
|
|
|
|
|
class QtWrapper:
|
|
def __init__(self, widget):
|
|
self.widget = widget
|
|
self.displayed_robots_id_list = []
|
|
|
|
# lang = rospy.get_param('lang')
|
|
lang = "pol"
|
|
|
|
if lang == "eng":
|
|
self.language = ET
|
|
self.log_info("Started english version of plugin NEW")
|
|
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)
|