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

removed double connect

Browse Source 
Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>
This commit is contained in:
Jakub Delicat 2023-08-11 15:30:59 +02:00
parent b6d3ea76e3
commit b08d9a4254
2 changed files with 151 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
safety_master_plugin/master_plugin.py
View File

@ -14,10 +14,9 @@ from safety_master_plugin.qt_master_wrapper import QtWrapper
class MasterPlugin(Plugin): class MasterPlugin(Plugin):
def __init__(self, context):
def __init__(self,context):
super(MasterPlugin, self).__init__(context) super(MasterPlugin, self).__init__(context)
self.setObjectName('Master Plugin - L1.5 safety system') self.setObjectName("Master Plugin - L1.5 safety system")
self._widget = MasterWidget(context) self._widget = MasterWidget(context)
self._ros_wrapper = ROSWrapper() self._ros_wrapper = ROSWrapper()
@ -35,11 +34,14 @@ class MasterPlugin(Plugin):
self._qt_wrapper.set_sliders_to_initial_values() self._qt_wrapper.set_sliders_to_initial_values()
# Setup functions to get _master_info states from _ros_wrapper # Setup functions to get _master_info states from _ros_wrapper
self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state) self._ros_wrapper.setup_get_master_stop_state_function(
self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions) self._master_info.get_master_stop_state
)
self._ros_wrapper.setup_get_restrictions_function(
self._master_info.get_restrictions
)
self.set_callback_functions() self.set_callback_functions()
self._qt_wrapper.connect_signals()
# timer to consecutively send twist messages # timer to consecutively send twist messages
self._update_parameter_timer = QTimer(self) self._update_parameter_timer = QTimer(self)
@ -54,29 +56,36 @@ class MasterPlugin(Plugin):
self.set_Qt_callback_functions() self.set_Qt_callback_functions()
def set_ROS_callback_functions(self): def set_ROS_callback_functions(self):
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._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update) self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
def set_Qt_callback_functions(self): def set_Qt_callback_functions(self):
self._qt_wrapper.set_master_stop_state_updated_callback(self.handle_master_stop_state_updated) self._qt_wrapper.set_master_stop_state_updated_callback(
self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated) self.handle_master_stop_state_updated
)
self._qt_wrapper.set_restrictions_updated_callback(
self.handle_restrictions_updated
)
def handle_robots_list_update(self, robots_id_list):
def handle_robots_list_update(self,robots_id_list):
self.update_robots_list(robots_id_list) self.update_robots_list(robots_id_list)
def handle_robot_info_update(self,robot_info): def handle_robot_info_update(self, robot_info):
self.update_robot_info(robot_info) self.update_robot_info(robot_info)
def handle_master_stop_state_updated(self): def handle_master_stop_state_updated(self):
"""Handles master stop state when it changes e. g. on button click and confirmed."""
print("woking!!!")
if self._master_info.master_stop_state == SS.RUNNING: if self._master_info.master_stop_state == SS.RUNNING:
self.master_stopped() self.master_stopped()
elif self._master_info.master_stop_state == SS.STOPPED: elif self._master_info.master_stop_state == SS.STOPPED:
self.master_started() self.master_started()
else: else:
raise ValueError('master_stop_state value undefined') raise ValueError("master_stop_state value undefined")
def handle_restrictions_updated(self,restrictions): def handle_restrictions_updated(self, restrictions):
self.restrictions_changed(restrictions) self.restrictions_changed(restrictions)
def update_robots_list(self, robots_id_list): def update_robots_list(self, robots_id_list):
@ -90,15 +99,15 @@ class MasterPlugin(Plugin):
self._master_info.robots_id_list = robots_id_list self._master_info.robots_id_list = robots_id_list
def new_robot_registered(self,robot_id): def new_robot_registered(self, robot_id):
self._ros_wrapper.add_robot_subscriber(robot_id) self._ros_wrapper.add_robot_subscriber(robot_id)
self.cbhandler.add_robot_signal.emit(robot_id) self.cbhandler.add_robot_signal.emit(robot_id)
def robot_unregistered(self,robot_id): def robot_unregistered(self, robot_id):
# self._ros_wrapper.remove_robot_subscriber(robot_id) # self._ros_wrapper.remove_robot_subscriber(robot_id)
self.cbhandler.remove_robot_signal.emit(robot_id) self.cbhandler.remove_robot_signal.emit(robot_id)
def update_robot_info(self,robot_info): def update_robot_info(self, robot_info):
self.cbhandler.robot_info_signal.emit(robot_info) self.cbhandler.robot_info_signal.emit(robot_info)
def master_started(self): def master_started(self):
@ -108,9 +117,9 @@ class MasterPlugin(Plugin):
def master_stopped(self): def master_stopped(self):
self._master_info.master_stop_state = SS.STOPPED self._master_info.master_stop_state = SS.STOPPED
# self._ros_wrapper.master_stopped() self._ros_wrapper.master_stopped()
self._qt_wrapper.master_stopped() self._qt_wrapper.master_stopped()
def restrictions_changed(self,restrictions): def restrictions_changed(self, restrictions):
self._master_info.restrictions = restrictions self._master_info.restrictions = restrictions
self._ros_wrapper.restrictions_changed(restrictions) self._ros_wrapper.restrictions_changed(restrictions)

