diff --git a/safety_master_plugin/master_plugin.py b/safety_master_plugin/master_plugin.py index 50f7cf3..808ee68 100755 --- a/safety_master_plugin/master_plugin.py +++ b/safety_master_plugin/master_plugin.py @@ -45,11 +45,13 @@ class MasterPlugin(Plugin): # timer to consecutively send twist messages self._update_parameter_timer = QTimer(self) - self._update_parameter_timer.timeout.connect(self._ros_wrapper.timer_callback) + self._update_parameter_timer.timeout.connect( + self._ros_wrapper.qt_timer_callback + ) self._update_parameter_timer.start(100) # Stopping master at the beginning - self.master_stopped() + self.disable_robots() def set_callback_functions(self): self.set_ROS_callback_functions() @@ -77,10 +79,10 @@ class MasterPlugin(Plugin): def handle_master_stop_state_updated(self): """Handles master stop state when it changes e. g. on button click and confirmed.""" - if self._master_info.master_stop_state == SS.RUNNING: - self.master_stopped() - elif self._master_info.master_stop_state == SS.STOPPED: - self.master_started() + if self._master_info.master_stop_state == SS.STOPPED: + self.enable_robots() + elif self._master_info.master_stop_state == SS.RUNNING: + self.disable_robots() else: raise ValueError("master_stop_state value undefined") @@ -103,21 +105,21 @@ class MasterPlugin(Plugin): self.cbhandler.add_robot_signal.emit(robot_id) def robot_unregistered(self, robot_id): - # self._ros_wrapper.remove_robot_subscriber(robot_id) + self._ros_wrapper.remove_robot_subscriber(robot_id) self.cbhandler.remove_robot_signal.emit(robot_id) def update_robot_info(self, robot_info): self.cbhandler.robot_info_signal.emit(robot_info) - def master_started(self): + def enable_robots(self): self._master_info.master_stop_state = SS.RUNNING - self._ros_wrapper.master_started() - self._qt_wrapper.master_started() + self._ros_wrapper.when_robots_enabled() + self._qt_wrapper.when_robots_enabled() - def master_stopped(self): + def disable_robots(self): self._master_info.master_stop_state = SS.STOPPED - self._ros_wrapper.master_stopped() - self._qt_wrapper.master_stopped() + self._ros_wrapper.when_robots_disabled() + self._qt_wrapper.when_robots_disabled() def restrictions_changed(self, restrictions): self._master_info.restrictions = restrictions diff --git a/safety_master_plugin/qt_master_wrapper.py b/safety_master_plugin/qt_master_wrapper.py index a25aeca..fc8444d 100644 --- a/safety_master_plugin/qt_master_wrapper.py +++ b/safety_master_plugin/qt_master_wrapper.py @@ -137,17 +137,17 @@ class QtWrapper: self.master_stop_state_updated_callback() def display_master_stop_on(self): - self.widget.masterstopButton.setStyleSheet( - "QPushButton { color: black; background-color: green; font: bold 20px}" - ) - self.widget.masterstopButton.setText("Zatrzymaj roboty") - - def display_master_stop_off(self): self.widget.masterstopButton.setStyleSheet( "QPushButton { color: black; background-color: red; font: bold 20px}" ) self.widget.masterstopButton.setText("Odblokuj roboty") + def display_master_stop_off(self): + self.widget.masterstopButton.setStyleSheet( + "QPushButton { color: black; background-color: green; font: bold 20px}" + ) + self.widget.masterstopButton.setText("Zatrzymaj roboty") + def update_robot_info(self, robot_info): items_to_add = [] items_to_add.append(QStandardItem("pioneer{}".format(robot_info.robot_id))) @@ -197,14 +197,14 @@ class QtWrapper: item.setEditable(0) self.stdItemModel.setItem(row_number, i, item) - def master_stopped(self): + def when_robots_disabled(self): self.master_stop_state = SS.STOPPED - self.display_master_stop_off() + self.display_master_stop_on() self.log_info("Przycisk masterSTOP zostal naciśnięty. Zatrzymuję roboty") - def master_started(self): + def when_robots_enabled(self): self.master_stop_state = SS.RUNNING - self.display_master_stop_on() + self.display_master_stop_off() self.log_info("Przycisk masterSTOP odciśnięty") def add_robot(self, robot_id): diff --git a/safety_master_plugin/ros_master_wrapper.py b/safety_master_plugin/ros_master_wrapper.py index 9d24c9c..44dc13c 100644 --- a/safety_master_plugin/ros_master_wrapper.py +++ b/safety_master_plugin/ros_master_wrapper.py @@ -1,7 +1,13 @@ #!/usr/bin/env python import rclpy from rclpy.node import Node, NodeNameNonExistentError -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, QoSDurabilityPolicy +from rclpy.qos import ( + QoSProfile, + QoSReliabilityPolicy, + QoSHistoryPolicy, + QoSLivelinessPolicy, + QoSDurabilityPolicy, +) from std_msgs.msg import Bool from ros2aria_msgs.msg import RobotInfoMsg @@ -12,10 +18,10 @@ from safety_master_plugin.master_restrictions import Restrictions from safety_master_plugin.stop_state import StopState as SS -class ROSWrapper(Node): +class ROSWrapper(Node): def __init__(self): - super().__init__('safety_master_plugin') + super().__init__("safety_master_plugin") self.robots_list_update_callback = None self.robot_info_update_callback = None @@ -30,37 +36,47 @@ class ROSWrapper(Node): durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, history=QoSHistoryPolicy.SYSTEM_DEFAULT, liveliness=QoSLivelinessPolicy.AUTOMATIC, - depth=10 + depth=10, ) self.subs = [] - def setup_subscribers_and_publishers(self): - self.master_stop_publisher = self.create_publisher(Bool, '/pioneers/master_stop', 10) - self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10) + self.master_stop_publisher = self.create_publisher( + Bool, "/pioneers/master_stop", 10 + ) + self.restrictions_publisher = self.create_publisher( + RestrictionsMsg, "/pioneers/restrictions", 10 + ) - def add_robot_subscriber(self,robot_id): + def add_robot_subscriber(self, robot_id): if robot_id not in self.robot_info_subscribers: - topic_name = '/pioneer{0}/robot_info'.format(robot_id) - self.robot_info_subscribers[robot_id] = self.create_subscription(RobotInfoMsg, topic_name, self.handle_robot_info_update, 10) + topic_name = "/pioneer{0}/robot_info".format(robot_id) + self.robot_info_subscribers[robot_id] = self.create_subscription( + RobotInfoMsg, topic_name, self.handle_robot_info_update, 10 + ) else: - raise RuntimeError('Subscriber for pioneer{0} already in dictionary'.format(robot_id)) + raise RuntimeError( + "Subscriber for pioneer{0} already in dictionary".format(robot_id) + ) - # def remove_robot_subscriber(self,robot_id): - # if robot_id in self.robot_info_subscribers: - # self.unsubscribe(self.robot_info_subscribers[robot_id]) - # del self.robot_info_subscribers[robot_id] - # else: - # raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id)) + def remove_robot_subscriber(self, robot_id): + if robot_id in self.robot_info_subscribers: + self.unsubscribe(self.robot_info_subscribers[robot_id]) + del self.robot_info_subscribers[robot_id] + else: + raise RuntimeError( + "Subscriber for PIONEER{0} not in dictionary. Cannot be removed".format( + robot_id + ) + ) - # def unsubscribe(self,subscriber): - # if subscriber != None: - # subscriber.unregister() - - # def unregister_publisher(self,publisher): - # if publisher != None: - # publisher.unregister() + def unsubscribe(self, subscriber): + if subscriber is not None: + subscriber.destroy() + def unregister_publisher(self, publisher): + if publisher is not None: + publisher.destroy() def publish_periodic_update(self): restrictions = self.get_restrictions() @@ -73,12 +89,14 @@ class ROSWrapper(Node): stop_msg = Bool() if stop_state == SS.RUNNING: - stop_msg.data = True - elif stop_state == SS.STOPPED: stop_msg.data = False + elif stop_state == SS.STOPPED: + stop_msg.data = True else: stop_msg.data = False - raise ValueError('stop_state UNKNOWN when attempting to publish periodic update') + raise ValueError( + "stop_state UNKNOWN when attempting to publish periodic update" + ) self.restrictions_publisher.publish(restrictions_msg) self.master_stop_publisher.publish(stop_msg) @@ -92,12 +110,12 @@ class ROSWrapper(Node): def unregister_node(self): self.cancel_subscribers_and_publishers() - rospy.signal_shutdown('Closing safety master plugin') + rospy.signal_shutdown("Closing safety master plugin") - def setup_get_master_stop_state_function(self,function): + def setup_get_master_stop_state_function(self, function): self.get_master_stop_state = function - def setup_get_restrictions_function(self,function): + def setup_get_restrictions_function(self, function): self.get_restrictions = function # ROSWrapper Callbacks @@ -108,7 +126,9 @@ class ROSWrapper(Node): for pioneer_id in range(6): published_topics_list = [] try: - published_topics_list = self.get_publisher_names_and_types_by_node('ros2aria', '/pioneer'+str(pioneer_id)) + published_topics_list = self.get_publisher_names_and_types_by_node( + "ros2aria", "/pioneer" + str(pioneer_id) + ) except NodeNameNonExistentError: pass @@ -122,25 +142,24 @@ class ROSWrapper(Node): _robot_info.update_robot_info(msg) self.robot_info_update_callback(_robot_info) - # MasterPlugin Callbacks - def set_robots_list_update_callback(self,callback_function): + def set_robots_list_update_callback(self, callback_function): self.robots_list_update_callback = callback_function - def set_robot_info_update_callback(self,callback_function): + def set_robot_info_update_callback(self, callback_function): self.robot_info_update_callback = callback_function - def master_started(self): - msg = Bool() - msg.data = True - self.master_stop_publisher.publish(msg) - - def master_stopped(self): + def when_robots_enabled(self): msg = Bool() msg.data = False self.master_stop_publisher.publish(msg) - def restrictions_changed(self,restrictions): + def when_robots_disabled(self): + msg = Bool() + msg.data = True + self.master_stop_publisher.publish(msg) + + def restrictions_changed(self, restrictions): msg = RestrictionsMsg() msg.distance.data = restrictions.distance @@ -149,10 +168,10 @@ class ROSWrapper(Node): self.restrictions_publisher.publish(msg) - def timer_callback(self): + def qt_timer_callback(self): if self.publish_periodic_update is not None: self.publish_periodic_update() - + if self.get_robots_list is not None: self.get_robots_list()