L1.5/safety_user_plugin
L1.5/safety_user_plugin
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

fixed includes

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-16 11:47:09 +02:00
parent 53784eecbe
commit ce405ab4f1
7 changed files with 382 additions and 287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
safety_user_plugin/english.py
View File

@ -1,16 +1,16 @@
#!/usr/bin/env python #!/usr/bin/env python
# coding=utf-8 # coding=utf-8
from language import LanguageTexts from safety_user_plugin.language import LanguageTexts
class EnglishTexts(LanguageTexts): class EnglishTexts(LanguageTexts):
# Warning popup for robot unlocking # Warning popup for robot unlocking
warning_text_1 = "ATTENTION" warning_text_1 = "ATTENTION"
warning_text_2 = "Are You sure You want to unlock the robot?" warning_text_2 = "Are You sure You want to unlock the robot?"
# Button texts # Button texts
select_text = 'Select' select_text = "Select"
stop_robot_text = "STOP robot" stop_robot_text = "STOP robot"
unlock_robot_text = "UNLOCK robot" unlock_robot_text = "UNLOCK robot"
@ -18,35 +18,36 @@ class EnglishTexts(LanguageTexts):
disengage_clutch_text = "Disengage clutch" disengage_clutch_text = "Disengage clutch"
engage_clutch_text = "Engage clutch" engage_clutch_text = "Engage clutch"
release_robot_text = 'Release robot' release_robot_text = "Release robot"
# Logger info texts # Logger info texts
robot_selected_text = 'PIONIER{0} selected!' robot_selected_text = "PIONIER{0} selected!"
robot_released_text = 'Robot released' robot_released_text = "Robot released"
stopped_and_engaged_text = "Robot will be stopped and clutch engaged" stopped_and_engaged_text = "Robot will be stopped and clutch engaged"
master_stop_clicked_text = 'MasterSTOP button pressed. Stopping the robots' master_stop_clicked_text = "MasterSTOP button pressed. Stopping the robots"
master_stop_released_text = 'MasterSTOP button released. Robot can be unlocked' master_stop_released_text = "MasterSTOP button released. Robot can be unlocked"
robot_stopped_text = 'Robot stopped' robot_stopped_text = "Robot stopped"
robot_started_text = 'Robot started' robot_started_text = "Robot started"
clutch_disengaged_text = 'Disengaging the clutch' clutch_disengaged_text = "Disengaging the clutch"
clutch_engaged_text = 'Engaging the clutch' clutch_engaged_text = "Engaging the clutch"
obstacle_detected_text = 'Obstacle detected. Robot stopped'
obstacle_removed_text = 'Obstacle removed'
obstacle_detected_text = "Obstacle detected. Robot stopped"
obstacle_removed_text = "Obstacle removed"
# Logger error/problem messages # Logger error/problem messages
selection_error_text = 'Select the PIONEER from robots list first' selection_error_text = "Select the PIONEER from robots list first"
cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!" cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!"
cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!" cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!"
cannot_select_robot_text = "Robot can't be selected. It is already selected by another group" cannot_select_robot_text = (
connection_to_robot_lost_text = 'Connection lost with robot PIONIER{0}' "Robot can't be selected. It is already selected by another group"
connection_to_master_lost_text = 'Connection lost with masterSTOP. Ask the teacher to run it' )
connection_to_robot_lost_text = "Connection lost with robot PIONIER{0}"
connection_to_master_lost_text = (
"Connection lost with masterSTOP. Ask the teacher to run it"
)
# Description labels # Description labels
linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Linear velocity</span></p></body></html>' linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Linear velocity</span></p></body></html>'
@ -59,7 +60,5 @@ class EnglishTexts(LanguageTexts):
restrictions_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Restrictions</span></p></body></html>' restrictions_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Restrictions</span></p></body></html>'
history_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Message history</span></p></body></html>' history_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Message history</span></p></body></html>'
def __init__(self): def __init__(self):
pass pass

53
safety_user_plugin/polish.py
View File

@ -1,16 +1,16 @@
#!/usr/bin/env python #!/usr/bin/env python
# coding=utf-8 # coding=utf-8
from language import LanguageTexts from safety_user_plugin.language import LanguageTexts
class PolishTexts(LanguageTexts): class PolishTexts(LanguageTexts):
# Warning popup for robot unlocking # Warning popup for robot unlocking
warning_text_1 = "UWAGA" warning_text_1 = "UWAGA"
warning_text_2 = "Na pewno chcesz odblokować robota?" warning_text_2 = "Na pewno chcesz odblokować robota?"
# Button texts # Button texts
select_text = 'Wybierz' select_text = "Wybierz"
stop_robot_text = "Zatrzymaj robota" stop_robot_text = "Zatrzymaj robota"
unlock_robot_text = "Odblokuj robota" unlock_robot_text = "Odblokuj robota"
@ -18,35 +18,42 @@ class PolishTexts(LanguageTexts):
disengage_clutch_text = "Rozłącz sprzęgło" disengage_clutch_text = "Rozłącz sprzęgło"
engage_clutch_text = "Połącz sprzęgło" engage_clutch_text = "Połącz sprzęgło"
release_robot_text = 'Zwolnij robota' release_robot_text = "Zwolnij robota"
# Logger info texts # Logger info texts
robot_selected_text = 'PIONIER{0} wybrany!' robot_selected_text = "PIONIER{0} wybrany!"
robot_released_text = 'Odłączam robota' robot_released_text = "Odłączam robota"
stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty" stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty"
master_stop_clicked_text = 'Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty' master_stop_clicked_text = (
master_stop_released_text = 'Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota' "Przycisk masterSTOP został naciśnięty. Zatrzymuje roboty"
)
master_stop_released_text = (
"Przycisk masterSTOP odciśnięty. Mozesz uruchomić robota"
)
robot_stopped_text = 'Robot zatrzymany' robot_stopped_text = "Robot zatrzymany"
robot_started_text = 'Robot wystartował' robot_started_text = "Robot wystartował"
clutch_disengaged_text = 'Rozprzęgam sprzęgło' clutch_disengaged_text = "Rozprzęgam sprzęgło"
clutch_engaged_text = 'Sprzęgam sprzegło' clutch_engaged_text = "Sprzęgam sprzegło"
obstacle_detected_text = 'Przeszkoda wykryta. Zatrzymuję robota'
obstacle_removed_text = 'Przeszkoda usunięta'
obstacle_detected_text = "Przeszkoda wykryta. Zatrzymuję robota"
obstacle_removed_text = "Przeszkoda usunięta"
# Logger error/problem messages # Logger error/problem messages
selection_error_text = 'Najpierw wybierz PIONIERA z listy robotów' selection_error_text = "Najpierw wybierz PIONIERA z listy robotów"
cannot_start_masterstop_text = 'Nie można wystartować robota. MasterSTOP wciśnięty!' cannot_start_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!"
cannot_start_obstacle_text = 'Nie mozna wystartować. Przeszkoda w polu działania robota' cannot_start_obstacle_text = (
cannot_select_robot_text = 'Nie mozna wybrać robota. Robot został już wybrany przez inną grupę' "Nie mozna wystartować. Przeszkoda w polu działania robota"
connection_to_robot_lost_text = 'Utrata połączenia z robotem PIONIER{0}' )
connection_to_master_lost_text = 'Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie' cannot_select_robot_text = (
"Nie mozna wybrać robota. Robot został już wybrany przez inną grupę"
)
connection_to_robot_lost_text = "Utrata połączenia z robotem PIONIER{0}"
connection_to_master_lost_text = (
"Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie"
)
# Description labels # Description labels
linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Prędkość liniowa</span></p></body></html>' linear_text = '<html><head/><body><p align="center"><span style=" font-weight:600;">Prędkość liniowa</span></p></body></html>'