191
safety_master_plugin/qt_master_wrapper.py
View File

@ -22,21 +22,25 @@ from python_qt_binding.QtCore import Qt
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 = []
self.master_stop_state_updated_callback = None
self.restrictions_updated_callback = None
self.master_stop_state = None self.master_stop_state = None
self.init_robots_list() self.init_robots_list()
# self.disable_sliders_tracking() # self.disable_sliders_tracking()
ui_directory_path = '{0}/ui'.format(get_package_share_directory('safety_master_plugin')) ui_directory_path = "{0}/ui".format(
get_package_share_directory("safety_master_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) self.widget.distanceImage.setPixmap(self.distance_pixmap)
self.widget.angularImage.setPixmap(self.angular_pixmap) self.widget.angularImage.setPixmap(self.angular_pixmap)
self.widget.linearImage.setPixmap(self.linear_pixmap) self.widget.linearImage.setPixmap(self.linear_pixmap)
@ -51,189 +55,216 @@ class QtWrapper:
self.widget.linearSlider.setValue(100) self.widget.linearSlider.setValue(100)
def init_robots_list(self): def init_robots_list(self):
self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList) self.stdItemModel = QStandardItemModel(0, 6, self.widget.robotsList)
self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' ) self.stdItemModel.setHeaderData(0, Qt.Horizontal, "Robot")
self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' ) self.stdItemModel.setHeaderData(1, Qt.Horizontal, "Stan baterii")
self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' ) self.stdItemModel.setHeaderData(2, Qt.Horizontal, "Stan robota")
self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' ) self.stdItemModel.setHeaderData(3, Qt.Horizontal, "Stan sprzęgła")
self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' ) self.stdItemModel.setHeaderData(4, Qt.Horizontal, "Prędk. lin.")
self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' ) self.stdItemModel.setHeaderData(5, Qt.Horizontal, "Prędk. obr.")
self.widget.robotsList.setModel(self.stdItemModel) self.widget.robotsList.setModel(self.stdItemModel)
self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch) self.widget.robotsList.horizontalHeader().setSectionResizeMode(
self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) 0, QHeaderView.Stretch
self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch) )
self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch) self.widget.robotsList.horizontalHeader().setSectionResizeMode(
self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch) 1, QHeaderView.Stretch
self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, 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.horizontalHeader().setSectionsMovable(False)
# self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows) # self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows)
self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection) self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection)
def disable_sliders_tracking(self): def disable_sliders_tracking(self):
self.widget.distanceSlider.setTracking(False) self.widget.distanceSlider.setTracking(False)
self.widget.angularSlider.setTracking(False) self.widget.angularSlider.setTracking(False)
self.widget.linearSlider.setTracking(False) self.widget.linearSlider.setTracking(False)
def set_master_stop_state_updated_callback(self, callback_function):
def set_master_stop_state_updated_callback(self,callback_function):
self.master_stop_state_updated_callback = callback_function self.master_stop_state_updated_callback = callback_function
def set_restrictions_updated_callback(self,callback_function): def set_restrictions_updated_callback(self, callback_function):
self.restrictions_updated_callback = callback_function self.restrictions_updated_callback = callback_function
def handle_restrictions_update(self): def handle_restrictions_update(self):
distance = (float(self.widget.distanceSlider.value())/100) print("double?")
angular_velocity = (float(self.widget.angularSlider.value())/100) distance = float(self.widget.distanceSlider.value()) / 100
linear_velocity = (float(self.widget.linearSlider.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.distanceLabel.setText("{:.2f}".format(distance))
self.widget.angularLabel.setText("{:.2f}".format(angular_velocity)) self.widget.angularLabel.setText("{:.2f}".format(angular_velocity))
self.widget.linearLabel.setText("{:.2f}".format(linear_velocity)) self.widget.linearLabel.setText("{:.2f}".format(linear_velocity))
restrictions = Restrictions(distance,angular_velocity,linear_velocity) restrictions = Restrictions(distance, angular_velocity, linear_velocity)
self.restrictions_updated_callback(restrictions) self.restrictions_updated_callback(restrictions)
def connect_signals(self): def connect_signals(self):
self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update) self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update)
self.widget.angularSlider.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.linearSlider.valueChanged.connect(self.handle_restrictions_update)
self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked) self.widget.masterstopButton.clicked.connect(
self.handle_master_stop_button_clicked
)
self.set_sliders_to_initial_values() self.set_sliders_to_initial_values()
def handle_masterstopButton_clicked(self): def handle_master_stop_button_clicked(self):
if self.master_stop_state == SS.RUNNING: print("CLick\t")
self.master_stop_state_updated_callback() # self.master_stop_state_updated_callback()
else: # if self.master_stop_state == SS.RUNNING:
reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokować wszystkie roboty?",QMessageBox.Yes,QMessageBox.No) # print("Klikam aby zablokować")
# self.master_stop_state_updated_callback()
if reply == QMessageBox.Yes: # else:
self.master_stop_state_updated_callback() # 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): def display_master_stop_on(self):
self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}") self.widget.masterstopButton.setStyleSheet(
self.widget.masterstopButton.setText('Zatrzymaj roboty') "QPushButton { color: black; background-color: green; font: bold 20px}"
)
self.widget.masterstopButton.setText("Zatrzymaj roboty")
def display_master_stop_off(self): def display_master_stop_off(self):
self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}") self.widget.masterstopButton.setStyleSheet(
self.widget.masterstopButton.setText('Odblokuj roboty') "QPushButton { color: black; background-color: red; font: bold 20px}"
)
self.widget.masterstopButton.setText("Odblokuj roboty")
def update_robot_info(self, robot_info):
def update_robot_info(self,robot_info):
items_to_add = [] items_to_add = []
items_to_add.append(QStandardItem('pioneer{}'.format(robot_info.robot_id))) items_to_add.append(QStandardItem("pioneer{}".format(robot_info.robot_id)))
items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.battery_voltage))) items_to_add.append(QStandardItem("{:.2f}".format(robot_info.battery_voltage)))
if robot_info.state == SS.RUNNING: if robot_info.state == SS.RUNNING:
item = QStandardItem('Działa') item = QStandardItem("Działa")
item.setBackground(QBrush(Qt.green)) item.setBackground(QBrush(Qt.green))
items_to_add.append(item) items_to_add.append(item)
elif robot_info.state == SS.STOPPED: elif robot_info.state == SS.STOPPED:
item = QStandardItem('Zatrzymany') item = QStandardItem("Zatrzymany")
item.setBackground(QBrush(Qt.red)) item.setBackground(QBrush(Qt.red))
items_to_add.append(item) items_to_add.append(item)
else: else:
raise ValueError('Undefined value of robot_info.robot_state') raise ValueError("Undefined value of robot_info.robot_state")
if robot_info.clutch == CS.ENGAGED: if robot_info.clutch == CS.ENGAGED:
item = QStandardItem('Sprzęgnięte') item = QStandardItem("Sprzęgnięte")
item.setBackground(QBrush(Qt.green)) item.setBackground(QBrush(Qt.green))
items_to_add.append(item) items_to_add.append(item)
elif robot_info.clutch == CS.DISENGAGED: elif robot_info.clutch == CS.DISENGAGED:
item = QStandardItem('Rozprzęgnięte') item = QStandardItem("Rozprzęgnięte")
item.setBackground(QBrush(Qt.red)) item.setBackground(QBrush(Qt.red))
items_to_add.append(item) items_to_add.append(item)
else: else:
raise ValueError('Undefined value of robot_info.clutch_state') 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) linear_vel = math.sqrt(
items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel))) 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_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2]))) items_list = self.stdItemModel.findItems(
"pioneer{0}".format(robot_info.robot_id), Qt.MatchExactly
)
items_list = self.stdItemModel.findItems('pioneer{0}'.format(robot_info.robot_id),Qt.MatchExactly)
if len(items_list) > 0: if len(items_list) > 0:
for item in items_list: for item in items_list:
row_number = item.row() row_number = item.row()
else: else:
row_number = self.stdItemModel.rowCount() row_number = self.stdItemModel.rowCount()
for i,item in enumerate(items_to_add): for i, item in enumerate(items_to_add):
item.setTextAlignment(Qt.AlignCenter) item.setTextAlignment(Qt.AlignCenter)
item.setEditable(0) item.setEditable(0)
self.stdItemModel.setItem(row_number,i,item) self.stdItemModel.setItem(row_number, i, item)
def master_stopped(self): def master_stopped(self):
self.master_stop_state = SS.STOPPED self.master_stop_state = SS.STOPPED
self.display_master_stop_off() self.display_master_stop_off()
self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty') self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty")
def master_started(self): def master_started(self):
self.master_stop_state = SS.RUNNING self.master_stop_state = SS.RUNNING
self.display_master_stop_on() self.display_master_stop_on()
self.log_info('Przycisk masterSTOP odciśnięty') self.log_info("Przycisk masterSTOP odciśnięty")
def add_robot(self,robot_id):
def add_robot(self, robot_id):
if robot_id not in self.displayed_robots_id_list: if robot_id not in self.displayed_robots_id_list:
new_robot_info=QStandardItem('pioneer{}'.format(robot_id)) new_robot_info = QStandardItem("pioneer{}".format(robot_id))
new_robot_info.setTextAlignment(Qt.AlignCenter) new_robot_info.setTextAlignment(Qt.AlignCenter)
new_robot_info.setEditable(0) new_robot_info.setEditable(0)
# new_robot_info.setBackground(QBrush(Qt.white)) # new_robot_info.setBackground(QBrush(Qt.white))
self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info) self.stdItemModel.setItem(self.stdItemModel.rowCount(), 0, new_robot_info)
self.displayed_robots_id_list.append(robot_id) self.displayed_robots_id_list.append(robot_id)
self.log_info('pioneer{0} połączony'.format(robot_id)) self.log_info("pioneer{0} połączony".format(robot_id))
else: else:
raise RuntimeError('Adding robot, which is already in master GUI') raise RuntimeError("Adding robot, which is already in master GUI")
def remove_robot(self,robot_id): def remove_robot(self, robot_id):
items_list = self.stdItemModel.findItems('pioneer{0}'.format(robot_id),Qt.MatchExactly) items_list = self.stdItemModel.findItems(
"pioneer{0}".format(robot_id), Qt.MatchExactly
)
if len(items_list) > 0: if len(items_list) > 0:
for item in items_list: for item in items_list:
row_number = item.row() row_number = item.row()
self.stdItemModel.removeRow(row_number) self.stdItemModel.removeRow(row_number)
self.log_info('pioneer{0} rozłączony'.format(robot_id)) self.log_info("pioneer{0} rozłączony".format(robot_id))
self.displayed_robots_id_list.remove(robot_id) self.displayed_robots_id_list.remove(robot_id)
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">' + str(time) + '. ' + info_text + '</font><br>') # 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.widget.logsBrowser.insertHtml(
'<font color="black"> {0} {1}</font><br>'.format(time, info_text)
)
self.scroll_to_bottom() self.scroll_to_bottom()
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()
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()
def scroll_to_bottom(self): def scroll_to_bottom(self):