From 1c621bf6845259f1df9eaf3f5bb921990cf28e99 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 23 Aug 2023 19:43:24 +0200 Subject: [PATCH] Working with one robot Signed-off-by: Jakub Delicat --- safety_user_plugin/english.py | 3 + safety_user_plugin/polish.py | 3 + safety_user_plugin/qt_wrapper.py | 3 + safety_user_plugin/ros_wrapper.py | 102 ++++++++++-------------------- safety_user_plugin/user_plugin.py | 24 ++----- 5 files changed, 48 insertions(+), 87 deletions(-) diff --git a/safety_user_plugin/english.py b/safety_user_plugin/english.py index 3023397..a6a8331 100644 --- a/safety_user_plugin/english.py +++ b/safety_user_plugin/english.py @@ -38,6 +38,9 @@ class EnglishTexts(LanguageTexts): obstacle_removed_text = "Obstacle removed" # Logger error/problem messages + master_plugin_inactive_text = ( + "Master is inactive. Ask the teacher about the master plugin." + ) selection_error_text = "Select the PIONEER from robots list first" cannot_start_masterstop_text = "Robot can't be started. MasterSTOP pressed!" cannot_start_obstacle_text = "Robot can't be started. Obstacle detected!" diff --git a/safety_user_plugin/polish.py b/safety_user_plugin/polish.py index f5ee653..8bbc0cc 100644 --- a/safety_user_plugin/polish.py +++ b/safety_user_plugin/polish.py @@ -42,6 +42,9 @@ class PolishTexts(LanguageTexts): obstacle_removed_text = "Przeszkoda usunięta" # Logger error/problem messages + master_plugin_inactive_text = ( + "Master nieaktywny. Zapytaj prowadzącego o odpalenie programu." + ) selection_error_text = "Najpierw wybierz PIONEERA z listy robotów" cannot_start_masterstop_text = "Nie można wystartować robota. MasterSTOP wciśnięty!" cannot_start_obstacle_text = ( diff --git a/safety_user_plugin/qt_wrapper.py b/safety_user_plugin/qt_wrapper.py index 61bf232..66d3af3 100644 --- a/safety_user_plugin/qt_wrapper.py +++ b/safety_user_plugin/qt_wrapper.py @@ -140,6 +140,9 @@ class QtWrapper: def handle_not_selected_error(self): self.log_error(self.language.selection_error_text) + def log_master_inactive(self): + self.log_warning(self.language.master_plugin_inactive_text) + def handle_openButton_clicked(self): selected_robot_id = self.get_selected_robot_id() diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index 767683e..33ef8d7 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/safety_user_plugin/ros_wrapper.py @@ -29,6 +29,8 @@ class ROSWrapper(Node): self.selected_robot_info_update_callback = None self.master_stop_update_callback = None self.restrictions_update_callback = None + self.master_connection_lost_callback = None + self.robot_connection_lost_callback = None self.get_user_stop_state = None self.get_clutch_state = None @@ -92,11 +94,10 @@ class ROSWrapper(Node): # rospy.Duration(0.1), self.publish_periodic_update # ) # print("NEW") - # if self.connection_timer != None: - # self.shutdown_timer(self.connection_timer) - # self.connection_timer = rospy.Timer( - # rospy.Duration(5), self.handle_robot_connection_lost - # ) + self.shutdown_timer(self.connection_timer) + self.connection_timer = self.create_timer( + 5.0, self.handle_robot_connection_lost + ) # if self.master_connection_timer != None: # self.shutdown_timer(self.master_connection_timer) @@ -109,18 +110,21 @@ class ROSWrapper(Node): # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) def unsubscribe(self, subscriber): - if subscriber != None: - subscriber.destroy() + if subscriber is not None: + self.destroy_subscription(subscriber) + subscriber = None def unregister_publisher(self, publisher): - if publisher != None: - publisher.destroy() + while publisher is not None: + self.destroy_publisher(publisher) + publisher = None def shutdown_timer(self, timer): - if timer != None: - timer.shutdown() + while timer is not None: + self.destroy_timer(timer) + timer = None - def publish_periodic_update(self, event): + def publish_periodic_update(self): stop_state = self.get_user_stop_state() clutch_state = self.get_clutch_state() @@ -149,11 +153,11 @@ class ROSWrapper(Node): # "/PIONEER{0}/RosAria/user_stop".format(key) # ) - def handle_robot_connection_lost(self, event): + def handle_robot_connection_lost(self): self.shutdown_timer(self.connection_timer) self.robot_connection_lost_callback() - def handle_master_connection_lost(self, event): + def handle_master_connection_lost(self): self.shutdown_timer(self.master_connection_timer) self.master_connection_lost_callback() @@ -165,38 +169,21 @@ class ROSWrapper(Node): 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 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: - raise RuntimeError( - "Attempting to restart connection_timer when it is not initialized" - ) + self.shutdown_timer(self.connection_timer) + self.connection_timer = self.create_timer( + 5.0, self.handle_robot_connection_lost + ) def restart_master_connection_timer(self): - if self.master_connection_timer != None: - print("Destroy connection timer") - # self.master_connection_timer.destroy() - # 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" - ) + self.shutdown_timer(self.master_connection_timer) + self.master_connection_timer = self.create_timer( + 1.5, self.handle_master_connection_lost + ) def unregister_node(self): self.cancel_subscribers_and_publishers() + print("Unregister node.") # rospy.signal_shutdown("Closing safety user plugin") def select_robot(self, robot_id): @@ -211,14 +198,12 @@ class ROSWrapper(Node): # ROSWrapper Callbacks def get_robots_list_and_refresh_subscribers(self): robots_id_list = self._get_robots_list() - # self._create_hz_subscribers(robots_id_list) - # self._remove_hz_subscribers(robots_id_list) self.robots_list_update_callback(robots_id_list) def _get_robots_list(self): robots_id_list = [] # NOTE: Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info - for pioneer_id in range(6): + for pioneer_id in range(1, 7): published_topics_list = [] try: published_topics_list = self.get_publisher_names_and_types_by_node( @@ -231,40 +216,20 @@ class ROSWrapper(Node): robots_id_list.append(pioneer_id) return robots_id_list - def _create_hz_subscribers(self, robots_id_list): - # Adding new hz subscribers - for robot_id in robots_id_list: - if robot_id not in self.robots_hz_subscribers_dict: - # Add hz subscriber - print("ADDED SUB") - # self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1) - # self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber( - # "/PIONEER{0}/RosAria/user_stop".format(robot_id), - # Bool, - # self.rostopics_hz_dict[robot_id].callback_hz, - # callback_args="/PIONEER{0}/RosAria/user_stop".format(robot_id), - # ) - - def _remove_hz_subscribers(self, robots_id_list): - # Removing old hz subscribers - for robot_key in self.robots_hz_subscribers_dict: - if robot_key not in robots_id_list: - # Remove old subscriber - self.unsubscribe(self.robots_hz_subscribers_dict[robot_id]) - self.robots_hz_subscribers_dict[robot_id] = None - self.rostopics_hz_dict[robot_id] = None - def release_robot(self): self.disengage_clutch() self.user_robot_disabled() self.unsubscribe(self.robot_info_subscriber) + self.unsubscribe(self.restrictions_subscriber) + self.unsubscribe(self.master_stop_subscriber) + self.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.connection_timer) self.shutdown_timer(self.master_connection_timer) def handle_selected_robot_info_update(self, msg): # Restarting connection timer to avoid raising robot_connection_lost_callback - # self.restart_connection_timer() + self.restart_connection_timer() _robot_info = RobotInfo(0) _robot_info.update_robot_info(msg) @@ -272,8 +237,7 @@ class ROSWrapper(Node): def handle_master_stop_update(self, msg): # Restarting master connection timer to avoid raising master_connection_lost_callback - # TODO: Czy trzeba resetować? - # self.restart_master_connection_timer() + self.restart_master_connection_timer() master_stop_state = SS.UNKNOWN if msg.data is False: diff --git a/safety_user_plugin/user_plugin.py b/safety_user_plugin/user_plugin.py index d4311ec..9fede0b 100755 --- a/safety_user_plugin/user_plugin.py +++ b/safety_user_plugin/user_plugin.py @@ -34,7 +34,6 @@ class UserPlugin(Plugin): def __init__(self, context): super(UserPlugin, self).__init__(context) self.setObjectName("User Plugin - L1.5 safety system") - # setStyleSheet("background-color:black;") self.cbhandler = CallbackHandler() self._widget = UserWidget(context) @@ -117,7 +116,7 @@ class UserPlugin(Plugin): self._qt_wrapper.update_robots_list_gui(robots_id_list) def handle_selected_robot_info_update(self, robot_info): - if robot_info != None: + if robot_info is not None: self.update_selected_robot_info(robot_info) else: raise RuntimeError("Updated robot_info is NoneType object") @@ -137,7 +136,7 @@ class UserPlugin(Plugin): # Qt callback functions def handle_robot_selection(self, robot_id): - if self._user_info.selected_robot == None: + if self._user_info.selected_robot is None: # TODO: hz subskrypcje służą do sprawdzaNIA czy robot jest już zajęty # if self._ros_wrapper.robots_hz_value[robot_id] == None: self.select_robot(robot_id) @@ -165,9 +164,9 @@ class UserPlugin(Plugin): elif self._user_info.user_stop_state == SS.STOPPED: self.user_started() else: - raise ValueError("user_stop_state value undefined") + self._qt_wrapper.log_master_inactive() else: - raise ValueError("master_stop_state value undefined") + self._qt_wrapper.log_master_inactive() def handle_clutch_switched(self): if self._user_info.clutch_state == CS.ENGAGED: @@ -196,42 +195,35 @@ class UserPlugin(Plugin): if self._user_info.selected_robot != None: self.user_stopped() self.call_qt_wrapper_method("master_stopped") - # self._qt_wrapper.master_stopped() def master_started(self): self._user_info.master_stop_state = SS.RUNNING if self._user_info.selected_robot != None: self.call_qt_wrapper_method("master_started") - # self._qt_wrapper.master_started() def user_stopped(self): self._user_info.user_stop_state = SS.STOPPED self._ros_wrapper.user_robot_disabled() self.call_qt_wrapper_method("user_stopped") - # self._qt_wrapper.user_stopped() def user_started(self): self._user_info.user_stop_state = SS.RUNNING self._ros_wrapper.user_robot_enabled() self.call_qt_wrapper_method("user_started") - # self._qt_wrapper.user_started() def engage_clutch(self): self._user_info.clutch_state = CS.ENGAGED self._ros_wrapper.engage_clutch() self.call_qt_wrapper_method("engage_clutch") - # self._qt_wrapper.engage_clutch() def disengage_clutch(self): self._user_info.clutch_state = CS.DISENGAGED self._ros_wrapper.disengage_clutch() self.call_qt_wrapper_method("disengage_clutch") - # self._qt_wrapper.disengage_clutch() def select_robot(self, robot_id): self._ros_wrapper.select_robot(robot_id) - # self._qt_wrapper.select_robot(robot_id) self.call_qt_wrapper_method_with_argument("select_robot", [robot_id]) self._user_info.select_robot(robot_id) @@ -247,21 +239,18 @@ class UserPlugin(Plugin): def release_robot(self): self._ros_wrapper.release_robot() self.call_qt_wrapper_method("release_robot") - # self._qt_wrapper.release_robot() self._user_info.release_robot() self._user_info.master_stop_state = SS.UNKNOWN def connection_to_robot_lost(self, lost_robot_id): - # pass self._ros_wrapper.release_robot() self._user_info.release_robot() self.call_qt_wrapper_method_with_argument( "connection_to_robot_lost", [lost_robot_id] ) - # self._qt_wrapper.connection_to_robot_lost(lost_robot_id) def connection_to_master_lost(self): - if self._user_info.selected_robot != None: + if self._user_info.selected_robot is not None: self.master_stopped() self.call_qt_wrapper_method("connection_to_master_lost") @@ -273,7 +262,7 @@ class UserPlugin(Plugin): self.call_qt_wrapper_method("obstacle_removed") def update_selected_robot_info(self, robot_info): - if self._user_info.selected_robot != None: + if self._user_info.selected_robot is not None: if ( robot_info.obstacle_detected is True and self._user_info.selected_robot.obstacle_detected is False @@ -289,4 +278,3 @@ class UserPlugin(Plugin): self.call_qt_wrapper_method_with_argument( "update_selected_robot_info", [robot_info] ) - # self._qt_wrapper.update_selected_robot_info(robot_info)