diff --git a/safety_user_plugin/qt_wrapper.py b/safety_user_plugin/qt_wrapper.py index 743f0ad..61bf232 100644 --- a/safety_user_plugin/qt_wrapper.py +++ b/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() diff --git a/safety_user_plugin/ros_wrapper.py b/safety_user_plugin/ros_wrapper.py index c146208..767683e 100644 --- a/safety_user_plugin/ros_wrapper.py +++ b/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) diff --git a/ui/Cancel.jpg b/ui/Cancel.jpg deleted file mode 100644 index ff6a442..0000000 Binary files a/ui/Cancel.jpg and /dev/null differ diff --git a/ui/Cancel.png b/ui/Cancel.png new file mode 100644 index 0000000..d41b5df Binary files /dev/null and b/ui/Cancel.png differ diff --git a/ui/Ok.jpg b/ui/Ok.jpg deleted file mode 100644 index f052a71..0000000 Binary files a/ui/Ok.jpg and /dev/null differ diff --git a/ui/Ok.png b/ui/Ok.png new file mode 100644 index 0000000..009707a Binary files /dev/null and b/ui/Ok.png differ