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)
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)
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")
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)
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]
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()
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")