added subscribers | icons

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

safety_user_plugin/qt_wrapper.py

@@ -53,8 +53,8 @@ class QtWrapper:
         self.widget.angularImage.setPixmap(self.angular_pixmap)
         self.widget.linearImage.setPixmap(self.linear_pixmap.scaled(20, 48))
 
-        self.ok_pixmap = QPixmap("{0}/Ok.jpg".format(ui_directory_path))
-        self.cancel_pixmap = QPixmap("{0}/Cancel.jpg".format(ui_directory_path))
+        self.ok_pixmap = QPixmap("{0}/Ok.png".format(ui_directory_path))
+        self.cancel_pixmap = QPixmap("{0}/Cancel.png".format(ui_directory_path))
 
     def disconnect_signals(self):
         self.widget.clutchButton.clicked.disconnect()

safety_user_plugin/ros_wrapper.py

@@ -59,22 +59,25 @@ class ROSWrapper(Node):
         )
 
     def setup_subscribers_and_publishers(self, robot_id):
-        robot_name = "PIONEER" + str(robot_id)
+        self.robot_info_subscriber = self.create_subscription(
+            RobotInfoMsg,
+            f"/pioneer{robot_id}/robot_info",
+            self.handle_selected_robot_info_update,
+            qos_profile=self._qos,
+        )
 
-        # self.robot_info_subscriber = rospy.Subscriber(
-        #     "/{0}/RosAria/robot_info".format(robot_name),
-        #     RobotInfoMsg,
-        #     self.handle_selected_robot_info_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(
-        #     "/PIONEER/restrictions", RestrictionsMsg, self.handle_restrictions_update
-        # )
+        self.restrictions_subscriber = self.create_subscription(
+            RestrictionsMsg,
+            "/pioneers/restrictions",
+            self.handle_restrictions_update,
+            qos_profile=self._qos,
+        )
 
         self.user_stop_publisher = self.create_publisher(
             Bool, f"/pioneer{robot_id}/user_stop", 10
@@ -119,7 +122,7 @@ class ROSWrapper(Node):
 
     def publish_periodic_update(self, event):
         stop_state = self.get_user_stop_state()
-        # clutch_state = self.get_clutch_state()
+        clutch_state = self.get_clutch_state()
 
         if stop_state == SS.RUNNING:
             self.user_robot_enabled()
@@ -130,12 +133,14 @@ class ROSWrapper(Node):
                 "stop_state UNKNOWN when attempting to publish periodic update"
             )
 
-        # if clutch_state == CS.ENGAGED:
-        #     self.engage_clutch()
-        # elif clutch_state == CS.DISENGAGED:
-        #     self.disengage_clutch()
-        # else:
-        #     raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
+        if clutch_state == CS.ENGAGED:
+            self.engage_clutch()
+        elif clutch_state == CS.DISENGAGED:
+            self.disengage_clutch()
+        else:
+            raise ValueError(
+                "clutch_state UNKNOWN when attempting to publish periodic update"
+            )
 
     def update_hz_values(self):
         print("update_hz_values")
@@ -212,7 +217,7 @@ class ROSWrapper(Node):
 
     def _get_robots_list(self):
         robots_id_list = []
-        # Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
+        # NOTE: Można to zrobić lepiej np. czekać na publikowaną wiadomość na robot_info
         for pioneer_id in range(6):
             published_topics_list = []
             try:
@@ -259,7 +264,7 @@ class ROSWrapper(Node):
 
     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)