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, k_p, k_i, k_d, d_t, l_bound=None, u_bound=None):
'''
Initialize the the PID controller parameters.
Parameters:
k_p: Proportional gain
k_i: Integral gain
k_d: Derivative gain
d_t: Time interval between calculating to output control
l_bound: Lower bound for PID output (whats the lowest thrust to drive.)
u_bound: Upper bound for PID output (whats the maximum thrsust to drive.)
Returns:
N/A
'''
self.k_p = k_p
self.k_i = k_i
self.k_d = k_d
self.d_t = d_t
self.l_bound = l_bound
self.u_bound = u_bound
self.integral = 0.0
self.integral_min = -5.0
self.integral_max = 5.0
self.previous_error = 0.0
#Timer to check difference time in between control calculations
self.PID_timer = util_timer.Timer()
def __init__(self, task_dict,drive_functions):
'''
Initialize the Gate task give the task description dictionary.
Parameters:
task_dict: A python dictionary containing the parameters for
the waypoint task.
Dictionary Form:
---------------------------
{
"type": "Gate_No_Visision"
"name": <task_name>
"timeout": <timeout_time_minutes>
"line_up_position":<the Yaw/North/East/Depth position to line up too infront of the gate>
"stabilization_time": <The time (sec) to wait for stabilization at line_up_position>
"move_forward_dist": <The number of feet to move forward in the x direction of the sub to drive through the gate>
}
drive_functions: An already initialized object of drive functions. This
class contains a variety of drive functions that are useful to complete
tasks.
'''
Task.__init__(self)
self.task_dict = task_dict
#Unpack the information about the task
self.name = self.task_dict["name"]
self.type = "Gate_No_Vision"
self.timeout = self.task_dict["timeout"] * 60.0 #In Seconds now.
#Timer to be used to keep track of how long the task has been going.
self.timeout_timer = util_timer.Timer()
self.line_up_position = self.task_dict["line_up_position"]
self.position_buffer_zone = self.task_dict["position_buffer_zone"]
self.depth_buffer_zone = self.task_dict["depth_buffer_zone"]
self.yaw_buffer_zone = self.task_dict["yaw_buffer_zone"]
self.stabilization_time = self.task_dict["stabilization_time"]
self.move_forward_dist = self.task_dict["move_forward_dist"]
self.drive_functions = drive_functions
#If true, then the sub will attempt to go through the gate backwards
self.go_through_gate_backwards = self.task_dict["go_through_gate_backwards"]
if(self.go_through_gate_backwards):
self.move_forward_dist *= -1 #Reverse the direction to go forward to go backwards
desired_yaw = self.line_up_position[0] + 180 #Flip the yaw 180 deg to go backwards
if(desired_yaw >= 360):
self.line_up_position[0] = 360 - desired_yaw
print(self.line_up_position[0])
else:
self.line_up_position[0] = desired_yaw
def __init__(self, task_dict, drive_functions):
'''
Initialize the waypoint task given the waypoint dict.
Parameters:
task_dict: A python dictionary containing the parameters
for the waitpoint task.
Dictionary Form:
-------------------
{
"type": "Waypoint"
"name": <task_name>
"timeout": <timeout_time_minutes>
"position_buffer_zone": <buffer zone for each (North, East) position (ft)>
"depth_buffer_zone": <buffer zone for depth moves (ft)>
"yaw_buffer_zone": <buffer zone for yaw moves (deg)>
"waypoint_file": <location of csv file containing waypoints>
}
drive_functions: An already initialized object of drive_functions. This
class contains a variety of drive functions that are useful
to complete tasks.
'''
Task.__init__(self)
self.task_dict = task_dict
#Unpack the information about the task
self.name = self.task_dict["name"]
self.type = "Waypoint"
self.timeout = float(self.task_dict["timeout"] * 60.0)
self.waypoint_file = self.task_dict["waypoint_file"]
#Buffer zone (bubble) for North/East positions to be considered in at correct coordinate
self.position_buffer_zone = self.task_dict["position_buffer_zone"]
#The buffer zone used to be considered to make it to a certain depth when doing a depth dive
self.depth_buffer_zone = self.task_dict["depth_buffer_zone"]
#The buffer zone to be used for yaw to consider making it to a certain yaw position
self.yaw_buffer_zone = self.task_dict["yaw_buffer_zone"]
self.drive_functions = drive_functions
#Initialize the timeout timer
self.timeout_timer = util_timer.Timer()
self.unpack_waypoints()
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):
'''
Basic outline initialization of a task.
Parameters:
timeout: How long the task has before it is exited.
Returns:
N/A
'''
#Dictionary containing all of the task information.
self.task_dict = None
#Provide a name for your task
self.name = None
#Provide a type for your task. This type will be used by the Mission Commander
#to part
self.type = None
self.timeout = None
self.timeout_timer = util_timer.Timer()
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 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")
