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

Added master_connection_lost handling

Browse Source
This commit is contained in:
Olek Bojda 2018-09-25 13:31:56 +02:00
parent a3061fa7d3
commit 18be3242c6
3 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
safety_user_plugin/scripts/qt_wrapper.py
View File

@ -242,6 +242,8 @@ class QtWrapper:
self.connect_signals() self.connect_signals()
self.log_info('Utrata polaczenia z robotem PIONIER' + str(lost_robot_id)) 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): def scroll_to_bottom(self):
scrollbar = self.widget.logsBrowser.verticalScrollBar() scrollbar = self.widget.logsBrowser.verticalScrollBar()

19
safety_user_plugin/scripts/ros_wrapper.py
View File

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

7
safety_user_plugin/scripts/user_plugin.py
View File

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