L1.5/l1.5-safety-system
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Fixed problem with connection_lost_callback firing when releasing and again selecting robot. Everything seems to work fine

Browse Source
This commit is contained in:
Olek Bojda 2018-09-27 12:43:52 +02:00
parent f3547bca5a
commit 4d8b68cd7d
2 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -48,8 +48,16 @@ class ROSWrapper:
self.user_stop_publisher = rospy.Publisher('/RosAria/{0}/user_stop'.format(robot_name),Bool,queue_size = 1) 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) 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) 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) 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) self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
def unsubscribe(self,subscriber): def unsubscribe(self,subscriber):
@ -160,6 +168,7 @@ class ROSWrapper:
self.unsubscribe(self.robot_info_subscriber) self.unsubscribe(self.robot_info_subscriber)
self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.periodic_timer)
self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.connection_timer)
self.shutdown_timer(self.master_connection_timer)
def handle_selected_robot_info_update(self,msg): def handle_selected_robot_info_update(self,msg):
# Restarting connection timer to avoid raising robot_connection_lost_callback # Restarting connection timer to avoid raising robot_connection_lost_callback

12
safety_user_plugin/scripts/user_plugin.py
View File

@ -28,7 +28,6 @@ class CallbackHandler(QObject):
def list_connect(self,slot): def list_connect(self,slot):
self.signal_with_list_argument.connect(slot) self.signal_with_list_argument.connect(slot)
# def handle
class UserPlugin(Plugin): class UserPlugin(Plugin):
@ -162,16 +161,20 @@ class UserPlugin(Plugin):
# Operations # Operations
def master_stopped(self): def master_stopped(self):
self._user_info.master_stop_state = SS.STOPPED
if self._user_info.selected_robot != None:
self.user_stopped() self.user_stopped()
self.call_qt_wrapper_method('master_stopped') self.call_qt_wrapper_method('master_stopped')
# self._qt_wrapper.master_stopped() # self._qt_wrapper.master_stopped()
self._user_info.master_stop_state = SS.STOPPED
def master_started(self): def master_started(self):
# self._qt_wrapper.master_started()
self.call_qt_wrapper_method('master_started')
self._user_info.master_stop_state = SS.RUNNING 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): def user_stopped(self):
self._user_info.user_stop_state = SS.STOPPED self._user_info.user_stop_state = SS.STOPPED
self._ros_wrapper.user_stopped() self._ros_wrapper.user_stopped()
@ -220,6 +223,7 @@ class UserPlugin(Plugin):
# self._qt_wrapper.connection_to_robot_lost(lost_robot_id) # self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
def connection_to_master_lost(self): def connection_to_master_lost(self):
if self._user_info.selected_robot != None:
self.master_stopped() self.master_stopped()
self.call_qt_wrapper_method('connection_to_master_lost') self.call_qt_wrapper_method('connection_to_master_lost')