diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index addc4f2..cf6591b 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -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 \ No newline at end of file diff --git a/safety_user_plugin/scripts/restrictions.py b/safety_user_plugin/scripts/restrictions.py new file mode 100644 index 0000000..5c0cf41 --- /dev/null +++ b/safety_user_plugin/scripts/restrictions.py @@ -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 \ No newline at end of file diff --git a/safety_user_plugin/scripts/robot_info.py b/safety_user_plugin/scripts/robot_info.py index 2348b26..ce48297 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/safety_user_plugin/scripts/robot_info.py @@ -1,9 +1,11 @@ #!/usr/bin/env python - - class RobotInfo: - def __init__(self,robot_id): - self.robot_id = robot_id - raise(NotImplementedError) \ No newline at end of file + 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 \ No newline at end of file diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index da78459..3b49efc 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -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 \ No newline at end of file + 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) + diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py index 092a736..a221817 100644 --- a/safety_user_plugin/scripts/user_info.py +++ b/safety_user_plugin/scripts/user_info.py @@ -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 \ No newline at end of file + 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 \ No newline at end of file diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index ff69b70..e04e039 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -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 \ No newline at end of file + self._ros_wrapper.select_robot(robot_id) + self._qt_wrapper.select_robot(robot_id) + self._user_info.select_robot(robot_id) \ No newline at end of file