diff --git a/safety_master_plugin/master_plugin.py b/safety_master_plugin/master_plugin.py index 9cf10d9..cd16379 100755 --- a/safety_master_plugin/master_plugin.py +++ b/safety_master_plugin/master_plugin.py @@ -14,10 +14,9 @@ from safety_master_plugin.qt_master_wrapper import QtWrapper class MasterPlugin(Plugin): - - def __init__(self,context): + def __init__(self, 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._ros_wrapper = ROSWrapper() @@ -33,13 +32,16 @@ class MasterPlugin(Plugin): self._qt_wrapper.connect_signals() self._qt_wrapper.set_sliders_to_initial_values() - + # 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_restrictions_function(self._master_info.get_restrictions) + self._ros_wrapper.setup_get_master_stop_state_function( + self._master_info.get_master_stop_state + ) + self._ros_wrapper.setup_get_restrictions_function( + self._master_info.get_restrictions + ) self.set_callback_functions() - self._qt_wrapper.connect_signals() # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) @@ -54,31 +56,38 @@ class MasterPlugin(Plugin): self.set_Qt_callback_functions() 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) 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_restrictions_updated_callback(self.handle_restrictions_updated) - + self._qt_wrapper.set_master_stop_state_updated_callback( + 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) - def handle_robot_info_update(self,robot_info): + def handle_robot_info_update(self, robot_info): self.update_robot_info(robot_info) 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: self.master_stopped() elif self._master_info.master_stop_state == SS.STOPPED: - self.master_started() + self.master_started() 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) - + def update_robots_list(self, robots_id_list): for robot_id in robots_id_list: if robot_id not in self._master_info.robots_id_list: @@ -90,15 +99,15 @@ class MasterPlugin(Plugin): 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.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.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) def master_started(self): @@ -108,9 +117,9 @@ class MasterPlugin(Plugin): def master_stopped(self): self._master_info.master_stop_state = SS.STOPPED - # self._ros_wrapper.master_stopped() + self._ros_wrapper.master_stopped() self._qt_wrapper.master_stopped() - def restrictions_changed(self,restrictions): + def restrictions_changed(self, restrictions): self._master_info.restrictions = restrictions - self._ros_wrapper.restrictions_changed(restrictions) \ No newline at end of file + self._ros_wrapper.restrictions_changed(restrictions) diff --git a/safety_master_plugin/qt_master_wrapper.py b/safety_master_plugin/qt_master_wrapper.py index efbb508..baaf576 100644 --- a/safety_master_plugin/qt_master_wrapper.py +++ b/safety_master_plugin/qt_master_wrapper.py @@ -22,21 +22,25 @@ from python_qt_binding.QtCore import Qt class QtWrapper: - - def __init__(self,widget): + def __init__(self, widget): self.widget = widget self.displayed_robots_id_list = [] + self.master_stop_state_updated_callback = None + self.restrictions_updated_callback = None + self.master_stop_state = None self.init_robots_list() # self.disable_sliders_tracking() - ui_directory_path = '{0}/ui'.format(get_package_share_directory('safety_master_plugin')) + 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.angular_pixmap = QPixmap('{0}/Angular.png'.format(ui_directory_path)) - self.linear_pixmap = QPixmap('{0}/Linear.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.linear_pixmap = QPixmap("{0}/Linear.png".format(ui_directory_path)) self.widget.distanceImage.setPixmap(self.distance_pixmap) self.widget.angularImage.setPixmap(self.angular_pixmap) self.widget.linearImage.setPixmap(self.linear_pixmap) @@ -51,189 +55,216 @@ class QtWrapper: self.widget.linearSlider.setValue(100) def init_robots_list(self): - self.stdItemModel = QStandardItemModel( 0, 6, self.widget.robotsList) - self.stdItemModel.setHeaderData( 0, Qt.Horizontal, 'Robot' ) - self.stdItemModel.setHeaderData( 1, Qt.Horizontal, 'Stan baterii' ) - self.stdItemModel.setHeaderData( 2, Qt.Horizontal, 'Stan robota' ) - self.stdItemModel.setHeaderData( 3, Qt.Horizontal, 'Stan sprzęgła' ) - self.stdItemModel.setHeaderData( 4, Qt.Horizontal, 'Prędk. lin.' ) - self.stdItemModel.setHeaderData( 5, Qt.Horizontal, 'Prędk. obr.' ) + self.stdItemModel = QStandardItemModel(0, 6, self.widget.robotsList) + self.stdItemModel.setHeaderData(0, Qt.Horizontal, "Robot") + self.stdItemModel.setHeaderData(1, Qt.Horizontal, "Stan baterii") + self.stdItemModel.setHeaderData(2, Qt.Horizontal, "Stan robota") + self.stdItemModel.setHeaderData(3, Qt.Horizontal, "Stan sprzęgła") + self.stdItemModel.setHeaderData(4, Qt.Horizontal, "Prędk. lin.") + self.stdItemModel.setHeaderData(5, Qt.Horizontal, "Prędk. obr.") self.widget.robotsList.setModel(self.stdItemModel) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(0, QHeaderView.Stretch) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(2, QHeaderView.Stretch) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(3, QHeaderView.Stretch) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(4, QHeaderView.Stretch) - self.widget.robotsList.horizontalHeader().setSectionResizeMode(5, QHeaderView.Stretch) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 0, QHeaderView.Stretch + ) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 1, QHeaderView.Stretch + ) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 2, QHeaderView.Stretch + ) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 3, QHeaderView.Stretch + ) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 4, QHeaderView.Stretch + ) + self.widget.robotsList.horizontalHeader().setSectionResizeMode( + 5, QHeaderView.Stretch + ) self.widget.robotsList.horizontalHeader().setSectionsMovable(False) # self.widget.robotsList.setSelectionBehavior(QAbstractItemView.SelectRows) self.widget.robotsList.setSelectionMode(QAbstractItemView.NoSelection) - def disable_sliders_tracking(self): self.widget.distanceSlider.setTracking(False) self.widget.angularSlider.setTracking(False) self.widget.linearSlider.setTracking(False) - - def set_master_stop_state_updated_callback(self,callback_function): + def set_master_stop_state_updated_callback(self, callback_function): self.master_stop_state_updated_callback = callback_function - def set_restrictions_updated_callback(self,callback_function): + def set_restrictions_updated_callback(self, callback_function): self.restrictions_updated_callback = callback_function def handle_restrictions_update(self): - distance = (float(self.widget.distanceSlider.value())/100) - angular_velocity = (float(self.widget.angularSlider.value())/100) - linear_velocity = (float(self.widget.linearSlider.value())/100) + print("double?") + distance = float(self.widget.distanceSlider.value()) / 100 + angular_velocity = float(self.widget.angularSlider.value()) / 100 + linear_velocity = float(self.widget.linearSlider.value()) / 100 self.widget.distanceLabel.setText("{:.2f}".format(distance)) self.widget.angularLabel.setText("{:.2f}".format(angular_velocity)) self.widget.linearLabel.setText("{:.2f}".format(linear_velocity)) - restrictions = Restrictions(distance,angular_velocity,linear_velocity) + restrictions = Restrictions(distance, angular_velocity, linear_velocity) self.restrictions_updated_callback(restrictions) - def connect_signals(self): self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update) self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update) self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update) - self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked) + self.widget.masterstopButton.clicked.connect( + self.handle_master_stop_button_clicked + ) self.set_sliders_to_initial_values() - def handle_masterstopButton_clicked(self): - if self.master_stop_state == SS.RUNNING: - self.master_stop_state_updated_callback() - else: - reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokować wszystkie roboty?",QMessageBox.Yes,QMessageBox.No) - - if reply == QMessageBox.Yes: - self.master_stop_state_updated_callback() + def handle_master_stop_button_clicked(self): + print("CLick\t") + # self.master_stop_state_updated_callback() + # if self.master_stop_state == SS.RUNNING: + # print("Klikam aby zablokować") + # self.master_stop_state_updated_callback() + # else: + # reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokować wszystkie roboty?",QMessageBox.Yes,QMessageBox.No) + # if reply == QMessageBox.Yes: + # self.master_stop_state_updated_callback() def display_master_stop_on(self): - self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}") - self.widget.masterstopButton.setText('Zatrzymaj roboty') + self.widget.masterstopButton.setStyleSheet( + "QPushButton { color: black; background-color: green; font: bold 20px}" + ) + self.widget.masterstopButton.setText("Zatrzymaj roboty") def display_master_stop_off(self): - self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}") - self.widget.masterstopButton.setText('Odblokuj roboty') + self.widget.masterstopButton.setStyleSheet( + "QPushButton { color: black; background-color: red; font: bold 20px}" + ) + self.widget.masterstopButton.setText("Odblokuj roboty") - - def update_robot_info(self,robot_info): + def update_robot_info(self, robot_info): items_to_add = [] - 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("pioneer{}".format(robot_info.robot_id))) + items_to_add.append(QStandardItem("{:.2f}".format(robot_info.battery_voltage))) if robot_info.state == SS.RUNNING: - item = QStandardItem('Działa') + item = QStandardItem("Działa") item.setBackground(QBrush(Qt.green)) items_to_add.append(item) elif robot_info.state == SS.STOPPED: - item = QStandardItem('Zatrzymany') + item = QStandardItem("Zatrzymany") item.setBackground(QBrush(Qt.red)) items_to_add.append(item) 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: - item = QStandardItem('Sprzęgnięte') + item = QStandardItem("Sprzęgnięte") item.setBackground(QBrush(Qt.green)) items_to_add.append(item) elif robot_info.clutch == CS.DISENGAGED: - item = QStandardItem('Rozprzęgnięte') + item = QStandardItem("Rozprzęgnięte") item.setBackground(QBrush(Qt.red)) items_to_add.append(item) 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) - items_to_add.append(QStandardItem('{:.2f}'.format(linear_vel))) - - items_to_add.append(QStandardItem('{:.2f}'.format(robot_info.angular_velocity[2]))) + linear_vel = math.sqrt( + robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2 + ) + items_to_add.append(QStandardItem("{:.2f}".format(linear_vel))) + items_to_add.append( + QStandardItem("{:.2f}".format(robot_info.angular_velocity[2])) + ) - - items_list = self.stdItemModel.findItems('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: for item in items_list: row_number = item.row() else: 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.setEditable(0) - self.stdItemModel.setItem(row_number,i,item) + self.stdItemModel.setItem(row_number, i, item) def master_stopped(self): self.master_stop_state = SS.STOPPED self.display_master_stop_off() - self.log_info('Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty') + self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty") def master_started(self): self.master_stop_state = SS.RUNNING self.display_master_stop_on() - self.log_info('Przycisk masterSTOP odciśnięty') - - - def add_robot(self,robot_id): + self.log_info("Przycisk masterSTOP odciśnięty") + def add_robot(self, robot_id): if robot_id not in self.displayed_robots_id_list: - new_robot_info=QStandardItem('pioneer{}'.format(robot_id)) + new_robot_info = QStandardItem("pioneer{}".format(robot_id)) new_robot_info.setTextAlignment(Qt.AlignCenter) new_robot_info.setEditable(0) # new_robot_info.setBackground(QBrush(Qt.white)) - self.stdItemModel.setItem(self.stdItemModel.rowCount(),0,new_robot_info) + self.stdItemModel.setItem(self.stdItemModel.rowCount(), 0, new_robot_info) 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: - 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): - items_list = self.stdItemModel.findItems('pioneer{0}'.format(robot_id),Qt.MatchExactly) + def remove_robot(self, robot_id): + items_list = self.stdItemModel.findItems( + "pioneer{0}".format(robot_id), Qt.MatchExactly + ) if len(items_list) > 0: for item in items_list: row_number = item.row() self.stdItemModel.removeRow(row_number) - - self.log_info('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) - def log_info(self,info_text): - time = datetime.datetime.now().strftime('[%H:%M:%S]') + 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('' + str(time) + '. ' + info_text + '
') - self.widget.logsBrowser.insertHtml(' {0} {1}
'.format(time,info_text)) + self.widget.logsBrowser.insertHtml( + ' {0} {1}
'.format(time, info_text) + ) self.scroll_to_bottom() - def log_warning(self,warning_text): - time = datetime.datetime.now().strftime('[%H:%M:%S]') - - cursor = self.widget.logsBrowser.textCursor() - cursor.movePosition(QTextCursor.End) - self.widget.logsBrowser.setTextCursor(cursor) - self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) - self.widget.logsBrowser.insertHtml(' {0} {1}
'.format(time,warning_text)) - self.scroll_to_bottom() - - def log_error(self,error_text): - time = datetime.datetime.now().strftime('[%H:%M:%S]') + 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(' {0} {1}
'.format(time,error_text)) + self.widget.logsBrowser.insertHtml( + ' {0} {1}
'.format(time, warning_text) + ) + self.scroll_to_bottom() + + def log_error(self, error_text): + time = datetime.datetime.now().strftime("[%H:%M:%S]") + + cursor = self.widget.logsBrowser.textCursor() + cursor.movePosition(QTextCursor.End) + self.widget.logsBrowser.setTextCursor(cursor) + self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End) + self.widget.logsBrowser.insertHtml( + ' {0} {1}
'.format(time, error_text) + ) self.scroll_to_bottom() def scroll_to_bottom(self):