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

Further changes in user plugin

Browse Source
This commit is contained in:
Olek Bojda 2018-09-18 01:46:10 +02:00
parent abad617bc8
commit 1652612df7
6 changed files with 238 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
safety_user_plugin/scripts/qt_wrapper.py
View File

@ -25,17 +25,17 @@ class QtWrapper:
def update_selected_robot_info(self,robot_info):
raise NotImplementedError
def master_stopped():
def master_stopped(self):
raise NotImplementedError
def master_started():
def master_started(self):
raise NotImplementedError
def user_stopped():
def user_stopped(self):
raise NotImplementedError
def user_started():
def user_started(self):
raise NotImplementedError
def master_is_stopped_notify():
def master_is_stopped_notify(self):
raise NotImplementedError

8
safety_user_plugin/scripts/restrictions.py Normal file
View File

@ -0,0 +1,8 @@
#!/usr/bin/env python
class Restrictions:
def __init__(self,restrictions_msg):
self.distance = restrictions_msg.distance
self.linear_velocity = restrictions_msg.linear_velocity
self.angular_velocity = restrictions_msg.angular_velocity

12
safety_user_plugin/scripts/robot_info.py
View File

@ -1,9 +1,11 @@
#!/usr/bin/env python
class RobotInfo:
def __init__(self,robot_id):
self.robot_id = robot_id
raise(NotImplementedError)
def __init__(self,robot_info_msg):
self.robot_id = robot_info_msg.robot_id
self.battery_voltage = robot_info_msg.battery_voltage
self.linear_velocity = robot_info_msg.linear_velocity
self.angular_velocity = robot_info_msg.angular_velocity
self.state = robot_info_msg.state
self.clutch = robot_info_msg.clutch

199
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -1,31 +1,206 @@
#!/usr/bin/env python
import rospy
import rospkg
from std_msgs.msg import Bool
from safety_user_plugin.msg import RobotInfoMsg
from safety_user_plugin.msg import RestrictionsMsg
from robot_info import RobotInfo
from restrictions import Restrictions
from enums.clutch_state import ClutchState as CS
from enums.stop_state import StopState as SS
class ROSWrapper:
def __init__(self):
pass
self.robots_list_update_callback = None
self.selected_robot_info_update_callback = None
self.master_stop_update_callback = None
self.restrictions_update_callback = None
self.get_user_stop_state = None
self.get_clutch_state = None
self.robot_info_subscriber = None
self.master_stop_subscriber = None
self.restrictions_subscriber = None
self.user_stop_publisher = None
self.clutch_publisher = None
self.periodic_timer = None
def setup_node(self):
raise NotImplementedError
rospy.init_node('safety_user_plugin', anonymous=True)
def setup_subscribers_and_publishers(self,robot_id):
robot_name = 'PIONIER' + str(robot_id)
self.robot_info_subscriber = rospy.Subscriber('/RosAria/'+robot_name+'/robot_info', RobotInfoMsg, self.handle_selected_robot_info_update)
self.master_stop_subscriber = rospy.Subscriber('/RosAria/master_stop', Bool, self.handle_master_stop_update)
self.restrictions_subscriber = rospy.Subscriber('/RosAria/restrictions', RestrictionsMsg, self.handle_restrictions_update)
self.user_stop_publisher = rospy.Publisher('/RosAria/'+robot_name+'/user_stop',Bool,queue_size = 1)
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)
def unsubscribe(self,subscriber):
if subscriber != None:
subscriber.unsubscribe()
def unregister_publisher(self,publisher):
if publisher != None:
publisher.unregister()
def shutdown_timer(self,timer):
if timer != None:
timer.shutdown()
def publish_periodic_update(self,event):
stop_state = self.get_user_stop_state()
clutch_state = self.get_clutch_state()
if stop_state == SS.RUNNING:
self.user_started()
elif stop_state == SS.STOPPED:
self.user_stopped()
else:
# TODO Raise wrong value error
raise NotImplementedError
if clutch_state == CS.ENGAGED:
self.engage_clutch()
elif clutch_state == CS.DISENGAGED:
self.disengage_clutch()
else:
# TODO Raise wrong value error
raise NotImplementedError
def cancel_subscribers_and_publishers(self):
self.shutdown_timer(self.periodic_timer)
self.unsubscribe(self.robot_info_subscriber)
self.unsubscribe(self.master_stop_subscriber)
self.unsubscribe(self.restrictions_subscriber)
self.unregister_publisher(self.user_stop_publisher)
self.unregister_publisher(self.clutch_publisher)
self.periodic_timer = None
self.robot_info_subscriber = None
self.master_stop_subscriber = None
self.restrictions_subscriber = None
self.user_stop_publisher = None
self.clutch_publisher = None
def unregister_node(self):
self.cancel_subscribers_and_publishers()
rospy.signal_shutdown('Closing safety user plugin')
def select_robot(self,robot_id):
raise NotImplementedError
return False
self.setup_subscribers_and_publishers(robot_id)
def setup_get_user_stop_state_function(self,function):
self.get_user_stop_state = function
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):
robots_id_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') and topic_find('robot_state'):
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_id_list.append(robot_number)
self.robots_list_update_callback(robots_id_list)
def handle_selected_robot_info_update(self,msg):
_robot_info = RobotInfo(msg)
self.selected_robot_info_update_callback(_robot_info)
def handle_master_stop_update(self,msg):
master_stop_state = SS.UNKNOWN
if msg.data == True:
master_stop_state = SS.RUNNING
else:
master_stop_state = SS.STOPPED
self.master_stop_update_callback(master_stop_state)
def handle_restrictions_update(self,msg):
restrictions = Restrictions(msg)
restrictions_update_callback(restrictions)
# UserPlugin Callbacks
def set_robots_list_update_callback(self,callback_function):
self.robots_list_update_callback = callback_function
def unregister_node(self):
raise NotImplementedError
def set_selected_robot_info_update_callback(self,callback_function):
self.selected_robot_info_update_callback = callback_function
def set_master_stop_update_callback(self,callback_function):
self.master_stop_update_callback = callback_function
def set_restrictions_update_callback(self,callback_function):
self.restrictions_update_callback = callback_function
def engage_clutch(self):
raise NotImplementedError
msg = Bool()
msg.data = True
self.clutch_publisher.Publish(msg)
def disengage_clutch(self):
raise NotImplementedError
def user_stopped():
raise NotImplementedError
msg = Bool()
msg.data = False
self.clutch_publisher.Publish(msg)
def user_started():
raise NotImplementedError
msg = Bool()
msg.data = True
self.user_stop_publisher.Publish(msg)
def user_stopped():
msg = Bool()
msg.data = False
self.user_stop_publisher.Publish(msg)

