diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index 9dda2cf..c813968 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -5,6 +5,10 @@ import rospkg import math +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + + class QtWrapper: def __init__(self,widget): @@ -12,13 +16,32 @@ class QtWrapper: self.logger_counter = 0 self.displayed_robots_id_list = [] - def connect_robot_signals(self): - raise NotImplementedError + def disconnect_signals(self): + self.widget.clutchButton.clicked.disconnect() + self.widget.stopButton.clicked.disconnect() + self.widget.choiceButton.clicked.disconnect() - def connect_signals(self): - self.widget.openButton.clicked.connect(self.handle_openButton_clicked) + def connect_robot_signals(self): self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback) self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked_callback) + self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked) + + def connect_signals(self): + self.widget.clutchButton.clicked.connect(self.handle_not_selected_error) + self.widget.stopButton.clicked.connect(self.handle_not_selected_error) + self.widget.choiceButton.clicked.connect(self.handle_openButton_clicked) + + + def handle_emitted_signal(self,method_name): + exec('self.{0}()'.format(method_name)) + + def handle_emitted_signal_with_list_argument(self,method_name,argument): + if method_name == 'update_selected_robot_info': + self.update_selected_robot_info(argument[0]) + + else: + method_with_argument = 'self.{0}({1})'.format(method_name,argument[0]) + exec(method_with_argument) def get_selected_robot_id(self): current_text = self.widget.robotsList.currentText() @@ -28,6 +51,8 @@ class QtWrapper: else: return None + def handle_not_selected_error(self): + self.log_error('Najpierw wybierz PIONIERA z listy robotow') def handle_openButton_clicked(self): selected_robot_id = self.get_selected_robot_id() @@ -35,12 +60,17 @@ class QtWrapper: if selected_robot_id != None: self.handle_openButton_clicked_callback(selected_robot_id) else: - # TODO - Log info - raise NotImplementedError + self.log_error('Najpierw wybierz PIONIERA z listy robotow') + + def handle_closeButton_clicked(self): + self.handle_closeButton_clicked_callback() def set_robot_selection_callback(self,callback): self.handle_openButton_clicked_callback = callback + def set_robot_release_callback(self,callback): + self.handle_closeButton_clicked_callback = callback + def set_user_stop_state_updated_callback(self,callback): self.handle_stopButton_clicked_callback = callback @@ -48,10 +78,18 @@ class QtWrapper: self.handle_clutchButton_clicked_callback = callback def select_robot(self,robot_id): - self.log_info('Robot PIONIER' + str(robot_id) + ' wybrany!') + self.disconnect_signals() + self.connect_robot_signals() + self.log_info('PIONIER' + str(robot_id) + ' wybrany!') + self.log_info('Robot zostanie zatrzymany i sprzegniety') + + self.widget.choiceButton.setText('Zwolnij robota') def release_robot(self): + self.disconnect_signals() + self.connect_signals() self.log_info('Odlaczam robota') + self.clear_robot_info() def update_robots_list_gui(self,robots_id_list): robots_id_list.sort() @@ -98,34 +136,44 @@ class QtWrapper: self.widget.linearLabel.setText("{:.2f}".format(linear_vel)) self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2])) - if robot_info.clutch == True: - self.engage_clutch() + if robot_info.clutch == CS.ENGAGED: + self.widget.clutchLabel.setText('1') else: - self.disengage_clutch() + self.widget.clutchLabel.setText('0') - if robot_info.state == True: - self.engage_clutch() + if robot_info.state == SS.RUNNING: + self.widget.stateLabel.setText('1') else: - self.disengage_clutch() + self.widget.stateLabel.setText('0') def master_stopped(self): + # raise NotImplementedError self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty') def master_started(self): + # raise NotImplementedError self.log_info('Przycisk masterSTOP odcisniety. Mozesz uruchomic robota') def user_stopped(self): - self.log_info('Zatrzymuje robota') + # raise NotImplementedError + self.log_info('Robot zatrzymany') self.widget.stateLabel.setText('0') def user_started(self): + # raise NotImplementedError self.log_info('Robot wystartowal') self.widget.stateLabel.setText('1') def disengage_clutch(self): + # raise NotImplementedError + self.log_info('Rozprzegam sprzeglo') self.widget.clutchLabel.setText('0') + # pass def engage_clutch(self): + # raise NotImplementedError + self.log_info('Sprzegam sprzeglo') + # self.signal.('disengage_clutch') self.widget.clutchLabel.setText('1') def master_is_stopped_notify(self): @@ -160,4 +208,15 @@ class QtWrapper: def scroll_to_bottom(self): scrollbar = self.widget.logsBrowser.verticalScrollBar() - scrollbar.setValue(scrollbar.maximum()) \ No newline at end of file + scrollbar.setValue(scrollbar.maximum()) + + + def clear_robot_info(self): + self.widget.idLabel.setText('-') + self.widget.batteryLabel.setText('-') + self.widget.linearLabel.setText('-') + self.widget.angularLabel.setText('-') + self.widget.stateLabel.setText('-') + self.widget.clutchLabel.setText('-') + + self.widget.choiceButton.setText('Wybierz') \ No newline at end of file diff --git a/safety_user_plugin/scripts/robot_info.py b/safety_user_plugin/scripts/robot_info.py index 63c2f9e..4f50b36 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/safety_user_plugin/scripts/robot_info.py @@ -1,5 +1,8 @@ #!/usr/bin/env python +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + class RobotInfo: def __init__(self,robot_id): @@ -15,5 +18,13 @@ class RobotInfo: 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.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z] - self.state = robot_info_msg.state.data - self.clutch = robot_info_msg.clutch.data \ No newline at end of file + + if robot_info_msg.state.data == True: + self.state = SS.RUNNING + else: + self.state = SS.STOPPED + + if robot_info_msg.clutch.data == True: + self.clutch = CS.ENGAGED + else: + self.clutch = CS.DISENGAGED \ No newline at end of file diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index fc86415..3bd0ac1 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -71,16 +71,14 @@ class ROSWrapper: elif stop_state == SS.STOPPED: self.user_stopped() else: - # TODO Raise wrong value error - raise NotImplementedError + raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') if clutch_state == CS.ENGAGED: self.engage_clutch() elif clutch_state == CS.DISENGAGED: self.disengage_clutch() else: - # TODO Raise wrong value error - raise NotImplementedError + raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update') def handle_robot_connection_lost(self,event): self.shutdown_timer(self.connection_timer) @@ -106,8 +104,7 @@ class ROSWrapper: self.connection_timer.shutdown() self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost) else: - # TODO - raise connection_timer_not_setup error - raise NotImplementedError + raise RuntimeError('Attempting to restart connection_timer when it is not initialized') def unregister_node(self): self.cancel_subscribers_and_publishers() diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index c25ff8a..b6f185a 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -3,6 +3,8 @@ import os from qt_gui.plugin import Plugin +from python_qt_binding.QtCore import QObject +from python_qt_binding.QtCore import pyqtSignal from qt_wrapper import QtWrapper from ros_wrapper import ROSWrapper @@ -13,6 +15,20 @@ from enums.clutch_state import ClutchState as CS from enums.stop_state import StopState as SS +class CallbackHandler(QObject): + signal = pyqtSignal(str) + signal_with_list_argument = pyqtSignal(str,list) + + def __init__(self): + super(CallbackHandler, self).__init__() + + def connect(self,slot): + self.signal.connect(slot) + + def list_connect(self,slot): + self.signal_with_list_argument.connect(slot) + + # def handle class UserPlugin(Plugin): @@ -20,7 +36,7 @@ class UserPlugin(Plugin): super(UserPlugin, self).__init__(context) self.setObjectName('User Plugin - L1.5 safety system') # setStyleSheet("background-color:black;") - + self.cbhandler = CallbackHandler() self._widget = UserWidget(context) self._qt_wrapper = QtWrapper(self._widget) @@ -41,8 +57,16 @@ class UserPlugin(Plugin): # At the end! self._qt_wrapper.connect_signals() 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_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument) - # self._qt_wrapper.update_robots_list_gui([5,6,2]) + + + def call_qt_wrapper_method(self,method_name): + self.cbhandler.signal.emit(method_name) + + def call_qt_wrapper_method_with_argument(self,method_name,argument): + self.cbhandler.signal_with_list_argument.emit(method_name,argument) def shutdown_plugin(self): self._ros_wrapper.unregister_node() @@ -59,6 +83,7 @@ class UserPlugin(Plugin): def set_Qt_callback_functions(self): 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_user_stop_state_updated_callback(self.handle_user_stop_state_updated) self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched) @@ -71,17 +96,20 @@ class UserPlugin(Plugin): self._qt_wrapper.update_robots_list_gui(robots_id_list) def handle_selected_robot_info_update(self,robot_info): - self.update_selected_robot_info(robot_info) + if robot_info != None: + self.update_selected_robot_info(robot_info) + else: + raise RuntimeError('Updated robot_info is NoneType object') def handle_master_stop_update(self,master_stop_state): - self._user_info.master_stop_state = master_stop_state - if master_stop_state == SS.STOPPED: - self.master_stopped() - elif master_stop_state == SS.RUNNING: - self.master_started() - else: - # TODO - Wrong masterstop value exception - raise NotImplementedError + if (master_stop_state != self._user_info.master_stop_state): + self._user_info.master_stop_state = master_stop_state + if master_stop_state == SS.STOPPED: + self.master_stopped() + elif master_stop_state == SS.RUNNING: + self.master_started() + else: + raise ValueError('Undefined master_stop_state received') def handle_restrictions_update(self,restrictions): self._qt_wrapper.update_restrictions_gui(restrictions) @@ -91,19 +119,14 @@ class UserPlugin(Plugin): def handle_robot_selection(self,robot_id): if self._user_info.selected_robot == None: self.select_robot(robot_id) - # Stop robot and engage clutch after taking control over it to put it in starting state - self.user_stopped() - self.engage_clutch() else: - # TODO - raise user already selected robot exception - raise NotImplementedError + raise RuntimeError('User already selected robot') def handle_robot_release(self): if self._user_info.selected_robot != None: self.release_robot() else: - # TODO - raise no robot selected error - raise NotImplementedError + raise RuntimeError('Cannot release. No robot selected') def handle_user_stop_state_updated(self): if self._user_info.master_stop_state == SS.STOPPED: @@ -114,11 +137,9 @@ class UserPlugin(Plugin): elif self._user_info.user_stop_state == SS.STOPPED: self.user_started() else: - # TODO - Wrong userstop value exception - raise NotImplementedError + raise ValueError('user_stop_state value undefined') else: - # TODO - raise switching unknown masterstop value exception - raise NotImplementedError + raise ValueError('master_stop_state value undefined') def handle_clutch_switched(self): if self._user_info.clutch_state == CS.ENGAGED: @@ -126,63 +147,76 @@ class UserPlugin(Plugin): elif self._user_info.clutch_state == CS.DISENGAGED: self.engage_clutch() else: - # TODO - raise switching unknown clutch value exception - raise NotImplementedError + raise ValueError('clutch_state value undefined') def handle_robot_connection_lost(self): if self._user_info.selected_robot != None: lost_robot_id = self._user_info.selected_robot.robot_id self.connection_to_robot_lost(lost_robot_id) else: - raise NotImplementedError + raise RuntimeError('Connection lost callback received when no robot was selected') # Operations def master_stopped(self): self.user_stopped() - self._qt_wrapper.master_stopped() + self.call_qt_wrapper_method('master_stopped') + # self._qt_wrapper.master_stopped() self._user_info.master_stop_state = SS.STOPPED def master_started(self): - self._qt_wrapper.master_started() + # self._qt_wrapper.master_started() + self.call_qt_wrapper_method('master_started') self._user_info.master_stop_state = SS.RUNNING def user_stopped(self): self._user_info.user_stop_state = SS.STOPPED self._ros_wrapper.user_stopped() - self._qt_wrapper.user_stopped() + self.call_qt_wrapper_method('user_stopped') + # self._qt_wrapper.user_stopped() def user_started(self): self._user_info.user_stop_state = SS.RUNNING self._ros_wrapper.user_started() - self._qt_wrapper.user_started() + self.call_qt_wrapper_method('user_started') + # self._qt_wrapper.user_started() def engage_clutch(self): self._user_info.clutch_state = CS.ENGAGED self._ros_wrapper.engage_clutch() - self._qt_wrapper.engage_clutch() + self.call_qt_wrapper_method('engage_clutch') + # self._qt_wrapper.engage_clutch() def disengage_clutch(self): self._user_info.clutch_state = CS.DISENGAGED self._ros_wrapper.disengage_clutch() - self._qt_wrapper.disengage_clutch() + self.call_qt_wrapper_method('disengage_clutch') + # self._qt_wrapper.disengage_clutch() def select_robot(self,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._user_info.select_robot(robot_id) + # Stop robot and engage clutch after taking control over it to put it in starting state + self.user_stopped() + self.engage_clutch() + def release_robot(self): self._ros_wrapper.release_robot() - self._qt_wrapper.release_robot() + self.call_qt_wrapper_method('release_robot') + # self._qt_wrapper.release_robot() self._user_info.release_robot() def connection_to_robot_lost(self,lost_robot_id): # pass self._ros_wrapper.release_robot() self._user_info.release_robot() - self._qt_wrapper.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) def update_selected_robot_info(self,robot_info): self._user_info.update_selected_robot_info(robot_info) - self._qt_wrapper.update_selected_robot_info(robot_info) \ No newline at end of file + self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info]) + # self._qt_wrapper.update_selected_robot_info(robot_info) \ No newline at end of file diff --git a/safety_user_plugin/ui/user_view.ui b/safety_user_plugin/ui/user_view.ui index 5dde685..0935d86 100644 --- a/safety_user_plugin/ui/user_view.ui +++ b/safety_user_plugin/ui/user_view.ui @@ -39,7 +39,7 @@ - + 0 @@ -53,7 +53,7 @@ - Otwórz + Wybierz