diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index 239631c..19e12a9 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -3,44 +3,56 @@ import os import rospy import rospkg -from python_qt_binding.QtGui import QPalette -from python_qt_binding.QtWidgets import QPushButton +# from python_qt_binding.QtGui import QPalette +# from python_qt_binding.QtWidgets import QPushButton # from python_qt_binding.QtWidgets import QWidget, QDialog class QtWrapper: def __init__(self,widget): - self.widget = widget - # self.widget.setStyleSheet("background-color:black;") - # self.widget.setBackgroundRole(QPalette.Button) - # self.widget.setStyleSheet("QPushButton { background-color: yellow }") self.widget.logsBrowser.setText('Sukces') + def connect_robot_signals(self): + raise NotImplementedError + def connect_signals(self): - # self.widget.openButton.clicked.connect(self.handle_openButton_clicked) - self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked) + self.widget.openButton.clicked.connect(self.handle_openButton_clicked) + self.widget.clutchButton.clicked.connect(self.handle_clutchButton_clicked_callback) + self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked_callback) + + def get_selected_robot_id(self): + raise NotImplementedError + + def handle_openButton_clicked(self): + selected_robot_id = self.get_selected_robot_id() + + if selected_robot_id != None: + self.handle_openButton_clicked_callback(selected_robot_id) + else: + # TODO - Log info + raise NotImplementedError def set_robot_selection_callback(self,callback): - # self.handle_openButton_clicked = callback - pass + self.handle_openButton_clicked_callback = callback def set_user_stop_state_updated_callback(self,callback): - # raise NotImplementedError - pass + self.handle_stopButton_clicked_callback = callback def set_clutch_switched_callback(self,callback): - self.handle_clutchButton_clicked = callback + self.handle_clutchButton_clicked_callback = callback def select_robot(self,robot_id): - raise NotImplementedError + self.widget.logsBrowser.setText('Sukcesx1') def release_robot(self): - raise NotImplementedError + self.widget.logsBrowser.setText('Sukcesx2') def update_robots_list_gui(self,robots_id_list): - # raise NotImplementedError - pass + robots_id_list.sort() + id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list) + self.widget.robotsList.addItems(id_strings_list) + def update_selected_robot_info(self,robot_info): raise NotImplementedError @@ -58,7 +70,8 @@ class QtWrapper: raise NotImplementedError def engage_clutch(self): - raise NotImplementedError + # raise NotImplementedError + pass def disengage_clutch(self): raise NotImplementedError diff --git a/safety_user_plugin/scripts/robot_info.py b/safety_user_plugin/scripts/robot_info.py index cc9493f..63c2f9e 100644 --- a/safety_user_plugin/scripts/robot_info.py +++ b/safety_user_plugin/scripts/robot_info.py @@ -2,7 +2,15 @@ class RobotInfo: - def __init__(self,robot_info_msg): + def __init__(self,robot_id): + self.robot_id = robot_id + self.battery_voltage = None + self.linear_velocity = None + self.angular_velocity = None + self.state = None + self.clutch = None + + def update_robot_info(self,robot_info_msg): self.robot_id = robot_info_msg.robot_id.data self.battery_voltage = robot_info_msg.battery_voltage.data self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z] diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index 051e2a9..8e6f184 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -15,8 +15,6 @@ from enums.stop_state import StopState as SS class ROSWrapper: def __init__(self): - # self.setup_node() - self.robots_list_update_callback = None self.selected_robot_info_update_callback = None self.master_stop_update_callback = None @@ -54,7 +52,7 @@ class ROSWrapper: def unsubscribe(self,subscriber): if subscriber != None: - subscriber.unsubscribe() + subscriber.unregister() def unregister_publisher(self,publisher): if publisher != None: @@ -147,11 +145,17 @@ class ROSWrapper: self.robots_list_update_callback(robots_id_list) + + def release_robot(self): + self.unsubscribe(self.robot_info_subscriber) + self.shutdown_timer(self.periodic_timer) + self.shutdown_timer(self.connection_timer) + def handle_selected_robot_info_update(self,msg): # Restarting connection timer to avoid raising robot_connection_lost_callback self.restart_connection_timer() - _robot_info = RobotInfo(msg) + _robot_info = RobotInfo(0).update_robot_info(msg) self.selected_robot_info_update_callback(_robot_info) def handle_master_stop_update(self,msg): diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index b50c579..fcedb45 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/safety_user_plugin/scripts/user_plugin.py @@ -42,6 +42,8 @@ class UserPlugin(Plugin): self._qt_wrapper.connect_signals() self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) + self._qt_wrapper.update_robots_list_gui([5,6,2]) + def shutdown_plugin(self): self._ros_wrapper.unregister_node() @@ -105,12 +107,12 @@ class UserPlugin(Plugin): raise NotImplementedError def handle_user_stop_state_updated(self): - if self._user_info.master_stop_state == CS.STOPPED: + if self._user_info.master_stop_state == SS.STOPPED: self._qt_wrapper.master_is_stopped_notify() - elif self._user_info.master_stop_state == CS.RUNNING: - if self._user_info.user_stop_state == CS.RUNNING: + elif self._user_info.master_stop_state == SS.RUNNING: + if self._user_info.user_stop_state == SS.RUNNING: self.user_stopped() - elif self._user_info.user_stop_state == CS.STOPPED: + elif self._user_info.user_stop_state == SS.STOPPED: self.user_started() else: # TODO - Wrong userstop value exception @@ -141,19 +143,19 @@ class UserPlugin(Plugin): def master_stopped(self): self.user_stopped() self._qt_wrapper.master_stopped() - self._user_info.master_stop_state = CS.STOPPED + self._user_info.master_stop_state = SS.STOPPED def master_started(self): self._qt_wrapper.master_started() - self._user_info.master_stop_state = CS.RUNNING + self._user_info.master_stop_state = SS.RUNNING def user_stopped(self): - self._user_info.user_stop_state = CS.STOPPED + self._user_info.user_stop_state = SS.STOPPED self._ros_wrapper.user_stopped() self._qt_wrapper.user_stopped() def user_started(self): - self._user_info.user_stop_state = CS.RUNNING + self._user_info.user_stop_state = SS.RUNNING self._ros_wrapper.user_started() self._qt_wrapper.user_started()