Most of functionalities of user plugin works

safety_user_plugin/scripts/qt_wrapper.py

@@ -5,6 +5,10 @@ import rospkg
 
 import math
 
+from enums.clutch_state import ClutchState as CS
+from enums.stop_state import StopState as SS
+
+
 class QtWrapper:
 
     def __init__(self,widget):
@@ -12,13 +16,32 @@ class QtWrapper:
         self.logger_counter = 0
         self.displayed_robots_id_list = []
 
-    def connect_robot_signals(self):
+    def disconnect_signals(self):
-        raise NotImplementedError
+        self.widget.clutchButton.clicked.disconnect()
+        self.widget.stopButton.clicked.disconnect()
+        self.widget.choiceButton.clicked.disconnect()
 
-    def connect_signals(self):
+    def connect_robot_signals(self):
-        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)
+        self.widget.choiceButton.clicked.connect(self.handle_closeButton_clicked)
+
+    def connect_signals(self):
+        self.widget.clutchButton.clicked.connect(self.handle_not_selected_error)
+        self.widget.stopButton.clicked.connect(self.handle_not_selected_error)
+        self.widget.choiceButton.clicked.connect(self.handle_openButton_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 get_selected_robot_id(self):
         current_text = self.widget.robotsList.currentText()
@@ -28,6 +51,8 @@ class QtWrapper:
         else:
             return None
 
+    def handle_not_selected_error(self):
+        self.log_error('Najpierw wybierz PIONIERA z listy robotow')
 
     def handle_openButton_clicked(self):
         selected_robot_id = self.get_selected_robot_id()
@@ -35,12 +60,17 @@ class QtWrapper:
         if selected_robot_id != None:
             self.handle_openButton_clicked_callback(selected_robot_id)
         else:
-            # TODO - Log info
+            self.log_error('Najpierw wybierz PIONIERA z listy robotow')
-            raise NotImplementedError
+
+    def handle_closeButton_clicked(self):
+        self.handle_closeButton_clicked_callback()
 
     def set_robot_selection_callback(self,callback):
         self.handle_openButton_clicked_callback = callback
 
+    def set_robot_release_callback(self,callback):
+        self.handle_closeButton_clicked_callback = callback
+
     def set_user_stop_state_updated_callback(self,callback):
         self.handle_stopButton_clicked_callback = callback
 
@@ -48,10 +78,18 @@ class QtWrapper:
         self.handle_clutchButton_clicked_callback = callback
 
     def select_robot(self,robot_id):
-        self.log_info('Robot PIONIER' + str(robot_id) + ' wybrany!')
+        self.disconnect_signals()
+        self.connect_robot_signals()
+        self.log_info('PIONIER' + str(robot_id) + ' wybrany!') 
+        self.log_info('Robot zostanie zatrzymany i sprzegniety')
+
+        self.widget.choiceButton.setText('Zwolnij robota')
 
     def release_robot(self):
+        self.disconnect_signals()
+        self.connect_signals()
         self.log_info('Odlaczam robota')
+        self.clear_robot_info()
 
     def update_robots_list_gui(self,robots_id_list):
         robots_id_list.sort()
@@ -98,34 +136,44 @@ class QtWrapper:
         self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
         self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2]))
         
-        if robot_info.clutch == True:
+        if robot_info.clutch == CS.ENGAGED:
-            self.engage_clutch()
+            self.widget.clutchLabel.setText('1')
         else:
-            self.disengage_clutch()
+            self.widget.clutchLabel.setText('0')
 
-        if robot_info.state == True:
+        if robot_info.state == SS.RUNNING:
-            self.engage_clutch()
+            self.widget.stateLabel.setText('1')
         else:
-            self.disengage_clutch()
+            self.widget.stateLabel.setText('0')
 
     def master_stopped(self):
+        # raise NotImplementedError
         self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty')
 
     def master_started(self):
+        # raise NotImplementedError
         self.log_info('Przycisk masterSTOP odcisniety. Mozesz uruchomic robota')
 
     def user_stopped(self):
