From 18be3242c6b70f0c2247f63f76ca1d3bbc2ab45a Mon Sep 17 00:00:00 2001 From: Olek Bojda Date: Tue, 25 Sep 2018 13:31:56 +0200 Subject: [PATCH] Added master_connection_lost handling --- safety_user_plugin/scripts/qt_wrapper.py | 2 ++ safety_user_plugin/scripts/ros_wrapper.py | 19 +++++++++++++++++++ safety_user_plugin/scripts/user_plugin.py | 7 +++++++ 3 files changed, 28 insertions(+) diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index 4b0c71f..9c33223 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/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() diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index 190b604..cfaff97 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/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 diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index b6f185a..cea9176 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/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])