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

Further development of architecture for user plugin

Browse Source
This commit is contained in:
Olek Bojda 2018-09-18 14:56:21 +02:00
parent ba57aae639
commit 388759b8aa
4 changed files with 97 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
safety_user_plugin/scripts/qt_wrapper.py
View File

@ -14,14 +14,18 @@ class QtWrapper:
self.widget.textBrowser.setText('Sukces')
def connect_signals(self):
raise NotImplementedError
pass
def select_robot(self,robot_id):
raise NotImplementedError
def update_robots_list_gui(self,robots_id_list):
def release_robot(self):
raise NotImplementedError
def update_robots_list_gui(self,robots_id_list):
# raise NotImplementedError
pass
def update_selected_robot_info(self,robot_info):
raise NotImplementedError
@ -37,5 +41,30 @@ class QtWrapper:
def user_started(self):
raise NotImplementedError
def engage_clutch(self):
raise NotImplementedError
def disengage_clutch(self):
raise NotImplementedError
def master_is_stopped_notify(self):
raise NotImplementedError
def update_restrictions_gui(self,restrictions):
raise NotImplementedError
def set_robot_selection_callback(self,callback):
# raise NotImplementedError
pass
def set_user_stop_state_updated_callback(self,callback):
# raise NotImplementedError
pass
def set_clutch_switched_callback(self,callback):
# raise NotImplementedError
pass
def connection_to_robot_lost(self,lost_robot_id):
raise NotImplementedError

60
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -15,6 +15,8 @@ from enums.stop_state import StopState as SS
class ROSWrapper:
def __init__(self):
# self.setup_node()
self.robots_list_update_callback = None
self.selected_robot_info_update_callback = None
self.master_stop_update_callback = None
@ -32,6 +34,9 @@ class ROSWrapper:
self.periodic_timer = None
self.robots_list_timer = None
self.connection_timer = None
self.setup_subscribers_and_publishers(6)
def setup_node(self):
rospy.init_node('safety_user_plugin', anonymous=True)
@ -47,6 +52,7 @@ class ROSWrapper:
self.clutch_publisher = rospy.Publisher('/RosAria/'+robot_name+'/clutch',Bool,queue_size = 1)
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)
def unsubscribe(self,subscriber):
if subscriber != None:
@ -80,6 +86,10 @@ class ROSWrapper:
# TODO Raise wrong value error
raise NotImplementedError
def handle_robot_connection_lost(self,event):
shutdown_timer(self.connection_timer)
self.robot_connection_lost_callback()
def cancel_subscribers_and_publishers(self):
self.shutdown_timer(self.periodic_timer)
self.unsubscribe(self.robot_info_subscriber)
@ -95,6 +105,14 @@ class ROSWrapper:
self.user_stop_publisher = None
self.clutch_publisher = None
def restart_connection_timer(self):
if self.connection_timer != None:
self.connection_timer.shutdown()
self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost)
else:
# TODO - raise connection_timer_not_setup error
raise NotImplementedError
def unregister_node(self):
self.cancel_subscribers_and_publishers()
rospy.signal_shutdown('Closing safety user plugin')
@ -108,31 +126,6 @@ class ROSWrapper:
def setup_get_clutch_state_function(self,function):
self.get_clutch_state = function
def scan_for_robots(self):
robots_list = []
published_topics_list = rospy.get_published_topics(namespace='/')
published_topics = []
for list_ in published_topics_list:
published_topics.append(list_[0])
for topic in published_topics:
if topic.find('RosAria') ==-1 or topic.find('robot_state') == -1:
pass
else:
robot_number = topic.split('/')
robot_number = robot_number[1]
robot_number = robot_number[7:]
if len(robot_number) > 0:
robot_number = int(robot_number)
robots_list.append(robot_number)
return robots_list
# ROSWrapper Callbacks
def get_robots_list(self,event):
robots_id_list = []
@ -157,6 +150,9 @@ class ROSWrapper:
self.robots_list_update_callback(robots_id_list)
def handle_selected_robot_info_update(self,msg):
# Restarting connection timer to avoid raising robot_connection_lost_callback
self.restart_connection_timer()
_robot_info = RobotInfo(msg)
self.selected_robot_info_update_callback(_robot_info)
@ -187,24 +183,26 @@ class ROSWrapper:
def set_restrictions_update_callback(self,callback_function):
self.restrictions_update_callback = callback_function
def set_robot_connection_lost_callback(self,callback_function):
self.robot_connection_lost_callback = callback_function
def engage_clutch(self):
msg = Bool()
msg.data = True
self.clutch_publisher.Publish(msg)
self.clutch_publisher.publish(msg)
def disengage_clutch(self):
msg = Bool()
msg.data = False
self.clutch_publisher.Publish(msg)
self.clutch_publisher.publish(msg)
def user_started():
def user_started(self):
msg = Bool()
msg.data = True
self.user_stop_publisher.Publish(msg)
self.user_stop_publisher.publish(msg)
def user_stopped():
def user_stopped(self):
msg = Bool()
msg.data = False
self.user_stop_publisher.Publish(msg)
self.user_stop_publisher.publish(msg)

