Further development of user plugin

safety_user_plugin/scripts/enums/clutch_state.py

@@ -0,0 +1,6 @@
+
+
+class ClutchState:
+	ENGAGED = 4
+	DISENGAGED = 5
+	UNKNOWN = 6

safety_user_plugin/scripts/enums/stop_state.py

@@ -0,0 +1,6 @@
+
+
+class StopState:
+	STOPPED = 1
+	RUNNING = 2
+	UNKNOWN = 3

safety_user_plugin/scripts/qt_wrapper.py

@@ -24,3 +24,18 @@ class QtWrapper:
 
     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

safety_user_plugin/scripts/robot_info.py

@@ -4,5 +4,6 @@
 
 class RobotInfo:
 
-	def __init__(self):
+    def __init__(self,robot_id):
+        self.robot_id = robot_id
         raise(NotImplementedError)

safety_user_plugin/scripts/ros_wrapper.py

@@ -14,3 +14,18 @@ class ROSWrapper:
 
     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

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.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
+        self.selected_robot = RobotInfo(robot_id)
 
-	def update_robots_list(self,robots_id_list):
+    def deselect_robot(self):
+        self.selected_robot = None
+
+    def update_robots_id_list(self,robots_id_list):
         self.robots_id_list = robots_id_list

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._user_info.selected_robot == None:
+            self.select_robot(robot_id)
+        else:
+            # TODO - raise user already selected robot exception
+            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_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
-        else:
-            # TODO - raise user already selected robot exception
-            raise NotImplementedError
-
-    def handle_user_stop_button_update(self,button_state):
-        raise NotImplementedError
-
-    def handle_clutch_switch(self,clutch_state):
-        raise NotImplementedError