From 4f36a75249e997f170ddcba1c0396b9c0b8581e6 Mon Sep 17 00:00:00 2001 From: Olek Bojda Date: Tue, 18 Sep 2018 20:21:30 +0200 Subject: [PATCH] Further work --- safety_user_plugin/scripts/qt_wrapper.py | 82 +++++++++++++++++------ safety_user_plugin/scripts/user_plugin.py | 17 +++-- safety_user_plugin/ui/user_view.ui | 2 +- 3 files changed, 75 insertions(+), 26 deletions(-) diff --git a/safety_user_plugin/scripts/qt_wrapper.py b/safety_user_plugin/scripts/qt_wrapper.py index 19e12a9..430f9f9 100644 --- a/safety_user_plugin/scripts/qt_wrapper.py +++ b/safety_user_plugin/scripts/qt_wrapper.py @@ -3,15 +3,13 @@ import os import rospy import rospkg -# from python_qt_binding.QtGui import QPalette -# from python_qt_binding.QtWidgets import QPushButton -# from python_qt_binding.QtWidgets import QWidget, QDialog +import math 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 - - def engage_clutch(self): - # raise NotImplementedError - pass + self.log_info('Robot wystartowal') + self.widget.stateLabel.setText('1') 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('' + str(self.logger_counter) + '. ' + info_text + '
') + 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('' + str(self.logger_counter) + '. ' + warning_text + '
') + 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('' + str(self.logger_counter) + '. ' + error_text + '
') + 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 \ No newline at end of file + 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()) \ No newline at end of file diff --git a/safety_user_plugin/scripts/user_plugin.py b/safety_user_plugin/scripts/user_plugin.py index fcedb45..23ce6d6 100755 --- a/safety_user_plugin/scripts/user_plugin.py +++ b/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._qt_wrapper.update_selected_robot_info(robot_info) + self.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._qt_wrapper.connection_to_robot_lost(lost_robot_id) + self.connection_to_robot_lost(lost_robot_id) else: raise NotImplementedError @@ -177,4 +175,13 @@ class UserPlugin(Plugin): def release_robot(self): self._ros_wrapper.release_robot() self._qt_wrapper.release_robot() - self._user_info.release_robot() \ No newline at end of file + 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) \ No newline at end of file diff --git a/safety_user_plugin/ui/user_view.ui b/safety_user_plugin/ui/user_view.ui index 9755a81..f71d8ed 100644 --- a/safety_user_plugin/ui/user_view.ui +++ b/safety_user_plugin/ui/user_view.ui @@ -172,7 +172,7 @@ - + 100