diff --git a/safety_user_plugin/scripts/enums/__init__.py b/safety_user_plugin/scripts/enums/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/safety_user_plugin/scripts/enums/clutch_state.py b/safety_user_plugin/scripts/enums/clutch_state.py new file mode 100644 index 0000000..9479b62 --- /dev/null +++ b/safety_user_plugin/scripts/enums/clutch_state.py @@ -0,0 +1,6 @@ + + +class ClutchState: + ENGAGED = 4 + DISENGAGED = 5 + UNKNOWN = 6 \ No newline at end of file diff --git a/safety_user_plugin/scripts/enums/stop_state.py b/safety_user_plugin/scripts/enums/stop_state.py new file mode 100644 index 0000000..17d5939 --- /dev/null +++ b/safety_user_plugin/scripts/enums/stop_state.py @@ -0,0 +1,6 @@ + + +class StopState: + STOPPED = 1 + RUNNING = 2 + UNKNOWN = 3 \ No newline at end of file diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index ef962ec..addc4f2 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -7,20 +7,35 @@ import rospkg class QtWrapper: - def __init__(self,widget): + def __init__(self,widget): - self.widget = widget + self.widget = widget - self.widget.textBrowser.setText('Sukces') + self.widget.textBrowser.setText('Sukces') - def connect_signals(self): - raise NotImplementedError + def connect_signals(self): + raise NotImplementedError - def select_robot(self,robot_id): - raise NotImplementedError + def select_robot(self,robot_id): + raise NotImplementedError - def update_robots_list_gui(self,robots_id_list): - raise NotImplementedError + def update_robots_list_gui(self,robots_id_list): + raise NotImplementedError - def update_selected_robot_info(self,robot_info): - raise NotImplementedError \ No newline at end of file + def update_selected_robot_info(self,robot_info): + raise NotImplementedError + + def master_stopped(): + raise NotImplementedError + + def master_started(): + raise NotImplementedError + + def user_stopped(): + raise NotImplementedError + + def user_started(): + raise NotImplementedError + + def master_is_stopped_notify(): + raise NotImplementedError \ 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 1301632..2348b26 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/safety_user_plugin/scripts/robot_info.py @@ -4,5 +4,6 @@ class RobotInfo: - def __init__(self): - raise(NotImplementedError) \ No newline at end of file + def __init__(self,robot_id): + self.robot_id = robot_id + raise(NotImplementedError) \ 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 effa329..da78459 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -2,15 +2,30 @@ class ROSWrapper: - def __init__(self): - pass + def __init__(self): + pass - def setup_node(self): - raise NotImplementedError + def setup_node(self): + raise NotImplementedError - def select_robot(self,robot_id): - raise NotImplementedError - return False + def select_robot(self,robot_id): + raise NotImplementedError + return False - def set_robots_list_update_callback(self,callback_function): - self.robots_list_update_callback = callback_function + def set_robots_list_update_callback(self,callback_function): + self.robots_list_update_callback = callback_function + + def unregister_node(self): + raise NotImplementedError + + def engage_clutch(self): + raise NotImplementedError + + def disengage_clutch(self): + raise NotImplementedError + + def user_stopped(): + raise NotImplementedError + + def user_started(): + raise NotImplementedError \ No newline at end of file diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py index fbe87e1..092a736 100644 --- a/safety_user_plugin/scripts/user_info.py +++ b/safety_user_plugin/scripts/user_info.py @@ -1,13 +1,21 @@ #!/usr/bin/env python +from robot_info import RobotInfo + class UserInfo: - def __init__(self): - self.selected_robot_id = None - self.robots_id_list = [] + def __init__(self): + self.selected_robot = None + self.robots_id_list = [] + self.user_stop_state = None + self.master_stop_state = None + self.clutch_state = None - def select_robot(self,robot_id): - self.selected_robot_id = robot_id + def select_robot(self,robot_id): + self.selected_robot = RobotInfo(robot_id) - def update_robots_list(self,robots_id_list): - self.robots_id_list = robots_id_list \ No newline at end of file + def deselect_robot(self): + self.selected_robot = None + + def update_robots_id_list(self,robots_id_list): + self.robots_id_list = robots_id_list \ No newline at end of file diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index aef3ff4..ff69b70 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -11,6 +11,9 @@ from ros_wrapper import ROSWrapper from user_widget import UserWidget from user_info import UserInfo +from enums.clutch_state import ClutchState as CS +from enums.stop_state import StopState as SS + class UserPlugin(Plugin): def __init__(self,context): @@ -22,12 +25,20 @@ class UserPlugin(Plugin): self._ros_wrapper = ROSWrapper() self._user_info = UserInfo() + self._user_info.user_stop_state = SS.STOPPED + self._user_info.master_stop_state = SS.UNKNOWN + self._user_info.clutch_state = CS.UNKNOWN + # self.register_callback_functions() # At the end! # self._qt_wrapper.connect_signals() # self._ros_wrapper.setup_node() + def shutdown_plugin(self): + # TODO unregister all publishers here + self._ros_wrapper.unregister_node() + def register_callback_functions(self): raise NotImplementedError # self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) @@ -35,30 +46,89 @@ class UserPlugin(Plugin): # ROS callback functions def handle_robots_list_update(self,robots_id_list): - self._user_info.update_robots_list(robots_id_list) + self._user_info.update_robots_id_list(robots_id_list) self._qt_wrapper.update_robots_list_gui(robots_id_list) def handle_selected_robot_info_update(self,robot_info): self._qt_wrapper.update_selected_robot_info(self,robot_info) - def handle_master_stop_update(self,topic_state): - raise(NotImplementedError) + 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 + + def handle_restrictions_update(self,restrictions): + raise NotImplementedError + # Qt callback functions def handle_robot_selection(self,robot_id): - if self._user_info.selected_robot_id is None: - if self._ros_wrapper.select_robot(robot_id) == True: - self._qt_wrapper.select_robot(robot_id) - self._user_info.select_robot(robot_id) - else: - # TODO - raise ROS subscriptions problem exception - raise NotImplementedError + if self._user_info.selected_robot == None: + self.select_robot(robot_id) else: # TODO - raise user already selected robot exception raise NotImplementedError - def handle_user_stop_button_update(self,button_state): - raise NotImplementedError + def handle_user_stop_state_updated(self): + if self._user_info.master_stop_state == CS.STOPPED: + self._qt_wrapper.master_is_stopped_notify() + elif self._user_info.master_stop_state == CS.RUNNING: + if self._user_info.user_stop_state == CS.RUNNING: + self.user_stopped() + elif self._user_info.user_stop_state == CS.STOPPED: + self.user_started() + else: + # TODO - Wrong userstop value exception + raise NotImplementedError + else: + # TODO - raise switching unknown masterstop value exception + raise NotImplementedError - def handle_clutch_switch(self,clutch_state): - raise NotImplementedError \ No newline at end of file + def handle_clutch_switched(self): + if self._user_info.clutch_state == CS.ENGAGED: + self.disengage_clutch() + elif self._user_info.clutch_state == CS.DISENGAGED: + self.engage_clutch() + else: + # TODO - raise switching unknown clutch value exception + raise NotImplementedError + + def master_stopped(self): + self.user_stopped() + self._qt_wrapper.master_stopped() + self._user_info.master_stop_state = CS.STOPPED + + def master_started(self): + self._qt_wrapper.master_started() + self._user_info.master_stop_state = CS.RUNNING + + def user_stopped(self): + self._user_info.user_stop_state = CS.STOPPED + self._ros_wrapper.user_stopped() + self._qt_wrapper.user_stopped() + + def user_started(self): + self._user_info.user_stop_state = CS.RUNNING + self._ros_wrapper.user_started() + self._qt_wrapper.user_started() + + def engage_clutch(self): + self._user_info.clutch_state = CS.ENGAGED + self._ros_wrapper.engage_clutch() + + def disengage_clutch(self): + self._user_info.clutch_state = CS.DISENGAGED + self._ros_wrapper.disengage_clutch() + + def select_robot(self): + if self._ros_wrapper.select_robot(robot_id) == True: + self._qt_wrapper.select_robot(robot_id) + self._user_info.select_robot(robot_id) + else: + # TODO - raise ROS subscriptions problem exception + raise NotImplementedError \ No newline at end of file