-        self.log_info('Zatrzymuje robota')
+        # raise NotImplementedError
+        self.log_info('Robot zatrzymany')
         self.widget.stateLabel.setText('0')
 
     def user_started(self):
+        # raise NotImplementedError
         self.log_info('Robot wystartowal')
         self.widget.stateLabel.setText('1')
 
     def disengage_clutch(self):
+        # raise NotImplementedError
+        self.log_info('Rozprzegam sprzeglo')
         self.widget.clutchLabel.setText('0')
+        # pass
 
     def engage_clutch(self):
+        # raise NotImplementedError
+        self.log_info('Sprzegam sprzeglo')
+        # self.signal.('disengage_clutch')
         self.widget.clutchLabel.setText('1')
 
     def master_is_stopped_notify(self):
@@ -161,3 +209,14 @@ class QtWrapper:
     def scroll_to_bottom(self):
         scrollbar = self.widget.logsBrowser.verticalScrollBar()
         scrollbar.setValue(scrollbar.maximum())
+
+
+    def clear_robot_info(self):
+        self.widget.idLabel.setText('-')
+        self.widget.batteryLabel.setText('-')
+        self.widget.linearLabel.setText('-')
+        self.widget.angularLabel.setText('-')
+        self.widget.stateLabel.setText('-')
+        self.widget.clutchLabel.setText('-')
+
+        self.widget.choiceButton.setText('Wybierz')

safety_user_plugin/scripts/robot_info.py

@@ -1,5 +1,8 @@
 #!/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):
@@ -15,5 +18,13 @@ class RobotInfo:
         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]
-        self.state = robot_info_msg.state.data
+
-        self.clutch = robot_info_msg.clutch.data
+        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_user_plugin/scripts/ros_wrapper.py

@@ -71,16 +71,14 @@ class ROSWrapper:
         elif stop_state == SS.STOPPED:
             self.user_stopped()
         else:
-            # TODO Raise wrong value error
+            raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
-            raise NotImplementedError
 
         if clutch_state == CS.ENGAGED:
             self.engage_clutch()
         elif clutch_state == CS.DISENGAGED:
             self.disengage_clutch()
         else:
-            # TODO Raise wrong value error
+            raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
-            raise NotImplementedError  
 
     def handle_robot_connection_lost(self,event):
         self.shutdown_timer(self.connection_timer)
@@ -106,8 +104,7 @@ class ROSWrapper:
             self.connection_timer.shutdown()
             self.connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_robot_connection_lost)
         else:
-            # TODO - raise connection_timer_not_setup error
+            raise RuntimeError('Attempting to restart connection_timer when it is not initialized')
-            raise NotImplementedError
 
     def unregister_node(self):
         self.cancel_subscribers_and_publishers()

safety_user_plugin/scripts/user_plugin.py

@@ -3,6 +3,8 @@
 import os
 
 from qt_gui.plugin import Plugin
+from python_qt_binding.QtCore import QObject
+from python_qt_binding.QtCore import pyqtSignal
 
 from qt_wrapper import QtWrapper
 from ros_wrapper import ROSWrapper
@@ -13,6 +15,20 @@ from enums.clutch_state import ClutchState as CS
 from enums.stop_state import StopState as SS
 
 
+class CallbackHandler(QObject):
+    signal = pyqtSignal(str)
+    signal_with_list_argument = pyqtSignal(str,list)
+
+    def __init__(self):
+        super(CallbackHandler, self).__init__()    
+
+    def connect(self,slot):
+        self.signal.connect(slot)
+
+    def list_connect(self,slot):
+        self.signal_with_list_argument.connect(slot)
+
+    # def handle
 
 class UserPlugin(Plugin):
 
@@ -20,7 +36,7 @@ class UserPlugin(Plugin):
         super(UserPlugin, self).__init__(context)
         self.setObjectName('User Plugin - L1.5 safety system')
         # setStyleSheet("background-color:black;")
-        
+        self.cbhandler = CallbackHandler()
 
         self._widget = UserWidget(context)
         self._qt_wrapper = QtWrapper(self._widget)
