예제 #1
0
    def __init__(self):
        '''
        Initialize the layout for the widget by setting its color and instantiating its
        components:

        Parameters:
            N/A

        Returns:
            N/A
        '''
        QWidget.__init__(self)

        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        self.pid_gui_node = mechos.Node("PID_TUNER_GUI", '192.168.1.2',
                                        '192.168.1.14')

        #Publisher to tell the navigation/movement controller when new PID values are saved.
        self.pid_configs_update_publisher = self.pid_gui_node.create_publisher(
            "UPDATE_PID_CONFIGS", Bool(), protocol="tcp")

        #Subscriber to get PID ERRORS
        #self.pid_errors_subscriber = self.pid_gui_node.create_subscriber("PE", self._update_error_plot, configs["sub_port"])
        #self.pid_error_proto = pid_errors_pb2.PID_ERRORS()

        #Mechos parameter server
        #Initialize parameter server client to get and set parameters related to sub
        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Set background color of the widget
        #nav_gui_palette = self.palette()
        #nav_gui_palette.setColor(self.backgroundRole(), QColor(64, 64, 64))
        #self.setPalette(nav_gui_palette)

        #Create widgets main layout structure
        self.primary_linking_layout = QVBoxLayout(self)
        self.setLayout(self.primary_linking_layout)

        #Options widget
        self.options_linking_layout = QGridLayout()

        self._error_plotter()
        self._PID_controller_select()
        self._PID_sliders()
        self.set_desired_position = Set_Desired_Position_GUI()

        #Set up QTimer to update the PID errors
        self.pid_error_update_timer = QTimer()

        #self.pid_error_update_timer.timeout.connect(lambda: self.pid_gui_node.spinOnce(self.pid_errors_subscriber))

        self.primary_linking_layout.addLayout(self.options_linking_layout, 1)
        self.primary_linking_layout.addWidget(self.set_desired_position, 2)
        #Start PID errors update errors. Update 100 timers a second
        self.pid_error_update_timer.start(10)
예제 #2
0
    def __init__(self):
        '''
        Initialize the mission planning widget.

        Parameters:
            N/A
        Returns:
            N/A
        '''
        QWidget.__init__(self)

        #Get mechos network configurations
        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #MechOS parameter server (this is where we will save the waypoint file)
        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Call in the ui for mission select
        self.mission_select_widget = uic.loadUi("mission_select.ui", self)

        self.mission_select_node = mechos.Node("MISSION_SELECT_GUI",
                                               '192.168.1.2', '192.168.1.14')
        self.update_mission_info_publisher = self.mission_select_node.create_publisher(
            "MISSON_SELECT", Bool(), protocol="tcp")

        #Connect the mission select button to update the mission in the parameter
        #server and tell the mission commander that the mission file has changed.
        self.mission_select_widget.save_mission_btn.clicked.connect(
            self._update_mission_file)
        currently_set_mission_file = self.param_serv.get_param(
            "Missions/mission_file")
        self.mission_select_widget.mission_file_line_edit.setText(
            currently_set_mission_file)

        self._update_mission_file()

        self.linking_layout = QGridLayout(self)
        self.setLayout(self.linking_layout)

        self.setMinimumSize(449, 330)
