diff --git a/safety_user_plugin/english.py b/safety_user_plugin/english.py index 3593d18..3023397 100644 --- a/safety_user_plugin/english.py +++ b/safety_user_plugin/english.py @@ -21,7 +21,7 @@ class EnglishTexts(LanguageTexts): release_robot_text = "Release robot" # Logger info texts - robot_selected_text = "PIONIER{0} selected!" + robot_selected_text = "PIONEER{0} selected!" robot_released_text = "Robot released" stopped_and_engaged_text = "Robot will be stopped and clutch engaged" @@ -44,7 +44,7 @@ class EnglishTexts(LanguageTexts): cannot_select_robot_text = ( "Robot can't be selected. It is already selected by another group" ) - connection_to_robot_lost_text = "Connection lost with robot PIONIER{0}" + connection_to_robot_lost_text = "Connection lost with robot PIONEER{0}" connection_to_master_lost_text = ( "Connection lost with masterSTOP. Ask the teacher to run it" ) diff --git a/safety_user_plugin/polish.py b/safety_user_plugin/polish.py index d68f225..f5ee653 100644 --- a/safety_user_plugin/polish.py +++ b/safety_user_plugin/polish.py @@ -21,7 +21,7 @@ class PolishTexts(LanguageTexts): release_robot_text = "Zwolnij robota" # Logger info texts - robot_selected_text = "PIONIER{0} wybrany!" + robot_selected_text = "PIONEER{0} wybrany!" robot_released_text = "Odłączam robota" stopped_and_engaged_text = "Robot zostanie zatrzymany i sprzęgnięty" @@ -42,7 +42,7 @@ class PolishTexts(LanguageTexts): obstacle_removed_text = "Przeszkoda usunięta" # Logger error/problem messages - selection_error_text = "Najpierw wybierz PIONIERA z listy robotów" + 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 = ( "Nie mozna wystartować. Przeszkoda w polu działania robota" @@ -50,7 +50,7 @@ class PolishTexts(LanguageTexts): cannot_select_robot_text = ( "Nie mozna wybrać robota. Robot został już wybrany przez inną grupę" ) - connection_to_robot_lost_text = "Utrata połączenia z robotem PIONIER{0}" + connection_to_robot_lost_text = "Utrata połączenia z robotem PIONEER{0}" connection_to_master_lost_text = ( "Utrata połączenia z masterstopem. Poproś prowadzącego o jego uruchomienie" ) diff --git a/safety_user_plugin/qt_wrapper.py b/safety_user_plugin/qt_wrapper.py index e79fd85..743f0ad 100644 --- a/safety_user_plugin/qt_wrapper.py +++ b/safety_user_plugin/qt_wrapper.py @@ -102,7 +102,7 @@ class QtWrapper: def get_selected_robot_id(self): current_text = self.widget.robotsList.currentText() - if "PIONIER" in current_text: + if "PIONEER" in current_text: return int(current_text[7:]) else: return None @@ -179,7 +179,7 @@ class QtWrapper: def update_robots_list_gui(self, robots_id_list): robots_id_list.sort() - id_strings_list = (("PIONIER" + str(x)) for x in robots_id_list) + id_strings_list = (("PIONEER" + str(x)) for x in robots_id_list) robots_to_add = [] robots_to_remove = [] @@ -208,7 +208,7 @@ class QtWrapper: return def add_robot_to_list(self, robot_id): - self.widget.robotsList.addItem("PIONIER{0}".format(robot_id)) + self.widget.robotsList.addItem("PIONEER{0}".format(robot_id)) self.displayed_robots_id_list.append(robot_id) def update_selected_robot_info(self, robot_info): @@ -216,7 +216,7 @@ class QtWrapper: robot_info.linear_velocity[0] ** 2 + robot_info.linear_velocity[1] ** 2 ) - self.widget.idLabel.setText("PIONIER{0}".format(robot_info.robot_id)) + self.widget.idLabel.setText("PIONEER{0}".format(robot_info.robot_id)) self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage)) self.widget.linearLabel.setText("{:.2f}".format(linear_vel)) self.widget.angularLabel.setText( diff --git a/safety_user_plugin/robot_info.py b/safety_user_plugin/robot_info.py index fda0a49..999d6a4 100644 --- a/safety_user_plugin/robot_info.py +++ b/safety_user_plugin/robot_info.py @@ -29,12 +29,12 @@ class RobotInfo: ] self.obstacle_detected = robot_info_msg.obstacle_detected.data - if robot_info_msg.state.data == True: + if robot_info_msg.state.data is True: self.state = SS.RUNNING else: self.state = SS.STOPPED - if robot_info_msg.clutch.data == True: + if robot_info_msg.clutch.data is True: self.clutch = CS.ENGAGED else: self.clutch = CS.DISENGAGED diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index e8a35e1..c146208 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/safety_user_plugin/ros_wrapper.py @@ -1,6 +1,13 @@ #!/usr/bin/env python import rclpy from rclpy.node import Node, NodeNameNonExistentError +from rclpy.qos import ( + QoSProfile, + QoSReliabilityPolicy, + QoSHistoryPolicy, + QoSLivelinessPolicy, + QoSDurabilityPolicy, +) from std_msgs.msg import Bool from ros2aria_msgs.msg import RobotInfoMsg @@ -43,30 +50,38 @@ class ROSWrapper(Node): self.rostopics_hz_dict = {} self.robots_hz_value = {} - # def setup_node(self): - # rospy.init_node('safety_user_plugin', anonymous=True) + self._qos = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE, + history=QoSHistoryPolicy.SYSTEM_DEFAULT, + liveliness=QoSLivelinessPolicy.AUTOMATIC, + depth=10, + ) def setup_subscribers_and_publishers(self, robot_id): - robot_name = "PIONIER" + str(robot_id) + robot_name = "PIONEER" + str(robot_id) # self.robot_info_subscriber = rospy.Subscriber( # "/{0}/RosAria/robot_info".format(robot_name), # RobotInfoMsg, # self.handle_selected_robot_info_update, # ) - # self.master_stop_subscriber = rospy.Subscriber( - # "/PIONIER/master_stop", Bool, self.handle_master_stop_update - # ) + self.master_stop_subscriber = self.create_subscription( + Bool, + "/pioneers/master_stop", + self.handle_master_stop_update, + qos_profile=self._qos, + ) # self.restrictions_subscriber = rospy.Subscriber( - # "/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update + # "/PIONEER/restrictions", RestrictionsMsg, self.handle_restrictions_update # ) - # self.user_stop_publisher = rospy.Publisher( - # "/{0}/RosAria/user_stop".format(robot_name), Bool, queue_size=1 - # ) - # self.clutch_publisher = rospy.Publisher( - # "/{0}/RosAria/clutch".format(robot_name), Bool, queue_size=1 - # ) + self.user_stop_publisher = self.create_publisher( + Bool, f"/pioneer{robot_id}/user_stop", 10 + ) + self.clutch_publisher = self.create_publisher( + Bool, f"/pioneer{robot_id}/clutch", 10 + ) # if self.periodic_timer != None: # self.shutdown_timer(self.periodic_timer) @@ -92,11 +107,11 @@ class ROSWrapper(Node): def unsubscribe(self, subscriber): if subscriber != None: - subscriber.unregister() + subscriber.destroy() def unregister_publisher(self, publisher): if publisher != None: - publisher.unregister() + publisher.destroy() def shutdown_timer(self, timer): if timer != None: @@ -107,9 +122,9 @@ class ROSWrapper(Node): # clutch_state = self.get_clutch_state() if stop_state == SS.RUNNING: - self.user_started() + self.user_robot_enabled() elif stop_state == SS.STOPPED: - self.user_stopped() + self.user_robot_disabled() else: raise ValueError( "stop_state UNKNOWN when attempting to publish periodic update" @@ -126,7 +141,7 @@ class ROSWrapper(Node): print("update_hz_values") # for key in self.rostopics_hz_dict: # self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz( - # "/PIONIER{0}/RosAria/user_stop".format(key) + # "/PIONEER{0}/RosAria/user_stop".format(key) # ) def handle_robot_connection_lost(self, event): @@ -165,7 +180,8 @@ class ROSWrapper(Node): def restart_master_connection_timer(self): if self.master_connection_timer != None: - self.master_connection_timer.shutdown() + print("Destroy connection timer") + # self.master_connection_timer.destroy() # self.master_connection_timer = rospy.Timer( # rospy.Duration(1.5), self.handle_master_connection_lost # ) @@ -190,8 +206,8 @@ 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._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): @@ -218,10 +234,10 @@ class ROSWrapper(Node): print("ADDED SUB") # self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1) # self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber( - # "/PIONIER{0}/RosAria/user_stop".format(robot_id), + # "/PIONEER{0}/RosAria/user_stop".format(robot_id), # Bool, # self.rostopics_hz_dict[robot_id].callback_hz, - # callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id), + # callback_args="/PIONEER{0}/RosAria/user_stop".format(robot_id), # ) def _remove_hz_subscribers(self, robots_id_list): @@ -234,6 +250,8 @@ class ROSWrapper(Node): 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.shutdown_timer(self.periodic_timer) self.shutdown_timer(self.connection_timer) @@ -249,10 +267,11 @@ class ROSWrapper(Node): def handle_master_stop_update(self, msg): # Restarting master connection timer to avoid raising master_connection_lost_callback - self.restart_master_connection_timer() + # TODO: Czy trzeba resetować? + # self.restart_master_connection_timer() master_stop_state = SS.UNKNOWN - if msg.data == True: + if msg.data is False: master_stop_state = SS.RUNNING else: master_stop_state = SS.STOPPED @@ -295,15 +314,15 @@ class ROSWrapper(Node): msg.data = False self.clutch_publisher.publish(msg) - def user_started(self): - msg = Bool() - msg.data = True - self.user_stop_publisher.publish(msg) - - def user_stopped(self): + def user_robot_enabled(self): msg = Bool() msg.data = False self.user_stop_publisher.publish(msg) + def user_robot_disabled(self): + msg = Bool() + msg.data = True + self.user_stop_publisher.publish(msg) + def qt_timer_callback(self): rclpy.spin_once(self, timeout_sec=0.001) diff --git a/safety_user_plugin/user_plugin.py b/safety_user_plugin/user_plugin.py index 4a581c0..d4311ec 100755 --- a/safety_user_plugin/user_plugin.py +++ b/safety_user_plugin/user_plugin.py @@ -54,7 +54,6 @@ class UserPlugin(Plugin): self._user_info.get_clutch_state ) - # self._ros_wrapper.setup_node() self.set_callback_functions() # At the end! @@ -139,10 +138,11 @@ class UserPlugin(Plugin): # Qt callback functions def handle_robot_selection(self, robot_id): if self._user_info.selected_robot == None: - if self._ros_wrapper.robots_hz_value[robot_id] == None: - self.select_robot(robot_id) - else: - self._qt_wrapper.robot_selected_by_another_group_notify() + # 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) + # else: + # self._qt_wrapper.robot_selected_by_another_group_notify() else: raise RuntimeError("User already selected robot") @@ -207,13 +207,13 @@ class UserPlugin(Plugin): def user_stopped(self): self._user_info.user_stop_state = SS.STOPPED - self._ros_wrapper.user_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_started() + self._ros_wrapper.user_robot_enabled() self.call_qt_wrapper_method("user_started") # self._qt_wrapper.user_started() @@ -275,13 +275,13 @@ class UserPlugin(Plugin): def update_selected_robot_info(self, robot_info): if self._user_info.selected_robot != None: if ( - robot_info.obstacle_detected == True - and self._user_info.selected_robot.obstacle_detected == False + robot_info.obstacle_detected is True + and self._user_info.selected_robot.obstacle_detected is False ): self.obstacle_detected() elif ( - robot_info.obstacle_detected == False - and self._user_info.selected_robot.obstacle_detected == True + robot_info.obstacle_detected is False + and self._user_info.selected_robot.obstacle_detected is True ): self.obstacle_removed()