134 lines
4.5 KiB
Python
Executable File
134 lines
4.5 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
import os
|
|
import rospy
|
|
import rospkg
|
|
|
|
from qt_gui.plugin import Plugin
|
|
|
|
from qt_wrapper import QtWrapper
|
|
from ros_wrapper import ROSWrapper
|
|
from user_widget import UserWidget
|
|
from user_info import UserInfo
|
|
|
|
from enums.clutch_state import ClutchState as CS
|
|
from enums.stop_state import StopState as SS
|
|
|
|
class UserPlugin(Plugin):
|
|
|
|
def __init__(self,context):
|
|
super(UserPlugin, self).__init__(context)
|
|
self.setObjectName('User Plugin - L1.5 safety system')
|
|
|
|
self._widget = UserWidget(context)
|
|
self._qt_wrapper = QtWrapper(self._widget)
|
|
self._ros_wrapper = ROSWrapper()
|
|
self._user_info = UserInfo()
|
|
|
|
self._user_info.user_stop_state = SS.STOPPED
|
|
self._user_info.master_stop_state = SS.UNKNOWN
|
|
self._user_info.clutch_state = CS.UNKNOWN
|
|
|
|
# self.register_callback_functions()
|
|
|
|
# At the end!
|
|
# self._qt_wrapper.connect_signals()
|
|
# self._ros_wrapper.setup_node()
|
|
|
|
def shutdown_plugin(self):
|
|
# TODO unregister all publishers here
|
|
self._ros_wrapper.unregister_node()
|
|
|
|
def register_callback_functions(self):
|
|
raise NotImplementedError
|
|
# self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update)
|
|
|
|
|
|
# ROS callback functions
|
|
def handle_robots_list_update(self,robots_id_list):
|
|
self._user_info.update_robots_id_list(robots_id_list)
|
|
self._qt_wrapper.update_robots_list_gui(robots_id_list)
|
|
|
|
def handle_selected_robot_info_update(self,robot_info):
|
|
self._qt_wrapper.update_selected_robot_info(self,robot_info)
|
|
|
|
def handle_master_stop_update(self,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
|
|
|
|
def handle_restrictions_update(self,restrictions):
|
|
raise NotImplementedError
|
|
|
|
|
|
# Qt callback functions
|
|
def handle_robot_selection(self,robot_id):
|
|
if self._user_info.selected_robot == None:
|
|
self.select_robot(robot_id)
|
|
else:
|
|
# TODO - raise user already selected robot exception
|
|
raise NotImplementedError
|
|
|
|
def handle_user_stop_state_updated(self):
|
|
if self._user_info.master_stop_state == CS.STOPPED:
|
|
self._qt_wrapper.master_is_stopped_notify()
|
|
elif self._user_info.master_stop_state == CS.RUNNING:
|
|
if self._user_info.user_stop_state == CS.RUNNING:
|
|
self.user_stopped()
|
|
elif self._user_info.user_stop_state == CS.STOPPED:
|
|
self.user_started()
|
|
else:
|
|
# TODO - Wrong userstop value exception
|
|
raise NotImplementedError
|
|
else:
|
|
# TODO - raise switching unknown masterstop value exception
|
|
raise NotImplementedError
|
|
|
|
def handle_clutch_switched(self):
|
|
if self._user_info.clutch_state == CS.ENGAGED:
|
|
self.disengage_clutch()
|
|
elif self._user_info.clutch_state == CS.DISENGAGED:
|
|
self.engage_clutch()
|
|
else:
|
|
# TODO - raise switching unknown clutch value exception
|
|
raise NotImplementedError
|
|
|
|
def master_stopped(self):
|
|
self.user_stopped()
|
|
self._qt_wrapper.master_stopped()
|
|
self._user_info.master_stop_state = CS.STOPPED
|
|
|
|
def master_started(self):
|
|
self._qt_wrapper.master_started()
|
|
self._user_info.master_stop_state = CS.RUNNING
|
|
|
|
def user_stopped(self):
|
|
self._user_info.user_stop_state = CS.STOPPED
|
|
self._ros_wrapper.user_stopped()
|
|
self._qt_wrapper.user_stopped()
|
|
|
|
def user_started(self):
|
|
self._user_info.user_stop_state = CS.RUNNING
|
|
self._ros_wrapper.user_started()
|
|
self._qt_wrapper.user_started()
|
|
|
|
def engage_clutch(self):
|
|
self._user_info.clutch_state = CS.ENGAGED
|
|
self._ros_wrapper.engage_clutch()
|
|
|
|
def disengage_clutch(self):
|
|
self._user_info.clutch_state = CS.DISENGAGED
|
|
self._ros_wrapper.disengage_clutch()
|
|
|
|
def select_robot(self):
|
|
if self._ros_wrapper.select_robot(robot_id) == True:
|
|
self._qt_wrapper.select_robot(robot_id)
|
|
self._user_info.select_robot(robot_id)
|
|
else:
|
|
# TODO - raise ROS subscriptions problem exception
|
|
raise NotImplementedError |