195
safety_user_plugin/qt_wrapper.py
View File

@ -2,53 +2,56 @@
# coding=utf-8 # coding=utf-8
import os import os
import rospy
import rospkg
import math import math
import datetime import datetime
from enums.clutch_state import ClutchState as CS from safety_user_plugin.enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS from safety_user_plugin.enums.stop_state import StopState as SS
from python_qt_binding.QtGui import QPixmap from python_qt_binding.QtGui import QPixmap
from python_qt_binding.QtGui import QTextCursor from python_qt_binding.QtGui import QTextCursor
from python_qt_binding.QtWidgets import QMessageBox from python_qt_binding.QtWidgets import QMessageBox
from polish import PolishTexts as PT from safety_user_plugin.polish import PolishTexts as PT
from english import EnglishTexts as ET from safety_user_plugin.english import EnglishTexts as ET
class QtWrapper: class QtWrapper:
def __init__(self, widget):
def __init__(self,widget):
self.widget = widget self.widget = widget
self.displayed_robots_id_list = [] self.displayed_robots_id_list = []
lang = rospy.get_param('lang') # lang = rospy.get_param('lang')
lang = "pol"
if lang == 'eng': if lang == "eng":
self.language = ET self.language = ET
self.log_info('Started english version of plugin NEW') self.log_info("Started english version of plugin NEW")
elif lang == 'pol': elif lang == "pol":
self.language = PT self.language = PT
self.log_info('Polska wersja pluginu uruchomiona') self.log_info("Polska wersja pluginu uruchomiona")
else: else:
raise ValueError('language parameter has value different than "eng" or "pol"') raise ValueError(
'language parameter has value different than "eng" or "pol"'
)
self.clear_robot_info() self.clear_robot_info()
ui_directory_path = '{0}/ui'.format(rospkg.RosPack().get_path('safety_user_plugin')) 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.distance_pixmap = QPixmap("{0}/Distance.png".format(ui_directory_path))
self.angular_pixmap = QPixmap('{0}/Angular.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.linear_pixmap = QPixmap("{0}/Linear.png".format(ui_directory_path))
self.widget.distanceImage.setPixmap(self.distance_pixmap.scaled(66,46)) self.widget.distanceImage.setPixmap(self.distance_pixmap.scaled(66, 46))
self.widget.angularImage.setPixmap(self.angular_pixmap) self.widget.angularImage.setPixmap(self.angular_pixmap)
self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20,48)) self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20, 48))
self.ok_pixmap = QPixmap('{0}/Ok.jpg'.format(ui_directory_path)) self.ok_pixmap = QPixmap("{0}/Ok.jpg".format(ui_directory_path))
self.cancel_pixmap = QPixmap('{0}/Cancel.jpg'.format(ui_directory_path)) self.cancel_pixmap = QPixmap("{0}/Cancel.jpg".format(ui_directory_path))
def disconnect_signals(self): def disconnect_signals(self):
self.widget.clutchButton.clicked.disconnect() self.widget.clutchButton.clicked.disconnect()
@ -56,7 +59,9 @@ class QtWrapper:
self.widget.choiceButton.clicked.disconnect() self.widget.choiceButton.clicked.disconnect()
def connect_robot_signals(self): def connect_robot_signals(self):
self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback) self.widget.clutchButton.clicked.connect(
self.handle_clutchButton_clicked_callback
)
self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked) self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked)
self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked) self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked)
@ -65,56 +70,67 @@ class QtWrapper:
self.widget.stopButton.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) 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(self,method_name): def handle_emitted_signal_with_list_argument(self, method_name, argument):
exec('self.{0}()'.format(method_name)) if method_name == "update_selected_robot_info":
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]) self.update_selected_robot_info(argument[0])
else: else:
method_with_argument = 'self.{0}({1})'.format(method_name,argument[0]) method_with_argument = "self.{0}({1})".format(method_name, argument[0])
exec(method_with_argument) exec(method_with_argument)
def handle_stopButton_clicked(self): def handle_stopButton_clicked(self):
if self.robot_state == SS.RUNNING: if self.robot_state == SS.RUNNING:
self.handle_stopButton_clicked_callback() self.handle_stopButton_clicked_callback()
else: else:
reply = QMessageBox.warning(self.widget,self.language.warning_text_1,self.language.warning_text_2,QMessageBox.Yes,QMessageBox.No) reply = QMessageBox.warning(
self.widget,
self.language.warning_text_1,
self.language.warning_text_2,
QMessageBox.Yes,
QMessageBox.No,
)
if reply == QMessageBox.Yes: if reply == QMessageBox.Yes:
self.handle_stopButton_clicked_callback() self.handle_stopButton_clicked_callback()
def get_selected_robot_id(self): def get_selected_robot_id(self):
current_text = self.widget.robotsList.currentText() current_text = self.widget.robotsList.currentText()
if 'PIONIER' in current_text: if "PIONIER" in current_text:
return int(current_text[7:]) return int(current_text[7:])
else: else:
return None return None
def display_clutch_on(self): def display_clutch_on(self):
# self.widget.clutchLabel.setPixmap(self.ok_pixmap.scaled(40,40)) # 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.setStyleSheet(
"QPushButton { color: black; background-color: green; font: bold 20px}"
)
self.widget.clutchButton.setText(self.language.disengage_clutch_text) self.widget.clutchButton.setText(self.language.disengage_clutch_text)
def display_clutch_off(self): def display_clutch_off(self):
# self.widget.clutchLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) # 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.setStyleSheet(
"QPushButton { color: black; background-color: red; font: bold 20px}"
)
self.widget.clutchButton.setText(self.language.engage_clutch_text) self.widget.clutchButton.setText(self.language.engage_clutch_text)
def display_state_on(self): def display_state_on(self):
self.widget.stateLabel.setPixmap(self.ok_pixmap.scaled(40,40)) 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.setStyleSheet(
"QPushButton { color: black; background-color: green; font: bold 20px}"
)
self.widget.stopButton.setText(self.language.stop_robot_text) self.widget.stopButton.setText(self.language.stop_robot_text)
self.robot_state = SS.RUNNING self.robot_state = SS.RUNNING
def display_state_off(self): def display_state_off(self):
self.widget.stateLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) 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.setStyleSheet(
"QPushButton { color: black; background-color: red; font: bold 20px}"
)
self.widget.stopButton.setText(self.language.unlock_robot_text) self.widget.stopButton.setText(self.language.unlock_robot_text)
self.robot_state = SS.STOPPED self.robot_state = SS.STOPPED
@ -132,19 +148,19 @@ class QtWrapper:
def handle_closeButton_clicked(self): def handle_closeButton_clicked(self):
self.handle_closeButton_clicked_callback() self.handle_closeButton_clicked_callback()
def set_robot_selection_callback(self,callback): def set_robot_selection_callback(self, callback):
self.handle_openButton_clicked_callback = callback self.handle_openButton_clicked_callback = callback
def set_robot_release_callback(self,callback): def set_robot_release_callback(self, callback):
self.handle_closeButton_clicked_callback = callback self.handle_closeButton_clicked_callback = callback
def set_user_stop_state_updated_callback(self,callback): def set_user_stop_state_updated_callback(self, callback):
self.handle_stopButton_clicked_callback = callback self.handle_stopButton_clicked_callback = callback
def set_clutch_switched_callback(self,callback): def set_clutch_switched_callback(self, callback):
self.handle_clutchButton_clicked_callback = callback self.handle_clutchButton_clicked_callback = callback
def select_robot(self,robot_id): def select_robot(self, robot_id):
self.disconnect_signals() self.disconnect_signals()
self.connect_robot_signals() self.connect_robot_signals()
self.log_info(self.language.robot_selected_text.format(robot_id)) self.log_info(self.language.robot_selected_text.format(robot_id))
@ -158,9 +174,9 @@ class QtWrapper:
self.log_info(self.language.robot_released_text) self.log_info(self.language.robot_released_text)
self.clear_robot_info() self.clear_robot_info()
def update_robots_list_gui(self,robots_id_list): def update_robots_list_gui(self, robots_id_list):
robots_id_list.sort() robots_id_list.sort()
id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list) id_strings_list = (("PIONIER" + str(x)) for x in robots_id_list)
robots_to_add = [] robots_to_add = []
robots_to_remove = [] robots_to_remove = []
@ -179,8 +195,7 @@ class QtWrapper:
for robot_id in robots_to_add: for robot_id in robots_to_add:
self.add_robot_to_list(robot_id) self.add_robot_to_list(robot_id)
def remove_robot_from_list(self, robot_id):
def remove_robot_from_list(self,robot_id):
count = self.widget.robotsList.count() count = self.widget.robotsList.count()
for i in range(count): for i in range(count):
@ -189,19 +204,21 @@ class QtWrapper:
self.displayed_robots_id_list.remove(robot_id) self.displayed_robots_id_list.remove(robot_id)
return return
def add_robot_to_list(self, robot_id):
def add_robot_to_list(self,robot_id): self.widget.robotsList.addItem("PIONIER{0}".format(robot_id))
self.widget.robotsList.addItem('PIONIER{0}'.format(robot_id))
self.displayed_robots_id_list.append(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
)
def update_selected_robot_info(self,robot_info): self.widget.idLabel.setText("PIONIER{0}".format(robot_info.robot_id))
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.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage))
self.widget.linearLabel.setText("{:.2f}".format(linear_vel)) self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2])) self.widget.angularLabel.setText(
"{:.2f}".format(robot_info.angular_velocity[2])
)
# self.log_info(str(robot_info.clutch == CS.ENGAGED)) # self.log_info(str(robot_info.clutch == CS.ENGAGED))
@ -216,16 +233,16 @@ class QtWrapper:
self.display_state_off() self.display_state_off()
if robot_info.obstacle_detected == True: if robot_info.obstacle_detected == True:
self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) self.widget.obstaclestopLabel.setPixmap(self.cancel_pixmap.scaled(40, 40))
else: else:
self.widget.obstaclestopLabel.setPixmap(self.ok_pixmap.scaled(40,40)) self.widget.obstaclestopLabel.setPixmap(self.ok_pixmap.scaled(40, 40))
def master_stopped(self): def master_stopped(self):
self.widget.masterstopLabel.setPixmap(self.cancel_pixmap.scaled(40,40)) self.widget.masterstopLabel.setPixmap(self.cancel_pixmap.scaled(40, 40))
self.log_info(self.language.master_stop_clicked_text) self.log_info(self.language.master_stop_clicked_text)
def master_started(self): def master_started(self):
self.widget.masterstopLabel.setPixmap(self.ok_pixmap.scaled(40,40)) self.widget.masterstopLabel.setPixmap(self.ok_pixmap.scaled(40, 40))
self.log_info(self.language.master_stop_released_text) self.log_info(self.language.master_stop_released_text)
def user_stopped(self): def user_stopped(self):
@ -253,44 +270,56 @@ class QtWrapper:
def robot_selected_by_another_group_notify(self): def robot_selected_by_another_group_notify(self):
self.log_error(self.language.cannot_select_robot_text) self.log_error(self.language.cannot_select_robot_text)
def update_restrictions_gui(self,restrictions): def update_restrictions_gui(self, restrictions):
self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance)) self.widget.distanceRestrictionLabel.setText(
self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity)) "{:.2f}".format(restrictions.distance)
self.widget.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity)) )
self.widget.linearRestrictionLabel.setText(
"{:.2f}".format(restrictions.linear_velocity)
)
self.widget.angularRestrictionLabel.setText(
"{:.2f}".format(restrictions.angular_velocity)
)
def log_info(self,info_text): def log_info(self, info_text):
time = datetime.datetime.now().strftime('[%H:%M:%S]') time = datetime.datetime.now().strftime("[%H:%M:%S]")
cursor = self.widget.logsBrowser.textCursor() cursor = self.widget.logsBrowser.textCursor()
cursor.movePosition(QTextCursor.End) cursor.movePosition(QTextCursor.End)
self.widget.logsBrowser.setTextCursor(cursor) self.widget.logsBrowser.setTextCursor(cursor)
self.widget.logsBrowser.insertHtml('<font color="black"> {0} {1}</font><br>'.format(time,info_text)) self.widget.logsBrowser.insertHtml(
'<font color="black"> {0} {1}</font><br>'.format(time, info_text)
)
self.scroll_to_bottom() self.scroll_to_bottom()
# self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_text) # self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_text)
def log_warning(self,warning_text): def log_warning(self, warning_text):
time = datetime.datetime.now().strftime('[%H:%M:%S]') time = datetime.datetime.now().strftime("[%H:%M:%S]")
cursor = self.widget.logsBrowser.textCursor() cursor = self.widget.logsBrowser.textCursor()
cursor.movePosition(QTextCursor.End) cursor.movePosition(QTextCursor.End)
self.widget.logsBrowser.setTextCursor(cursor) self.widget.logsBrowser.setTextCursor(cursor)
self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
self.widget.logsBrowser.insertHtml('<font color="orange"> {0} {1}</font><br>'.format(time,warning_text)) self.widget.logsBrowser.insertHtml(
'<font color="orange"> {0} {1}</font><br>'.format(time, warning_text)
)
self.scroll_to_bottom() self.scroll_to_bottom()
# self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text) # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text)
def log_error(self,error_text): def log_error(self, error_text):
time = datetime.datetime.now().strftime('[%H:%M:%S]') time = datetime.datetime.now().strftime("[%H:%M:%S]")
cursor = self.widget.logsBrowser.textCursor() cursor = self.widget.logsBrowser.textCursor()
cursor.movePosition(QTextCursor.End) cursor.movePosition(QTextCursor.End)
self.widget.logsBrowser.setTextCursor(cursor) self.widget.logsBrowser.setTextCursor(cursor)
self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
self.widget.logsBrowser.insertHtml('<font color="red"> {0} {1}</font><br>'.format(time,error_text)) self.widget.logsBrowser.insertHtml(
'<font color="red"> {0} {1}</font><br>'.format(time, error_text)
)
self.scroll_to_bottom() self.scroll_to_bottom()
# self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text) # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)
def connection_to_robot_lost(self,lost_robot_id): def connection_to_robot_lost(self, lost_robot_id):
self.clear_robot_info() self.clear_robot_info()
self.disconnect_signals() self.disconnect_signals()
self.connect_signals() self.connect_signals()
@ -311,20 +340,20 @@ class QtWrapper:
scrollbar.setValue(scrollbar.maximum()) scrollbar.setValue(scrollbar.maximum())
def clear_robot_info(self): def clear_robot_info(self):
self.widget.idLabel.setText('-') self.widget.idLabel.setText("-")
self.widget.batteryLabel.setText('-') self.widget.batteryLabel.setText("-")
self.widget.linearLabel.setText('-') self.widget.linearLabel.setText("-")
self.widget.angularLabel.setText('-') self.widget.angularLabel.setText("-")
self.widget.stateLabel.setText('-') self.widget.stateLabel.setText("-")
self.widget.masterstopLabel.setText('-') self.widget.masterstopLabel.setText("-")
self.widget.obstaclestopLabel.setText('-') self.widget.obstaclestopLabel.setText("-")
self.widget.choiceButton.setText(self.language.select_text) self.widget.choiceButton.setText(self.language.select_text)
self.widget.stopButton.setText('-') self.widget.stopButton.setText("-")
self.widget.stopButton.setStyleSheet("") self.widget.stopButton.setStyleSheet("")
self.widget.clutchButton.setText('-') self.widget.clutchButton.setText("-")
self.widget.clutchButton.setStyleSheet("") self.widget.clutchButton.setStyleSheet("")
self.widget.angularText.setText(self.language.angular_text) self.widget.angularText.setText(self.language.angular_text)

