def __init__(self, backplane_serial_obj):
'''
Initialize the backplane response thread to connect through serial to
backplane.
Parameters:
backplane_serial_obj: An already initialized serial communication object
to the backplane.
Returns:
N/A
'''
threading.Thread.__init__(self)
self.backplane_serial = backplane_serial_obj
self.header_byte = 0xEE
self.run_thread = True
self.daemon = True
#Get the mechos network parameters
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.backplane_response_timer = util_timer.Timer()
self.backplane_response_timer_interval = float(
self.param_serv.get_param("Timing/backplane_response"))
#A list(Queue) to store data received from backplane
self.backplane_data_queue = []
def __init__(self, com_port):
'''
Initialize communcication path with the Sparton AHRS.
Parameters:
com_port: The serial communication port communicating with the AHRS
board.
'''
super(AHRS, self).__init__()
#serial communication port of the AHRS.
self.ahrs_com_port = com_port
#create object for Sparton AHRS data packets
self.sparton_ahrs = SpartonAHRSDataPackets(com_port)
#Get the mechos network parameters
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"])
#Initialize a timer for consistent timing on data
self.ahrs_timer_interval = float(self.param_serv.get_param("Timing/AHRS"))
self.ahrs_timer = util_timer.Timer()
self.threading_lock = threading.Lock()
self.run_thread = True
self.daemon = True
self.ahrs_data = [0, 0, 0] #roll, pitch, yaw
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 thrusters and PID controllers on Perseverance.
Parameters:
error_publisher: A MechOS publisher to send the publish the current error
Returns:
N/A
'''
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Initialize parameter server client to get and set parameters related to
#the PID controller. This includes update time and PID contstants for
#each degree of freedom.
self.param_serv = mechos.Parameter_Server_Client(
configs["param_ip"], configs["param_port"])
self.param_serv.use_parameter_database(configs["param_server_path"])
#Initialize serial connection to the maestro
com_port = self.param_serv.get_param("COM_Ports/maestro")
maestro_serial_obj = serial.Serial(com_port, 9600)
#Initialize all 8 thrusters (max thrust 80%)
max_thrust = float(self.param_serv.get_param("Control/max_thrust"))
self.thrusters = [None, None, None, None, None, None, None, None]
self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
[1, -1, 0], max_thrust, True)
self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
[1, 0, 0], max_thrust, False)
self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
[1, 1, 0], max_thrust, False)
self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
[0, 1, 0], max_thrust, False)
self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
[-1, 1, 0], max_thrust, True)
self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
[-1, 0, 0], max_thrust, False)
self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
[-1, -1, 0], max_thrust, False)
self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
[0, -1, 0], max_thrust, False)
#Initialize the PID controllers for control system
self.set_up_PID_controllers()
def __init__(self):
#Get the mechos network parameters
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"])
backplane_com_port = self.param_serv.get_param("COM_Ports/backplane")
self.backplane_driver_thread = Backplane_Handler(backplane_com_port)
self.threading_lock = threading.Lock()
self.run_thread = True
self.backplane_driver_thread.start()
time.sleep(1.5) #Makes sure Backplane is up and running
def __init__(self, com_port):
'''
Initialize serial connection to the backplane, start the backplane response
thread.
Parameters:
com_port: The serial communication port that the backplane is connected
to.
Returns:
N/A
'''
threading.Thread.__init__(self)
backplane_serial_obj = serial.Serial(com_port, 9600)
#Initialize object request for data to the backplane
self.backplane_requests = Backplane_Requests(backplane_serial_obj)
#Initialize thread object for queuing up data received from backplane
self.backplane_response_thread = Backplane_Responses(
backplane_serial_obj)
self.depth_processing = Pressure_Depth_Transducers()
#Get the mechos network parameters
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.backplane_handler_timer = util_timer.Timer()
self.backplane_handler_timer_interval = float(
self.param_serv.get_param("Timing/backplane_handler"))
#start backplane response thread
self.backplane_response_thread.start()
self.threading_lock = threading.Lock()
self.run_thread = True
self.daemon = True
self.depth_data = 0.0
self.raw_depth_data = [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 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 communication with each pressure transducer. And set up MechOS
node to communicate data over the MechOS network.
Parameters:
N/A
Returns:
N/A
'''
#Initialize base classes
super(Pressure_Depth_Transducers, self).__init__()
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"])
#This is the variable that you append raw pressure data to once it is
#received from the backplane.
self.depth_scaling = [
float(self.param_serv.get_param("Sensors/trans_1_scaling")),
float(self.param_serv.get_param("Sensors/trans_2_scaling"))
]
self.depth_bias = [
float(self.param_serv.get_param("Sensors/trans_1_bias")),
float(self.param_serv.get_param("Sensors/trans_2_bias"))
]
#Initialize Kalman Filter Parameters( Note this needs to be edited per type of transducer and number of transducers)
#Currently set up for two transducers
A = np.array([[1]])
B = np.array([[0]])
R = np.array([[1e-6]])
C = np.array([[1], [1]])
Q = np.array([[1e-5, 0], [0, 1e-5]])
self.kf = Kalman_Filter(A, B, R, C, Q)
#Set initial conditions for Kalman filter state
self.mu = np.array([[0]])
self.cov = np.array([[10]])
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 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()
def __init__(self):
'''
Initialize the thrusters and PID controllers on Perseverance.
Parameters:
N/A
Returns:
N/A
'''
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
#Initialize parameter server client to get and set parameters related to
#the PID controller. This includes update time and PID contstants for
#each degree of freedom.
self.param_serv = mechos.Parameter_Server_Client(
configs["param_ip"], configs["param_port"])
self.param_serv.use_parameter_database(configs["param_server_path"])
#Initialize serial connection to the maestro
com_port = self.param_serv.get_param("COM_Ports/maestro")
maestro_serial_obj = serial.Serial(com_port, 9600)
#Initialize all 8 thrusters (max thrust 80%)
max_thrust = float(self.param_serv.get_param("Control/max_thrust"))
self.thrusters = [None, None, None, None, None, None, None, None]
self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
[1, -1, 0], max_thrust, True)
self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
[1, 0, 0], max_thrust, True)
self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
[1, 1, 0], max_thrust, False)
self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
[0, 1, 0], max_thrust, False)
self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
[-1, 1, 0], max_thrust, True)
self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
[-1, 0, 0], max_thrust, True)
self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
[-1, -1, 0], max_thrust, False)
self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
[0, -1, 0], max_thrust, True)
#Used to notify when to hold depth based on the remote control trigger for depth.
self.remote_desired_depth = 0
self.remote_depth_recorded = False
self.remote_yaw_min_thrust = float(
self.param_serv.get_param("Control/Remote/yaw_min"))
self.remote_yaw_max_thrust = float(
self.param_serv.get_param("Control/Remote/yaw_max"))
self.remote_x_min_thrust = float(
self.param_serv.get_param("Control/Remote/x_min"))
self.remote_x_max_thrust = float(
self.param_serv.get_param("Control/Remote/x_max"))
self.remote_y_min_thrust = float(
self.param_serv.get_param("Control/Remote/y_min"))
self.remote_y_max_thrust = float(
self.param_serv.get_param("Control/Remote/y_max"))
#Initialize the PID controllers for control system
self.set_up_PID_controllers(True)
def __init__(self):
'''
Initializes my distance calculator
Params:
N/A
Returns:
N/A
'''
self.detection = None
self.x_coordinate = None
self.second_x_coordinate = None
self.y_coordinate = None
self.second_y_coordinate = None
self.width = None
self.second_width = None
self.height = None
self.second_height = None
self.difference = None
self.threshold = None
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.focal_length_x = float(
self.param_serv.get_param(
"Vision/solvePnP/camera_matrix/focal_length_x"))
self.focal_length_y = float(
self.param_serv.get_param(
"Vision/solvePnP/camera_matrix/focal_length_y"))
self.optical_center_x = float(
self.param_serv.get_param(
"Vision/solvePnP/camera_matrix/optical_center_x"))
self.optical_center_y = float(
self.param_serv.get_param(
"Vision/solvePnP/camera_matrix/optical_center_y"))
self.camera_matrix = np.array(
[[self.focal_length_x, 0.0, self.optical_center_x],
[0.0, self.focal_length_y, self.optical_center_y],
[0.0, 0.0, 1.0]])
self.distort_var_1 = float(
self.param_serv.get_param("Vision/solvePnP/distortion_matrix/k1"))
self.distort_var_2 = float(
self.param_serv.get_param("Vision/solvePnP/distortion_matrix/k2"))
self.distort_var_3 = float(
self.param_serv.get_param("Vision/solvePnP/distortion_matrix/p1"))
self.distort_var_4 = float(
self.param_serv.get_param("Vision/solvePnP/distortion_matrix/p2"))
self.distort_var_5 = float(
self.param_serv.get_param("Vision/solvePnP/distortion_matrix/k3"))
self.distortion_matrix = np.array([[
self.distort_var_1, self.distort_var_2, self.distort_var_3,
self.distort_var_4, self.distort_var_5
]])
self.two_dim_points = None
self.three_dim_points = None
#if pressure data is popped from queue, process it
if "Press" in backplane_data.keys():
raw_depth_data = backplane_data["Press"]
depth_data = self.depth_processing.process_depth_data(
raw_depth_data)
if (depth_data != None):
with self.threading_lock:
self.raw_depth_data = raw_depth_data
self.depth_data = depth_data[0, 0]
except Exception as e:
print("[ERROR]: Cannot pop backplane data. Error:", e)
if __name__ == "__main__":
#Get the mechos network parameters
configs = MechOS_Network_Configs(
MECHOS_CONFIG_FILE_PATH)._get_network_parameters()
param_serv = mechos.Parameter_Server_Client(configs["param_ip"],
configs["param_port"])
param_serv.use_parameter_database(configs["param_server_path"])
com_port = param_serv.get_param("COM_Ports/backplane")
backplane_handler = Backplane_Handler(com_port)
backplane_handler.run()
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()
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 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")
