Further development of architecture for user plugin

safety_user_plugin/scripts/qt_wrapper.py

@@ -14,14 +14,18 @@ class QtWrapper:
         self.widget.textBrowser.setText('Sukces')
 
     def connect_signals(self):
-        raise NotImplementedError
+        pass
 
     def select_robot(self,robot_id):
         raise NotImplementedError
 
-    def update_robots_list_gui(self,robots_id_list):
+    def release_robot(self):
         raise NotImplementedError
 
+    def update_robots_list_gui(self,robots_id_list):
+        # raise NotImplementedError
+        pass
+
     def update_selected_robot_info(self,robot_info):
         raise NotImplementedError
 
@@ -37,5 +41,30 @@ class QtWrapper:
     def user_started(self):
         raise NotImplementedError
 
+    def engage_clutch(self):
+        raise NotImplementedError
+
+    def disengage_clutch(self):
+        raise NotImplementedError
+
     def master_is_stopped_notify(self):
+        raise NotImplementedError
+
+    def update_restrictions_gui(self,restrictions):
+        raise NotImplementedError
+
+
+    def set_robot_selection_callback(self,callback):
+        # raise NotImplementedError
+        pass
+
+    def set_user_stop_state_updated_callback(self,callback):
+        # raise NotImplementedError
+        pass
+
+    def set_clutch_switched_callback(self,callback):
+        # raise NotImplementedError
+        pass
+
+    def connection_to_robot_lost(self,lost_robot_id):
         raise NotImplementedError

safety_user_plugin/scripts/ros_wrapper.py

@@ -15,6 +15,8 @@ from enums.stop_state import StopState as SS
 class ROSWrapper:
 
     def __init__(self):
+        # self.setup_node()
+
         self.robots_list_update_callback = None
         self.selected_robot_info_update_callback = None
         self.master_stop_update_callback = None
@@ -32,6 +34,9 @@ class ROSWrapper:
 
         self.periodic_timer = None
         self.robots_list_timer = None
+        self.connection_timer = None
+
+        self.setup_subscribers_and_publishers(6)
 
     def setup_node(self):
         rospy.init_node('safety_user_plugin', anonymous=True)
@@ -47,6 +52,7 @@ class ROSWrapper:
         self.clutch_publisher = rospy.Publisher('/RosAria/'+robot_name+'/clutch',Bool,queue_size = 1)
 
         self.periodic_timer = rospy.Timer(rospy.Duration(0.2),self.publish_periodic_update)
+        self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost)
 
     def unsubscribe(self,subscriber):
         if subscriber != None:
@@ -80,6 +86,10 @@ class ROSWrapper:
             # TODO Raise wrong value error
             raise NotImplementedError  
 
+    def handle_robot_connection_lost(self,event):
+        shutdown_timer(self.connection_timer)
+        self.robot_connection_lost_callback()
+
     def cancel_subscribers_and_publishers(self):
         self.shutdown_timer(self.periodic_timer)
         self.unsubscribe(self.robot_info_subscriber)
@@ -95,6 +105,14 @@ class ROSWrapper:
         self.user_stop_publisher = None
         self.clutch_publisher = None
 
+    def restart_connection_timer(self):
+        if self.connection_timer != None:
+            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
+
     def unregister_node(self):
         self.cancel_subscribers_and_publishers()
         rospy.signal_shutdown('Closing safety user plugin')
@@ -108,31 +126,6 @@ class ROSWrapper:
     def setup_get_clutch_state_function(self,function):
         self.get_clutch_state = function
 
-
-    def scan_for_robots(self):
-        robots_list = []
-
-        published_topics_list = rospy.get_published_topics(namespace='/')
-        published_topics = []
-
-
-        for list_ in published_topics_list:
-            published_topics.append(list_[0])
-
-
-        for topic in published_topics:
-            if topic.find('RosAria') ==-1 or topic.find('robot_state') == -1:
-                pass
-            else:
-                robot_number = topic.split('/')
-                robot_number = robot_number[1]
-                robot_number = robot_number[7:]
-                if len(robot_number) > 0:
-                    robot_number = int(robot_number)
-                    robots_list.append(robot_number)
-
-        return robots_list
-
     # ROSWrapper Callbacks
     def get_robots_list(self,event):
         robots_id_list = []
