131 lines
4.7 KiB
Python
131 lines
4.7 KiB
Python
#!/usr/bin/env python
|
|
import os
|
|
import rospy
|
|
import rospkg
|
|
|
|
import math
|
|
|
|
class QtWrapper:
|
|
|
|
def __init__(self,widget):
|
|
self.widget = widget
|
|
self.logger_counter = 0
|
|
|
|
def connect_robot_signals(self):
|
|
raise NotImplementedError
|
|
|
|
def connect_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)
|
|
|
|
def get_selected_robot_id(self):
|
|
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()
|
|
|
|
if selected_robot_id != None:
|
|
self.handle_openButton_clicked_callback(selected_robot_id)
|
|
else:
|
|
# TODO - Log info
|
|
raise NotImplementedError
|
|
|
|
def set_robot_selection_callback(self,callback):
|
|
self.handle_openButton_clicked_callback = callback
|
|
|
|
def set_user_stop_state_updated_callback(self,callback):
|
|
self.handle_stopButton_clicked_callback = callback
|
|
|
|
def set_clutch_switched_callback(self,callback):
|
|
self.handle_clutchButton_clicked_callback = callback
|
|
|
|
def select_robot(self,robot_id):
|
|
self.log_info('Robot PIONIER' + str(robot_id) + ' wybrany!')
|
|
|
|
def release_robot(self):
|
|
self.log_info('Odlaczam robota')
|
|
|
|
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)
|
|
self.widget.robotsList.addItems(id_strings_list)
|
|
|
|
|
|
def update_selected_robot_info(self,robot_info):
|
|
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):
|
|
self.log_info('Przycisk masterSTOP zostal nacisniety. Zatrzymuje roboty')
|
|
|
|
def master_started(self):
|
|
self.log_info('Przycisk masterSTOP odcisniety. Mozesz uruchomic robota')
|
|
|
|
def user_stopped(self):
|
|
self.log_info('Zatrzymuje robota')
|
|
self.widget.stateLabel.setText('0')
|
|
|
|
def user_started(self):
|
|
self.log_info('Robot wystartowal')
|
|
self.widget.stateLabel.setText('1')
|
|
|
|
def disengage_clutch(self):
|
|
self.widget.clutchLabel.setText('0')
|
|
|
|
def engage_clutch(self):
|
|
self.widget.clutchLabel.setText('1')
|
|
|
|
def master_is_stopped_notify(self):
|
|
self.log_error('Nie mozna wystartowac robota. MasterSTOP wcisniety!')
|
|
|
|
def update_restrictions_gui(self,restrictions):
|
|
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):
|
|
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()) |