2
safety_user_plugin/scripts/user_info.py
View File

@ -14,7 +14,7 @@ class UserInfo:
def select_robot(self,robot_id):
self.selected_robot = RobotInfo(robot_id)
def deselect_robot(self):
def release_robot(self):
self.selected_robot = None
def update_robots_id_list(self,robots_id_list):

42
safety_user_plugin/scripts/user_plugin.py
View File

@ -31,12 +31,12 @@ class UserPlugin(Plugin):
self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state)
self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state)
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
# self.set_callback_functions()
# self._ros_wrapper.setup_node()
self.set_callback_functions()
# At the end!
# self._qt_wrapper.connect_signals()
# self._ros_wrapper.setup_node()
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
def shutdown_plugin(self):
self._ros_wrapper.unregister_node()
@ -46,13 +46,17 @@ class UserPlugin(Plugin):
self.set_ROS_callback_functions()
def set_ROS_callback_functions(self):
self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_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_robot_connection_lost_callback(self.handle_robot_connection_lost)
def set_Qt_callback_functions(self):
raise NotImplementedError
self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
self._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated)
self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched)
# ROS callback functions
@ -75,17 +79,27 @@ class UserPlugin(Plugin):
raise NotImplementedError
def handle_restrictions_update(self,restrictions):
raise NotImplementedError
self._qt_wrapper.update_restrictions_gui(restrictions)
# Qt callback functions
def handle_robot_selection(self,robot_id):
if self._user_info.selected_robot == None:
self.select_robot(robot_id)
# Stop robot and engage clutch after taking control over it to put it in starting state
self.user_stopped()
self.engage_clutch()
else:
# TODO - raise user already selected robot exception
raise NotImplementedError
def handle_robot_release(self):
if self._user_info.selected_robot != None:
self.release_robot()
else:
# TODO - raise no robot selected error
raise NotImplementedError
def handle_user_stop_state_updated(self):
if self._user_info.master_stop_state == CS.STOPPED:
self._qt_wrapper.master_is_stopped_notify()
@ -110,6 +124,13 @@ class UserPlugin(Plugin):
# TODO - raise switching unknown clutch value exception
raise NotImplementedError
def handle_robot_connection_lost(self):
lost_robot_id = self._user_info.selected_robot.robot_id
self.release_robot()
self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
# Operations
def master_stopped(self):
self.user_stopped()
self._qt_wrapper.master_stopped()
@ -132,12 +153,19 @@ class UserPlugin(Plugin):
def engage_clutch(self):
self._user_info.clutch_state = CS.ENGAGED
self._ros_wrapper.engage_clutch()
self._qt_wrapper.engage_clutch()
def disengage_clutch(self):
self._user_info.clutch_state = CS.DISENGAGED
self._ros_wrapper.disengage_clutch()
self._qt_wrapper.disengage_clutch()
def select_robot(self):
def select_robot(self,robot_id):
self._ros_wrapper.select_robot(robot_id)
self._qt_wrapper.select_robot(robot_id)
self._user_info.select_robot(robot_id)
def release_robot(self):
self._ros_wrapper.release_robot()
self._qt_wrapper.release_robot()
self._user_info.release_robot()