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