예제 #3
0
    def __init__(self):
        '''
        Initialize the mission given the mission .json file.
        Parameters:
            sensor_driver: The sensor driver thread object so
            the drive functions have access to the sensor data.
        Returns:
            N/A
        '''

        threading.Thread.__init__(self)

        self.mission_file = None

        #Initialize the drive functions
        self.drive_functions = Drive_Functions()

        #Get the mechos network parameters
        configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #Connect to parameters server
        self.param_serv = mechos.Parameter_Server_Client(configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #MechOS node to connect the mission commander to the mechos network
        self.mission_commander_node = mechos.Node("MISSION_COMMANDER", '192.168.1.14', '192.168.1.14')

        #subscriber to listen if the movement mode is set to be autonomous mission mode
        self.movement_mode_subscriber = self.mission_commander_node.create_subscriber("MOVEMENT_MODE", Int(), self._update_movement_mode_callback, protocol="tcp")
        #subscriber to listen if the mission informatin has changed.
        self.update_mission_info_subscriber = self.mission_commander_node.create_subscriber("MISSON_SELECT", Bool(), self._update_mission_info_callback, protocol="tcp")
        #subscriber to listen if neural network data is available
        self.neural_network_subscriber = self.mission_commander_node.create_subscriber("NEURAL_NET", Neural_Network_Message(), self._update_neural_net_callback, protocol="tcp")

        self.neural_net_data = [0, 0, 0, 0, 0, 0]

        #Publisher to be able to kill the sub within the mission
        self.kill_sub_publisher = self.mission_commander_node.create_publisher("KILL_SUB", Bool(), protocol="tcp")

        #Publisher to zero the position of the sub.
        self.zero_position_publisher = self.mission_commander_node.create_publisher("ZERO_POSITION", Bool(), protocol="tcp")

        #Set up serial com to read the autonomous button
        com_port = self.param_serv.get_param("COM_Ports/auto")
        self.auto_serial = serial.Serial(com_port, 9600)

        #Set up a thread to listen to request from the GUI
        self.command_listener_thread = threading.Thread(target=self._command_listener)
        self.command_listener_thread.daemon = True
        self.command_listener_thread_run = True
        self.command_listener_thread.start()

        self.mission_tasks = [] #A list of the mission tasks
        self.mission_data = None #The .json file structure loaded into python dictionary

        self.run_thread = True
        self.daemon = True

        self.mission_mode = False #If true, then the subs navigation system is ready for missions
        self.mission_live = False  #Mission live corresponds to the autonomous buttons state.

        #Variable to keep the current sensor data(position) of the sub.
        self.current_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


        #load the mission data
        self._update_mission_info_callback(None)
    def __init__(self):
        '''
        Initializes a Tabbed Display widget.

        Parameters:
            individual_tab: The individual_tab Qwidget
        '''
        QWidget.__init__(self)
        self.layout = QVBoxLayout(self)

        # Set the background color of the widget
        #tabbed_display_palette = self.palette()
        #tabbed_display_palette.setColor(self.backgroundRole(), QColor(64, 64, 64))
        #self.setPalette(tabbed_display_palette)

        # Initialize tab screen
        self.tabs = QTabWidget()
        self.tabs.setFixedSize(700, 700)

        # Set color of QTabWidget
        #tabs_palette = self.tabs.palette()
        #tabs_palette.setColor(QPalette.Window, QColor(64, 64, 64))
        #self.tabs.setPalette(tabs_palette)

        # Add tabs to layout
        self.layout.addWidget(self.tabs)
        self.setLayout(self.layout)

        # Call method _update_mode() when tab is changed
        self.tabs.currentChanged.connect(self._update_mode)

        # Create MechOS node
        configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
        self.tab_display_node = mechos.Node("MOVEMENT_MODE_TABS_GUI", '192.168.1.2', '192.168.1.14')
        self.movement_mode_publisher = self.tab_display_node.create_publisher("MOVEMENT_MODE", Int(), protocol="tcp")

        #Publisher to kill the sub when tabs are switched.
        self.sub_killed_publisher = self.tab_display_node.create_publisher("KILL_SUB", Bool(), protocol="tcp")
예제 #5
0
    def __init__(self):
        '''
            Initialize the layout for the widget by setting its color and instantiating
            its components.

            Parameter:
            N/A

            Returns:
            N/A
        '''
        QWidget.__init__(self)
        self.title = "Kill Switch"

        #Set background color of the widget
        #nav_gui_palette = self.palette()
        #nav_gui_palette.setColor(self.backgroundRole(), QColor(64, 64, 64))
        #self.setPalette(nav_gui_palette)

        #Create widgets main layout
        self.linking_layout = QGridLayout(self)
        self.linking_layout.setAlignment(Qt.AlignCenter)
        self.setLayout(self.linking_layout)
        self.setWindowTitle(self.title)

        #Set the value for kill status
        self.KILL_STATUS = "killed"

        #Init the button
        self.pushButton = QPushButton('Kill Sub', self)
        self.pushButton.setStyleSheet("background-color: red")

        #Connect the update function
        self.pushButton.clicked.connect(self._update_status)


        #Create MechOS node
        configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
        self.sub_killed_node = mechos.Node("KILL_SUB_GUI", '192.168.1.2', '192.168.1.14')
        self.sub_killed_publisher = self.sub_killed_node.create_publisher("KILL_SUB", Bool(), protocol="tcp")

        #Also create a killed button subscriber in case the sub kills it's self, so that way the
        #state changes in the GUI.
        self.sub_killed_subscriber = self.sub_killed_node.create_subscriber("KILL_SUB", Bool(), self._sub_killed_callback, protocol="tcp")

        #Set up a QTimer to update the PID errors
        self.sub_killed_subscriber_update_timer = QTimer()
        self.sub_killed_subscriber_update_timer.timeout.connect( \
                    lambda: self.sub_killed_node.spin_once())

        #Start the timer to check if the subs killed state has changed (every 100ms)
        self.sub_killed_subscriber_update_timer.start(100)
예제 #6
0
    def __init__(self):
        '''
        Initialize the Desired Position GUI

        Parameters:
            N/A
        '''
        QWidget.__init__(self)

        #Set the background color of the widget
        #nav_gui_palette = self.palette()
        #nav_gui_palette.setColor(self.backgroundRole(), QColor(64, 64, 64))
        #self.setPalette(nav_gui_palette)

        #Set up MechOS network configurations
        configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #MechOS node to receive data from the sub and display it
        self.set_position_node = mechos.Node("SET_POSITION_GUI", '192.168.1.2', '192.168.1.14')
        self.set_position_pub = self.set_position_node.create_publisher("DESIRED_POSITION", Desired_Position_Message(), protocol="tcp")
        self.zero_position_pub = self.set_position_node.create_publisher("ZERO_POSITION", Bool(), protocol="tcp")

        self.linking_layout = QVBoxLayout(self)
        self.setLayout(self.linking_layout)
        self._desired_position_inputs()

        self.desired_position = [0, 0, 0, 0, 0, 0]
예제 #7
0
    def __init__(self):
        QWidget.__init__(self)

        #Set the background color of the widget
        #waypoint_gui_palette = self.palette()
        #waypoint_gui_palette.setColor(self.backgroundRole(), QColor(64, 64, 64))
        #self.setPalette(waypoint_gui_palette)

        #Get mechos network configurations
        configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #MechOS parameter server (this is where we will save the waypoint file)
        self.param_serv = mechos.Parameter_Server_Client(configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Call in ui waypoint_widget design (build in QtDesigner)
        self.waypoint_widget = uic.loadUi("waypoint_widget.ui", self)

        self.waypoint_widget.enable_waypoint_collection_checkbox.stateChanged.connect(self._update_waypoint_enable)

        self.waypoint_widget.enable_waypoint_collection_checkbox.setChecked(False)
        #Create a mechos network publisher to publish the enable state of the
        #waypoints
        self.waypoint_node = mechos.Node("WAYPOINT_COLLECTION_GUI", '192.168.1.2', '192.168.1.14')
        self.waypoint_control_publisher = self.waypoint_node.create_publisher("ENABLE_WAYPOINT_COLLECTION", Bool(), protocol="tcp")

        #Connect button to save button to the parameter server.
        self.waypoint_widget.save_waypoint_file_btn.clicked.connect(self._update_save_waypoint_file)
        currently_set_waypoint_file = self.param_serv.get_param("Missions/waypoint_collect_file")
        self.waypoint_widget.waypoint_file_line_edit.setText(currently_set_waypoint_file)

        #Send disable waypoint message on start up.
        self._update_waypoint_enable()

        self.linking_layout = QGridLayout(self)
        self.setLayout(self.linking_layout)
        #self.linking_layout.addWidget(self.waypoint_widget, 0, 0)

        self.setMinimumSize(449, 330)

        #Start the remote control thread
        #Note: You MUST have the logitech plugged in.
        self.remote_control = Remote_Control_Input()
        self.remote_control.start()
예제 #8
0
    def __init__(self):
        '''
        Initialize all of the threads for gathering data from each of the sensors.
        This includes AHRS, Backplane, Pressure Transducers, and DVL.

        Parameters:
            N/A
        Returns:
            N/A
        '''

        threading.Thread.__init__(self)
        #Get the mechos network parameters
        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #Mechos nodes to send Sensor Data
        self.sensor_driver_node = mechos.Node("SENSOR_DRIVER", '192.168.1.14',
                                              '192.168.1.14')

        #Publisher to publish the sensor data to the network.
        #Sensd [roll, pitch, yaw, north pos., east pos., depth]
        self.nav_data_publisher = self.sensor_driver_node.create_publisher(
            "SENSOR_DATA", Float_Array(6), protocol="udp", queue_size=1)

        #Subscriber to receive a message to zero position of the currrent north/east position of the sub.
        self.zero_pos_subscriber = self.sensor_driver_node.create_subscriber(
            "ZERO_POSITION",
            Bool(),
            self._update_zero_position,
            protocol="tcp")

        #MechOS node to receive zero position message (zero position message is sent in the DP topic)
        #self.zero_pos_sub = self.sensor_driver_node.create_subscriber("DP", self._zero_pos_callback, configs["sub_port"])
        #self.zero_pos_proto = desired_position_pb2.DESIRED_POS() #Protocol buffer for receiving the zero position flag

        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Get com ports to connect to sensors
        backplane_com_port = self.param_serv.get_param("COM_Ports/backplane")
        ahrs_com_port = self.param_serv.get_param("COM_Ports/AHRS")
        dvl_com_port = self.param_serv.get_param("COM_Ports/DVL")

        #Initialize the backplane handler
        self.backplane_driver_thread = Backplane_Handler(backplane_com_port)

        #Initialize ahrs handler
        self.ahrs_driver_thread = AHRS(ahrs_com_port)

        #Initialize DVL thread
        self.dvl_driver_thread = DVL_THREAD(dvl_com_port)

        #Threading lock to access shared thread data safely
        self.threading_lock = threading.Lock()
        self.run_thread = True

        #Start sensor gathering threads
        self.backplane_driver_thread.start()
        self.ahrs_driver_thread.start()
        self.dvl_driver_thread.start()

        self.current_north_pos = 0
        self.current_east_pos = 0

        #Previous time at which the dvl was read, keeps consistent timining of the dvl
        self.prev_dvl_read_time = 0
        self.sensor_data = [0, 0, 0, 0, 0, 0]

        self.run_thread = True
        self.daemon = True
    def __init__(self):
        '''
        Initialize the navigation controller. This includes getting parameters from the
        parameter server and initializing subscribers to listen for command messages and sensor data

        Parameters:
            N/A
        Returns:
            N/A
        '''

        threading.Thread.__init__(self)
        self.daemon = True

        #Get the mechos network parameters
        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #MechOS node to connect movement controller to mechos network
        #The ip that the sub and mechoscore runs on are both 192.168.1.14
        self.navigation_controller_node = mechos.Node("NAVIGATION_CONTROLLER",
                                                      '192.168.1.14',
                                                      '192.168.1.14')

        #Subscribe to remote commands
        self.remote_control_subscriber = self.navigation_controller_node.create_subscriber(
            "REMOTE_CONTROL_COMMAND",
            Remote_Command_Message(),
            self._read_remote_control,
            protocol="udp",
            queue_size=1)

        #Subscriber to change movement mode
        self.movement_mode_subscriber = self.navigation_controller_node.create_subscriber(
            "MOVEMENT_MODE",
            Int(),
            self.__update_movement_mode_callback,
            protocol="tcp")

        #Update PID configurations button
        self.pid_configs_subscriber = self.navigation_controller_node.create_subscriber(
            "UPDATE_PID_CONFIGS",
            Bool(),
            self.__update_pid_configs_callback,
            protocol="tcp")

        #Subscriber to get the desired position set by the user/mission controller.
        self.desired_position_subscriber = self.navigation_controller_node.create_subscriber(
            "DESIRED_POSITION",
            Desired_Position_Message(),
            self.__unpack_desired_position_callback,
            protocol="tcp")

        #Subscriber to see if waypoint recording is enabled
        self.enable_waypoint_collection_subscriber = self.navigation_controller_node.create_subscriber(
            "ENABLE_WAYPOINT_COLLECTION",
            Bool(),
            self.__update_enable_waypoint_collection,
            protocol="tcp")

        #Subscriber to listen for thrust messages from the thruster test widget
        self.thruster_test_subscriber = self.navigation_controller_node.create_subscriber(
            "THRUSTS",
            Thruster_Message(),
            self.__update_thruster_test_callback,
            protocol="tcp")

        #Connect to parameters server
        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Subscriber to receive sensor data from the sensor driver node.
        self.nav_data_subscriber = self.navigation_controller_node.create_subscriber(
            "SENSOR_DATA",
            Float_Array(6),
            self.__update_sensor_data,
            protocol="udp",
            queue_size=1)

        #Subscriber to commands from the GUI and Mission commanderto listen if the sub is killed.
        self.sub_killed_subscriber = self.navigation_controller_node.create_subscriber(
            "KILL_SUB", Bool(), self._update_sub_killed_state, protocol="tcp")

        #Publisher to zero the NORTH/EAST position of the sub.
        self.zero_position_publisher = self.navigation_controller_node.create_publisher(
            "ZERO_POSITION", Bool(), protocol="tcp")

        #Get navigation controller timing
        self.nav_time_interval = float(
            self.param_serv.get_param("Timing/nav_controller"))
        self.nav_timer = util_timer.Timer()

        #Initial movement mode to match GUI.
        #0 --> PID tuner
        #1 --> Thruster tester
        self.movement_mode = 0

        #Allow the naviagtion controller thread to run
        self.run_thread = True

        #1--> Thrusters are softkilled (by software)
        #0--> Thrusters are unkilled and can be commanded.
        self.sub_killed = 1

        #Initialize 6 degree of freedom PID movement controller used for the sub.
        #Primary control system for the sub
        self.pid_controller = Movement_PID()

        #Initialize current position [roll, pitch, yaw, north_pos, east_pos, depth]
        self.current_position = [0, 0, 0, 0, 0, 0]
        self.pos_error = [0, 0, 0, 0, 0, 0]  #errors for all axies

        #Initialize desired position [roll, pitch, yaw, north_pos, east_pos, depth]
        self.desired_position = [0, 0, 0, 0, 0, 0]

        #Set up a thread to listen to a requests from GUI/mission_commander.
        # This includes movement mode, desired_position, new PID values, and sub killed command.
        self.command_listener_thread = threading.Thread(
            target=self._command_listener)
        self.command_listener_thread.daemon = True
        self.command_listener_thread_run = True
        self.command_listener_thread.start()

        #A thread to update the GUI/mission commander on the current position of the sub
        # and the current PID errors if the sub is in PID tunning mode
        self.update_command_thread = threading.Thread(
            target=self._update_command)
        self.update_command_thread.daemon = True
        self.update_command_thread_run = True
        self.update_command_thread.start()

        self.remote_commands = [0.0, 0.0, 0.0, 0.0, 0]
        self.waypoint_file = None
        self.enable_waypoint_collection = False

        self.daemon = True

        print("[INFO]: Sub Initially Killed")