fixed the logic of the buttons

Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>

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:
+        if self._master_info.master_stop_state == SS.STOPPED:
-            self.master_stopped()
+            self.enable_robots()
-        elif self._master_info.master_stop_state == SS.STOPPED:
+        elif self._master_info.master_stop_state == SS.RUNNING:
-            self.master_started()
+            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._ros_wrapper.when_robots_enabled()
-        self._qt_wrapper.master_started()
+        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._ros_wrapper.when_robots_disabled()
-        self._qt_wrapper.master_stopped()
+        self._qt_wrapper.when_robots_disabled()
 
     def restrictions_changed(self, restrictions):
         self._master_info.restrictions = restrictions

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):

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.master_stop_publisher = self.create_publisher(
-        self.restrictions_publisher = self.create_publisher(RestrictionsMsg, '/pioneers/restrictions', 10)
+            Bool, "/pioneers/master_stop", 10
+        )
+        self.restrictions_publisher = self.create_publisher(
+            RestrictionsMsg, "/pioneers/restrictions", 10
+        )
 
     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)
+            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)
+            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):
+    def remove_robot_subscriber(self, robot_id):
-    #     if robot_id in self.robot_info_subscribers:
+        if robot_id in self.robot_info_subscribers:
-    #         self.unsubscribe(self.robot_info_subscribers[robot_id])
+            self.unsubscribe(self.robot_info_subscribers[robot_id])
-    #         del self.robot_info_subscribers[robot_id]
+            del self.robot_info_subscribers[robot_id]
-    #     else:
+        else:
-    #         raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
+            raise RuntimeError(
+                "Subscriber for PIONEER{0} not in dictionary. Cannot be removed".format(
+                    robot_id
+                )
+            )
 
-    # def unsubscribe(self,subscriber):
+    def unsubscribe(self, subscriber):
-    #     if subscriber != None:
+        if subscriber is not None:
-    #         subscriber.unregister()
+            subscriber.destroy()
-
-    # def unregister_publisher(self,publisher):
-    #     if publisher != None:
-    #         publisher.unregister()
 
+    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,7 +110,7 @@ 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):
         self.get_master_stop_state = function
@@ -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,7 +142,6 @@ 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):
         self.robots_list_update_callback = callback_function
@@ -130,16 +149,16 @@ class ROSWrapper(Node):
     def set_robot_info_update_callback(self, callback_function):
         self.robot_info_update_callback = callback_function
 
-    def master_started(self):
+    def when_robots_enabled(self):
-        msg = Bool()
-        msg.data = True
-        self.master_stop_publisher.publish(msg)
-
-    def master_stopped(self):
         msg = Bool()
         msg.data = False
         self.master_stop_publisher.publish(msg)
 
+    def when_robots_disabled(self):
+        msg = Bool()
+        msg.data = True
+        self.master_stop_publisher.publish(msg)
+
     def restrictions_changed(self, restrictions):
         msg = RestrictionsMsg()
 
@@ -149,7 +168,7 @@ 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()