22
safety_user_plugin/robot_info.py
View File

@ -1,11 +1,11 @@
#!/usr/bin/env python #!/usr/bin/env python
from enums.clutch_state import ClutchState as CS from safety_user_plugin.enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS from safety_user_plugin.enums.stop_state import StopState as SS
class RobotInfo: class RobotInfo:
def __init__(self, robot_id):
def __init__(self,robot_id):
self.robot_id = robot_id self.robot_id = robot_id
self.battery_voltage = None self.battery_voltage = None
self.linear_velocity = None self.linear_velocity = None
@ -14,11 +14,19 @@ class RobotInfo:
self.clutch = None self.clutch = None
self.obstacle_detected = None self.obstacle_detected = None
def update_robot_info(self,robot_info_msg): def update_robot_info(self, robot_info_msg):
self.robot_id = robot_info_msg.robot_id.data self.robot_id = robot_info_msg.robot_id.data
self.battery_voltage = robot_info_msg.battery_voltage.data self.battery_voltage = robot_info_msg.battery_voltage.data
self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z] self.linear_velocity = [
self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z] robot_info_msg.twist.linear.x,
robot_info_msg.twist.linear.y,
robot_info_msg.twist.linear.z,
]
self.angular_velocity = [
robot_info_msg.twist.angular.x,
robot_info_msg.twist.angular.y,
robot_info_msg.twist.angular.z,
]
self.obstacle_detected = robot_info_msg.obstacle_detected.data self.obstacle_detected = robot_info_msg.obstacle_detected.data
if robot_info_msg.state.data == True: if robot_info_msg.state.data == True:

