Scheme of whole program prepared. Also masterstop button works

safety_master_plugin/scripts/master_info.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+
+# from robot_info import RobotInfo
+
+class MasterInfo:
+
+    def __init__(self):
+        self.robots_id_list = []
+        self.master_stop_state = None
+        self.restrictions = None
+
+    def get_master_stop_state(self):
+        return self.master_stop_state
+
+    def get_restrictions(self):
+        return self.restrictions

safety_master_plugin/scripts/master_plugin.py

@@ -1,8 +1,13 @@
+from ros_master_wrapper import ROSWrapper
+from qt_master_wrapper import QtWrapper
+from restrictions import Restrictions
 
+from master_info import MasterInfo
+from master_widget import MasterWidget
 
 from qt_gui.plugin import Plugin
 
-from master_widget import MasterWidget
+from enums.stop_state import StopState as SS
 
 class MasterPlugin(Plugin):
 
@@ -11,12 +16,85 @@ class MasterPlugin(Plugin):
         self.setObjectName('Master Plugin - L1.5 safety system')
 
         self._widget = MasterWidget(context)
-        # self._qt_wrapper = QtWrapper(self._widget)
-        # self._ros_wrapper = ROSWrapper()
+        self._qt_wrapper = QtWrapper(self._widget)
         
-        # self.set_callback_functions()
+        self._master_info = MasterInfo()
 
         
+        # self._qt_wrapper.connect_signals()
+        # self._qt_wrapper.set_sliders_to_initial_values()
+
+        self._ros_wrapper = ROSWrapper()
+        # Setup functions to get _master_info states from _ros_wrapper
+        self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state)
+        self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions)
+
+        self.set_callback_functions()
+        self._qt_wrapper.connect_signals()
+
+        # Stopping master at the beginning
+        self.master_stopped()
 
     def set_callback_functions(self):
