Further work

safety_user_plugin/scripts/qt_wrapper.py

@@ -3,15 +3,13 @@ import os
 import rospy
 import rospkg
 
-# from python_qt_binding.QtGui import QPalette
+import math
-# 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.logsBrowser.setText('Sukces')
+        self.logger_counter = 0
 
     def connect_robot_signals(self):
         raise NotImplementedError
@@ -22,7 +20,13 @@ class QtWrapper:
         self.widget.stopButton.clicked.connect(self.handle_stopButton_clicked_callback)
 
     def get_selected_robot_id(self):
-        raise NotImplementedError
+        current_text = self.widget.robotsList.currentText()
+
+        if 'PIONIER' in current_text:
+            return int(current_text[7:])
+        else:
+            return None
+
 
     def handle_openButton_clicked(self):
         selected_robot_id = self.get_selected_robot_id()
@@ -43,10 +47,10 @@ class QtWrapper:
         self.handle_clutchButton_clicked_callback = callback
 
     def select_robot(self,robot_id):
-        self.widget.logsBrowser.setText('Sukcesx1')
+        self.log_info('Robot PIONIER' + str(robot_id) + ' wybrany!')
 
     def release_robot(self):
-        self.widget.logsBrowser.setText('Sukcesx2')
+        self.log_info('Odlaczam robota')
 
     def update_robots_list_gui(self,robots_id_list):
         robots_id_list.sort()
@@ -55,35 +59,73 @@ class QtWrapper:
 
 
     def update_selected_robot_info(self,robot_info):
-        raise NotImplementedError
+        linear_vel = math.sqrt(robot_info.linear_velocity[0]**2 + robot_info.linear_velocity[1]**2)
+
+        self.widget.idLabel.setText('PIONIER' + str(robot_id))
+        self.widget.batteryLabel.setText("{:.2f}".format(robot_info.battery_voltage))
+        self.widget.linearLabel.setText("{:.2f}".format(linear_vel))
+        self.widget.angularLabel.setText("{:.2f}".format(robot_info.angular_velocity[2]))
+        
+        if robot_info.clutch == True:
+            self.engage_clutch()
+        else:
+            self.disengage_clutch()
+
+        if robot_info.state == True:
+            self.engage_clutch()
+        else:
+            self.disengage_clutch()
 
     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):
-        raise NotImplementedError
+        self.log_info('Zatrzymuje robota')
+        self.widget.stateLabel.setText('0')
 
     def user_started(self):
-        raise NotImplementedError
+        self.log_info('Robot wystartowal')
-
+        self.widget.stateLabel.setText('1')
-    def engage_clutch(self):
-        # raise NotImplementedError
-        pass
 
     def disengage_clutch(self):
-        raise NotImplementedError
+        self.widget.clutchLabel.setText('0')
+
+    def engage_clutch(self):
+        self.widget.clutchLabel.setText('1')
 
     def master_is_stopped_notify(self):
-        raise NotImplementedError
+        self.log_error('Nie mozna wystartowac robota. MasterSTOP wcisniety!')
 
     def update_restrictions_gui(self,restrictions):
-        raise NotImplementedError
+        self.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
+        self.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
+        self.angularRestrictionLabel.setText("{:.2f}".format(restrictions.angular_velocity))       
 
+    def log_info(self,info_text):
+        self.logger_counter += 1
+        self.widget.logsBrowser.insertHtml('<font color="black">' + str(self.logger_counter) + '. ' + 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):
+        self.logger_counter += 1
+        self.widget.logsBrowser.insertHtml('<font color="orange">' + str(self.logger_counter) + '. ' + 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):
+        self.logger_counter += 1
+        self.widget.logsBrowser.insertHtml('<font color="red">' + str(self.logger_counter) + '. ' + error_text + '</font><br>')
+        self.scroll_to_bottom()
+        # self.widget.logsBrowser.append(str(self.logger_counter) + '\t[ERROR]\t' + error_text)
 
     def connection_to_robot_lost(self,lost_robot_id):
-        raise NotImplementedError
+        self.log_info('Utrata polaczenia z robotem PIONIER' + str(lost_robot_id))
+
+
+    def scroll_to_bottom(self):
+        scrollbar = self.widget.logsBrowser.verticalScrollBar()
+        scrollbar.setValue(scrollbar.maximum())

safety_user_plugin/scripts/user_plugin.py

@@ -71,8 +71,7 @@ class UserPlugin(Plugin):
         self._qt_wrapper.update_robots_list_gui(robots_id_list)
 
     def handle_selected_robot_info_update(self,robot_info):
-        self._user_info.update_selected_robot_info(robot_info)
+        self.update_selected_robot_info(robot_info)
-        self._qt_wrapper.update_selected_robot_info(robot_info)
 
     def handle_master_stop_update(self,master_stop_state):
         self._user_info.master_stop_state = master_stop_state
@@ -133,8 +132,7 @@ class UserPlugin(Plugin):
     def handle_robot_connection_lost(self):
         if self._user_info.selected_robot != None:
             lost_robot_id = self._user_info.selected_robot.robot_id
-            self.release_robot()
+            self.connection_to_robot_lost(lost_robot_id)
-            self._qt_wrapper.connection_to_robot_lost(lost_robot_id)
         else:
             raise NotImplementedError
 
@@ -178,3 +176,12 @@ class UserPlugin(Plugin):
         self._ros_wrapper.release_robot()
         self._qt_wrapper.release_robot()
         self._user_info.release_robot()
+
+    def connection_to_robot_lost(self,lost_robot_id):
+        self._ros_wrapper.release_robot()
+        self._user_info.release_robot()
+        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)

safety_user_plugin/ui/user_view.ui

@@ -172,7 +172,7 @@
      <item>
       <layout class="QGridLayout" name="gridLayout">
        <item row="1" column="1">
-        <widget class="QLabel" name="distanceLabel">
+        <widget class="QLabel" name="distanceRestrictionLabel">
          <property name="minimumSize">
           <size>
            <width>100</width>