@@ -157,6 +150,9 @@ class ROSWrapper:
         self.robots_list_update_callback(robots_id_list)
 
     def handle_selected_robot_info_update(self,msg):
+        # Restarting connection timer to avoid raising robot_connection_lost_callback
+        self.restart_connection_timer()
+
         _robot_info = RobotInfo(msg)
         self.selected_robot_info_update_callback(_robot_info)
 
@@ -187,24 +183,26 @@ class ROSWrapper:
     def set_restrictions_update_callback(self,callback_function):
         self.restrictions_update_callback = callback_function
 
+    def set_robot_connection_lost_callback(self,callback_function):
+        self.robot_connection_lost_callback = callback_function
 
     def engage_clutch(self):
         msg = Bool()
         msg.data = True
-        self.clutch_publisher.Publish(msg)
+        self.clutch_publisher.publish(msg)
 
     def disengage_clutch(self):
         msg = Bool()
         msg.data = False
-        self.clutch_publisher.Publish(msg)
+        self.clutch_publisher.publish(msg)
 
-    def user_started():
+    def user_started(self):
         msg = Bool()
         msg.data = True
-        self.user_stop_publisher.Publish(msg)
+        self.user_stop_publisher.publish(msg)
 
-    def user_stopped():
+    def user_stopped(self):
         msg = Bool()
         msg.data = False
-        self.user_stop_publisher.Publish(msg)
+        self.user_stop_publisher.publish(msg)
 

safety_user_plugin/scripts/user_info.py

@@ -14,7 +14,7 @@ class UserInfo:
     def select_robot(self,robot_id):
         self.selected_robot = RobotInfo(robot_id)
 
-    def deselect_robot(self):
+    def release_robot(self):
         self.selected_robot = None
 
     def update_robots_id_list(self,robots_id_list):

safety_user_plugin/scripts/user_plugin.py

@@ -31,12 +31,12 @@ class UserPlugin(Plugin):
         self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
         self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state)
 
-        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        # self._ros_wrapper.setup_node()
-        # self.set_callback_functions()
+        self.set_callback_functions()
 
         # At the end!
         # self._qt_wrapper.connect_signals()
-        # self._ros_wrapper.setup_node()
+        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
 
     def shutdown_plugin(self):
         self._ros_wrapper.unregister_node()
@@ -46,13 +46,17 @@ class UserPlugin(Plugin):
         self.set_ROS_callback_functions()
         
     def set_ROS_callback_functions(self):
-        
         self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update)
         self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update)
         self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update)
+        self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost)
 
     def set_Qt_callback_functions(self):
-        raise NotImplementedError
+        self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
+        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)
+        
+       
 
 
     # ROS callback functions
@@ -75,17 +79,27 @@ class UserPlugin(Plugin):
             raise NotImplementedError             
 
     def handle_restrictions_update(self,restrictions):
-        raise NotImplementedError
+        self._qt_wrapper.update_restrictions_gui(restrictions)
 
 
     # Qt callback functions
     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
 
+    def handle_robot_release(self):
+        if self._user_info.selected_robot != None:
+            self.release_robot()
+        else:
+            # TODO - raise no robot selected error
+            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()
@@ -110,6 +124,13 @@ class UserPlugin(Plugin):
             # TODO - raise switching unknown clutch value exception
             raise NotImplementedError
 
+    def handle_robot_connection_lost(self):
+        lost_robot_id = self._user_info.selected_robot.robot_id
+        self.release_robot()
+        self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
+
+
+    # Operations
     def master_stopped(self):
         self.user_stopped()
         self._qt_wrapper.master_stopped()
@@ -132,12 +153,19 @@ class UserPlugin(Plugin):
     def engage_clutch(self):
         self._user_info.clutch_state = CS.ENGAGED
         self._ros_wrapper.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()
 
-    def select_robot(self):
+    def select_robot(self,robot_id):
         self._ros_wrapper.select_robot(robot_id)
         self._qt_wrapper.select_robot(robot_id)
-        self._user_info.select_robot(robot_id)
+        self._user_info.select_robot(robot_id)
+
+    def release_robot(self):
+        self._ros_wrapper.release_robot()
+        self._qt_wrapper.release_robot()
+        self._user_info.release_robot()