+        self.set_ROS_callback_functions()
+        self.set_Qt_callback_functions()
+
+    def set_ROS_callback_functions(self):
+        self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        self._ros_wrapper.set_robot_info_update_callback(self.handle_robot_info_update)
+
+    def set_Qt_callback_functions(self):
+        self._qt_wrapper.set_master_stop_state_updated_callback(self.handle_master_stop_state_updated)
+        self._qt_wrapper.set_restrictions_updated_callback(self.handle_restrictions_updated)
+        
+  
+    def handle_robots_list_update(self,robots_id_list):
+        self.update_robots_list(robots_id_list)
+
+    def handle_robot_info_update(self,robot_info):
+        self.update_robot_info(robot_info)
+
+    def handle_master_stop_state_updated(self):
+        if self._master_info.master_stop_state == SS.RUNNING:
+            self.master_stopped()
+        elif self._master_info.master_stop_state == SS.STOPPED:
+             self.master_started()          
+        else:
+            raise ValueError('master_stop_state value undefined') 
+
+    def handle_restrictions_updated(self,restrictions):
+        self.restrictions_changed(restrictions)
+    
+    def update_robots_list(self,robots_id_list):
+        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)
+        self._ros_wrapper.add_robot_subscriber(robot_id)
+
+    def robot_unregistered(self,robot_id):
+        raise NotImplementedError
+        # self._qt_wrapper.remove_robot_subscriber(robot_id)
+        self._ros_wrapper.remove_robot_subscriber(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
+
+    def master_started(self):
+        self._master_info.master_stop_state = SS.RUNNING
+        self._ros_wrapper.master_started()
+        self._qt_wrapper.master_started()
+
+    def master_stopped(self):
+        self._master_info.master_stop_state = SS.STOPPED
+        self._ros_wrapper.master_stopped()
+        self._qt_wrapper.master_stopped()
+
+    def restrictions_changed(self,restrictions):
+        self._master_info.restrictions = restrictions
+        self._ros_wrapper.restrictions_changed(restrictions)

safety_master_plugin/scripts/qt_master_wrapper.py

@@ -0,0 +1,170 @@
+#!/usr/bin/env python
+import os
+import rospy
+import rospkg
+
+import math
+import datetime
+
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
+
+from python_qt_binding.QtGui import QPixmap
+from python_qt_binding.QtGui import QTextCursor
+from python_qt_binding.QtWidgets import QMessageBox
+
+
+class QtWrapper:
+
+    def __init__(self,widget):
+        self.widget = widget
+        self.displayed_robots_id_list = []
+
+        self.disable_sliders_tracking()
+        # self.set_sliders_to_initial_values()
+
+        self.master_stop_state = None
+        # self.ok_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Ok.jpg')
+        # self.cancel_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Cancel.jpg')
+
+    def set_sliders_to_initial_values(self):
+        raise NotImplementedError
+
+    def disable_sliders_tracking(self):
+        self.widget.distanceSlider.setTracking(False)
+        self.widget.angularSlider.setTracking(False)
+        self.widget.linearSlider.setTracking(False)
+
+
+    def set_master_stop_state_updated_callback(self,callback_function):
+        self.master_stop_state_updated_callback = callback_function
+
+    def set_restrictions_updated_callback(self,callback_function):
+        self.restrictions_updated_callback = callback_function
+
+    def handle_restrictions_update(self):
+        raise NotImplementedError
+        # restrictions = Restrictions()
+        # Do something
+        # self.restrictions_updated_callback(restrictions)
+
+
+    def connect_signals(self):
+        # self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update)
+        # self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update)
+        # self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update)
+        self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked)
+
+    # def handle_emitted_signal(self,method_name):
+    #     exec('self.{0}()'.format(method_name))
+
+    # def handle_emitted_signal_with_list_argument(self,method_name,argument):
+    #     if method_name == 'update_selected_robot_info':
+    #         self.update_selected_robot_info(argument[0])
+
+    #     else:
+    #         method_with_argument = 'self.{0}({1})'.format(method_name,argument[0])
+    #         exec(method_with_argument)
+
+    def handle_masterstopButton_clicked(self):
+        if self.master_stop_state == SS.RUNNING:
+            self.master_stop_state_updated_callback()
+        else:
+            reply = QMessageBox.warning(self.widget,"UWAGA","Na pewno chcesz odblokowac wszystkie roboty?",QMessageBox.Yes,QMessageBox.No)
+            
+            if reply == QMessageBox.Yes:
+                self.master_stop_state_updated_callback()
+
+
+    def display_master_stop_on(self):
+        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: green; font: bold 20px}")
+        self.widget.masterstopButton.setText('Zatrzymaj roboty')
+
+    def display_master_stop_off(self):
+        self.widget.masterstopButton.setStyleSheet("QPushButton { color: black; background-color: red; font: bold 20px}")
+        self.widget.masterstopButton.setText('Odblokuj roboty')
+
+
+    def update_robots_list_gui(self,robots_id_list):
+        robots_id_list.sort()
+        id_strings_list = (('PIONIER'+str(x)) for x in robots_id_list)
+
+        robots_to_add = []
+        robots_to_remove = []
+
+        for robot_id in robots_id_list:
+            if robot_id not in self.displayed_robots_id_list:
+                robots_to_add.append(robot_id)
+
+        for robot_id in self.displayed_robots_id_list:
+            if robot_id not in robots_id_list:
+                robots_to_remove.append(robot_id)
+
+        for robot_id in robots_to_remove:
+            self.remove_robot_from_list(robot_id)
+
+        for robot_id in robots_to_add:
+            self.add_robot_to_list(robot_id)
+
+
+    # def remove_robot_from_list(self,robot_id):
+    #     count = self.widget.robotsList.count()
+
+    #     for i in range(count):
+    #         if str(robot_id) in self.widget.robotsList.itemText(i):
+    #             self.widget.robotsList.removeItem(i)
+    #             self.displayed_robots_id_list.remove(robot_id)
+    #             return
+            
+
+    # def add_robot_to_list(self,robot_id):
+    #     self.widget.robotsList.addItem('PIONIER'+str(robot_id))
+    #     self.displayed_robots_id_list.append(robot_id)
+
+
+    def master_stopped(self):
+        self.master_stop_state = SS.STOPPED
+        self.display_master_stop_off()
+        self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty')
+
+    def master_started(self):
+        self.master_stop_state = SS.RUNNING
+        self.display_master_stop_on()
+        self.log_info('Przycisk masterSTOP odcisniety')
+
+
+    def log_info(self,info_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        self.widget.logsBrowser.insertHtml('<font color="black">' + str(time) + '. ' + info_text + '</font><br>')
+        self.scroll_to_bottom()
+        # self.widget.logsBrowser.insertHtml(str(self.logger_counter) + '\t[INFO]\t' + info_text)
+
+    def log_warning(self,warning_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+        
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
+        self.widget.logsBrowser.insertHtml('<font color="orange">' + str(time) + '. ' + warning_text + '</font><br> ')
+        self.scroll_to_bottom()
+        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[WARN]\t' + warning_text)
+
+    def log_error(self,error_text):
+        time = datetime.datetime.now().strftime('[%H:%M:%S]')
+
+        cursor = self.widget.logsBrowser.textCursor()
+        cursor.movePosition(QTextCursor.End)
+        self.widget.logsBrowser.setTextCursor(cursor)
+        self.widget.logsBrowser.textCursor().movePosition(QTextCursor.End)
+        self.widget.logsBrowser.insertHtml('<font color="red">' + str(time) + '. ' + error_text + '</font><br>')
+        self.scroll_to_bottom()
+        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)
+
+    def scroll_to_bottom(self):
+        scrollbar = self.widget.logsBrowser.verticalScrollBar()
+        scrollbar.setValue(scrollbar.maximum())

safety_master_plugin/scripts/restrictions.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+
+class Restrictions:
+
+	def __init__(self,restrictions_msg):
+		self.distance = restrictions_msg.distance.data
+		self.linear_velocity = restrictions_msg.linear_velocity.data
+		self.angular_velocity = restrictions_msg.angular_velocity.data

safety_master_plugin/scripts/robot_info.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
+
+class RobotInfo:
+
+    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]
+        self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
+
+        if robot_info_msg.state.data == True:
+            self.state = SS.RUNNING
+        else:
+            self.state = SS.STOPPED
+
+        if robot_info_msg.clutch.data == True:
+            self.clutch = CS.ENGAGED
+        else:
+            self.clutch = CS.DISENGAGED

