diff --git a/plugin.xml b/plugin.xml index 0b2b84d..2eda22e 100644 --- a/plugin.xml +++ b/plugin.xml @@ -1,15 +1,15 @@ - + Safety system user plugin - + folder system-help - \ No newline at end of file + diff --git a/safety_user_plugin/qt_wrapper.py b/safety_user_plugin/qt_wrapper.py index aabf6a8..e79fd85 100644 --- a/safety_user_plugin/qt_wrapper.py +++ b/safety_user_plugin/qt_wrapper.py @@ -6,6 +6,9 @@ import os import math import datetime +from ament_index_python.packages import get_package_share_directory + + from safety_user_plugin.enums.clutch_state import ClutchState as CS from safety_user_plugin.enums.stop_state import StopState as SS @@ -39,7 +42,7 @@ class QtWrapper: self.clear_robot_info() ui_directory_path = "{0}/ui".format( - rospkg.RosPack().get_path("safety_user_plugin") + get_package_share_directory("safety_user_plugin") ) self.distance_pixmap = QPixmap("{0}/Distance.png".format(ui_directory_path)) diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index 783b259..fa018dd 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/safety_user_plugin/ros_wrapper.py @@ -33,7 +33,7 @@ class ROSWrapper: self.robots_list_timer = None self.connection_timer = None self.master_connection_timer = None - self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) + # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) self.robots_hz_subscribers_dict = {} self.rostopics_hz_dict = {} @@ -45,46 +45,46 @@ class ROSWrapper: def setup_subscribers_and_publishers(self, robot_id): robot_name = "PIONIER" + 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.restrictions_subscriber = rospy.Subscriber( - "/PIONIER/restrictions", RestrictionsMsg, self.handle_restrictions_update - ) + # 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.restrictions_subscriber = rospy.Subscriber( + # "/PIONIER/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 = 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 + # ) - if self.periodic_timer != None: - self.shutdown_timer(self.periodic_timer) - self.periodic_timer = rospy.Timer( - 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 - ) + # if self.periodic_timer != None: + # self.shutdown_timer(self.periodic_timer) + # self.periodic_timer = rospy.Timer( + # 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 + # ) - if self.master_connection_timer != None: - self.shutdown_timer(self.master_connection_timer) - self.master_connection_timer = rospy.Timer( - rospy.Duration(1.5), self.handle_master_connection_lost - ) + # if self.master_connection_timer != None: + # self.shutdown_timer(self.master_connection_timer) + # self.master_connection_timer = rospy.Timer( + # rospy.Duration(1.5), self.handle_master_connection_lost + # ) - if self.hz_update_timer != None: - self.shutdown_timer(self.hz_update_timer) - self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) + # if self.hz_update_timer != None: + # self.shutdown_timer(self.hz_update_timer) + # self.hz_update_timer = rospy.Timer(rospy.Duration(0.2), self.update_hz_values) def unsubscribe(self, subscriber): if subscriber != None: @@ -150,9 +150,9 @@ class ROSWrapper: 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 - ) + # 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" @@ -161,9 +161,9 @@ class ROSWrapper: def restart_master_connection_timer(self): if self.master_connection_timer != None: self.master_connection_timer.shutdown() - self.master_connection_timer = rospy.Timer( - rospy.Duration(1.5), self.handle_master_connection_lost - ) + # 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" @@ -171,7 +171,7 @@ class ROSWrapper: def unregister_node(self): self.cancel_subscribers_and_publishers() - rospy.signal_shutdown("Closing safety user plugin") + # rospy.signal_shutdown("Closing safety user plugin") def select_robot(self, robot_id): self.setup_subscribers_and_publishers(robot_id) @@ -186,11 +186,11 @@ class ROSWrapper: def get_robots_list(self, event): robots_id_list = [] - published_topics_list = rospy.get_published_topics(namespace="/") + # published_topics_list = rospy.get_published_topics(namespace="/") published_topics = [] - for list_ in published_topics_list: - published_topics.append(list_[0]) + # 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_info") == -1: @@ -207,13 +207,14 @@ class ROSWrapper: for robot_id in robots_id_list: if robot_id not in self.robots_hz_subscribers_dict: # Add hz subscriber - 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), - Bool, - self.rostopics_hz_dict[robot_id].callback_hz, - callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id), - ) + 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), + # Bool, + # self.rostopics_hz_dict[robot_id].callback_hz, + # callback_args="/PIONIER{0}/RosAria/user_stop".format(robot_id), + # ) # Removing old hz subscribers for robot_key in self.robots_hz_subscribers_dict: @@ -258,7 +259,7 @@ class ROSWrapper: # UserPlugin Callbacks def set_robots_list_update_callback(self, callback_function): self.robots_list_update_callback = callback_function - self.robots_list_timer = rospy.Timer(rospy.Duration(0.5), self.get_robots_list) + # self.robots_list_timer = rospy.Timer(rospy.Duration(0.5), self.get_robots_list) def set_selected_robot_info_update_callback(self, callback_function): self.selected_robot_info_update_callback = callback_function diff --git a/safety_user_plugin/user_widget.py b/safety_user_plugin/user_widget.py index 915a2eb..f2e038b 100644 --- a/safety_user_plugin/user_widget.py +++ b/safety_user_plugin/user_widget.py @@ -1,18 +1,22 @@ #!/usr/bin/env python import os -import rospkg +from ament_index_python.packages import get_package_share_directory from python_qt_binding.QtWidgets import QWidget from python_qt_binding import loadUi -class UserWidget(QWidget): - def __init__(self,context): +class UserWidget(QWidget): + def __init__(self, context): super(UserWidget, self).__init__() - ui_file = os.path.join(rospkg.RosPack().get_path('safety_user_plugin'), 'ui', 'user_view.ui') + ui_file = os.path.join( + get_package_share_directory("safety_user_plugin"), "ui", "user_view.ui" + ) loadUi(ui_file, self) - self.setObjectName('User Plugin - L1.5 safety system') + self.setObjectName("User Plugin - L1.5 safety system") if context.serial_number() > 1: - self.setWindowTitle(self.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self) \ No newline at end of file + self.setWindowTitle( + self.windowTitle() + (" (%d)" % context.serial_number()) + ) + context.add_widget(self)