Added master_connection_lost handling

safety_user_plugin/scripts/qt_wrapper.py

@@ -242,6 +242,8 @@ class QtWrapper:
         self.connect_signals()
         self.log_info('Utrata polaczenia z robotem PIONIER' + str(lost_robot_id))
 
+    def connection_to_master_lost(self):
+        self.log_info('Utrata polaczenia z masterstopem')
 
     def scroll_to_bottom(self):
         scrollbar = self.widget.logsBrowser.verticalScrollBar()

safety_user_plugin/scripts/ros_wrapper.py

@@ -33,6 +33,7 @@ class ROSWrapper:
         self.periodic_timer = None
         self.robots_list_timer = None
         self.connection_timer = None
+        self.master_connection_timer = None
 
     def setup_node(self):
         rospy.init_node('safety_user_plugin', anonymous=True)
@@ -49,6 +50,7 @@ class ROSWrapper:
 
         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)
+        self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
 
     def unsubscribe(self,subscriber):
         if subscriber != None:
@@ -84,6 +86,10 @@ class ROSWrapper:
         self.shutdown_timer(self.connection_timer)
         self.robot_connection_lost_callback()
 
+    def handle_master_connection_lost(self,event):
+        self.shutdown_timer(self.master_connection_timer)
+        self.master_connection_lost_callback()
+
     def cancel_subscribers_and_publishers(self):
         self.shutdown_timer(self.periodic_timer)
         self.unsubscribe(self.robot_info_subscriber)
@@ -106,6 +112,13 @@ class ROSWrapper:
         else:
             raise RuntimeError('Attempting to restart connection_timer when it is not initialized')
 
+    def restart_master_connection_timer(self):
+        if self.master_connection_timer != None:
+            self.master_connection_timer.shutdown()
+            self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
+        else:
+            raise RuntimeError('Attempting to restart master_connection_timer when it is not initialized')
+
     def unregister_node(self):
         self.cancel_subscribers_and_publishers()
         rospy.signal_shutdown('Closing safety user plugin')
@@ -157,6 +170,9 @@ class ROSWrapper:
         self.selected_robot_info_update_callback(_robot_info)
 
     def handle_master_stop_update(self,msg):
+        # Restarting master connection timer to avoid raising master_connection_lost_callback
+        self.restart_master_connection_timer()
+
         master_stop_state = SS.UNKNOWN
         if msg.data == True:
             master_stop_state = SS.RUNNING
@@ -186,6 +202,9 @@ class ROSWrapper:
     def set_robot_connection_lost_callback(self,callback_function):
         self.robot_connection_lost_callback = callback_function
 
+    def set_master_connection_lost_callback(self,callback_function):
+        self.master_connection_lost_callback = callback_function
+
     def engage_clutch(self):
         msg = Bool()
         msg.data = True

safety_user_plugin/scripts/user_plugin.py

@@ -80,6 +80,7 @@ class UserPlugin(Plugin):
         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)
+        self._ros_wrapper.set_master_connection_lost_callback(self.handle_master_connection_lost)
 
     def set_Qt_callback_functions(self):
         self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
@@ -156,6 +157,8 @@ class UserPlugin(Plugin):
         else:
             raise RuntimeError('Connection lost callback received when no robot was selected')
 
+    def handle_master_connection_lost(self):
+        self.connection_to_master_lost()
 
     # Operations
     def master_stopped(self):
@@ -216,6 +219,10 @@ class UserPlugin(Plugin):
         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 connection_to_master_lost(self):
+        self.master_stopped()
+        self.call_qt_wrapper_method('connection_to_master_lost')
+
     def update_selected_robot_info(self,robot_info):
         self._user_info.update_selected_robot_info(robot_info)
         self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])