Working with one robot

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

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!"

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 = (

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

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.shutdown_timer(self.connection_timer)
+        self.connection_timer = self.create_timer(
-        # self.connection_timer = rospy.Timer(
+            5.0, self.handle_robot_connection_lost
-        #     rospy.Duration(5), 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:
+        if subscriber is not None:
-            subscriber.destroy()
+            self.destroy_subscription(subscriber)
+            subscriber = None
 
     def unregister_publisher(self, publisher):
-        if publisher != None:
+        while publisher is not None:
-            publisher.destroy()
+            self.destroy_publisher(publisher)
+            publisher = None
 
     def shutdown_timer(self, timer):
-        if timer != None:
+        while timer is not None:
-            timer.shutdown()
+            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.shutdown_timer(self.connection_timer)
-            self.connection_timer.shutdown()
+        self.connection_timer = self.create_timer(
-            # self.connection_timer = rospy.Timer(
+            5.0, self.handle_robot_connection_lost
-            #     rospy.Duration(1.5), self.handle_robot_connection_lost
-            # )
-        else:
-            raise RuntimeError(
-                "Attempting to restart connection_timer when it is not initialized"
         )
 
     def restart_master_connection_timer(self):
-        if self.master_connection_timer != None:
+        self.shutdown_timer(self.master_connection_timer)
-            print("Destroy connection timer")
+        self.master_connection_timer = self.create_timer(
-            # self.master_connection_timer.destroy()
+            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"
         )
 
     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:

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)