diff --git a/safety_master_plugin/launch/master_plugin.launch b/safety_master_plugin/launch/master_plugin.launch index 02a7b0c..2f98cd1 100644 --- a/safety_master_plugin/launch/master_plugin.launch +++ b/safety_master_plugin/launch/master_plugin.launch @@ -1,7 +1,7 @@ \ No newline at end of file diff --git a/safety_master_plugin/scripts/master_callback_handler.py b/safety_master_plugin/scripts/master_callback_handler.py new file mode 100644 index 0000000..c616d5d --- /dev/null +++ b/safety_master_plugin/scripts/master_callback_handler.py @@ -0,0 +1,21 @@ +from python_qt_binding.QtCore import QObject +from python_qt_binding.QtCore import pyqtSignal +from robot_info import RobotInfo + +class CallbackHandler(QObject): + robot_info_signal = pyqtSignal(object) + add_robot_signal = pyqtSignal(int) + remove_robot_signal = pyqtSignal(int) + + + def __init__(self): + super(CallbackHandler, self).__init__() + + def connect_robot_info_signal(self,slot): + self.robot_info_signal.connect(slot) + + def connect_add_robot_signal(self,slot): + self.add_robot_signal.connect(slot) + + def connect_remove_robot_signal(self,slot): + self.remove_robot_signal.connect(slot) \ No newline at end of file diff --git a/safety_master_plugin/scripts/master_plugin.py b/safety_master_plugin/scripts/master_plugin.py index 44f0ca5..9464fb8 100755 --- a/safety_master_plugin/scripts/master_plugin.py +++ b/safety_master_plugin/scripts/master_plugin.py @@ -9,6 +9,8 @@ from qt_gui.plugin import Plugin from enums.stop_state import StopState as SS +from master_callback_handler import CallbackHandler + class MasterPlugin(Plugin): def __init__(self,context): @@ -20,6 +22,11 @@ class MasterPlugin(Plugin): self._qt_wrapper = QtWrapper(self._widget) self._master_info = MasterInfo() + self.cbhandler = CallbackHandler() + self.cbhandler.connect_robot_info_signal(self._qt_wrapper.update_robot_info) + self.cbhandler.connect_add_robot_signal(self._qt_wrapper.add_robot) + self.cbhandler.connect_remove_robot_signal(self._qt_wrapper.remove_robot) + # self._qt_wrapper.connect_signals() # self._qt_wrapper.set_sliders_to_initial_values() @@ -66,24 +73,34 @@ class MasterPlugin(Plugin): self.restrictions_changed(restrictions) def update_robots_list(self,robots_id_list): + + for robot_id in robots_id_list: + if robot_id not in self._master_info.robots_id_list: + self.new_robot_registered(robot_id) + + for robot_id in self._master_info.robots_id_list: + if robot_id not in robots_id_list: + self.robot_unregistered(robot_id) + self._master_info.robots_id_list = robots_id_list - self._qt_wrapper.update_robots_list_gui(robots_id_list) - # raise NotImplementedError def new_robot_registered(self,robot_id): - raise NotImplementedError - # self._qt_wrapper.add_robot_subscriber(robot_id) + # raise NotImplementedError self._ros_wrapper.add_robot_subscriber(robot_id) + self.cbhandler.add_robot_signal.emit(robot_id) + # self._qt_wrapper.add_robot(robot_id) def robot_unregistered(self,robot_id): - raise NotImplementedError - # self._qt_wrapper.remove_robot_subscriber(robot_id) + # raise NotImplementedError self._ros_wrapper.remove_robot_subscriber(robot_id) + self.cbhandler.remove_robot_signal.emit(robot_id) + # self._qt_wrapper.remove_robot(robot_id) def update_robot_info(self,robot_info): # TODO emit signal to qt_wrapper where it will update info about one of robots - self._qt_wrapper.update_robot_info(self,robot_info) - raise NotImplementedError + # self._qt_wrapper.update_robot_info(self,robot_info) + # raise NotImplementedError + self.cbhandler.robot_info_signal.emit(robot_info) def master_started(self): self._master_info.master_stop_state = SS.RUNNING diff --git a/safety_master_plugin/scripts/qt_master_wrapper.py b/safety_master_plugin/scripts/qt_master_wrapper.py index 26b7173..b9d25ea 100644 --- a/safety_master_plugin/scripts/qt_master_wrapper.py +++ b/safety_master_plugin/scripts/qt_master_wrapper.py @@ -125,6 +125,10 @@ class QtWrapper: self.add_robot_to_list(robot_id) + def update_robot_info(self,robot_info): + self.log_info(str(robot_info.robot_id)) + # raise NotImplementedError + # def remove_robot_from_list(self,robot_id): # count = self.widget.robotsList.count() @@ -151,6 +155,15 @@ class QtWrapper: self.log_info('Przycisk masterSTOP odcisniety') + def add_robot(self,robot_id): + self.log_info('PIONIER{0} polaczony'.format(robot_id)) + self.displayed_robots_id_list.append(robot_id) + + def remove_robot(self,robot_id): + self.log_info('PIONIER{0} rozlaczony'.format(robot_id)) + self.displayed_robots_id_list.remove(robot_id) + + def log_info(self,info_text): time = datetime.datetime.now().strftime('[%H:%M:%S]') diff --git a/safety_user_plugin/scripts/ros_wrapper.py b/safety_user_plugin/scripts/ros_wrapper.py index cfaff97..68eb294 100644 --- a/safety_user_plugin/scripts/ros_wrapper.py +++ b/safety_user_plugin/scripts/ros_wrapper.py @@ -35,8 +35,8 @@ class ROSWrapper: self.connection_timer = None self.master_connection_timer = None - def setup_node(self): - rospy.init_node('safety_user_plugin', anonymous=True) + # def setup_node(self): + # rospy.init_node('safety_user_plugin', anonymous=True) def setup_subscribers_and_publishers(self,robot_id): robot_name = 'PIONIER' + str(robot_id)