147
safety_user_plugin/ros_wrapper.py
View File

@ -1,20 +1,18 @@
#!/usr/bin/env python #!/usr/bin/env python
import rospy import rclpy
import rospkg
import rostopic
from std_msgs.msg import Bool from std_msgs.msg import Bool
from rosaria_msgs.msg import RobotInfoMsg from ros2aria_msgs.msg import RobotInfoMsg
from rosaria_msgs.msg import RestrictionsMsg from ros2aria_msgs.msg import RestrictionsMsg
from robot_info import RobotInfo from safety_user_plugin.robot_info import RobotInfo
from restrictions import Restrictions from safety_user_plugin.restrictions import Restrictions
from safety_user_plugin.enums.clutch_state import ClutchState as CS
from safety_user_plugin.enums.stop_state import StopState as SS
from enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS
class ROSWrapper: class ROSWrapper:
def __init__(self): def __init__(self):
self.robots_list_update_callback = None self.robots_list_update_callback = None
self.selected_robot_info_update_callback = None self.selected_robot_info_update_callback = None
@ -35,7 +33,7 @@ class ROSWrapper:
self.robots_list_timer = None self.robots_list_timer = None
self.connection_timer = None self.connection_timer = None
self.master_connection_timer = None self.master_connection_timer = None
self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values) self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
self.robots_hz_subscribers_dict = {} self.robots_hz_subscribers_dict = {}
self.rostopics_hz_dict = {} self.rostopics_hz_dict = {}
@ -44,45 +42,63 @@ class ROSWrapper:
# def setup_node(self): # def setup_node(self):
# rospy.init_node('safety_user_plugin', anonymous=True) # rospy.init_node('safety_user_plugin', anonymous=True)
def setup_subscribers_and_publishers(self,robot_id): def setup_subscribers_and_publishers(self, robot_id):
robot_name = 'PIONIER' + str(robot_id) robot_name = "PIONIER" + str(robot_id)
self.robot_info_subscriber = rospy.Subscriber('/{0}/RosAria/robot_info'.format(robot_name), RobotInfoMsg, self.handle_selected_robot_info_update) self.robot_info_subscriber = rospy.Subscriber(
self.master_stop_subscriber = rospy.Subscriber('/PIONIER/master_stop', Bool, self.handle_master_stop_update) "/{0}/RosAria/robot_info".format(robot_name),
self.restrictions_subscriber = rospy.Subscriber('/PIONIER/restrictions', RestrictionsMsg, self.handle_restrictions_update) RobotInfoMsg,
self.handle_selected_robot_info_update,
)
self.master_stop_subscriber = rospy.Subscriber(
"/PIONIER/master_stop", Bool, self.handle_master_stop_update
)
self.restrictions_subscriber = rospy.Subscriber(
"/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update
)
self.user_stop_publisher = rospy.Publisher('/{0}/RosAria/user_stop'.format(robot_name),Bool,queue_size = 1) self.user_stop_publisher = rospy.Publisher(
self.clutch_publisher = rospy.Publisher('/{0}/RosAria/clutch'.format(robot_name),Bool,queue_size = 1) "/{0}/RosAria/user_stop".format(robot_name), Bool, queue_size=1
)
self.clutch_publisher = rospy.Publisher(
"/{0}/RosAria/clutch".format(robot_name), Bool, queue_size=1
)
if self.periodic_timer != None: if self.periodic_timer != None:
self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.periodic_timer)
self.periodic_timer = rospy.Timer(rospy.Duration(0.1),self.publish_periodic_update) self.periodic_timer = rospy.Timer(
rospy.Duration(0.1), self.publish_periodic_update
)
print("NEW") print("NEW")
if self.connection_timer != None: if self.connection_timer != None:
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
self.connection_timer = rospy.Timer(rospy.Duration(5),self.handle_robot_connection_lost) self.connection_timer = rospy.Timer(
rospy.Duration(5), self.handle_robot_connection_lost
)
if self.master_connection_timer != None: if self.master_connection_timer != None:
self.shutdown_timer(self.master_connection_timer) self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost) self.master_connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_master_connection_lost
)
if self.hz_update_timer != None: if self.hz_update_timer != None:
self.shutdown_timer(self.hz_update_timer) self.shutdown_timer(self.hz_update_timer)
self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values) self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values)
def unsubscribe(self,subscriber): def unsubscribe(self, subscriber):
if subscriber != None: if subscriber != None:
subscriber.unregister() subscriber.unregister()
def unregister_publisher(self,publisher): def unregister_publisher(self, publisher):
if publisher != None: if publisher != None:
publisher.unregister() publisher.unregister()
def shutdown_timer(self,timer): def shutdown_timer(self, timer):
if timer != None: if timer != None:
timer.shutdown() timer.shutdown()
def publish_periodic_update(self,event): def publish_periodic_update(self, event):
stop_state = self.get_user_stop_state() stop_state = self.get_user_stop_state()
# clutch_state = self.get_clutch_state() # clutch_state = self.get_clutch_state()
@ -91,7 +107,9 @@ class ROSWrapper:
elif stop_state == SS.STOPPED: elif stop_state == SS.STOPPED:
self.user_stopped() self.user_stopped()
else: else:
raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') raise ValueError(
"stop_state UNKNOWN when attempting to publish periodic update"
)
# if clutch_state == CS.ENGAGED: # if clutch_state == CS.ENGAGED:
# self.engage_clutch() # self.engage_clutch()
@ -100,16 +118,17 @@ class ROSWrapper:
# else: # else:
# raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update') # raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
def update_hz_values(self,event): def update_hz_values(self, event):
for key in self.rostopics_hz_dict: for key in self.rostopics_hz_dict:
self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz('/PIONIER{0}/RosAria/user_stop'.format(key)) self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
"/PIONIER{0}/RosAria/user_stop".format(key)
)
def handle_robot_connection_lost(self, event):
def handle_robot_connection_lost(self,event):
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
self.robot_connection_lost_callback() self.robot_connection_lost_callback()
def handle_master_connection_lost(self,event): def handle_master_connection_lost(self, event):
self.shutdown_timer(self.master_connection_timer) self.shutdown_timer(self.master_connection_timer)
self.master_connection_lost_callback() self.master_connection_lost_callback()
@ -131,59 +150,70 @@ class ROSWrapper:
def restart_connection_timer(self): def restart_connection_timer(self):
if self.connection_timer != None: if self.connection_timer != None:
self.connection_timer.shutdown() self.connection_timer.shutdown()
self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost) self.connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_robot_connection_lost
)
else: else:
raise RuntimeError('Attempting to restart connection_timer when it is not initialized') raise RuntimeError(
"Attempting to restart connection_timer when it is not initialized"
)
def restart_master_connection_timer(self): def restart_master_connection_timer(self):
if self.master_connection_timer != None: if self.master_connection_timer != None:
self.master_connection_timer.shutdown() self.master_connection_timer.shutdown()
self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost) self.master_connection_timer = rospy.Timer(
rospy.Duration(1.5), self.handle_master_connection_lost
)
else: else:
raise RuntimeError('Attempting to restart master_connection_timer when it is not initialized') raise RuntimeError(
"Attempting to restart master_connection_timer when it is not initialized"
)
def unregister_node(self): def unregister_node(self):
self.cancel_subscribers_and_publishers() self.cancel_subscribers_and_publishers()
rospy.signal_shutdown('Closing safety user plugin') rospy.signal_shutdown("Closing safety user plugin")
def select_robot(self,robot_id): def select_robot(self, robot_id):
self.setup_subscribers_and_publishers(robot_id) self.setup_subscribers_and_publishers(robot_id)
def setup_get_user_stop_state_function(self,function): def setup_get_user_stop_state_function(self, function):
self.get_user_stop_state = function self.get_user_stop_state = function
def setup_get_clutch_state_function(self,function): def setup_get_clutch_state_function(self, function):
self.get_clutch_state = function self.get_clutch_state = function
# ROSWrapper Callbacks # ROSWrapper Callbacks
def get_robots_list(self,event): def get_robots_list(self, event):
robots_id_list = [] robots_id_list = []
published_topics_list = rospy.get_published_topics(namespace='/') published_topics_list = rospy.get_published_topics(namespace="/")
published_topics = [] published_topics = []
for list_ in published_topics_list: for list_ in published_topics_list:
published_topics.append(list_[0]) published_topics.append(list_[0])
for topic in published_topics: for topic in published_topics:
if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1: if topic.find("RosAria") == -1 or topic.find("robot_info") == -1:
pass pass
else: else:
robot_number = topic.split('/') robot_number = topic.split("/")
robot_number = robot_number[1] robot_number = robot_number[1]
robot_number = robot_number[7:] robot_number = robot_number[7:]
if len(robot_number) > 0: if len(robot_number) > 0:
robot_number = int(robot_number) robot_number = int(robot_number)
robots_id_list.append(robot_number) robots_id_list.append(robot_number)
# Adding new hz subscribers # Adding new hz subscribers
for robot_id in robots_id_list: for robot_id in robots_id_list:
if robot_id not in self.robots_hz_subscribers_dict: if robot_id not in self.robots_hz_subscribers_dict:
# Add hz subscriber # Add hz subscriber
self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1) self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber('/PIONIER{0}/RosAria/user_stop'.format(robot_id),Bool, self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber(
self.rostopics_hz_dict[robot_id].callback_hz,callback_args='/PIONIER{0}/RosAria/user_stop'.format(robot_id)) "/PIONIER{0}/RosAria/user_stop".format(robot_id),
Bool,
self.rostopics_hz_dict[robot_id].callback_hz,
callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id),
)
# Removing old hz subscribers # Removing old hz subscribers
for robot_key in self.robots_hz_subscribers_dict: for robot_key in self.robots_hz_subscribers_dict:
@ -193,17 +223,15 @@ class ROSWrapper:
self.robots_hz_subscribers_dict[robot_id] = None self.robots_hz_subscribers_dict[robot_id] = None
self.rostopics_hz_dict[robot_id] = None self.rostopics_hz_dict[robot_id] = None
self.robots_list_update_callback(robots_id_list) self.robots_list_update_callback(robots_id_list)
def release_robot(self): def release_robot(self):
self.unsubscribe(self.robot_info_subscriber) self.unsubscribe(self.robot_info_subscriber)
self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
self.shutdown_timer(self.master_connection_timer) self.shutdown_timer(self.master_connection_timer)
def handle_selected_robot_info_update(self,msg): def handle_selected_robot_info_update(self, msg):
# Restarting connection timer to avoid raising robot_connection_lost_callback # Restarting connection timer to avoid raising robot_connection_lost_callback
self.restart_connection_timer() self.restart_connection_timer()
@ -211,7 +239,7 @@ class ROSWrapper:
_robot_info.update_robot_info(msg) _robot_info.update_robot_info(msg)
self.selected_robot_info_update_callback(_robot_info) self.selected_robot_info_update_callback(_robot_info)
def handle_master_stop_update(self,msg): def handle_master_stop_update(self, msg):
# Restarting master connection timer to avoid raising master_connection_lost_callback # Restarting master connection timer to avoid raising master_connection_lost_callback
self.restart_master_connection_timer() self.restart_master_connection_timer()
@ -223,28 +251,28 @@ class ROSWrapper:
self.master_stop_update_callback(master_stop_state) self.master_stop_update_callback(master_stop_state)
def handle_restrictions_update(self,msg): def handle_restrictions_update(self, msg):
restrictions = Restrictions(msg) restrictions = Restrictions(msg)
self.restrictions_update_callback(restrictions) self.restrictions_update_callback(restrictions)
# UserPlugin Callbacks # UserPlugin Callbacks
def set_robots_list_update_callback(self,callback_function): def set_robots_list_update_callback(self, callback_function):
self.robots_list_update_callback = callback_function self.robots_list_update_callback = callback_function
self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list) self.robots_list_timer = rospy.Timer(rospy.Duration(0.5), self.get_robots_list)
def set_selected_robot_info_update_callback(self,callback_function): def set_selected_robot_info_update_callback(self, callback_function):
self.selected_robot_info_update_callback = callback_function self.selected_robot_info_update_callback = callback_function
def set_master_stop_update_callback(self,callback_function): def set_master_stop_update_callback(self, callback_function):
self.master_stop_update_callback = callback_function self.master_stop_update_callback = callback_function
def set_restrictions_update_callback(self,callback_function): def set_restrictions_update_callback(self, callback_function):
self.restrictions_update_callback = callback_function self.restrictions_update_callback = callback_function
def set_robot_connection_lost_callback(self,callback_function): def set_robot_connection_lost_callback(self, callback_function):
self.robot_connection_lost_callback = callback_function self.robot_connection_lost_callback = callback_function
def set_master_connection_lost_callback(self,callback_function): def set_master_connection_lost_callback(self, callback_function):
self.master_connection_lost_callback = callback_function self.master_connection_lost_callback = callback_function
def engage_clutch(self): def engage_clutch(self):
@ -266,4 +294,3 @@ class ROSWrapper:
msg = Bool() msg = Bool()
msg.data = False msg.data = False
self.user_stop_publisher.publish(msg) self.user_stop_publisher.publish(msg)