safety_master_plugin/scripts/ros_master_wrapper.py

@@ -0,0 +1,160 @@
+#!/usr/bin/env python
+import rospy
+import rospkg
+
+from std_msgs.msg import Bool
+from safety_user_plugin.msg import RobotInfoMsg
+from safety_user_plugin.msg import RestrictionsMsg
+
+from robot_info import RobotInfo
+from restrictions import Restrictions
+
+from enums.stop_state import StopState as SS
+
+class ROSWrapper:
+
+    def __init__(self):
+        self.robots_list_update_callback = None
+        self.robot_info_update_callback = None
+
+        self.robot_info_subscribers = {}
+        self.master_stop_publisher = None
+        self.restrictions_publisher = None
+
+        self.robots_list_timer = None
+        self.periodic_timer = None
+
+        self.setup_subscribers_and_publishers()
+    # def setup_node(self):
+    #     rospy.init_node('safety_user_plugin', anonymous=True)
+
+    def setup_subscribers_and_publishers(self):
+        self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1)
+        self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1)
+
+        # self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update)
+
+    def add_robot_subscriber(self,robot_id):
+        if robot_id not in self.robot_info_subscribers:
+            topic_name = '/RosAria/PIONIER{0}/robot_info'.format(robot_id)
+            new_sub = rospy.Subscriber(topic_name, RobotInfoMsg, self.handle_robot_info_update)
+            self.robot_info_subscribers[robot_id] = new_sub
+        else:
+            raise RuntimeError('Subscriber for PIONIER{0} already in dictionary'.format(robot_id))
+
+    def remove_robot_subscriber(self,robot_id):
+        if robot_id in self.robot_info_subscribers:
+            self.unsubscribe(self.robot_info_subscribers[robot_id])
+            del self.robot_info_subscribers[robot_id]
+        else:
+            raise RuntimeError('Subscriber for PIONIER{0} not in dictionary. Cannot be removed'.format(robot_id))
+
+    def unsubscribe(self,subscriber):
+        if subscriber != None:
+            subscriber.unregister()
+
+    def unregister_publisher(self,publisher):
+        if publisher != None:
+            publisher.unregister()
+
+    def shutdown_timer(self,timer):
+        if timer != None:
+            timer.shutdown()
+
+    def publish_periodic_update(self,event):
+        restrictions = self.get_restrictions()
+        restrictions_msg = RestrictionsMsg()
+        restrictions_msg.distance.data = restrictions.distance
+        restrictions_msg.linear_velocity.data = restrictions.linear_velocity
+        restrictions_msg.angular_velocity.data = restrictions.angular_velocity
+
+        stop_state = self.get_master_stop_state()
+        stop_msg = Bool()
+
+        if stop_state == SS.RUNNING:
+            stop_msg.data = True
+        elif stop_state == SS.STOPPED:
+            stop_msg.data = False
+        else:
+            stop_msg.data = False
+            raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
+
+        self.restrictions_publisher.publish(restrictions_msg)
+        self.master_stop_publisher.publish(stop_msg)
+
+    def cancel_subscribers_and_publishers(self):
+        self.shutdown_timer(self.robots_list_timer)
+        self.shutdown_timer(self.periodic_timer)
+
+        self.unregister_publisher(self.master_stop_publisher)
+        self.unregister_publisher(self.restrictions_publisher)
+
+        for key, value in self.robot_info_subscribers.iteritems():
+            self.unsubscribe(value)
+
+    def unregister_node(self):
+        self.cancel_subscribers_and_publishers()
+        rospy.signal_shutdown('Closing safety master plugin')
+
+    def setup_get_master_stop_state_function(self,function):
+        self.get_master_stop_state = function
+
+    def setup_get_restrictions_function(self,function):
+        self.get_restrictions = function
+
+    # ROSWrapper Callbacks
+    def get_robots_list(self,event):
+        robots_id_list = []
+
+        published_topics_list = rospy.get_published_topics(namespace='/')
+        published_topics = []
+
+        for list_ in published_topics_list:
+            published_topics.append(list_[0])
+
+        for topic in published_topics:
+            if topic.find('RosAria') ==-1 or topic.find('robot_info') == -1:
+                pass
+            else:
+                robot_number = topic.split('/')
+                robot_number = robot_number[2]
+                robot_number = robot_number[7:]
+                if len(robot_number) > 0:
+                    robot_number = int(robot_number)
+                    robots_id_list.append(robot_number)
+
+        self.robots_list_update_callback(robots_id_list)
+
+
+    def handle_robot_info_update(self,msg):
+        _robot_info = RobotInfo(0)
+        _robot_info.update_robot_info(msg)
+        self.robot_info_update_callback(_robot_info)
+
+
+    # MasterPlugin Callbacks
+    def set_robots_list_update_callback(self,callback_function):
+        self.robots_list_update_callback = callback_function
+        self.robots_list_timer = rospy.Timer(rospy.Duration(0.5),self.get_robots_list)
+
+    def set_robot_info_update_callback(self,callback_function):
+        self.robot_info_update_callback = callback_function
+
+    def master_started(self):
+        msg = Bool()
+        msg.data = True
+        self.master_stop_publisher.publish(msg)
+
+    def master_stopped(self):
+        msg = Bool()
+        msg.data = False
+        self.master_stop_publisher.publish(msg)
+
+    def restrictions_changed(self,restrictions):
+        msg = RestrictionsMsg()
+
+        msg.distance.data = restrictions.distance
+        msg.linear_velocity.data = restrictions.linear_velocity
+        msg.angular_velocity.data = restrictions.angular_velocity
+
+        self.restrictions_publisher.publish(msg)