Most of functionalities of user plugin works
This commit is contained in:
Olek Bojda
parent
daebe414e8
commit
948c26ce25
@ -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):
|
||||
raise NotImplementedError
|
||||
def disconnect_signals(self):
|
||||
self.widget.clutchButton.clicked.disconnect()
|
||||
self.widget.stopButton.clicked.disconnect()
|
||||
self.widget.choiceButton.clicked.disconnect()
|
||||
|
||||
def connect_signals(self):
|
||||
self.widget.openButton.clicked.connect(self.handle_openButton_clicked)
|
||||
def connect_robot_signals(self):
|
||||
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
|
||||
raise NotImplementedError
|
||||
self.log_error('Najpierw wybierz PIONIERA z listy robotow')
|
||||
|
||||
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:
|
||||
self.engage_clutch()
|
||||
if robot_info.clutch == CS.ENGAGED:
|
||||
self.widget.clutchLabel.setText('1')
|
||||
else:
|
||||
self.disengage_clutch()
|
||||
self.widget.clutchLabel.setText('0')
|
||||
|
||||
if robot_info.state == True:
|
||||
self.engage_clutch()
|
||||
if robot_info.state == SS.RUNNING:
|
||||
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')
|
||||
@ -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
|
||||
@ -71,16 +71,14 @@ class ROSWrapper:
|
||||
elif stop_state == SS.STOPPED:
|
||||
self.user_stopped()
|
||||
else:
|
||||
# TODO Raise wrong value error
|
||||
raise NotImplementedError
|
||||
raise ValueError('stop_state UNKNOWN when attempting to publish periodic update')
|
||||
|
||||
if clutch_state == CS.ENGAGED:
|
||||
self.engage_clutch()
|
||||
elif clutch_state == CS.DISENGAGED:
|
||||
self.disengage_clutch()
|
||||
else:
|
||||
# TODO Raise wrong value error
|
||||
raise NotImplementedError
|
||||
raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
|
||||
|
||||
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 NotImplementedError
|
||||
raise RuntimeError('Attempting to restart connection_timer when it is not initialized')
|
||||
|
||||
def unregister_node(self):
|
||||
self.cancel_subscribers_and_publishers()
|
||||
|
||||
@ -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):
|
||||
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):
|
||||
if (master_stop_state != self._user_info.master_stop_state):
|
||||
self._user_info.master_stop_state = master_stop_state
|
||||
if master_stop_state == SS.STOPPED:
|
||||
self.master_stopped()
|
||||
elif master_stop_state == SS.RUNNING:
|
||||
self.master_started()
|
||||
else:
|
||||
# TODO - Wrong masterstop value exception
|
||||
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 NotImplementedError
|
||||
raise RuntimeError('User already selected robot')
|
||||
|
||||
def handle_robot_release(self):
|
||||
if self._user_info.selected_robot != None:
|
||||
self.release_robot()
|
||||
else:
|
||||
# TODO - raise no robot selected error
|
||||
raise NotImplementedError
|
||||
raise RuntimeError('Cannot release. No robot selected')
|
||||
|
||||
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 NotImplementedError
|
||||
raise ValueError('user_stop_state value undefined')
|
||||
else:
|
||||
# TODO - raise switching unknown masterstop value exception
|
||||
raise NotImplementedError
|
||||
raise ValueError('master_stop_state value undefined')
|
||||
|
||||
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 NotImplementedError
|
||||
raise ValueError('clutch_state value undefined')
|
||||
|
||||
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)
|
||||
@ -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>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user