10
safety_user_plugin/user_info.py
View File

@ -1,9 +1,9 @@
#!/usr/bin/env python #!/usr/bin/env python
from robot_info import RobotInfo from safety_user_plugin.robot_info import RobotInfo
class UserInfo: class UserInfo:
def __init__(self): def __init__(self):
self.selected_robot = None self.selected_robot = None
self.robots_id_list = [] self.robots_id_list = []
@ -11,16 +11,16 @@ class UserInfo:
self.master_stop_state = None self.master_stop_state = None
self.clutch_state = None self.clutch_state = None
def select_robot(self,robot_id): def select_robot(self, robot_id):
self.selected_robot = RobotInfo(robot_id) self.selected_robot = RobotInfo(robot_id)
def release_robot(self): def release_robot(self):
self.selected_robot = None self.selected_robot = None
def update_robots_id_list(self,robots_id_list): def update_robots_id_list(self, robots_id_list):
self.robots_id_list = robots_id_list self.robots_id_list = robots_id_list
def update_selected_robot_info(self,new_robot_info): def update_selected_robot_info(self, new_robot_info):
self.selected_robot.robot_id = new_robot_info.robot_id self.selected_robot.robot_id = new_robot_info.robot_id
self.selected_robot.battery_voltage = new_robot_info.battery_voltage self.selected_robot.battery_voltage = new_robot_info.battery_voltage
self.selected_robot.linear_velocity = new_robot_info.linear_velocity self.selected_robot.linear_velocity = new_robot_info.linear_velocity

