From 388759b8aa26f29762f974ea302a4a4b0aa838fe Mon Sep 17 00:00:00 2001 From: Olek Bojda Date: Tue, 18 Sep 2018 14:56:21 +0200 Subject: [PATCH] Further development of architecture for user plugin --- safety_user_plugin/scripts/qt_wrapper.py | 33 ++++++++++++- safety_user_plugin/scripts/ros_wrapper.py | 60 +++++++++++------------ safety_user_plugin/scripts/user_info.py | 2 +- safety_user_plugin/scripts/user_plugin.py | 44 ++++++++++++++--- 4 files changed, 97 insertions(+), 42 deletions(-) diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index cf6591b..e4b6875 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -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 \ 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 955d1ce..833a2b1 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -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) diff --git a/safety_user_plugin/scripts/user_info.py b/safety_user_plugin/scripts/user_info.py index a221817..bf0ed6a 100644 --- a/safety_user_plugin/scripts/user_info.py +++ b/safety_user_plugin/scripts/user_info.py @@ -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): diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index 757e167..e5720df 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -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) \ No newline at end of file + 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() \ No newline at end of file