def __init__(self):
'''
Initialize the navigation and odometery data display widget.
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)
configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#MechOS node to receive data from the sub and display it
self.sensor_data_node = mechos.Node("NAVIGATION_ODOMETRY_GUI", '192.168.1.2', '192.168.1.14')
self.nav_data_subscriber = self.sensor_data_node.create_subscriber("SENSOR_DATA", Float_Array(6), self._update_nav_data, protocol="udp", queue_size=1)
self.linking_layout = QVBoxLayout(self)
self.setLayout(self.linking_layout)
self._orientation_layout_grid()
self._earth_pos_layout_grid()
self._relative_pos_layout_grid()
#Create a timer to update the data
self.update_nav_data_timer = QTimer()
self.update_nav_data_timer.timeout.connect(lambda: self.sensor_data_node.spin_once())
self.update_nav_data_timer.start(1)
def __init__(self):
'''
Initialize the input axes and the node, prepare for message sending
Parameters:
IP: dictionary which holds all the necessary components for network
transfer, including IP address, sockets, etc
MEM: dictionary used for local message transfer
Returns:
N/A
'''
threading.Thread.__init__(self)
self.daemon = True
self.remote_control_send_node = mechos.Node("REMOTE_CONTROL_SEND",
'192.168.1.2',
'192.168.1.14')
self.remote_control_publisher = self.remote_control_send_node.create_publisher(
'REMOTE_CONTROL_COMMAND',
Remote_Command_Message(),
protocol="udp",
queue_size=1)
#Disgusting pygame stuff
pygame.init()
pygame.joystick.init()
self._joystick = pygame.joystick.Joystick(0)
self._joystick.init()
self._axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0]
self._remote_depth_hold = False
self._record_waypoint = False
self._zero_waypoint = False
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):
'''
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 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):
'''
'''
self.desired_position_proto = desired_position_pb2.DESIRED_POS()
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Create a MechOS publisher to send desired position data
self.position_setter_node = mechos.Node(
"DESIRED_POSITION_SETTER_HELPER", '192.168.1.2', '192.168.1.14')
self.position_setter_publisher = self.position_setter_node.create_publisher(
"DESIRED_POSITION", Desired_Position_Message(), protocol="tcp")
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 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):
'''
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):
'''
Parameters:
N/A
Returns:
N/A
'''
self.timeout_timer = util_timer.Timer()
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Create mechos node
self.drive_functions_node = mechos.Node("DRIVE_FUNCTIONS",
'192.168.1.14', '192.168.1.14')
self.desired_position_publisher = self.drive_functions_node.create_publisher(
"DESIRED_POSITION", Desired_Position_Message(), protocol="tcp")
self.nav_data_subscriber = self.drive_functions_node.create_subscriber(
"SENSOR_DATA",
Float_Array(6),
self.__update_sensor_data,
protocol="udp",
queue_size=1)
#Start a thread to listen for sensor data.
self.sensor_data_thread = threading.Thread(
target=self._update_sensor_data_thread, daemon=True)
self.sensor_data_thread.start()
#A boolean to specifiy if the drive functions are availible to run
#This is used in case missions are canceled and does not want these functions to be able to use.
self.drive_functions_enabled = True
self.sensor_data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.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)
#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 structer
self.linking_layout = QGridLayout(self)
self.setLayout(self.linking_layout)
#Set up sub widgets for check boxes and sliders
self._thruster_check_boxes()
self._thruster_slider()
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#MechOS publisher to send thrust test messages to thruster controller
self.thruster_test_node = mechos.Node("THRUSTER_TEST_GUI",
'192.168.1.2', '192.168.1.14')
self.publisher = self.thruster_test_node.create_publisher(
"THRUSTS", Thruster_Message(), protocol="tcp")
self.thrusts = [0, 0, 0, 0, 0, 0, 0, 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 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):
'''
Initialize the movement controller. This includes connecting publishers
and subscribers to the MechOS network to configure and control the movement
operation.
Parameters:
N/A
Returns:
N/A
'''
#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
self.movement_controller_node = mechos.Node("MOV_CTRL", configs["ip"])
#Subscriber to change movement mode
self.movement_mode_subscriber = self.movement_controller_node.create_subscriber(
"MM", self.__update_movement_mode_callback, configs["sub_port"])
#Subscriber to get the desired position set by the user/mission controller.
self.desired_position_subscriber = self.movement_controller_node.create_subscriber(
"DP", self.__unpack_desired_position_callback, configs["sub_port"])
self.pid_errors_proto = pid_errors_pb2.PID_ERRORS()
#Publisher that published the PID errors for each degree of freedom
self.pid_errors_publisher = self.movement_controller_node.create_publisher(
"PE", configs["pub_port"])
#Subscriber to listen for thrust messages from the thruster test widget
self.thruster_test_subscriber = self.movement_controller_node.create_subscriber(
"TT", self.__update_thruster_test_callback, configs["sub_port"])
self.thruster_test_proto = thrusters_pb2.Thrusters(
) #Thruster protobuf message
#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"])
#TODO: Remove Position Estimator from Dynamics
#Initialize the position estimator thread. The position estimator
#will estimate the real time current position of the sub with respect to
#the origin set.
#self.position_estimator_thread = Position_Estimator()
#self.position_estimator_thread.start()
#Proto buffer containing all of the navigation data
self.nav_data_proto = navigation_data_pb2.NAV_DATA()
self.current_position_subscriber = self.movement_controller_node.create_subscriber(
"NAV", self.__get_position_callback, configs["sub_port"])
#Get movement controller timing
self.time_interval = float(
self.param_serv.get_param("Timing/movement_control"))
self.movement_mode = 1
self.run_thread = True
#Initialize 6 degree of freedom PID movement controller used for the sub.
self.pid_controller = Movement_PID()
#Initialize current position
self.current_position = [0, 0, 0, 0, 0, 0]
#Initialize desired position
self.desired_position = [0, 0, 0, 0, 0, 0]
self.desired_position_proto = desired_position_pb2.DESIRED_POS()
#Set up thread to update PID values. The GUI has the ability to change
#the proportional, integral, and derivative constants by setting them in
#the parameter server. These values should only be checked in the PID tunning
#modes.
self.pid_values_update_thread = threading.Thread(
target=self.__update_pid_values)
self.pid_values_update_thread.daemon = True
self.pid_values_update_thread_run = False
#Set up a thread to listen to a movement mode change.
self.movement_mode_thread = threading.Thread(
target=self.update_movement_mode_thread)
self.movement_mode_thread.daemon = True
self.movement_mode_thread_run = True
self.movement_mode_thread.start()
import numpy as np
from MechOS import mechos
import argparse
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
"--thrust",
type=int,
nargs=8,
help=
"Input the thrust percentage you want to test at. Range [-100, 100]",
required=True)
args = parser.parse_args()
print(args.thrust)
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#MechOS publisher to send thrust test messages to thruster controller
thruster_test_node = mechos.Node("THRUSTER_TEST_HELPER", '192.168.1.2',
'192.168.1.14')
publisher = thruster_test_node.create_publisher("THRUSTS",
Thruster_Message(),
protocol="tcp")
publisher.publish(args.thrust)
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")
def __init__(self, MEM, IP):
'''
Initializes values for encoded image streaming, begins zed capture and
loads in the neural network.
Parameters:
MEM: Dictionary containing Node name and the desired local memory location.
IP: Dictionary containing Node name and desired streaming settings: The IP address, send
and recieve sockets, and the streaming protocal.
'''
# IP and MEM RAM locations
node_base.__init__(self, MEM, IP)
# Instantiations
configs = MechOS_Network_Configs(MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
self.param_serv = mechos.Parameter_Server_Client(configs["param_ip"], configs["param_port"])
self.param_serv.use_parameter_database(configs["param_server_path"])
self.vision_node= mechos.Node("VISION", "192.168.1.14", "192.168.1.14")
self.neural_net_publisher = self.vision_node.create_publisher("NEURAL_NET", Neural_Network_Message(), protocol="tcp")
#self.neural_net_publisher = self.neural_network_node.create_publisher("NN", configs["pub_port"])
self.neural_net_timer = float(self.param_serv.get_param("Timing/neural_network"))
#--MESSAGING INFO--#
self.MAX_UDP_PACKET_SIZE = 1500
# The end byte of the image sent over udp
self.END_BYTE = bytes.fromhex('c0c0')*2
# the Max Packet Size the Port will accept
self.MAX_PACKET_SIZE = 1500
#--CAMERA INSTANCE--(using the zed camera)#
#front_camera_index = int(self.param_serv.get_param("Vision/front_camera_index"))
#Create a zed camera object
self.zed = sl.Camera()
self.zed_init_params = sl.InitParameters()
self.zed_init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720
self.zed_init_params.camera_fps = 30 #set fps at 30 fps
self.zed_init_params.depth_mode = sl.DEPTH_MODE.DEPTH_MODE_NONE
#Open the zed camera
err = self.zed.open(self.zed_init_params)
if(err != sl.ERROR_CODE.SUCCESS):
exit(1)
# Neural Network Loaded from instance in darknet module
darknet_path = self.param_serv.get_param("Vision/yolo/darknet_path")
config_file = self.param_serv.get_param("Vision/yolo/config_file")
weights_file = self.param_serv.get_param("Vision/yolo/weights_file")
metadata_file = self.param_serv.get_param("Vision/yolo/metadata_file")
config_file_path = (os.path.join(darknet_path, config_file)).encode()
weights_file_path = (os.path.join(darknet_path, weights_file)).encode()
metadata_file_path = (os.path.join(darknet_path, metadata_file)).encode()
print(config_file_path, weights_file_path)
self.net = load_net(config_file_path,
weights_file_path,
0)
self.meta = load_meta(metadata_file_path)
#Solvepnp distance calculator.
self.distance_calculator = Distance_Calculator()