153
safety_user_plugin/user_plugin.py
View File

@ -6,34 +6,33 @@ from qt_gui.plugin import Plugin
from python_qt_binding.QtCore import QObject from python_qt_binding.QtCore import QObject
from python_qt_binding.QtCore import pyqtSignal from python_qt_binding.QtCore import pyqtSignal
from qt_wrapper import QtWrapper from safety_user_plugin.qt_wrapper import QtWrapper
from ros_wrapper import ROSWrapper from safety_user_plugin.ros_wrapper import ROSWrapper
from user_widget import UserWidget from safety_user_plugin.user_widget import UserWidget
from user_info import UserInfo from safety_user_plugin.user_info import UserInfo
from enums.clutch_state import ClutchState as CS from safety_user_plugin.enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS from safety_user_plugin.enums.stop_state import StopState as SS
class CallbackHandler(QObject): class CallbackHandler(QObject):
signal = pyqtSignal(str) signal = pyqtSignal(str)
signal_with_list_argument = pyqtSignal(str,list) signal_with_list_argument = pyqtSignal(str, list)
def __init__(self): def __init__(self):
super(CallbackHandler, self).__init__() super(CallbackHandler, self).__init__()
def connect(self,slot): def connect(self, slot):
self.signal.connect(slot) self.signal.connect(slot)
def list_connect(self,slot): def list_connect(self, slot):
self.signal_with_list_argument.connect(slot) self.signal_with_list_argument.connect(slot)
class UserPlugin(Plugin): class UserPlugin(Plugin):
def __init__(self, context):
def __init__(self,context):
super(UserPlugin, self).__init__(context) super(UserPlugin, self).__init__(context)
self.setObjectName('User Plugin - L1.5 safety system') self.setObjectName("User Plugin - L1.5 safety system")
# setStyleSheet("background-color:black;") # setStyleSheet("background-color:black;")
self.cbhandler = CallbackHandler() self.cbhandler = CallbackHandler()
@ -47,25 +46,31 @@ class UserPlugin(Plugin):
self._user_info.clutch_state = CS.UNKNOWN self._user_info.clutch_state = CS.UNKNOWN
# Setup functions to get _user_info states from _ros_wrapper # Setup functions to get _user_info states from _ros_wrapper
self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state) self._ros_wrapper.setup_get_user_stop_state_function(
self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state) self._user_info.get_user_stop_state
)
self._ros_wrapper.setup_get_clutch_state_function(
self._user_info.get_clutch_state
)
# self._ros_wrapper.setup_node() # self._ros_wrapper.setup_node()
self.set_callback_functions() self.set_callback_functions()
# At the end! # At the end!
self._qt_wrapper.connect_signals() self._qt_wrapper.connect_signals()
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) self._ros_wrapper.set_robots_list_update_callback(
self.handle_robots_list_update
)
self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal) self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal)
self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument) self.cbhandler.signal_with_list_argument.connect(
self._qt_wrapper.handle_emitted_signal_with_list_argument
)
def call_qt_wrapper_method(self, method_name):
def call_qt_wrapper_method(self,method_name):
self.cbhandler.signal.emit(method_name) self.cbhandler.signal.emit(method_name)
def call_qt_wrapper_method_with_argument(self,method_name,argument): def call_qt_wrapper_method_with_argument(self, method_name, argument):
self.cbhandler.signal_with_list_argument.emit(method_name,argument) self.cbhandler.signal_with_list_argument.emit(method_name, argument)
def shutdown_plugin(self): def shutdown_plugin(self):
self._ros_wrapper.unregister_node() self._ros_wrapper.unregister_node()
@ -75,62 +80,70 @@ class UserPlugin(Plugin):
self.set_ROS_callback_functions() self.set_ROS_callback_functions()
def set_ROS_callback_functions(self): def set_ROS_callback_functions(self):
self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update) self._ros_wrapper.set_selected_robot_info_update_callback(
self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update) self.handle_selected_robot_info_update
self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update) )
self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost) self._ros_wrapper.set_master_stop_update_callback(
self._ros_wrapper.set_master_connection_lost_callback(self.handle_master_connection_lost) self.handle_master_stop_update
)
self._ros_wrapper.set_restrictions_update_callback(
self.handle_restrictions_update
)
self._ros_wrapper.set_robot_connection_lost_callback(
self.handle_robot_connection_lost
)
self._ros_wrapper.set_master_connection_lost_callback(
self.handle_master_connection_lost
)
def set_Qt_callback_functions(self): def set_Qt_callback_functions(self):
self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection) self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
self._qt_wrapper.set_robot_release_callback(self.handle_robot_release) self._qt_wrapper.set_robot_release_callback(self.handle_robot_release)
self._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated) self._qt_wrapper.set_user_stop_state_updated_callback(
self.handle_user_stop_state_updated
)
self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched) self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched)
# ROS callback functions # ROS callback functions
def handle_robots_list_update(self,robots_id_list): def handle_robots_list_update(self, robots_id_list):
self._user_info.update_robots_id_list(robots_id_list) self._user_info.update_robots_id_list(robots_id_list)
self._qt_wrapper.update_robots_list_gui(robots_id_list) self._qt_wrapper.update_robots_list_gui(robots_id_list)
def handle_selected_robot_info_update(self,robot_info): def handle_selected_robot_info_update(self, robot_info):
if robot_info != None: if robot_info != None:
self.update_selected_robot_info(robot_info) self.update_selected_robot_info(robot_info)
else: else:
raise RuntimeError('Updated robot_info is NoneType object') raise RuntimeError("Updated robot_info is NoneType object")
def handle_master_stop_update(self,master_stop_state): def handle_master_stop_update(self, master_stop_state):
if (master_stop_state != self._user_info.master_stop_state): if master_stop_state != self._user_info.master_stop_state:
self._user_info.master_stop_state = master_stop_state self._user_info.master_stop_state = master_stop_state
if master_stop_state == SS.STOPPED: if master_stop_state == SS.STOPPED:
self.master_stopped() self.master_stopped()
elif master_stop_state == SS.RUNNING: elif master_stop_state == SS.RUNNING:
self.master_started() self.master_started()
else: else:
raise ValueError('Undefined master_stop_state received') raise ValueError("Undefined master_stop_state received")
def handle_restrictions_update(self,restrictions): def handle_restrictions_update(self, restrictions):
self._qt_wrapper.update_restrictions_gui(restrictions) self._qt_wrapper.update_restrictions_gui(restrictions)
# Qt callback functions # Qt callback functions
def handle_robot_selection(self,robot_id): def handle_robot_selection(self, robot_id):
if self._user_info.selected_robot == None: if self._user_info.selected_robot == None:
if self._ros_wrapper.robots_hz_value[robot_id] == None: if self._ros_wrapper.robots_hz_value[robot_id] == None:
self.select_robot(robot_id) self.select_robot(robot_id)
else: else:
self._qt_wrapper.robot_selected_by_another_group_notify() self._qt_wrapper.robot_selected_by_another_group_notify()
else: else:
raise RuntimeError('User already selected robot') raise RuntimeError("User already selected robot")
def handle_robot_release(self): def handle_robot_release(self):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
self.release_robot() self.release_robot()
else: else:
raise RuntimeError('Cannot release. No robot selected') raise RuntimeError("Cannot release. No robot selected")
def handle_user_stop_state_updated(self): def handle_user_stop_state_updated(self):
if self._user_info.selected_robot.obstacle_detected == True: if self._user_info.selected_robot.obstacle_detected == True:
@ -144,9 +157,9 @@ class UserPlugin(Plugin):
elif self._user_info.user_stop_state == SS.STOPPED: elif self._user_info.user_stop_state == SS.STOPPED:
self.user_started() self.user_started()
else: else:
raise ValueError('user_stop_state value undefined') raise ValueError("user_stop_state value undefined")
else: else:
raise ValueError('master_stop_state value undefined') raise ValueError("master_stop_state value undefined")
def handle_clutch_switched(self): def handle_clutch_switched(self):
if self._user_info.clutch_state == CS.ENGAGED: if self._user_info.clutch_state == CS.ENGAGED:
@ -154,14 +167,16 @@ class UserPlugin(Plugin):
elif self._user_info.clutch_state == CS.DISENGAGED: elif self._user_info.clutch_state == CS.DISENGAGED:
self.engage_clutch() self.engage_clutch()
else: else:
raise ValueError('clutch_state value undefined') raise ValueError("clutch_state value undefined")
def handle_robot_connection_lost(self): def handle_robot_connection_lost(self):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
lost_robot_id = self._user_info.selected_robot.robot_id lost_robot_id = self._user_info.selected_robot.robot_id
self.connection_to_robot_lost(lost_robot_id) self.connection_to_robot_lost(lost_robot_id)
else: else:
raise RuntimeError('Connection lost callback received when no robot was selected') raise RuntimeError(
"Connection lost callback received when no robot was selected"
)
def handle_master_connection_lost(self): def handle_master_connection_lost(self):
self.connection_to_master_lost() self.connection_to_master_lost()
@ -172,44 +187,44 @@ class UserPlugin(Plugin):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
self.user_stopped() self.user_stopped()
self.call_qt_wrapper_method('master_stopped') self.call_qt_wrapper_method("master_stopped")
# self._qt_wrapper.master_stopped() # self._qt_wrapper.master_stopped()
def master_started(self): def master_started(self):
self._user_info.master_stop_state = SS.RUNNING self._user_info.master_stop_state = SS.RUNNING
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
self.call_qt_wrapper_method('master_started') self.call_qt_wrapper_method("master_started")
# self._qt_wrapper.master_started() # self._qt_wrapper.master_started()
def user_stopped(self): def user_stopped(self):
self._user_info.user_stop_state = SS.STOPPED self._user_info.user_stop_state = SS.STOPPED
self._ros_wrapper.user_stopped() self._ros_wrapper.user_stopped()
self.call_qt_wrapper_method('user_stopped') self.call_qt_wrapper_method("user_stopped")
# self._qt_wrapper.user_stopped() # self._qt_wrapper.user_stopped()
def user_started(self): def user_started(self):
self._user_info.user_stop_state = SS.RUNNING self._user_info.user_stop_state = SS.RUNNING
self._ros_wrapper.user_started() self._ros_wrapper.user_started()
self.call_qt_wrapper_method('user_started') self.call_qt_wrapper_method("user_started")
# self._qt_wrapper.user_started() # self._qt_wrapper.user_started()
def engage_clutch(self): def engage_clutch(self):
self._user_info.clutch_state = CS.ENGAGED self._user_info.clutch_state = CS.ENGAGED
self._ros_wrapper.engage_clutch() self._ros_wrapper.engage_clutch()
self.call_qt_wrapper_method('engage_clutch') self.call_qt_wrapper_method("engage_clutch")
# self._qt_wrapper.engage_clutch() # self._qt_wrapper.engage_clutch()
def disengage_clutch(self): def disengage_clutch(self):
self._user_info.clutch_state = CS.DISENGAGED self._user_info.clutch_state = CS.DISENGAGED
self._ros_wrapper.disengage_clutch() self._ros_wrapper.disengage_clutch()
self.call_qt_wrapper_method('disengage_clutch') self.call_qt_wrapper_method("disengage_clutch")
# self._qt_wrapper.disengage_clutch() # self._qt_wrapper.disengage_clutch()
def select_robot(self,robot_id): def select_robot(self, robot_id):
self._ros_wrapper.select_robot(robot_id) self._ros_wrapper.select_robot(robot_id)
# self._qt_wrapper.select_robot(robot_id) # self._qt_wrapper.select_robot(robot_id)
self.call_qt_wrapper_method_with_argument('select_robot',[robot_id]) self.call_qt_wrapper_method_with_argument("select_robot", [robot_id])
self._user_info.select_robot(robot_id) self._user_info.select_robot(robot_id)
# Stop robot and engage clutch after taking control over it to put it in starting state # Stop robot and engage clutch after taking control over it to put it in starting state
@ -223,37 +238,47 @@ class UserPlugin(Plugin):
def release_robot(self): def release_robot(self):
self._ros_wrapper.release_robot() self._ros_wrapper.release_robot()
self.call_qt_wrapper_method('release_robot') self.call_qt_wrapper_method("release_robot")
# self._qt_wrapper.release_robot() # self._qt_wrapper.release_robot()
self._user_info.release_robot() self._user_info.release_robot()
self._user_info.master_stop_state = SS.UNKNOWN self._user_info.master_stop_state = SS.UNKNOWN
def connection_to_robot_lost(self,lost_robot_id): def connection_to_robot_lost(self, lost_robot_id):
# pass # pass
self._ros_wrapper.release_robot() self._ros_wrapper.release_robot()
self._user_info.release_robot() self._user_info.release_robot()
self.call_qt_wrapper_method_with_argument('connection_to_robot_lost',[lost_robot_id]) self.call_qt_wrapper_method_with_argument(
"connection_to_robot_lost", [lost_robot_id]
)
# self._qt_wrapper.connection_to_robot_lost(lost_robot_id) # self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
def connection_to_master_lost(self): def connection_to_master_lost(self):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
self.master_stopped() self.master_stopped()
self.call_qt_wrapper_method('connection_to_master_lost') self.call_qt_wrapper_method("connection_to_master_lost")
def obstacle_detected(self): def obstacle_detected(self):
self.call_qt_wrapper_method('obstacle_detected') self.call_qt_wrapper_method("obstacle_detected")
self.user_stopped() self.user_stopped()
def obstacle_removed(self): def obstacle_removed(self):
self.call_qt_wrapper_method('obstacle_removed') self.call_qt_wrapper_method("obstacle_removed")
def update_selected_robot_info(self,robot_info): def update_selected_robot_info(self, robot_info):
if self._user_info.selected_robot != None: if self._user_info.selected_robot != None:
if robot_info.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected == False: if (
robot_info.obstacle_detected == True
and self._user_info.selected_robot.obstacle_detected == False
):
self.obstacle_detected() self.obstacle_detected()
elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True: elif (
robot_info.obstacle_detected == False
and self._user_info.selected_robot.obstacle_detected == True
):
self.obstacle_removed() self.obstacle_removed()
self._user_info.update_selected_robot_info(robot_info) self._user_info.update_selected_robot_info(robot_info)
self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info]) self.call_qt_wrapper_method_with_argument(
"update_selected_robot_info", [robot_info]
)
# self._qt_wrapper.update_selected_robot_info(robot_info) # self._qt_wrapper.update_selected_robot_info(robot_info)