11
safety_user_plugin/scripts/user_info.py
View File

@ -18,4 +18,13 @@ class UserInfo:
self.selected_robot = None
def update_robots_id_list(self,robots_id_list):
self.robots_id_list = robots_id_list
self.robots_id_list = robots_id_list
def update_selected_robot_info(self,new_robot_info):
raise NotImplementedError
def get_user_stop_state(self):
return self.user_stop_state
def get_clutch_state(self):
return self.clutch_state

34
safety_user_plugin/scripts/user_plugin.py
View File

@ -1,8 +1,6 @@
#!/usr/bin/env python
import os
import rospy
import rospkg
from qt_gui.plugin import Plugin
@ -29,19 +27,31 @@ class UserPlugin(Plugin):
self._user_info.master_stop_state = SS.UNKNOWN
self._user_info.clutch_state = CS.UNKNOWN
# self.register_callback_functions()
# Setup functions to get _user_info states from _ros_wrapper
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.set_callback_functions()
# At the end!
# self._qt_wrapper.connect_signals()
# self._ros_wrapper.setup_node()
def shutdown_plugin(self):
# TODO unregister all publishers here
self._ros_wrapper.unregister_node()
def register_callback_functions(self):
def set_callback_functions(self):
self.set_Qt_callback_functions()
self.set_ROS_callback_functions()
def set_ROS_callback_functions(self):
self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
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)
def set_Qt_callback_functions(self):
raise NotImplementedError
# self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
# ROS callback functions
@ -50,7 +60,8 @@ class UserPlugin(Plugin):
self._qt_wrapper.update_robots_list_gui(robots_id_list)
def handle_selected_robot_info_update(self,robot_info):
self._qt_wrapper.update_selected_robot_info(self,robot_info)
self._user_info.update_selected_robot_info(robot_info)
self._qt_wrapper.update_selected_robot_info(robot_info)
def handle_master_stop_update(self,master_stop_state):
self._user_info.master_stop_state = master_stop_state
@ -126,9 +137,6 @@ class UserPlugin(Plugin):
self._ros_wrapper.disengage_clutch()
def select_robot(self):
if self._ros_wrapper.select_robot(robot_id) == True:
self._qt_wrapper.select_robot(robot_id)
self._user_info.select_robot(robot_id)
else:
# TODO - raise ROS subscriptions problem exception
raise NotImplementedError
self._ros_wrapper.select_robot(robot_id)
self._qt_wrapper.select_robot(robot_id)
self._user_info.select_robot(robot_id)