#!/usr/bin/env python import os 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') # setStyleSheet("background-color:black;") 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 # Setup functions to get _user_info states from _ros_wrapper self._ros_wrapper.setup_get_user_stop_state_function(self._user_info.get_user_stop_state) self._ros_wrapper.setup_get_clutch_state_function(self._user_info.get_clutch_state) # self._ros_wrapper.setup_node() self.set_callback_functions() # At the end! self._qt_wrapper.connect_signals() self._ros_wrapper.set_robots_list_update_callback(self.handle_robots_list_update) def shutdown_plugin(self): self._ros_wrapper.unregister_node() def set_callback_functions(self): self.set_Qt_callback_functions() self.set_ROS_callback_functions() def set_ROS_callback_functions(self): self._ros_wrapper.set_selected_robot_info_update_callback(self.handle_selected_robot_info_update) self._ros_wrapper.set_master_stop_update_callback(self.handle_master_stop_update) self._ros_wrapper.set_restrictions_update_callback(self.handle_restrictions_update) self._ros_wrapper.set_robot_connection_lost_callback(self.handle_robot_connection_lost) def set_Qt_callback_functions(self): self._qt_wrapper.set_robot_selection_callback(self.handle_robot_selection) 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) # 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._user_info.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 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): self._qt_wrapper.update_restrictions_gui(restrictions) # Qt callback functions 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 def handle_robot_release(self): if self._user_info.selected_robot != None: self.release_robot() else: # TODO - raise no robot selected error 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 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) else: raise NotImplementedError # Operations 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() 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() def select_robot(self,robot_id): self._ros_wrapper.select_robot(robot_id) self._qt_wrapper.select_robot(robot_id) self._user_info.select_robot(robot_id) def release_robot(self): self._ros_wrapper.release_robot() self._qt_wrapper.release_robot() self._user_info.release_robot()