@@ -41,8 +57,16 @@ class UserPlugin(Plugin):
         # At the end!
         self._qt_wrapper.connect_signals()
         self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
+        self.cbhandler.signal.connect(self._qt_wrapper.handle_emitted_signal)
+        self.cbhandler.signal_with_list_argument.connect(self._qt_wrapper.handle_emitted_signal_with_list_argument)
 
-        # self._qt_wrapper.update_robots_list_gui([5,6,2])
+
+
+    def call_qt_wrapper_method(self,method_name):
+        self.cbhandler.signal.emit(method_name)
+
+    def call_qt_wrapper_method_with_argument(self,method_name,argument):
+        self.cbhandler.signal_with_list_argument.emit(method_name,argument)
 
     def shutdown_plugin(self):
         self._ros_wrapper.unregister_node()
@@ -59,6 +83,7 @@ class UserPlugin(Plugin):
 
     def set_Qt_callback_functions(self):
         self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection)
+        self._qt_wrapper.set_robot_release_callback(self.handle_robot_release)
         self._qt_wrapper.set_user_stop_state_updated_callback(self.handle_user_stop_state_updated)
         self._qt_wrapper.set_clutch_switched_callback(self.handle_clutch_switched)
         
@@ -71,17 +96,20 @@ class UserPlugin(Plugin):
         self._qt_wrapper.update_robots_list_gui(robots_id_list)
 
     def handle_selected_robot_info_update(self,robot_info):
-        self.update_selected_robot_info(robot_info)
+        if robot_info != None:
+            self.update_selected_robot_info(robot_info)
+        else:
+            raise RuntimeError('Updated robot_info is NoneType object')
 
     def handle_master_stop_update(self,master_stop_state):
-        self._user_info.master_stop_state = master_stop_state
+        if (master_stop_state != self._user_info.master_stop_state):
-        if master_stop_state == SS.STOPPED:
+            self._user_info.master_stop_state = master_stop_state
-            self.master_stopped()
+            if master_stop_state == SS.STOPPED:
-        elif master_stop_state == SS.RUNNING:
+                self.master_stopped()
-            self.master_started()
+            elif master_stop_state == SS.RUNNING:
-        else:
+                self.master_started()
-            # TODO - Wrong masterstop value exception
+            else:
-            raise NotImplementedError             
+                raise ValueError('Undefined master_stop_state received')           
 
     def handle_restrictions_update(self,restrictions):
         self._qt_wrapper.update_restrictions_gui(restrictions)
@@ -91,19 +119,14 @@ class UserPlugin(Plugin):
     def handle_robot_selection(self,robot_id):
         if self._user_info.selected_robot == None:
             self.select_robot(robot_id)
-            # Stop robot and engage clutch after taking control over it to put it in starting state
-            self.user_stopped()
-            self.engage_clutch()
         else:
-            # TODO - raise user already selected robot exception
+            raise RuntimeError('User already selected robot')
-            raise NotImplementedError
 
     def handle_robot_release(self):
         if self._user_info.selected_robot != None:
             self.release_robot()
         else:
-            # TODO - raise no robot selected error
+            raise RuntimeError('Cannot release. No robot selected')
-            raise NotImplementedError
 
     def handle_user_stop_state_updated(self):
         if self._user_info.master_stop_state == SS.STOPPED:
@@ -114,11 +137,9 @@ class UserPlugin(Plugin):
             elif self._user_info.user_stop_state == SS.STOPPED:
                 self.user_started()
             else:
-                # TODO - Wrong userstop value exception
+                raise ValueError('user_stop_state value undefined')            
-                raise NotImplementedError             
         else:
-            # TODO - raise switching unknown masterstop value exception
+            raise ValueError('master_stop_state value undefined') 
-            raise NotImplementedError
 
     def handle_clutch_switched(self):
         if self._user_info.clutch_state == CS.ENGAGED:
@@ -126,63 +147,76 @@ class UserPlugin(Plugin):
         elif self._user_info.clutch_state == CS.DISENGAGED:
             self.engage_clutch()
         else:
