diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index 642ec76..68f32ff 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -48,8 +48,16 @@ class ROSWrapper: self.user_stop_publisher = rospy.Publisher('/RosAria/{0}/user_stop'.format(robot_name),Bool,queue_size = 1) self.clutch_publisher = rospy.Publisher('/RosAria/{0}/clutch'.format(robot_name),Bool,queue_size = 1) + if self.periodic_timer != None: + self.shutdown_timer(self.periodic_timer) self.periodic_timer = rospy.Timer(rospy.Duration(0.2),self.publish_periodic_update) + + if self.connection_timer != None: + self.shutdown_timer(self.connection_timer) self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost) + + if self.master_connection_timer != None: + self.shutdown_timer(self.master_connection_timer) self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost) def unsubscribe(self,subscriber): @@ -160,6 +168,7 @@ class ROSWrapper: self.unsubscribe(self.robot_info_subscriber) self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.connection_timer) + self.shutdown_timer(self.master_connection_timer) def handle_selected_robot_info_update(self,msg): # Restarting connection timer to avoid raising robot_connection_lost_callback diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index cea9176..c982be3 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -28,7 +28,6 @@ class CallbackHandler(QObject): def list_connect(self,slot): self.signal_with_list_argument.connect(slot) - # def handle class UserPlugin(Plugin): @@ -162,15 +161,19 @@ class UserPlugin(Plugin): # Operations def master_stopped(self): - self.user_stopped() - self.call_qt_wrapper_method('master_stopped') - # self._qt_wrapper.master_stopped() self._user_info.master_stop_state = SS.STOPPED + + if self._user_info.selected_robot != None: + self.user_stopped() + self.call_qt_wrapper_method('master_stopped') + # self._qt_wrapper.master_stopped() def master_started(self): - # self._qt_wrapper.master_started() - self.call_qt_wrapper_method('master_started') self._user_info.master_stop_state = SS.RUNNING + + if self._user_info.selected_robot != None: + self.call_qt_wrapper_method('master_started') + # self._qt_wrapper.master_started() def user_stopped(self): self._user_info.user_stop_state = SS.STOPPED @@ -220,8 +223,9 @@ class UserPlugin(Plugin): # 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') + if self._user_info.selected_robot != None: + 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)