-            # TODO - raise switching unknown clutch value exception
+            raise ValueError('clutch_state value undefined') 
-            raise NotImplementedError
 
     def handle_robot_connection_lost(self):
         if self._user_info.selected_robot != None:
             lost_robot_id = self._user_info.selected_robot.robot_id
             self.connection_to_robot_lost(lost_robot_id)
         else:
-            raise NotImplementedError
+            raise RuntimeError('Connection lost callback received when no robot was selected')
 
 
     # Operations
     def master_stopped(self):
         self.user_stopped()
-        self._qt_wrapper.master_stopped()
+        self.call_qt_wrapper_method('master_stopped')
+        # self._qt_wrapper.master_stopped()
         self._user_info.master_stop_state = SS.STOPPED
 
     def master_started(self):
-        self._qt_wrapper.master_started()
+        # self._qt_wrapper.master_started()
+        self.call_qt_wrapper_method('master_started')
         self._user_info.master_stop_state = SS.RUNNING
 
     def user_stopped(self):
         self._user_info.user_stop_state = SS.STOPPED
         self._ros_wrapper.user_stopped()
-        self._qt_wrapper.user_stopped()
+        self.call_qt_wrapper_method('user_stopped')
+        # self._qt_wrapper.user_stopped()
 
     def user_started(self):
         self._user_info.user_stop_state = SS.RUNNING
         self._ros_wrapper.user_started()
-        self._qt_wrapper.user_started()
+        self.call_qt_wrapper_method('user_started')
+        # self._qt_wrapper.user_started()
 
     def engage_clutch(self):
         self._user_info.clutch_state = CS.ENGAGED
         self._ros_wrapper.engage_clutch()
-        self._qt_wrapper.engage_clutch()
+        self.call_qt_wrapper_method('engage_clutch')
+        # self._qt_wrapper.engage_clutch()
 
     def disengage_clutch(self):
         self._user_info.clutch_state = CS.DISENGAGED
         self._ros_wrapper.disengage_clutch()
-        self._qt_wrapper.disengage_clutch()
+        self.call_qt_wrapper_method('disengage_clutch')
+        # self._qt_wrapper.disengage_clutch()
 
     def select_robot(self,robot_id):
         self._ros_wrapper.select_robot(robot_id)
-        self._qt_wrapper.select_robot(robot_id)
+        # self._qt_wrapper.select_robot(robot_id)
+        self.call_qt_wrapper_method_with_argument('select_robot',[robot_id])
         self._user_info.select_robot(robot_id)
 
+        # Stop robot and engage clutch after taking control over it to put it in starting state
+        self.user_stopped()
+        self.engage_clutch()
+
     def release_robot(self):
         self._ros_wrapper.release_robot()
-        self._qt_wrapper.release_robot()
+        self.call_qt_wrapper_method('release_robot')
+        # self._qt_wrapper.release_robot()
         self._user_info.release_robot()
 
     def connection_to_robot_lost(self,lost_robot_id):
         # pass
         self._ros_wrapper.release_robot()
         self._user_info.release_robot()
-        self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
+        self.call_qt_wrapper_method_with_argument('connection_to_robot_lost',[lost_robot_id])
+        # self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
 
     def update_selected_robot_info(self,robot_info):
         self._user_info.update_selected_robot_info(robot_info)
-        self._qt_wrapper.update_selected_robot_info(robot_info)
+        self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
+        # self._qt_wrapper.update_selected_robot_info(robot_info)

safety_user_plugin/ui/user_view.ui

@@ -39,7 +39,7 @@
       </widget>
      </item>
      <item>
-      <widget class="QPushButton" name="openButton">
+      <widget class="QPushButton" name="choiceButton">
        <property name="sizePolicy">
         <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
          <horstretch>0</horstretch>
@@ -53,7 +53,7 @@
         </size>
        </property>
        <property name="text">
-        <string>Otwórz</string>
+        <string>Wybierz</string>
        </property>
       </widget>
      </item>