From caf99c03e550e499c0775d13400448e33d82dae5 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Fri, 8 Nov 2024 15:15:25 +0100 Subject: [PATCH] refactor(style): applied formatting and style changes --- .pre-commit-config.yaml | 1 + .../acoustics_data_record_lib.py | 11 +- .../acoustics_data_record_node.py | 101 ++++++---- ...nch.py => acoustics_data_record_launch.py} | 6 +- .../utilities/display_acoustics_data_live.py | 116 +++++++----- .../acoustics_interface_lib.py | 73 ++++---- .../acoustics_interface_node.py | 126 +++++++++---- .../launch/acoustics_interface_launch.py | 6 +- auv_setup/launch/orca.launch.py | 11 +- auv_setup/launch/topside.launch.py | 7 +- mission/LCD/sources/ip_lib.py | 3 +- mission/LCD/sources/lcd.py | 13 +- mission/LCD/sources/lcd_lib.py | 22 ++- mission/LCD/sources/power_sense_module_lib.py | 24 ++- mission/LCD/sources/pressure_sensor_lib.py | 3 +- mission/LCD/sources/temperature_sensor_lib.py | 9 +- .../blackbox/blackbox/blackbox_log_data.py | 72 +++---- mission/blackbox/blackbox/blackbox_node.py | 77 +++++--- mission/blackbox/launch/blackbox.launch.py | 3 +- mission/gui_auv/auv_gui/auv_gui.py | 52 ++++-- .../power_sense_module_lib.py | 23 ++- .../power_sense_module_node.py | 56 ++++-- .../pressure_sensor_lib.py | 3 +- .../pressure_sensor_node.py | 42 +++-- .../temperature_sensor_lib.py | 9 +- .../temperature_sensor_node.py | 50 +++-- .../launch/internal_status_auv.launch.py | 3 +- .../joystick_interface_auv.py | 90 ++++++--- .../launch/joystick_interface_auv.launch.py | 3 +- .../tests/test_joystick_interface_auv.py | 12 +- .../eigen_vector6d_typedef.hpp | 4 +- .../pseudoinverse_allocator.hpp | 36 ++-- .../thrust_allocator_ros.hpp | 88 ++++----- .../thrust_allocator_utils.hpp | 128 +++++++------ .../launch/thrust_allocator_auv.launch.py | 3 +- .../src/pseudoinverse_allocator.cpp | 10 +- .../src/thrust_allocator_auv_node.cpp | 14 +- .../src/thrust_allocator_ros.cpp | 176 +++++++++--------- .../launch/thruster_interface_auv.launch.py | 3 +- .../thruster_interface_auv_driver_lib.py | 32 ++-- .../thruster_interface_auv_node.py | 58 ++++-- 41 files changed, 927 insertions(+), 652 deletions(-) rename acoustics/acoustics_data_record/launch/{acoustics_data_record.launch.py => acoustics_data_record_launch.py} (93%) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 36cb74d32..c867df9f1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -56,6 +56,7 @@ repos: "--select=D", "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", "--fix", + "--unsafe-fixes" ] stages: [pre-commit] pass_filenames: true diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py index 7d921d65e..109c626a2 100755 --- a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_lib.py @@ -29,7 +29,9 @@ def __init__(self, ros2_package_directory: str = "") -> None: ] # Make new .csv file for logging blackbox data ---------- - with open(self.data_file_location, mode="w", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="w", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow(self.csv_headers) @@ -47,8 +49,7 @@ def log_data_to_csv_file( tdoa: list[float] = [0.0], position: list[float] = [0.0], ) -> None: - """ - Logs the provided data to a CSV file. + """Logs the provided data to a CSV file. Parameters: self (object): The instance of the class. @@ -69,7 +70,9 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="a", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py index 86f7d1ae2..9b1d57c8b 100755 --- a/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py +++ b/acoustics/acoustics_data_record/acoustics_data_record/acoustics_data_record_node.py @@ -14,9 +14,13 @@ class AcousticsDataRecordNode(Node): def __init__(self) -> None: # Variables for setting upp logging correctly - hydrophone_data_size = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data + hydrophone_data_size = ( + (2**10) * 3 + ) # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data dsp_data_size = 2**10 # DSP (Digital Signal Processing) has 2^10 long data - tdoa_data_size = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for + tdoa_data_size = ( + 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for + ) position_data_size = 3 # position only has X, Y, Z basically 3 elements # Initialize ROS2 node @@ -24,19 +28,29 @@ def __init__(self) -> None: # Initialize Subscribers ---------- # Start listening to Hydrophone data - self.subscriber_hydrophone1 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5) + self.subscriber_hydrophone1 = self.create_subscription( + Int32MultiArray, "/acoustics/hydrophone1", self.hydrophone1_callback, 5 + ) self.hydrophone1_data = array.array("i", [0] * hydrophone_data_size) - self.subscriber_hydrophone2 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5) + self.subscriber_hydrophone2 = self.create_subscription( + Int32MultiArray, "/acoustics/hydrophone2", self.hydrophone2_callback, 5 + ) self.hydrophone2_data = array.array("i", [0] * hydrophone_data_size) - self.subscriber_hydrophone3 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5) + self.subscriber_hydrophone3 = self.create_subscription( + Int32MultiArray, "/acoustics/hydrophone3", self.hydrophone3_callback, 5 + ) self.hydrophone3_data = array.array("i", [0] * hydrophone_data_size) - self.subscriber_hydrophone4 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5) + self.subscriber_hydrophone4 = self.create_subscription( + Int32MultiArray, "/acoustics/hydrophone4", self.hydrophone4_callback, 5 + ) self.hydrophone4_data = array.array("i", [0] * hydrophone_data_size) - self.subscriber_hydrophone5 = self.create_subscription(Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5) + self.subscriber_hydrophone5 = self.create_subscription( + Int32MultiArray, "/acoustics/hydrophone5", self.hydrophone5_callback, 5 + ) self.hydrophone5_data = array.array("i", [0] * hydrophone_data_size) # Start listening to DSP (Digital Signal Processing) data @@ -48,10 +62,14 @@ def __init__(self) -> None: ) self.filter_response_data = array.array("i", [0] * dsp_data_size) - self.subscriber_fft = self.create_subscription(Int32MultiArray, "/acoustics/fft", self.fft_callback, 5) + self.subscriber_fft = self.create_subscription( + Int32MultiArray, "/acoustics/fft", self.fft_callback, 5 + ) self.fft_data = array.array("i", [0] * dsp_data_size) - self.subscriber_peaks = self.create_subscription(Int32MultiArray, "/acoustics/peaks", self.peaks_callback, 5) + self.subscriber_peaks = self.create_subscription( + Int32MultiArray, "/acoustics/peaks", self.peaks_callback, 5 + ) self.peaks_data = array.array("i", [0] * dsp_data_size) # Start listening to Multilateration data @@ -63,23 +81,38 @@ def __init__(self) -> None: ) self.tdoa_data = array.array("f", [0.0] * tdoa_data_size) - self.subscriber_position_response = self.create_subscription(Float32MultiArray, "/acoustics/position", self.position_callback, 5) + self.subscriber_position_response = self.create_subscription( + Float32MultiArray, "/acoustics/position", self.position_callback, 5 + ) self.position_data = array.array("f", [0.0] * position_data_size) # Initialize logger ---------- # Get package directory location - ros2_package_directory_location = get_package_share_directory("acoustics_data_record") - ros2_package_directory_location = ros2_package_directory_location + "/../../../../" # go back to workspace + ros2_package_directory_location = get_package_share_directory( + "acoustics_data_record" + ) ros2_package_directory_location = ( - ros2_package_directory_location + "src/vortex-auv/acoustics/acoustics_data_record/" + ros2_package_directory_location + "/../../../../" + ) # go back to workspace + ros2_package_directory_location = ( + ros2_package_directory_location + + "src/vortex-auv/acoustics/acoustics_data_record/" ) # Navigate to this package # Make blackbox logging file - self.acoustics_data_record = AcousticsDataRecordLib(ros2_package_directory=ros2_package_directory_location) + self.acoustics_data_record = AcousticsDataRecordLib( + ros2_package_directory=ros2_package_directory_location + ) # Logs all the newest data 1 time(s) per second - self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, very slow - data_loging_rate = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + self.declare_parameter( + "acoustics.data_logging_rate", 1.0 + ) # Providing a default value 1.0 => 1 samplings per second, very slow + data_loging_rate = ( + self.get_parameter("acoustics.data_logging_rate") + .get_parameter_value() + .double_value + ) timer_period = 1.0 / data_loging_rate self.logger_timer = self.create_timer(timer_period, self.logger) @@ -100,8 +133,7 @@ def __init__(self) -> None: # Callback methods for different topics def hydrophone1_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for hydrophone1 topic. + """Callback method for hydrophone1 topic. Args: msg (Int32MultiArray): Message containing hydrophone1 data. @@ -109,8 +141,7 @@ def hydrophone1_callback(self, msg: Int32MultiArray) -> None: self.hydrophone1_data = msg.data def hydrophone2_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for hydrophone2 topic. + """Callback method for hydrophone2 topic. Args: msg (Int32MultiArray): Message containing hydrophone2 data. @@ -118,8 +149,7 @@ def hydrophone2_callback(self, msg: Int32MultiArray) -> None: self.hydrophone2_data = msg.data def hydrophone3_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for hydrophone3 topic. + """Callback method for hydrophone3 topic. Args: msg (Int32MultiArray): Message containing hydrophone3 data. @@ -127,8 +157,7 @@ def hydrophone3_callback(self, msg: Int32MultiArray) -> None: self.hydrophone3_data = msg.data def hydrophone4_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for hydrophone4 topic. + """Callback method for hydrophone4 topic. Args: msg (Int32MultiArray): Message containing hydrophone4 data. @@ -136,8 +165,7 @@ def hydrophone4_callback(self, msg: Int32MultiArray) -> None: self.hydrophone4_data = msg.data def hydrophone5_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for hydrophone5 topic. + """Callback method for hydrophone5 topic. Args: msg (Int32MultiArray): Message containing hydrophone5 data. @@ -145,8 +173,7 @@ def hydrophone5_callback(self, msg: Int32MultiArray) -> None: self.hydrophone5_data = msg.data def filter_response_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for filter_response topic. + """Callback method for filter_response topic. Args: msg (Int32MultiArray): Message containing filter response data. @@ -154,8 +181,7 @@ def filter_response_callback(self, msg: Int32MultiArray) -> None: self.filter_response_data = msg.data def fft_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for fft topic. + """Callback method for fft topic. Args: msg (Int32MultiArray): Message containing FFT data. @@ -163,8 +189,7 @@ def fft_callback(self, msg: Int32MultiArray) -> None: self.fft_data = msg.data def peaks_callback(self, msg: Int32MultiArray) -> None: - """ - Callback method for peaks topic. + """Callback method for peaks topic. Args: msg (Int32MultiArray): Message containing peaks data. @@ -172,8 +197,7 @@ def peaks_callback(self, msg: Int32MultiArray) -> None: self.peaks_data = msg.data def tdoa_callback(self, msg: Float32MultiArray) -> None: - """ - Callback method for time_difference_of_arrival topic. + """Callback method for time_difference_of_arrival topic. Args: msg (Float32MultiArray): Message containing TDOA data. @@ -181,8 +205,7 @@ def tdoa_callback(self, msg: Float32MultiArray) -> None: self.tdoa_data = msg.data def position_callback(self, msg: Float32MultiArray) -> None: - """ - Callback method for position topic. + """Callback method for position topic. Args: msg (Float32MultiArray): Message containing position data. @@ -191,8 +214,7 @@ def position_callback(self, msg: Float32MultiArray) -> None: # The logger that logs all the data def logger(self) -> None: - """ - Logs all the data to a CSV file using the AcousticsDataRecordLib. + """Logs all the data to a CSV file using the AcousticsDataRecordLib. This method is called periodically based on the data logging rate. """ @@ -211,8 +233,7 @@ def logger(self) -> None: def main() -> None: - """ - Main function to initialize and run the ROS2 node for acoustics data recording. + """Main function to initialize and run the ROS2 node for acoustics data recording. This function performs the following steps: 1. Initializes the ROS2 communication. diff --git a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py b/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py similarity index 93% rename from acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py rename to acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py index 085c82c38..7e3a8dee5 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py @@ -1,13 +1,13 @@ import os from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription from launch_ros.actions import Node +from launch import LaunchDescription + def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the acoustics_data_record node. + """Generates a launch description for the acoustics_data_record node. This function constructs the path to the YAML configuration file for the acoustics_interface package and returns a LaunchDescription object that diff --git a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py index 30af95235..b903e56a8 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -13,9 +13,13 @@ from matplotlib import animation, gridspec # Variables for setting upp data structures correctly -HYDROPHONE_DATA_SIZE = (2**10) * 3 # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data +HYDROPHONE_DATA_SIZE = ( + (2**10) * 3 +) # 1 hydrophone buffer is 2^10 long, Each hydrophone data has 3 buffers full of this data DSP_DATA_SIZE = 2**10 # DSP (Digital Signal Processing) has 2^10 long data -TDOA_DATA_SIZE = 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for +TDOA_DATA_SIZE = ( + 5 # TDOA (Time Difference Of Arrival) has 5 hydrophones it has times for +) POSITION_DATA_SIZE = 3 # position only has X, Y, Z basically 3 elements # Important variables for later processing of data @@ -28,29 +32,35 @@ # Create an outer GridSpec for the two columns outer_gs = gridspec.GridSpec(1, 2, figure=fig, width_ratios=[1, 1]) # Create an inner GridSpec for the first column -gs_hydrophone = gridspec.GridSpecFromSubplotSpec(5, 1, subplot_spec=outer_gs[0], hspace=0.1) +gs_hydrophone = gridspec.GridSpecFromSubplotSpec( + 5, 1, subplot_spec=outer_gs[0], hspace=0.1 +) # Create an inner GridSpec for the second column, with height ratios for the 70%/30% split -gs_dsp = gridspec.GridSpecFromSubplotSpec(2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3) +gs_dsp = gridspec.GridSpecFromSubplotSpec( + 2, 1, subplot_spec=outer_gs[1], height_ratios=[7, 3], hspace=0.3 +) -hydrophoneAxis = [None] * 5 +hydrophone_axis = [None] * 5 # Add subplots in the first column for hydrophone data for i in range(5): - hydrophoneAxis[i] = fig.add_subplot(gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None) - hydrophoneAxis[i].label_outer() + hydrophone_axis[i] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophone_axis[0] if i else None + ) + hydrophone_axis[i].label_outer() fig.text(0.25, 0.965, "Hydrophone Data", ha="center") # Add subplots in the second column FFTAxis = fig.add_subplot(gs_dsp[0]) -filterAxis = fig.add_subplot(gs_dsp[1]) +filter_axis = fig.add_subplot(gs_dsp[1]) # Plot type so that the size is dynamic plt.tight_layout() # Select nice color pallet for graphs -colorSoftPurple = (168 / 255, 140 / 255, 220 / 255) -colorSoftBlue = (135 / 255, 206 / 255, 250 / 255) -colorSoftGreen = (122 / 255, 200 / 255, 122 / 255) +color_soft_purple = (168 / 255, 140 / 255, 220 / 255) +color_soft_blue = (135 / 255, 206 / 255, 250 / 255) +color_soft_green = (122 / 255, 200 / 255, 122 / 255) # .CSV Setup ================================================== # Get Directory of the .csv files @@ -58,47 +68,48 @@ ACOUSTICS_CSV_FILE_DIR = PACKAGE_DIR + "/acoustics_data" # List of all the acoustic files -acousticsCSVFiles = csv_files = glob.glob(ACOUSTICS_CSV_FILE_DIR + "/acoustics_data_" + "*.csv") +acoustics_csv_files = csv_files = glob.glob( + ACOUSTICS_CSV_FILE_DIR + "/acoustics_data_" + "*.csv" +) # Get the latest csv file name for acoustics data -acousticsCSVFile = max(acousticsCSVFiles, key=os.path.getctime) +acoustics_csv_file = max(acoustics_csv_files, key=os.path.getctime) def convert_pandas_object_to_int_array(pandas_object: pd.Series) -> list: - """ - Convert a pandas object containing a string representation of an integer array to a list of integers. + """Convert a pandas object containing a string representation of an integer array to a list of integers. Args: - pandasObject (pandas.Series): A pandas Series object containing a string representation of an integer array. + pandas_object (pandas.Series): A pandas Series object containing a string representation of an integer array. Returns: list: A list of integers extracted from the pandas object. """ - pandas_string = pandas_object.iloc[0].strip("array('i', ").rstrip(")") + pandas_string = pandas_object.iloc[0].replace("array('i', ", "").replace(")", "") pandas_int_array = [int(x.strip()) for x in pandas_string.strip("[]").split(",")] return pandas_int_array def convert_pandas_object_to_float_array(pandas_object: pd.Series) -> list: - """ - Convert a pandas object containing a string representation of a float array to a list of floats. + """Convert a pandas object containing a string representation of a float array to a list of floats. Args: - pandasObject (pandas.Series): A pandas Series object containing a string representation of a float array. + pandas_object (pandas.Series): A pandas Series object containing a string representation of a float array. Returns: list: A list of floats extracted from the pandas object. """ - pandas_string = pandas_object.iloc[0].strip("array('f', ").rstrip(")") - pandas_float_array = [float(x.strip()) for x in pandas_string.strip("[]").split(",")] + pandas_string = pandas_object.iloc[0].replace("array('f', ", "").replace(")", "") + pandas_float_array = [ + float(x.strip()) for x in pandas_string.strip("[]").split(",") + ] return pandas_float_array def get_acoustics_data() -> list: - """ - Retrieves and processes the latest acoustics data from a CSV file. + """Retrieves and processes the latest acoustics data from a CSV file. This function reads the latest acoustics data from a specified CSV file and processes it to extract various data points including hydrophone data, unfiltered data, filtered data, FFT data, peaks data, TDOA data, and @@ -143,16 +154,26 @@ def get_acoustics_data() -> list: positon_data = [0.0] * POSITION_DATA_SIZE # Read latest acoustics data ---------- - acoustics_data_frame = pd.read_csv(acousticsCSVFile) + acoustics_data_frame = pd.read_csv(acoustics_csv_file) latest_acoustics_data = acoustics_data_frame.tail(1) try: # Get latest hydrophone data - hydrophone1 = convert_pandas_object_to_int_array(latest_acoustics_data["Hydrophone1"]) - hydrophone2 = convert_pandas_object_to_int_array(latest_acoustics_data["Hydrophone2"]) - hydrophone3 = convert_pandas_object_to_int_array(latest_acoustics_data["Hydrophone3"]) - hydrophone4 = convert_pandas_object_to_int_array(latest_acoustics_data["Hydrophone4"]) - hydrophone5 = convert_pandas_object_to_int_array(latest_acoustics_data["Hydrophone5"]) + hydrophone1 = convert_pandas_object_to_int_array( + latest_acoustics_data["Hydrophone1"] + ) + hydrophone2 = convert_pandas_object_to_int_array( + latest_acoustics_data["Hydrophone2"] + ) + hydrophone3 = convert_pandas_object_to_int_array( + latest_acoustics_data["Hydrophone3"] + ) + hydrophone4 = convert_pandas_object_to_int_array( + latest_acoustics_data["Hydrophone4"] + ) + hydrophone5 = convert_pandas_object_to_int_array( + latest_acoustics_data["Hydrophone5"] + ) # Unfiltered data is special as it is the same as Hydrohone 1 first 1024 values # This is because Acoustics PCB uses Hydrophone 1 to perform DSP @@ -170,7 +191,9 @@ def get_acoustics_data() -> list: # Get multilateration data tdoa_data = convert_pandas_object_to_float_array(latest_acoustics_data["TDOA"]) - positon_data = convert_pandas_object_to_float_array(latest_acoustics_data["Position"]) + positon_data = convert_pandas_object_to_float_array( + latest_acoustics_data["Position"] + ) except Exception as e: print(f"ERROR: Couldn't read acoustics data. Exception: {e}") @@ -182,7 +205,9 @@ def get_acoustics_data() -> list: max_frequency_index = int(MAX_FREQUENCY_TO_SHOW * sample_length / SAMPLE_RATE) fft_amplitude_data = fft_data[0:max_frequency_index] - fft_frequency_data = [(i * (SAMPLE_RATE / sample_length)) for i in range(sample_length)] + fft_frequency_data = [ + (i * (SAMPLE_RATE / sample_length)) for i in range(sample_length) + ] fft_frequency_data = fft_frequency_data[0:max_frequency_index] # Peaks data is special as each peak data value is a array of [Amplitude, Frequency, Phase] of the peak @@ -219,8 +244,7 @@ def get_acoustics_data() -> list: def display_live_data() -> None: - """ - Display live acoustics data by plotting hydrophone data, filter response, and FFT data. + """Display live acoustics data by plotting hydrophone data, filter response, and FFT data. Retrieves the latest acoustics data and separates it into hydrophone data, unfiltered data, filtered data, FFT amplitude and frequency data, and peak amplitude and frequency data. @@ -268,24 +292,28 @@ def display_live_data() -> None: # Plot hydrophone data for hydrophone_index in range(5): x_hydrophone = list(range(len(hydrophone_data[hydrophone_index][::]))) - hydrophoneAxis[hydrophone_index].clear() - hydrophoneAxis[hydrophone_index].plot( + hydrophone_axis[hydrophone_index].clear() + hydrophone_axis[hydrophone_index].plot( x_hydrophone, hydrophone_data[hydrophone_index], label=f"Hydrophone {hydrophone_index + 1}", - color=colorSoftBlue, + color=color_soft_blue, alpha=1, ) - hydrophoneAxis[hydrophone_index].legend(loc="upper right", fontsize="xx-small") + hydrophone_axis[hydrophone_index].legend(loc="upper right", fontsize="xx-small") # Plot Filter response x_raw = list(range(len(unfiltered_data))) x_filter = list(range(len(filter_data))) - filterAxis.clear() - filterAxis.set_title("Filter response") - filterAxis.plot(x_raw, unfiltered_data, label="Raw", color=colorSoftBlue, alpha=0.5) - filterAxis.plot(x_filter, filter_data, label="Filter", color=colorSoftGreen, alpha=0.7) - filterAxis.legend(loc="upper right", fontsize="xx-small") + filter_axis.clear() + filter_axis.set_title("Filter response") + filter_axis.plot( + x_raw, unfiltered_data, label="Raw", color=color_soft_blue, alpha=0.5 + ) + filter_axis.plot( + x_filter, filter_data, label="Filter", color=color_soft_green, alpha=0.7 + ) + filter_axis.legend(loc="upper right", fontsize="xx-small") # Plot FFT data FFTAxis.clear() @@ -296,7 +324,7 @@ def display_live_data() -> None: fft_frequency_data, fft_amplitude_data, label="FFT", - color=colorSoftPurple, + color=color_soft_purple, alpha=1, width=500, ) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index adf1dcfe7..a9c1c9817 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,13 +1,13 @@ # Setting up libraries import errno import time -from socket import AF_INET, SOCK_DGRAM, error, socket +from socket import AF_INET, SOCK_DGRAM, socket class TeensyCommunicationUDP: - """ - This class is responsible for the RPI side of teensy-RPI UDP communication. It is - implemented with a singleton pattern for convenience. + """This class is responsible for the RPI side of teensy-RPI UDP communication. + + It is implemented with a singleton pattern for convenience. Note: Private members are denoted by _member_name @@ -55,9 +55,9 @@ class TeensyCommunicationUDP: _INITIALIZATION_MESSAGE = "HELLO :D" # This is a message only sent once to establish 2 way communication between Teensy and client - _clientSocket = socket(AF_INET, SOCK_DGRAM) + _client_socket = socket(AF_INET, SOCK_DGRAM) - _timeoutMax = 10 + _timeout_max = 10 _data_string = "" _data_target = "" acoustics_data = { @@ -75,22 +75,23 @@ class TeensyCommunicationUDP: @classmethod def init_communication(cls, frequencies_of_interest: list[tuple[int, int]]) -> None: - """ - Sets up communication with teensy + """Sets up communication with teensy. Parameters: frequenciesOfInterest (list[tuple[int, int]]): List of frequencies to look for """ - assert len(frequencies_of_interest) == 10, "Frequency list has to have exactly 10 entries" + assert ( + len(frequencies_of_interest) == 10 + ), "Frequency list has to have exactly 10 entries" _frequencies_of_interest = frequencies_of_interest cls.MY_IP = cls._get_ip() # Socket setup - cls._clientSocket.settimeout(cls._TIMEOUT) - cls._clientSocket.bind((cls.MY_IP, cls._MY_PORT)) - cls._clientSocket.setblocking(False) + cls._client_socket.settimeout(cls._TIMEOUT) + cls._client_socket.bind((cls.MY_IP, cls._MY_PORT)) + cls._client_socket.setblocking(False) cls._send_acknowledge_signal() time_start = time.time() @@ -100,7 +101,7 @@ def init_communication(cls, frequencies_of_interest: list[tuple[int, int]]) -> N print("Did not receive READY signal. Will wait.") time.sleep(1) - if time.time() - time_start > cls._timeoutMax: + if time.time() - time_start > cls._timeout_max: print("Gave up on receiving READY. Sending acknowledge signal again") # Start over time_start = time.time() @@ -111,9 +112,7 @@ def init_communication(cls, frequencies_of_interest: list[tuple[int, int]]) -> N @classmethod def fetch_data(cls) -> None: - """ - Gets data from teensy and stores it in `acoustics_data` - """ + """Gets data from teensy and stores it in `acoustics_data`.""" i = 0 while True: @@ -138,9 +137,7 @@ def fetch_data(cls) -> None: @classmethod def _write_to_target(cls) -> None: - """ - Writes to the current target in `acoustics_data` and clears the data string - """ + """Writes to the current target in `acoustics_data` and clears the data string.""" if cls._data_target in {"TDOA", "LOCATION"}: data = cls._parse_data_string(is_float=True) else: @@ -156,17 +153,16 @@ def _write_to_target(cls) -> None: @classmethod def _get_raw_data(cls) -> str | None: - """ - Gets a message from teensy + """Gets a message from teensy. Returns: The message in the UDP buffer if there is one """ try: - rec_data, _ = cls._clientSocket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) + rec_data, _ = cls._client_socket.recvfrom(cls._MAX_PACKAGE_SIZE_RECEIVED) message_received = rec_data.decode() return message_received - except error as e: # `error` is really `socket.error` + except OSError as e: # `error` is really `socket.error` if e.errno == errno.EWOULDBLOCK: pass else: @@ -174,8 +170,7 @@ def _get_raw_data(cls) -> str | None: @classmethod def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: - """ - Converts _data_string to a list + """Converts _data_string to a list. Parameters: is_float (bool): whether _data_string should be seen as a list of floats or ints @@ -199,9 +194,7 @@ def _parse_data_string(cls, is_float: bool) -> list[float] | list[int] | None: # https://stackoverflow.com/questions/166506/finding-local-ip-addresses-using-pythons-stdlib @classmethod def _get_ip(cls) -> None: - """ - Gets the device's IP address - """ + """Gets the device's IP address.""" s = socket(AF_INET, SOCK_DGRAM) s.settimeout(0) try: @@ -217,11 +210,11 @@ def _get_ip(cls) -> None: @classmethod def _send_acknowledge_signal(cls) -> None: - """ - Sends "HELLO :D to teensy - """ + """Sends "HELLO :D to teensy.""" try: - cls._clientSocket.sendto(cls._INITIALIZATION_MESSAGE.encode(), cls._address) + cls._client_socket.sendto( + cls._INITIALIZATION_MESSAGE.encode(), cls._address + ) print("DEBUGGING: Sent acknowledge package") except Exception as e: print("Error from send_acknowledge_signal") @@ -229,8 +222,7 @@ def _send_acknowledge_signal(cls) -> None: @classmethod def _check_if_available(cls) -> None: - """ - Checks if READY has been received + """Checks if READY has been received. Note: The while loop here may not be necessary, it is just there to make absolutely sure that *all* the data in the UDP buffer is read out when waiting for ready signal, to avoid strange bugs @@ -259,22 +251,25 @@ def _check_if_available(cls) -> None: return False @classmethod - def _send_frequencies_of_interest(cls, frequencies_of_interest: list[tuple[float, float]]) -> None: - """ - Sends the list of frequencies with variance to teensy + def _send_frequencies_of_interest( + cls, frequencies_of_interest: list[tuple[float, float]] + ) -> None: + """Sends the list of frequencies with variance to teensy. Parameters: frequenciesOfInterest (list[tuple[float, float]]): The list of frequencies w/ variance """ try: # Format (CSV): xxx,x,xx,x...,x (frequency list comes first, then variances) - assert len(frequencies_of_interest) == 10, "List of frequencies has to be ten entries long!" + assert ( + len(frequencies_of_interest) == 10 + ), "List of frequencies has to be ten entries long!" # ten messages in total, one message for each entry to work around the max packet size for frequency, variance in frequencies_of_interest: frequency_variance_msg = f"{str(frequency)},{str(variance)}," # print(self.address); - cls._clientSocket.sendto(frequency_variance_msg.encode(), cls._address) + cls._client_socket.sendto(frequency_variance_msg.encode(), cls._address) except Exception as e: print(f"Unexpected error while sending frequency data: {e}") diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 4c2e4f60d..87b2d31d7 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -2,14 +2,14 @@ import rclpy import rclpy.logging -from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP from rclpy.node import Node from std_msgs.msg import Float32MultiArray, Int32MultiArray +from acoustics_interface.acoustics_interface_lib import TeensyCommunicationUDP + class AcousticsInterfaceNode(Node): - """ - Publishes Acoustics data to ROS2 + """Publishes Acoustics data to ROS2. Methods: data_update() -> None: @@ -19,37 +19,67 @@ class AcousticsInterfaceNode(Node): """ def __init__(self) -> None: - """ - Sets up acoustics logging and publishers, also sets up teensy communication - """ + """Sets up acoustics logging and publishers, also sets up teensy communication.""" super().__init__("acoustics_interface") rclpy.logging.initialize() - self._hydrophone_1_publisher = self.create_publisher(Int32MultiArray, "/acoustics/hydrophone1", 5) - self._hydrophone_2_publisher = self.create_publisher(Int32MultiArray, "/acoustics/hydrophone2", 5) - self._hydrophone_3_publisher = self.create_publisher(Int32MultiArray, "/acoustics/hydrophone3", 5) - self._hydrophone_4_publisher = self.create_publisher(Int32MultiArray, "/acoustics/hydrophone4", 5) - self._hydrophone_5_publisher = self.create_publisher(Int32MultiArray, "/acoustics/hydrophone5", 5) - - self._filter_response_publisher = self.create_publisher(Int32MultiArray, "/acoustics/filter_response", 5) - self._fft_publisher = self.create_publisher(Int32MultiArray, "/acoustics/fft", 5) - self._peak_publisher = self.create_publisher(Int32MultiArray, "/acoustics/peaks", 5) - self._tdoa_publisher = self.create_publisher(Float32MultiArray, "/acoustics/time_difference_of_arrival", 5) - self._position_publisher = self.create_publisher(Float32MultiArray, "/acoustics/position", 5) + self._hydrophone_1_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/hydrophone1", 5 + ) + self._hydrophone_2_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/hydrophone2", 5 + ) + self._hydrophone_3_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/hydrophone3", 5 + ) + self._hydrophone_4_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/hydrophone4", 5 + ) + self._hydrophone_5_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/hydrophone5", 5 + ) + + self._filter_response_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/filter_response", 5 + ) + self._fft_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/fft", 5 + ) + self._peak_publisher = self.create_publisher( + Int32MultiArray, "/acoustics/peaks", 5 + ) + self._tdoa_publisher = self.create_publisher( + Float32MultiArray, "/acoustics/time_difference_of_arrival", 5 + ) + self._position_publisher = self.create_publisher( + Float32MultiArray, "/acoustics/position", 5 + ) # Logs all the newest data - self.declare_parameter("acoustics.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, very slow - data_logging_rate = self.get_parameter("acoustics.data_logging_rate").get_parameter_value().double_value + self.declare_parameter( + "acoustics.data_logging_rate", 1.0 + ) # Providing a default value 1.0 => 1 samplings per second, very slow + data_logging_rate = ( + self.get_parameter("acoustics.data_logging_rate") + .get_parameter_value() + .double_value + ) timer_period = 1.0 / data_logging_rate self._timer_data_update = self.create_timer(0.001, self.data_update) - self._timer_data_publisher = self.create_timer(timer_period, self.data_publisher) + self._timer_data_publisher = self.create_timer( + timer_period, self.data_publisher + ) # Declare Frequency of interest parameters to send to Acoustics PCB to look out for # This list has to be exactly 10 entries long (20 elements (10 frequencies + 10 variances)) # format [(FREQUENCY, FREQUENCY_VARIANCE), ...] self.declare_parameter("acoustics.frequencies_of_interest", [0] * 20) - frequencies_of_interest_parameters = self.get_parameter("acoustics.frequencies_of_interest").get_parameter_value().integer_array_value + frequencies_of_interest_parameters = ( + self.get_parameter("acoustics.frequencies_of_interest") + .get_parameter_value() + .integer_array_value + ) frequencies_of_interest = [] for i in range(0, len(frequencies_of_interest_parameters), 2): @@ -70,8 +100,7 @@ def __init__(self) -> None: self.get_logger().info("Successfully connected to Acoustics PCB MCU :D") def data_update(self) -> None: - """ - Fetches data using the TeensyCommunicationUDP class. + """Fetches data using the TeensyCommunicationUDP class. This method calls the fetch_data method from the TeensyCommunicationUDP class to update the data. @@ -79,24 +108,45 @@ def data_update(self) -> None: TeensyCommunicationUDP.fetch_data() def data_publisher(self) -> None: - """Publishes to topics""" - self._hydrophone_1_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"])) - self._hydrophone_2_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_2"])) - self._hydrophone_3_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_3"])) - self._hydrophone_4_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_4"])) - self._hydrophone_5_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_5"])) - - self._filter_response_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["SAMPLES_FILTERED"])) - self._fft_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["FFT"])) - self._peak_publisher.publish(Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["PEAK"])) - - self._tdoa_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["TDOA"])) - self._position_publisher.publish(Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"])) + """Publishes to topics.""" + self._hydrophone_1_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"]) + ) + self._hydrophone_2_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_2"]) + ) + self._hydrophone_3_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_3"]) + ) + self._hydrophone_4_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_4"]) + ) + self._hydrophone_5_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_5"]) + ) + + self._filter_response_publisher.publish( + Int32MultiArray( + data=TeensyCommunicationUDP.acoustics_data["SAMPLES_FILTERED"] + ) + ) + self._fft_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["FFT"]) + ) + self._peak_publisher.publish( + Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["PEAK"]) + ) + + self._tdoa_publisher.publish( + Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["TDOA"]) + ) + self._position_publisher.publish( + Float32MultiArray(data=TeensyCommunicationUDP.acoustics_data["LOCATION"]) + ) def main(args: list = None) -> None: - """ - Entry point for the acoustics interface node. + """Entry point for the acoustics interface node. This function initializes the ROS 2 Python client library, creates an instance of the AcousticsInterfaceNode, and starts spinning the node to process callbacks. diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index a283d3b22..b040b403b 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -1,13 +1,13 @@ import os from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription from launch_ros.actions import Node +from launch import LaunchDescription + def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the acoustics_interface node. + """Generates a launch description for the acoustics_interface node. This function constructs the path to the YAML configuration file for the acoustics_interface node and returns a LaunchDescription object that diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 13a57acbe..12b3b0bc4 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -7,8 +7,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the ORCA AUV setup. + """Generates a launch description for the ORCA AUV setup. This function sets up the environment variable for ROS console formatting and includes the launch descriptions for the thruster allocator and thruster @@ -20,7 +19,9 @@ def generate_launch_description() -> LaunchDescription: thruster interface. """ # Set environment variable - set_env_var = SetEnvironmentVariable(name="ROSCONSOLE_FORMAT", value="[${severity}] [${time}] [${node}]: ${message}") + set_env_var = SetEnvironmentVariable( + name="ROSCONSOLE_FORMAT", value="[${severity}] [${time}] [${node}]: ${message}" + ) # Thruster Allocator launch thrust_allocator_launch = IncludeLaunchDescription( @@ -45,4 +46,6 @@ def generate_launch_description() -> LaunchDescription: ) # Return launch description - return LaunchDescription([set_env_var, thrust_allocator_launch, thruster_interface_launch]) + return LaunchDescription( + [set_env_var, thrust_allocator_launch, thruster_interface_launch] + ) diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index c0073bf3d..fff86a3b2 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -8,8 +8,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generate the launch description for the topside AUV setup. + """Generate the launch description for the topside AUV setup. This function sets up the environment variable for ROS console formatting, initializes the joystick node with specific parameters and remappings, and @@ -20,7 +19,9 @@ def generate_launch_description() -> LaunchDescription: variable setting, joystick node, and joystick interface launch. """ # Set environment variable - set_env_var = SetEnvironmentVariable(name="ROSCONSOLE_FORMAT", value="[${severity}] [${time}] [${node}]: ${message}") + set_env_var = SetEnvironmentVariable( + name="ROSCONSOLE_FORMAT", value="[${severity}] [${time}] [${node}]: ${message}" + ) # Joystick node joy_node = Node( diff --git a/mission/LCD/sources/ip_lib.py b/mission/LCD/sources/ip_lib.py index 6828b6515..71eb15dc1 100644 --- a/mission/LCD/sources/ip_lib.py +++ b/mission/LCD/sources/ip_lib.py @@ -9,8 +9,7 @@ def __init__(self) -> None: self.cmd = ["hostname", "-I"] def get_ip(self) -> str: - """ - Executes a shell command to retrieve the IP address. + """Executes a shell command to retrieve the IP address. Returns: str: The IP address as a string. diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 83ae63dbc..1af56d0fe 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -21,8 +21,7 @@ # Formatting function for nices LCD screen layout def format_line(value: str, unit: str) -> str: - """ - Formats a string to fit within a 16-character display, appending a unit with spacing. + """Formats a string to fit within a 16-character display, appending a unit with spacing. Args: value (str): The value to be displayed. @@ -33,7 +32,9 @@ def format_line(value: str, unit: str) -> str: """ spaces_available = 16 value_length = len(value) - unit_length = len(unit) + 1 # +1 to make sure there is spacing between value and unit + unit_length = ( + len(unit) + 1 + ) # +1 to make sure there is spacing between value and unit empty_space_length = spaces_available - (value_length + unit_length) empty_space_length = max(empty_space_length, 0) @@ -53,7 +54,7 @@ def format_line(value: str, unit: str) -> str: # IP ---------- TIME_DISPLAYING = 5 UPDATES_PER_SECOND = 1 - for i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): + for _i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): LINE_1 = "IP: " LINE_2 = str(IP.get_ip()) LCD.write_to_screen(LINE_1, LINE_2) @@ -62,7 +63,7 @@ def format_line(value: str, unit: str) -> str: # Voltage and Current ---------- TIME_DISPLAYING = 5 UPDATES_PER_SECOND = 2 - for i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): + for _i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): LINE_1 = format_line(str(round(PSM.get_voltage(), 3)), "V") LINE_2 = format_line(str(round(PSM.get_current(), 3)), "A") LCD.write_to_screen(LINE_1, LINE_2) @@ -71,7 +72,7 @@ def format_line(value: str, unit: str) -> str: # Pressure and Temperature ---------- TIME_DISPLAYING = 5 UPDATES_PER_SECOND = 1 - for i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): + for _i in range(TIME_DISPLAYING * UPDATES_PER_SECOND): LINE_1 = format_line(str(round(Pressure.get_pressure(), 1)), "hPa") LINE_2 = format_line(str(round(Temperature.get_temperature(), 1)), "*C") LCD.write_to_screen(LINE_1, LINE_2) diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index f77484a5c..70b8e5d6e 100644 --- a/mission/LCD/sources/lcd_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -24,8 +24,7 @@ def __init__(self) -> None: ) def write_to_screen(self, line1: str = "", line2: str = "") -> None: - """ - Writes two lines of text to the LCD screen. + """Writes two lines of text to the LCD screen. This method clears the LCD screen and then writes the provided text to the screen. Each line of text is truncated to a maximum of 16 @@ -48,8 +47,7 @@ def write_to_screen(self, line1: str = "", line2: str = "") -> None: self._lcd.write_string(line2) def fancy_animation(self, animation_speed: float = 0.4) -> None: - """ - Displays a fancy animation on the LCD screen where Pac-Man and a ghost chase each other. + """Displays a fancy animation on the LCD screen where Pac-Man and a ghost chase each other. Args: animation_speed (float): Speed of the animation. Default is 0.4. The actual speed is calculated as 1 / animation_speed. @@ -108,7 +106,9 @@ def fancy_animation(self, animation_speed: float = 0.4) -> None: # Display sequence steps = 20 - for a in range(steps): # Increase range to allow characters to exit screen completely + for a in range( + steps + ): # Increase range to allow characters to exit screen completely self._lcd.clear() # Pac-Man position and animation @@ -121,7 +121,9 @@ def fancy_animation(self, animation_speed: float = 0.4) -> None: self._lcd.write_string(chr(1)) # Mouth closed # Ghost position and animation - if 3 < a < steps + 4: # Start later and continue until the ghost is off-screen + if ( + 3 < a < steps + 4 + ): # Start later and continue until the ghost is off-screen ghost_pos = (0, a - 4) # Maintain spacing self._lcd.cursor_pos = ghost_pos self._lcd.write_string(chr(2)) @@ -136,7 +138,9 @@ def fancy_animation(self, animation_speed: float = 0.4) -> None: # Display sequence steps = 26 - for a in range(steps + 4): # Adjusted range to ensure all characters exit screen + for a in range( + steps + 4 + ): # Adjusted range to ensure all characters exit screen self._lcd.clear() # Ghost position and animation @@ -147,7 +151,9 @@ def fancy_animation(self, animation_speed: float = 0.4) -> None: self._lcd.write_string(chr(2)) # Pac-Man position and animation - pac_man_start_pos = ghost_start_pos + 4 # Starts 4 positions to the right of the ghost initially + pac_man_start_pos = ( + ghost_start_pos + 4 + ) # Starts 4 positions to the right of the ghost initially pac_man_current_pos = pac_man_start_pos - a if 0 <= pac_man_current_pos < 16: self._lcd.cursor_pos = (1, pac_man_current_pos) diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index e57195806..3713bd1ad 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -23,8 +23,12 @@ def __init__(self) -> None: self.channel_voltage = None self.channel_current = None try: - self.channel_voltage = MCP342x(self.bus, self.i2c_adress, channel=1, resolution=18) # voltage - self.channel_current = MCP342x(self.bus, self.i2c_adress, channel=0, resolution=18) # current + self.channel_voltage = MCP342x( + self.bus, self.i2c_adress, channel=1, resolution=18 + ) # voltage + self.channel_current = MCP342x( + self.bus, self.i2c_adress, channel=0, resolution=18 + ) # current except Exception as error: print(f"ERROR: Failed connecting to PSM: {error}") @@ -34,8 +38,7 @@ def __init__(self) -> None: self.psm_to_battery_current_offset = 0.330 # V def get_voltage(self) -> float: - """ - Retrieves the system voltage by reading and converting the channel voltage. + """Retrieves the system voltage by reading and converting the channel voltage. This method attempts to read the voltage from the power sense module (PSM) and convert it to the system voltage using a predefined conversion factor. If an @@ -47,16 +50,16 @@ def get_voltage(self) -> float: """ # Sometimes an I/O timeout or error happens, it will run again when the error disappears try: - system_voltage = self.channel_voltage.convert_and_read() * self.psm_to_battery_voltage + system_voltage = ( + self.channel_voltage.convert_and_read() * self.psm_to_battery_voltage + ) return system_voltage except Exception as error: print(f"ERROR: Failed retrieving voltage from PSM: {error}") return 0.0 def get_current(self) -> float: - """ - Retrieves the current system current by reading from the current channel, - applying an offset, and scaling the result. + """Retrieves the current system current by reading from the current channel, applying an offset, and scaling the result. Returns: float: The calculated system current. Returns 0.0 if an error occurs. @@ -65,7 +68,10 @@ def get_current(self) -> float: Exception: If there is an error in reading or converting the current. """ try: - system_current = (self.channel_current.convert_and_read() - self.psm_to_battery_current_offset) * self.psm_to_battery_current_scale_factor + system_current = ( + self.channel_current.convert_and_read() + - self.psm_to_battery_current_offset + ) * self.psm_to_battery_current_scale_factor return system_current except Exception as error: print(f"ERROR: Failed retrieving current from PSM: {error}") diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index 8e89b6960..7a72c6617 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -30,8 +30,7 @@ def __init__(self) -> None: time.sleep(1) def get_pressure(self) -> float: - """ - Retrieves the current pressure from the pressure sensor. + """Retrieves the current pressure from the pressure sensor. Returns: float: The current pressure value. Returns 0.0 if an error occurs. diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index e911fac2c..14a0b3942 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -1,9 +1,9 @@ #!/usr/bin/python3 -""" -! NOTE: +"""! NOTE. + ! For now we don't have a external sensor to measure internal temperature ! Instead we just use Internal Computer temperature sensor to gaugue temperature of the environment approximately -! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV +! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV. """ # Python Libraries @@ -16,8 +16,7 @@ def __init__(self) -> None: self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" def get_temperature(self) -> float: - """ - Reads the internal temperature from the specified sensor file location. + """Reads the internal temperature from the specified sensor file location. Returns: float: The temperature in Celsius. If an error occurs, returns 0.0. diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 0344897c3..76f84e618 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -48,14 +48,17 @@ def __init__(self, ros2_package_directory: str = "") -> None: self.manage_csv_files() # Make new .csv file for logging blackbox data ---------- - with open(self.data_file_location, mode="w", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="w", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow(self.csv_headers) # Methods for inside use of the class ---------- - def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_000_000) -> None: - """ - Manages CSV files in the blackbox data directory by deleting old files and ensuring the total size does not exceed a specified limit. + def manage_csv_files( + self, max_file_age_in_days: int = 7, max_size_kb: int = 3_000_000 + ) -> None: + """Manages CSV files in the blackbox data directory by deleting old files and ensuring the total size does not exceed a specified limit. Args: max_file_age_in_days (int, optional): The maximum age of files in days before they are deleted. Defaults to 7 days. @@ -77,10 +80,16 @@ def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_0 older_than_time = current_time - timedelta(days=max_file_age_in_days) # Compile a regular expression pattern for matching the expected filename format - pattern = re.compile(r"blackbox_data_(\d{4}-\d{2}-\d{2}_\d{2}:\d{2}:\d{2})\.csv") + pattern = re.compile( + r"blackbox_data_(\d{4}-\d{2}-\d{2}_\d{2}:\d{2}:\d{2})\.csv" + ) # List all .csv files in the blackbox data directory - csv_files = [f for f in os.listdir(self.blackbox_data_directory) if f.endswith(".csv") and f.startswith("blackbox_data_")] + csv_files = [ + f + for f in os.listdir(self.blackbox_data_directory) + if f.endswith(".csv") and f.startswith("blackbox_data_") + ] for csv_file in csv_files: match = pattern.match(csv_file) @@ -92,7 +101,9 @@ def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_0 try: file_time = datetime.strptime(match.group(1), "%Y-%m-%d_%H:%M:%S") except ValueError as e: - print(f"Error parsing file timestamp, skipping file: {csv_file}. Error: {e}") + print( + f"Error parsing file timestamp, skipping file: {csv_file}. Error: {e}" + ) continue if file_time < older_than_time: @@ -103,20 +114,28 @@ def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_0 # Calculate the total size of remaining .csv files total_size_kb = ( sum( - os.path.getsize(os.path.join(self.blackbox_data_directory, f)) for f in os.listdir(self.blackbox_data_directory) if f.endswith(".csv") + os.path.getsize(os.path.join(self.blackbox_data_directory, f)) + for f in os.listdir(self.blackbox_data_directory) + if f.endswith(".csv") ) / 1024 ) csv_files = [ - f for f in os.listdir(self.blackbox_data_directory) if f.endswith(".csv") and f.startswith("blackbox_data_") and pattern.match(f) + f + for f in os.listdir(self.blackbox_data_directory) + if f.endswith(".csv") + and f.startswith("blackbox_data_") + and pattern.match(f) ] # Delete oldest files if total size exceeds max_size_kb while total_size_kb > max_size_kb: # Sort .csv files by timestamp (oldest first) csv_files_sorted = sorted( csv_files, - key=lambda x: datetime.strptime(pattern.match(x).group(1), "%Y-%m-%d_%H:%M:%S"), + key=lambda x: datetime.strptime( + pattern.match(x).group(1), "%Y-%m-%d_%H:%M:%S" + ), ) if not csv_files_sorted: @@ -137,7 +156,9 @@ def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_0 ) / 1024 ) - csv_files.remove(oldest_file) # Ensure the deleted file is removed from the list + csv_files.remove( + oldest_file + ) # Ensure the deleted file is removed from the list print(f"Now the total size of .csv files is: {total_size_kb:.2f} KB") # Methods for external uses ---------- @@ -164,29 +185,8 @@ def log_data_to_csv_file( pwm_7: int = 0, pwm_8: int = 0, ) -> None: - """ - Logs the provided data to a CSV file. - Parameters: - - psm_current (float): The current of the power supply module. - - psm_voltage (float): The voltage of the power supply module. - - pressure_internal (float): The internal pressure. - - temperature_ambient (float): The ambient temperature. - - thruster_forces_1 (float): The force of thruster 1. - - thruster_forces_2 (float): The force of thruster 2. - - thruster_forces_3 (float): The force of thruster 3. - - thruster_forces_4 (float): The force of thruster 4. - - thruster_forces_5 (float): The force of thruster 5. - - thruster_forces_6 (float): The force of thruster 6. - - thruster_forces_7 (float): The force of thruster 7. - - thruster_forces_8 (float): The force of thruster 8. - - pwm_1 (int): The PWM signal for thruster 1. - - pwm_2 (int): The PWM signal for thruster 2. - - pwm_3 (int): The PWM signal for thruster 3. - - pwm_4 (int): The PWM signal for thruster 4. - - pwm_5 (int): The PWM signal for thruster 5. - - pwm_6 (int): The PWM signal for thruster 6. - - pwm_7 (int): The PWM signal for thruster 7. - - pwm_8 (int): The PWM signal for thruster 8. + """Logs the provided data to a CSV file. + This method appends a new row to the CSV file specified by `self.data_file_location`. The row contains the current time and the provided data values. """ @@ -194,7 +194,9 @@ def log_data_to_csv_file( current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] # Write to .csv file - with open(self.data_file_location, mode="a", newline="", encoding="utf-8") as csv_file: + with open( + self.data_file_location, mode="a", newline="", encoding="utf-8" + ) as csv_file: writer = csv.writer(csv_file) writer.writerow( [ diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index b82f17233..ace7e6bb4 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -5,7 +5,6 @@ import rclpy from ament_index_python.packages import get_package_share_directory -from blackbox.blackbox_log_data import BlackBoxLogData from rclpy.node import Node # ROS2 Topic Libraries @@ -14,6 +13,8 @@ # Custom Libraries from vortex_msgs.msg import ThrusterForces +from blackbox.blackbox_log_data import BlackBoxLogData + class BlackBoxNode(Node): def __init__(self) -> None: @@ -21,36 +22,62 @@ def __init__(self) -> None: super().__init__("blackbox_node") # Initialize sunscribers ---------- - self.psm_current_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/current", self.psm_current_callback, 1) + self.psm_current_subscriber = self.create_subscription( + Float32, "/auv/power_sense_module/current", self.psm_current_callback, 1 + ) self.psm_current_data = 0.0 - self.psm_voltage_subscriber = self.create_subscription(Float32, "/auv/power_sense_module/voltage", self.psm_voltage_callback, 1) + self.psm_voltage_subscriber = self.create_subscription( + Float32, "/auv/power_sense_module/voltage", self.psm_voltage_callback, 1 + ) self.psm_voltage_data = 0.0 - self.pressure_subscriber = self.create_subscription(Float32, "/auv/pressure", self.pressure_callback, 1) + self.pressure_subscriber = self.create_subscription( + Float32, "/auv/pressure", self.pressure_callback, 1 + ) self.pressure_data = 0.0 - self.temperature_subscriber = self.create_subscription(Float32, "/auv/temperature", self.temperature_callback, 1) + self.temperature_subscriber = self.create_subscription( + Float32, "/auv/temperature", self.temperature_callback, 1 + ) self.temperature_data = 0.0 - self.thruster_forces = self.create_subscription(ThrusterForces, "/thrust/thruster_forces", self.thruster_forces_callback, 1) - self.thruster_forces_data = array.array("f", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.thruster_forces = self.create_subscription( + ThrusterForces, "/thrust/thruster_forces", self.thruster_forces_callback, 1 + ) + self.thruster_forces_data = array.array( + "f", [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + ) - self.pwm = self.create_subscription(Int16MultiArray, "/pwm", self.pwm_callback, 1) + self.pwm = self.create_subscription( + Int16MultiArray, "/pwm", self.pwm_callback, 1 + ) self.pwm_data = array.array("i", [0, 0, 0, 0, 0, 0, 0, 0]) # Initialize logger ---------- # Get package directory location ros2_package_directory_location = get_package_share_directory("blackbox") - ros2_package_directory_location = ros2_package_directory_location + "/../../../../" # go back to workspace - ros2_package_directory_location = ros2_package_directory_location + "src/vortex-auv/mission/blackbox/" # Navigate to this package + ros2_package_directory_location = ( + ros2_package_directory_location + "/../../../../" + ) # go back to workspace + ros2_package_directory_location = ( + ros2_package_directory_location + "src/vortex-auv/mission/blackbox/" + ) # Navigate to this package # Make blackbox logging file - self.blackbox_log_data = BlackBoxLogData(ros2_package_directory=ros2_package_directory_location) + self.blackbox_log_data = BlackBoxLogData( + ros2_package_directory=ros2_package_directory_location + ) # Logs all the newest data 10 times per second - self.declare_parameter("blackbox.data_logging_rate", 1.0) # Providing a default value 1.0 => 1 samplings per second, very slow - data_logging_rate = self.get_parameter("blackbox.data_logging_rate").get_parameter_value().double_value + self.declare_parameter( + "blackbox.data_logging_rate", 1.0 + ) # Providing a default value 1.0 => 1 samplings per second, very slow + data_logging_rate = ( + self.get_parameter("blackbox.data_logging_rate") + .get_parameter_value() + .double_value + ) timer_period = 1.0 / data_logging_rate self.logger_timer = self.create_timer(timer_period, self.logger) @@ -67,8 +94,7 @@ def __init__(self) -> None: # Callback Methods ---------- def psm_current_callback(self, msg: Float32) -> None: - """ - Callback function for the power sense module (PSM) current topic. + """Callback function for the power sense module (PSM) current topic. This function is called whenever a new message is received on the "/auv/power_sense_module/current" topic. It updates the internal @@ -80,8 +106,7 @@ def psm_current_callback(self, msg: Float32) -> None: self.psm_current_data = msg.data def psm_voltage_callback(self, msg: Float32) -> None: - """ - Callback function for the power sense module (PSM) voltage topic. + """Callback function for the power sense module (PSM) voltage topic. This function is called whenever a new message is received on the "/auv/power_sense_module/voltage" topic. It updates the internal @@ -93,8 +118,7 @@ def psm_voltage_callback(self, msg: Float32) -> None: self.psm_voltage_data = msg.data def pressure_callback(self, msg: Float32) -> None: - """ - Callback function for the internal pressure topic. + """Callback function for the internal pressure topic. This function is called whenever a new message is received on the "/auv/pressure" topic. It updates the internal state with the latest @@ -106,8 +130,7 @@ def pressure_callback(self, msg: Float32) -> None: self.pressure_data = msg.data def temperature_callback(self, msg: Float32) -> None: - """ - Callback function for the ambient temperature topic. + """Callback function for the ambient temperature topic. This function is called whenever a new message is received on the "/auv/temperature" topic. It updates the internal state with the latest @@ -119,8 +142,7 @@ def temperature_callback(self, msg: Float32) -> None: self.temperature_data = msg.data def thruster_forces_callback(self, msg: ThrusterForces) -> None: - """ - Callback function for the thruster forces topic. + """Callback function for the thruster forces topic. This function is called whenever a new message is received on the "/thrust/thruster_forces" topic. It updates the internal state with the @@ -132,8 +154,7 @@ def thruster_forces_callback(self, msg: ThrusterForces) -> None: self.thruster_forces_data = msg.thrust def pwm_callback(self, msg: Int16MultiArray) -> None: - """ - Callback function for the PWM signals topic. + """Callback function for the PWM signals topic. This function is called whenever a new message is received on the "/pwm" topic. It updates the internal state with the latest PWM signals data. @@ -144,8 +165,7 @@ def pwm_callback(self, msg: Int16MultiArray) -> None: self.pwm_data = msg.data def logger(self) -> None: - """ - Logs various sensor and actuator data to a CSV file. + """Logs various sensor and actuator data to a CSV file. This method collects data from multiple sensors and actuators, including power system module (PSM) current and voltage, internal pressure, ambient @@ -186,8 +206,7 @@ def logger(self) -> None: def main(args: list = None) -> None: - """ - Entry point for the blackbox_node. + """Entry point for the blackbox_node. This function initializes the ROS2 environment, starts the BlackBoxNode, and keeps it spinning until ROS2 is shut down. Once ROS2 ends, it destroys diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index 5016f49b8..02f8844a0 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -6,8 +6,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the blackbox node. + """Generates a launch description for the blackbox node. This function constructs the path to the YAML configuration file for the blackbox node and returns a LaunchDescription object that includes the diff --git a/mission/gui_auv/auv_gui/auv_gui.py b/mission/gui_auv/auv_gui/auv_gui.py index c0ea054fe..9257c691b 100644 --- a/mission/gui_auv/auv_gui/auv_gui.py +++ b/mission/gui_auv/auv_gui/auv_gui.py @@ -26,8 +26,7 @@ def quaternion_to_euler(x: float, y: float, z: float, w: float) -> List[float]: - """ - Convert a quaternion to Euler angles (roll, pitch, yaw). + """Convert a quaternion to Euler angles (roll, pitch, yaw). Args: x (float): The x component of the quaternion. @@ -38,7 +37,6 @@ def quaternion_to_euler(x: float, y: float, z: float, w: float) -> List[float]: Returns: List[float]: A list of Euler angles [roll, pitch, yaw]. """ - # Roll (x-axis rotation) sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) @@ -75,13 +73,23 @@ def __init__(self) -> None: self.declare_parameter("history_length", 30) odom_topic = self.get_parameter("odom_topic").get_parameter_value().string_value - current_topic = self.get_parameter("current_topic").get_parameter_value().string_value - voltage_topic = self.get_parameter("voltage_topic").get_parameter_value().string_value - temperature_topic = self.get_parameter("temperature_topic").get_parameter_value().string_value - pressure_topic = self.get_parameter("pressure_topic").get_parameter_value().string_value + current_topic = ( + self.get_parameter("current_topic").get_parameter_value().string_value + ) + voltage_topic = ( + self.get_parameter("voltage_topic").get_parameter_value().string_value + ) + temperature_topic = ( + self.get_parameter("temperature_topic").get_parameter_value().string_value + ) + pressure_topic = ( + self.get_parameter("pressure_topic").get_parameter_value().string_value + ) # Subscriber to the /nucleus/odom topic - self.subscription = self.create_subscription(Odometry, odom_topic, self.odom_callback, 10) + self.subscription = self.create_subscription( + Odometry, odom_topic, self.odom_callback, 10 + ) # Variables to store odometry data self.xpos_data: List[float] = [] # x position @@ -98,10 +106,18 @@ def __init__(self) -> None: self.yaw: Optional[float] = None # Subscribe to internal status topics - self.current_subscriber = self.create_subscription(Float32, current_topic, self.current_callback, 5) - self.voltage_subscriber = self.create_subscription(Float32, voltage_topic, self.voltage_callback, 5) - self.temperature_subscriber = self.create_subscription(Float32, temperature_topic, self.temperature_callback, 5) - self.pressure_subscriber = self.create_subscription(Float32, pressure_topic, self.pressure_callback, 5) + self.current_subscriber = self.create_subscription( + Float32, current_topic, self.current_callback, 5 + ) + self.voltage_subscriber = self.create_subscription( + Float32, voltage_topic, self.voltage_callback, 5 + ) + self.temperature_subscriber = self.create_subscription( + Float32, temperature_topic, self.temperature_callback, 5 + ) + self.pressure_subscriber = self.create_subscription( + Float32, pressure_topic, self.pressure_callback, 5 + ) # Variables for internal status self.current = 0.0 @@ -129,7 +145,9 @@ def odom_callback(self, msg: Odometry) -> None: self.roll, self.pitch, self.yaw = quaternion_to_euler(x, y, z, w) # Limit the stored data for real-time plotting (avoid memory overflow) - max_data_points = self.get_parameter("history_length").get_parameter_value().integer_value + max_data_points = ( + self.get_parameter("history_length").get_parameter_value().integer_value + ) if len(self.x_data) > max_data_points: self.xpos_data.pop(0) self.ypos_data.pop(0) @@ -183,7 +201,9 @@ def __init__(self, gui_node: GuiNode, parent: Optional[QWidget] = None) -> None: self.z_data: List[float] = [] (self.line,) = self.ax.plot([], [], [], 'b-') - def update_plot(self, x_data: List[float], y_data: List[float], z_data: List[float]) -> None: + def update_plot( + self, x_data: List[float], y_data: List[float], z_data: List[float] + ) -> None: """Update the 3D plot with the latest odometry data.""" # Convert lists to numpy arrays to ensure compatibility with the plot functions x_data = np.array(x_data, dtype=float) @@ -264,7 +284,9 @@ def main(args: Optional[List[str]] = None) -> None: # Use a QTimer to update plot, current position, and internal status in the main thread def update_gui() -> None: - plot_canvas.update_plot(ros_node.xpos_data, ros_node.ypos_data, ros_node.zpos_data) + plot_canvas.update_plot( + ros_node.xpos_data, ros_node.ypos_data, ros_node.zpos_data + ) if len(ros_node.xpos_data) > 0 and ros_node.roll is not None: position_text = f"Current Position:\nX: {ros_node.xpos_data[-1]:.2f}\nY: {ros_node.ypos_data[-1]:.2f}\nZ: {ros_node.zpos_data[-1]:.2f}" orientation_text = f"Current Orientation:\nRoll: {ros_node.roll:.2f}\nPitch: {ros_node.pitch:.2f}\nYaw: {ros_node.yaw:.2f}" diff --git a/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py b/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py index 5f737210e..b7cf34f74 100755 --- a/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py +++ b/mission/internal_status_auv/internal_status_auv/power_sense_module_lib.py @@ -23,8 +23,12 @@ def __init__(self) -> None: self.channel_voltage = None self.channel_current = None try: - self.channel_voltage = MCP342x(self.bus, self.i2c_adress, channel=1, resolution=18) # voltage - self.channel_current = MCP342x(self.bus, self.i2c_adress, channel=0, resolution=18) # current + self.channel_voltage = MCP342x( + self.bus, self.i2c_adress, channel=1, resolution=18 + ) # voltage + self.channel_current = MCP342x( + self.bus, self.i2c_adress, channel=0, resolution=18 + ) # current except Exception as error: print(f"ERROR: Failed connecting to PSM: {error}") @@ -34,8 +38,7 @@ def __init__(self) -> None: self.psm_to_battery_current_offset = 0.330 # V def get_voltage(self) -> float: - """ - Retrieves the voltage measurement from the Power Sense Module (PSM). + """Retrieves the voltage measurement from the Power Sense Module (PSM). This method reads the voltage from the PSM's voltage channel and multiplies it by the PSM-to-battery voltage conversion ratio to obtain the actual system @@ -48,15 +51,16 @@ def get_voltage(self) -> float: """ # Sometimes an I/O timeout or error happens, it will run again when the error disappears try: - system_voltage = self.channel_voltage.convert_and_read() * self.psm_to_battery_voltage + system_voltage = ( + self.channel_voltage.convert_and_read() * self.psm_to_battery_voltage + ) return system_voltage except Exception as error: print(f"ERROR: Failed retrieving voltage from PSM: {error}") return 0.0 def get_current(self) -> float: - """ - Retrieves the current measurement from the Power Sense Module (PSM). + """Retrieves the current measurement from the Power Sense Module (PSM). This method reads the current from the PSM's current channel, adjusts it based on the PSM-to-battery current scale factor and offset, and returns the calculated @@ -68,7 +72,10 @@ def get_current(self) -> float: The current value in amperes. If an error occurs during reading, returns 0.0. """ try: - system_current = (self.channel_current.convert_and_read() - self.psm_to_battery_current_offset) * self.psm_to_battery_current_scale_factor + system_current = ( + self.channel_current.convert_and_read() + - self.psm_to_battery_current_offset + ) * self.psm_to_battery_current_scale_factor return system_current except Exception as error: print(f"ERROR: Failed retrieving current from PSM: {error}") diff --git a/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py b/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py index 2e94de425..851f9da4f 100755 --- a/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py +++ b/mission/internal_status_auv/internal_status_auv/power_sense_module_node.py @@ -1,12 +1,13 @@ #!/usr/bin/env python3 # ROS2 Libraries # Custom Libraries -import internal_status_auv.power_sense_module_lib import rclpy from rclpy.logging import get_logger from rclpy.node import Node from std_msgs.msg import Float32 +import internal_status_auv.power_sense_module_lib + class PowerSenseModulePublisher(Node): def __init__(self) -> None: @@ -15,8 +16,12 @@ def __init__(self) -> None: self.psm = internal_status_auv.power_sense_module_lib.PowerSenseModule() # Create publishers ---------- - self.publisher_current = self.create_publisher(Float32, "/auv/power_sense_module/current", 5) - self.publisher_voltage = self.create_publisher(Float32, "/auv/power_sense_module/voltage", 5) + self.publisher_current = self.create_publisher( + Float32, "/auv/power_sense_module/current", 5 + ) + self.publisher_voltage = self.create_publisher( + Float32, "/auv/power_sense_module/voltage", 5 + ) # Data gathering cycle ---------- self.current = 0.0 @@ -25,7 +30,11 @@ def __init__(self) -> None: self.declare_parameter( "internal_status.power_sense_module_read_rate", 10.0 ) # Providing a default value 10.0 => 0.1 second delay per data gathering - read_rate = self.get_parameter("internal_status.power_sense_module_read_rate").get_parameter_value().double_value + read_rate = ( + self.get_parameter("internal_status.power_sense_module_read_rate") + .get_parameter_value() + .double_value + ) read_timer_period = 1.0 / read_rate self.read_timer = self.create_timer(read_timer_period, self.read_timer_callback) @@ -33,22 +42,35 @@ def __init__(self) -> None: self.logger = get_logger("power_sense_module") self.declare_parameter("internal_status.voltage_min", 14.5) - self.voltage_min = self.get_parameter("internal_status.voltage_min").get_parameter_value().double_value + self.voltage_min = ( + self.get_parameter("internal_status.voltage_min") + .get_parameter_value() + .double_value + ) self.declare_parameter("internal_status.voltage_max", 16.8) - self.voltage_max = self.get_parameter("internal_status.voltage_max").get_parameter_value().double_value + self.voltage_max = ( + self.get_parameter("internal_status.voltage_max") + .get_parameter_value() + .double_value + ) self.declare_parameter("internal_status.power_sense_module_warning_rate", 0.1) - warning_rate = self.get_parameter("internal_status.power_sense_module_warning_rate").get_parameter_value().double_value + warning_rate = ( + self.get_parameter("internal_status.power_sense_module_warning_rate") + .get_parameter_value() + .double_value + ) warning_timer_period = 1.0 / warning_rate - self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) + self.warning_timer = self.create_timer( + warning_timer_period, self.warning_timer_callback + ) # Debugging ---------- self.get_logger().info('"power_sense_module_publisher" has been started') def read_timer_callback(self) -> None: - """ - Callback function triggered by the read timer. + """Callback function triggered by the read timer. This function retrieves the current and voltage data from the power sense module and publishes it to the corresponding ROS2 topics. @@ -64,12 +86,15 @@ def read_timer_callback(self) -> None: current_msg.data = self.current voltage_msg.data = self.voltage - self.publisher_current.publish(current_msg) # publish current value to the "current topic" - self.publisher_voltage.publish(voltage_msg) # publish voltage value to the "voltge topic" + self.publisher_current.publish( + current_msg + ) # publish current value to the "current topic" + self.publisher_voltage.publish( + voltage_msg + ) # publish voltage value to the "voltge topic" def warning_timer_callback(self) -> None: - """ - Callback function triggered by the warning timer. + """Callback function triggered by the warning timer. This function checks if the voltage levels are outside of the acceptable range and logs a warning if the voltage is either too low or too high. @@ -81,8 +106,7 @@ def warning_timer_callback(self) -> None: def main(args: list = None) -> None: - """ - Main function to initialize and spin the ROS2 node. + """Main function to initialize and spin the ROS2 node. This function initializes the rclpy library, creates an instance of the PowerSenseModulePublisher node, and starts spinning to keep the node running diff --git a/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py b/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py index cbe6f8391..763bf8ad7 100755 --- a/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py +++ b/mission/internal_status_auv/internal_status_auv/pressure_sensor_lib.py @@ -30,8 +30,7 @@ def __init__(self) -> None: time.sleep(1) def get_pressure(self) -> float: - """ - Retrieves the current pressure reading from the sensor. + """Retrieves the current pressure reading from the sensor. This method attempts to read the pressure from the connected MPRLS sensor. If the reading is successful, the pressure value is returned. diff --git a/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py b/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py index 74d1b04b8..fe55ec3be 100755 --- a/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py +++ b/mission/internal_status_auv/internal_status_auv/pressure_sensor_node.py @@ -1,12 +1,13 @@ #!/usr/bin/env python3 # ROS2 Libraries # Custom Libraries -import internal_status_auv.pressure_sensor_lib import rclpy from rclpy.logging import get_logger from rclpy.node import Node from std_msgs.msg import Float32 +import internal_status_auv.pressure_sensor_lib + class PressurePublisher(Node): def __init__(self) -> None: @@ -22,8 +23,14 @@ def __init__(self) -> None: # Data gathering cycle ---------- self.pressure = 0.0 - self.declare_parameter("internal_status.pressure_read_rate", 1.0) # Providing a default value 1.0 => 1 second delay per data gathering - read_rate = self.get_parameter("internal_status.pressure_read_rate").get_parameter_value().double_value + self.declare_parameter( + "internal_status.pressure_read_rate", 1.0 + ) # Providing a default value 1.0 => 1 second delay per data gathering + read_rate = ( + self.get_parameter("internal_status.pressure_read_rate") + .get_parameter_value() + .double_value + ) timer_period = 1.0 / read_rate self.timer = self.create_timer(timer_period, self.timer_callback) @@ -31,19 +38,28 @@ def __init__(self) -> None: self.logger = get_logger("pressure_sensor") self.declare_parameter("internal_status.pressure_critical_level", 1000.0) - self.pressure_critical_level = self.get_parameter("internal_status.pressure_critical_level").get_parameter_value().double_value + self.pressure_critical_level = ( + self.get_parameter("internal_status.pressure_critical_level") + .get_parameter_value() + .double_value + ) self.declare_parameter("internal_status.pressure_warning_rate", 0.1) - warning_rate = self.get_parameter("internal_status.pressure_warning_rate").get_parameter_value().double_value + warning_rate = ( + self.get_parameter("internal_status.pressure_warning_rate") + .get_parameter_value() + .double_value + ) warning_timer_period = 1.0 / warning_rate - self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) + self.warning_timer = self.create_timer( + warning_timer_period, self.warning_timer_callback + ) # Debugging ---------- self.get_logger().info('"pressure_sensor_publisher" has been started') def timer_callback(self) -> None: - """ - Callback function triggered by the main timer. + """Callback function triggered by the main timer. This function retrieves the pressure data from the sensor and publishes it to the "/auv/pressure" topic. @@ -57,19 +73,19 @@ def timer_callback(self) -> None: self.publisher_pressure.publish(pressure_msg) def warning_timer_callback(self) -> None: - """ - Callback function triggered by the warning timer. + """Callback function triggered by the warning timer. This function checks if the pressure exceeds the critical level. If so, a fatal warning is logged indicating a possible leak in the AUV. """ if self.pressure > self.pressure_critical_level: - self.logger.fatal(f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!") + self.logger.fatal( + f"WARNING: Internal pressure to HIGH: {self.pressure} hPa! Drone might be LEAKING!" + ) def main(args: list = None) -> None: - """ - Main function to initialize and spin the ROS2 node. + """Main function to initialize and spin the ROS2 node. This function initializes the rclpy library, creates an instance of the PressurePublisher node, and starts spinning to keep the node diff --git a/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py b/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py index b83e79375..c0bada58b 100755 --- a/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py +++ b/mission/internal_status_auv/internal_status_auv/temperature_sensor_lib.py @@ -1,9 +1,9 @@ #!/usr/bin/python3 -""" -! NOTE: +"""! NOTE. + ! For now we don't have a external sensor to measure internal temperature ! Instead we just use Internal Computer temperature sensor to gaugue temperature of the environment approximately -! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV +! In the future someone should implement a external temperature sensor for measuting a more accurate state of the temperatuer on the inside of the AUV. """ # Python Libraries @@ -16,8 +16,7 @@ def __init__(self) -> None: self.temperature_sensor_file_location = "/sys/class/thermal/thermal_zone0/temp" def get_temperature(self) -> float: - """ - Gets the current temperature from the internal computer's sensor. + """Gets the current temperature from the internal computer's sensor. This method reads the temperature value from the internal sensor file, which is in milli°C, converts it into Celsius, and returns the result. diff --git a/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py b/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py index f39aa563c..49f6d5299 100755 --- a/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py +++ b/mission/internal_status_auv/internal_status_auv/temperature_sensor_node.py @@ -1,29 +1,40 @@ #!/usr/bin/python3 # ROS2 Libraries # Custom Libraries -import internal_status_auv.temperature_sensor_lib import rclpy from rclpy.logging import get_logger from rclpy.node import Node from std_msgs.msg import Float32 +import internal_status_auv.temperature_sensor_lib + class TemperaturePublisher(Node): def __init__(self) -> None: # Pressure sensor setup ---------- - self.temperature = internal_status_auv.temperature_sensor_lib.TemperatureSensor() + self.temperature = ( + internal_status_auv.temperature_sensor_lib.TemperatureSensor() + ) # Node setup ---------- super().__init__("temperature_sensor_publisher") # Create publishers ---------- - self.publisher_temperature = self.create_publisher(Float32, "/auv/temperature", 5) + self.publisher_temperature = self.create_publisher( + Float32, "/auv/temperature", 5 + ) # Data gathering cycle ---------- self.temperature = 0.0 - self.declare_parameter("internal_status.temperature_read_rate", 0.1) # Providing a default value 0.1 => 10 second delay per data gathering - read_rate = self.get_parameter("internal_status.temperature_read_rate").get_parameter_value().double_value + self.declare_parameter( + "internal_status.temperature_read_rate", 0.1 + ) # Providing a default value 0.1 => 10 second delay per data gathering + read_rate = ( + self.get_parameter("internal_status.temperature_read_rate") + .get_parameter_value() + .double_value + ) timer_period = 1.0 / read_rate self.timer = self.create_timer(timer_period, self.timer_callback) @@ -31,19 +42,28 @@ def __init__(self) -> None: self.logger = get_logger("temperature_sensor") self.declare_parameter("internal_status.temperature_critical_level", 90.0) - self.temperature_critical_level = self.get_parameter("internal_status.temperature_critical_level").get_parameter_value().double_value + self.temperature_critical_level = ( + self.get_parameter("internal_status.temperature_critical_level") + .get_parameter_value() + .double_value + ) self.declare_parameter("internal_status.temperature_warning_rate", 0.1) - warning_rate = self.get_parameter("internal_status.temperature_warning_rate").get_parameter_value().double_value + warning_rate = ( + self.get_parameter("internal_status.temperature_warning_rate") + .get_parameter_value() + .double_value + ) warning_timer_period = 1.0 / warning_rate - self.warning_timer = self.create_timer(warning_timer_period, self.warning_timer_callback) + self.warning_timer = self.create_timer( + warning_timer_period, self.warning_timer_callback + ) # Debugging ---------- self.get_logger().info('"temperature_sensor_publisher" has been started') def timer_callback(self) -> None: - """ - Callback function triggered by the main timer. + """Callback function triggered by the main timer. This function retrieves the temperature data from the sensor and publishes it to the "/auv/temperature" topic. @@ -57,19 +77,19 @@ def timer_callback(self) -> None: self.publisher_temperature.publish(temperature_msg) def warning_timer_callback(self) -> None: - """ - Callback function triggered by the warning timer. + """Callback function triggered by the warning timer. This function checks if the temperature exceeds the critical level. If so, a fatal warning is logged indicating a possible overheating situation. """ if self.temperature > self.temperature_critical_level: - self.logger.fatal(f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!") + self.logger.fatal( + f"WARNING: Temperature inside the Drone to HIGH: {self.temperature} *C! Drone might be overheating!" + ) def main(args: list = None) -> None: - """ - Main function to initialize and spin the ROS2 node. + """Main function to initialize and spin the ROS2 node. This function initializes the rclpy library, creates an instance of the TemperaturePublisher node, and starts spinning to keep the node running diff --git a/mission/internal_status_auv/launch/internal_status_auv.launch.py b/mission/internal_status_auv/launch/internal_status_auv.launch.py index 0b491a903..399018e33 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -6,8 +6,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a LaunchDescription object that defines the nodes to be launched. + """Generates a LaunchDescription object that defines the nodes to be launched. This function creates a launch configuration for three sensor nodes: Power Sense Module Node, Pressure Sensor Node, and Temperature Sensor Node. diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py index dd4aa3bf0..4bf665328 100755 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py @@ -89,7 +89,9 @@ def __init__(self) -> None: self.joystick_axes_map_ = [] - self.joy_subscriber_ = self.create_subscription(Joy, "joystick/joy", self.joystick_cb, 5) + self.joy_subscriber_ = self.create_subscription( + Joy, "joystick/joy", self.joystick_cb, 5 + ) self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 5) self.declare_parameter("surge_scale_factor", 60.0) @@ -108,11 +110,17 @@ def __init__(self) -> None: self.joystick_pitch_scaling_ = self.get_parameter("pitch_scale_factor").value # Killswitch publisher - self.software_killswitch_signal_publisher_ = self.create_publisher(Bool, "softwareKillSwitch", 10) - self.software_killswitch_signal_publisher_.publish(Bool(data=True)) # Killswitch is active + self.software_killswitch_signal_publisher_ = self.create_publisher( + Bool, "softwareKillSwitch", 10 + ) + self.software_killswitch_signal_publisher_.publish( + Bool(data=True) + ) # Killswitch is active # Operational mode publisher - self.operational_mode_signal_publisher_ = self.create_publisher(String, "softwareOperationMode", 10) + self.operational_mode_signal_publisher_ = self.create_publisher( + String, "softwareOperationMode", 10 + ) # Signal that we are in XBOX mode self.operational_mode_signal_publisher_.publish(String(data="XBOX")) @@ -126,8 +134,7 @@ def create_wrench_message( pitch: float, yaw: float, ) -> Wrench: - """ - Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. + """Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. Args: surge (float): The x component of the force vector. @@ -150,27 +157,21 @@ def create_wrench_message( return wrench_msg def transition_to_xbox_mode(self) -> None: - """ - Turns off the controller and signals that the operational mode has switched to Xbox mode. - """ + """Turns off the controller and signals that the operational mode has switched to Xbox mode.""" self.operational_mode_signal_publisher_.publish(String(data="XBOX")) self.state_ = States.XBOX_MODE def transition_to_autonomous_mode(self) -> None: - """ - Publishes a zero force wrench message and signals that the system is turning on autonomous mode. - """ + """Publishes a zero force wrench message and signals that the system is turning on autonomous mode.""" wrench_msg = self.create_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) self.wrench_publisher_.publish(wrench_msg) self.operational_mode_signal_publisher_.publish(String(data="autonomous mode")) self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: - """ - Callback function that receives joy messages and converts them into - wrench messages to be sent to the thruster allocation node. - Handles software killswitch and control mode buttons, - and transitions between different states of operation. + """Callback function that receives joy messages and converts them into wrench messages to be sent to the thruster allocation node. + + Handles software killswitch and control mode buttons, and transitions between different states of operation. Args: msg: A ROS message containing the joy input data. @@ -178,7 +179,6 @@ def joystick_cb(self, msg: Joy) -> Wrench: Returns: A ROS message containing the wrench data that was sent to the thruster allocation node. """ - current_time = self.get_clock().now().to_msg().sec buttons = {} @@ -219,12 +219,36 @@ def joystick_cb(self, msg: Joy) -> Wrench: right_shoulder = buttons.get("RB", 0) # Extract axis values - surge = axes.get("vertical_axis_left_stick", 0.0) * self.joystick_surge_scaling_ * self.precise_manuevering_scaling_ - sway = -axes.get("horizontal_axis_left_stick", 0.0) * self.joystick_sway_scaling_ * self.precise_manuevering_scaling_ - heave = (left_trigger - right_trigger) * self.joystick_heave_scaling_ * self.precise_manuevering_scaling_ - roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_ * self.precise_manuevering_scaling_ - pitch = -axes.get("vertical_axis_right_stick", 0.0) * self.joystick_pitch_scaling_ * self.precise_manuevering_scaling_ - yaw = -axes.get("horizontal_axis_right_stick", 0.0) * self.joystick_yaw_scaling_ * self.precise_manuevering_scaling_ + surge = ( + axes.get("vertical_axis_left_stick", 0.0) + * self.joystick_surge_scaling_ + * self.precise_manuevering_scaling_ + ) + sway = ( + -axes.get("horizontal_axis_left_stick", 0.0) + * self.joystick_sway_scaling_ + * self.precise_manuevering_scaling_ + ) + heave = ( + (left_trigger - right_trigger) + * self.joystick_heave_scaling_ + * self.precise_manuevering_scaling_ + ) + roll = ( + (right_shoulder - left_shoulder) + * self.joystick_roll_scaling_ + * self.precise_manuevering_scaling_ + ) + pitch = ( + -axes.get("vertical_axis_right_stick", 0.0) + * self.joystick_pitch_scaling_ + * self.precise_manuevering_scaling_ + ) + yaw = ( + -axes.get("horizontal_axis_right_stick", 0.0) + * self.joystick_yaw_scaling_ + * self.precise_manuevering_scaling_ + ) # Debounce for the buttons if current_time - self.last_button_press_time_ < self.debounce_duration_: @@ -234,7 +258,12 @@ def joystick_cb(self, msg: Joy) -> Wrench: precise_manuevering_mode_button = False # If any button is pressed, update the last button press time - if software_control_mode_button or xbox_control_mode_button or software_killswitch_button or precise_manuevering_mode_button: + if ( + software_control_mode_button + or xbox_control_mode_button + or software_killswitch_button + or precise_manuevering_mode_button + ): self.last_button_press_time_ = current_time # Toggle killswitch on and off @@ -260,7 +289,9 @@ def joystick_cb(self, msg: Joy) -> Wrench: self.precise_manuevering_mode_ = not self.precise_manuevering_mode_ mode = "ENABLED" if self.precise_manuevering_mode_ else "DISABLED" self.get_logger().info(f"Precise maneuvering mode {mode}.") - self.precise_manuevering_scaling_ = 0.5 if self.precise_manuevering_mode_ else 1.0 + self.precise_manuevering_scaling_ = ( + 0.5 if self.precise_manuevering_mode_ else 1.0 + ) # Publish wrench message from joystick_interface to thrust allocation wrench_msg = self.create_wrench_message(surge, sway, heave, roll, pitch, yaw) @@ -282,10 +313,9 @@ def joystick_cb(self, msg: Joy) -> Wrench: def main() -> None: - """ - Initializes the ROS 2 client library, creates an instance of the JoystickInterface node, - and starts spinning the node to process callbacks. Once the node is shut down, it destroys - the node and shuts down the ROS 2 client library. + """Initializes the ROS 2 client library, creates an instance of the JoystickInterface node, and starts spinning the node to process callbacks. + + Once the node is shut down, it destroys the node and shuts down the ROS 2 client library. This function is the entry point for the joystick interface application. """ diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py index 1f0ecbc6e..f99e3b32d 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -6,8 +6,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the joystick_interface_auv node. + """Generates a launch description for the joystick_interface_auv node. This function creates a ROS 2 launch description that includes the joystick_interface_auv node. The node is configured to use the diff --git a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py index 707e9e28f..bf4c38e47 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -6,8 +6,7 @@ class TestJoystickInterface: # test that the wrench msg is created successfully def test_wrench_msg(self) -> None: - """ - Test the creation of a Wrench message using the JoystickInterface. + """Test the creation of a Wrench message using the JoystickInterface. This test initializes the ROS 2 client library, creates a Wrench message with specified force and torque values using the JoystickInterface, and @@ -34,8 +33,7 @@ def test_wrench_msg(self) -> None: # Test that the callback function will be able to interpret the joy msg def test_input_from_controller_into_wrench_msg(self) -> None: - """ - Test the conversion of joystick input to wrench message. + """Test the conversion of joystick input to wrench message. This test initializes the ROS 2 client library, creates a Joy message with specific axes and button values, and verifies that the joystick callback @@ -70,8 +68,7 @@ def test_input_from_controller_into_wrench_msg(self) -> None: # When the killswitch button is activated in the buttons list, it should output a wrench msg with only zeros def test_killswitch_button(self) -> None: - """ - Test the killswitch button functionality of the JoystickInterface. + """Test the killswitch button functionality of the JoystickInterface. This test initializes the ROS 2 client library, creates an instance of the JoystickInterface, and sets its state to XBOX_MODE. It then creates a Joy @@ -106,8 +103,7 @@ def test_killswitch_button(self) -> None: # When we move into XBOX mode it should still be able to return this wrench message def test_moving_in_of_xbox_mode(self) -> None: - """ - Test the joystick callback function in XBOX mode. + """Test the joystick callback function in XBOX mode. This test initializes the ROS 2 client library, creates an instance of the JoystickInterface, and sets its state to XBOX_MODE. It then creates a Joy diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp index c2f1e46f3..e06511362 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/eigen_vector6d_typedef.hpp @@ -9,7 +9,7 @@ #include namespace Eigen { - typedef Eigen::Matrix Vector6d; +typedef Eigen::Matrix Vector6d; } -#endif // VORTEX_EIGEN_TYPEDEFS_H +#endif // VORTEX_EIGEN_TYPEDEFS_H diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp index 6f53f56aa..e1d96ce9f 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/pseudoinverse_allocator.hpp @@ -15,25 +15,25 @@ * the input torques using the pseudoinverse allocator. */ class PseudoinverseAllocator { -public: - /** - * @brief Constructor for the PseudoinverseAllocator class. - * - * @param T_pinv The pseudoinverse of the thruster configuration matrix. - */ - explicit PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv); + public: + /** + * @brief Constructor for the PseudoinverseAllocator class. + * + * @param T_pinv The pseudoinverse of the thruster configuration matrix. + */ + explicit PseudoinverseAllocator(const Eigen::MatrixXd& T_pinv); - /** - * @brief Calculates the allocated thrust given the input torques using the - * pseudoinverse allocator. - * - * @param tau The input torques as a vector. - * @return The allocated thrust as a vector. - */ - Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd &tau); + /** + * @brief Calculates the allocated thrust given the input torques using the + * pseudoinverse allocator. + * + * @param tau The input torques as a vector. + * @return The allocated thrust as a vector. + */ + Eigen::VectorXd calculate_allocated_thrust(const Eigen::VectorXd& tau); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::MatrixXd T_pinv; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::MatrixXd T_pinv; }; -#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP +#endif // VORTEX_ALLOCATOR_PSEUDOINVERSE_ALLOCATOR_HPP diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp index 790d4b50f..0ef1a37f3 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_ros.hpp @@ -6,63 +6,63 @@ #ifndef VORTEX_ALLOCATOR_ROS_HPP #define VORTEX_ALLOCATOR_ROS_HPP -#include "thrust_allocator_auv/eigen_vector6d_typedef.hpp" -#include "thrust_allocator_auv/pseudoinverse_allocator.hpp" -#include "thrust_allocator_auv/thrust_allocator_utils.hpp" #include #include #include #include +#include "thrust_allocator_auv/eigen_vector6d_typedef.hpp" +#include "thrust_allocator_auv/pseudoinverse_allocator.hpp" +#include "thrust_allocator_auv/thrust_allocator_utils.hpp" class ThrustAllocator : public rclcpp::Node { -public: - explicit ThrustAllocator(); + public: + explicit ThrustAllocator(); - /** - * @brief Calculates the allocated - * thrust based on the body frame forces. It then saturates the output vector - * between min and max values and publishes the thruster forces to the topic - * "thrust/thruster_forces". - */ - void calculate_thrust_timer_cb(); + /** + * @brief Calculates the allocated + * thrust based on the body frame forces. It then saturates the output + * vector between min and max values and publishes the thruster forces to + * the topic "thrust/thruster_forces". + */ + void calculate_thrust_timer_cb(); -private: - /** - * @brief Callback function for the wrench input subscription. Extracts the - * surge, sway, heave, roll, pitch and yaw values from the received wrench msg - * and stores them in the body_frame_forces_ Eigen vector. - * @param msg The received geometry_msgs::msg::Wrench message. - */ - void wrench_cb(const geometry_msgs::msg::Wrench &msg); + private: + /** + * @brief Callback function for the wrench input subscription. Extracts the + * surge, sway, heave, roll, pitch and yaw values from the received wrench + * msg and stores them in the body_frame_forces_ Eigen vector. + * @param msg The received geometry_msgs::msg::Wrench message. + */ + void wrench_cb(const geometry_msgs::msg::Wrench& msg); - /** - * @brief Checks if the given Eigen vector contains any NaN or Inf values - * @param v The Eigen vector to check. - * @return True if the vector is healthy, false otherwise. - */ - bool healthy_wrench(const Eigen::VectorXd &v) const; + /** + * @brief Checks if the given Eigen vector contains any NaN or Inf values + * @param v The Eigen vector to check. + * @return True if the vector is healthy, false otherwise. + */ + bool healthy_wrench(const Eigen::VectorXd& v) const; - rclcpp::Publisher::SharedPtr - thruster_forces_publisher_; - rclcpp::Subscription::SharedPtr - wrench_subscriber_; - rclcpp::TimerBase::SharedPtr calculate_thrust_timer_; + rclcpp::Publisher::SharedPtr + thruster_forces_publisher_; + rclcpp::Subscription::SharedPtr + wrench_subscriber_; + rclcpp::TimerBase::SharedPtr calculate_thrust_timer_; - size_t count_; - Eigen::Vector3d center_of_mass_; - int num_dimensions_; - int num_thrusters_; - int min_thrust_; - int max_thrust_; + size_t count_; + Eigen::Vector3d center_of_mass_; + int num_dimensions_; + int num_thrusters_; + int min_thrust_; + int max_thrust_; - std::chrono::milliseconds thrust_update_period_; + std::chrono::milliseconds thrust_update_period_; - Eigen::MatrixXd thruster_force_direction_; - Eigen::MatrixXd thruster_position_; - Eigen::MatrixXd thrust_configuration_; + Eigen::MatrixXd thruster_force_direction_; + Eigen::MatrixXd thruster_position_; + Eigen::MatrixXd thrust_configuration_; - Eigen::Vector6d body_frame_forces_; - PseudoinverseAllocator pseudoinverse_allocator_; + Eigen::Vector6d body_frame_forces_; + PseudoinverseAllocator pseudoinverse_allocator_; }; -#endif // VORTEX_ALLOCATOR_ROS_HPP +#endif // VORTEX_ALLOCATOR_ROS_HPP diff --git a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp index 383782350..9ca8aac07 100644 --- a/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp +++ b/motion/thrust_allocator_auv/include/thrust_allocator_auv/thrust_allocator_utils.hpp @@ -22,36 +22,36 @@ * @return true if the matrix has any NaN or INF elements, false otherwise. */ template -inline bool is_invalid_matrix(const Eigen::MatrixBase &M) { - bool has_nan = !(M.array() == M.array()).all(); - bool has_inf = M.array().isInf().any(); - return has_nan || has_inf; +inline bool is_invalid_matrix(const Eigen::MatrixBase& M) { + bool has_nan = !(M.array() == M.array()).all(); + bool has_inf = M.array().isInf().any(); + return has_nan || has_inf; } inline Eigen::MatrixXd calculate_thrust_allocation_matrix( - const Eigen::MatrixXd &thruster_force_direction, - const Eigen::MatrixXd &thruster_position, - const Eigen::Vector3d ¢er_of_mass) { - // Initialize thrust allocation matrix - Eigen::MatrixXd thrust_allocation_matrix = Eigen::MatrixXd::Zero(6, 8); - - // Calculate thrust and moment contributions from each thruster - for (int i = 0; i < thruster_position.cols(); i++) { - Eigen::Vector3d pos = - thruster_position.col(i); // Thrust vector in body frame - Eigen::Vector3d F = - thruster_force_direction.col(i); // Position vector in body frame - - // Calculate position vector relative to the center of mass - pos -= center_of_mass; - - // Fill in the thrust allocation matrix - thrust_allocation_matrix.block<3, 1>(0, i) = F; - thrust_allocation_matrix.block<3, 1>(3, i) = pos.cross(F); - } - - thrust_allocation_matrix = thrust_allocation_matrix.array(); - return thrust_allocation_matrix; + const Eigen::MatrixXd& thruster_force_direction, + const Eigen::MatrixXd& thruster_position, + const Eigen::Vector3d& center_of_mass) { + // Initialize thrust allocation matrix + Eigen::MatrixXd thrust_allocation_matrix = Eigen::MatrixXd::Zero(6, 8); + + // Calculate thrust and moment contributions from each thruster + for (int i = 0; i < thruster_position.cols(); i++) { + Eigen::Vector3d pos = + thruster_position.col(i); // Thrust vector in body frame + Eigen::Vector3d F = + thruster_force_direction.col(i); // Position vector in body frame + + // Calculate position vector relative to the center of mass + pos -= center_of_mass; + + // Fill in the thrust allocation matrix + thrust_allocation_matrix.block<3, 1>(0, i) = F; + thrust_allocation_matrix.block<3, 1>(3, i) = pos.cross(F); + } + + thrust_allocation_matrix = thrust_allocation_matrix.array(); + return thrust_allocation_matrix; } /** @@ -61,12 +61,13 @@ inline Eigen::MatrixXd calculate_thrust_allocation_matrix( * @throws char* if the pseudoinverse is invalid. * @return The pseudoinverse of the given matrix. */ -inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) { - Eigen::MatrixXd pseudoinverse = T.transpose() * (T * T.transpose()).inverse(); - if (is_invalid_matrix(pseudoinverse)) { - throw std::runtime_error("Invalid Pseudoinverse Calculated"); - } - return pseudoinverse; +inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd& T) { + Eigen::MatrixXd pseudoinverse = + T.transpose() * (T * T.transpose()).inverse(); + if (is_invalid_matrix(pseudoinverse)) { + throw std::runtime_error("Invalid Pseudoinverse Calculated"); + } + return pseudoinverse; } /** @@ -79,17 +80,18 @@ inline Eigen::MatrixXd calculate_right_pseudoinverse(const Eigen::MatrixXd &T) { * @return True if all vector values are within the given range, false * otherwise. */ -inline bool saturate_vector_values(Eigen::VectorXd &vec, double min, +inline bool saturate_vector_values(Eigen::VectorXd& vec, + double min, double max) { - bool all_values_in_range = - std::all_of(vec.begin(), vec.end(), - [min, max](double val) { return val >= min && val <= max; }); + bool all_values_in_range = std::all_of( + vec.begin(), vec.end(), + [min, max](double val) { return val >= min && val <= max; }); - std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) { - return std::min(std::max(val, min), max); - }); + std::transform(vec.begin(), vec.end(), vec.begin(), [min, max](double val) { + return std::min(std::max(val, min), max); + }); - return all_values_in_range; + return all_values_in_range; } /** @@ -100,12 +102,12 @@ inline bool saturate_vector_values(Eigen::VectorXd &vec, double min, * @param msg The vortex_msgs::msg::ThrusterForces message to store the * converted values. */ -inline void array_eigen_to_msg(const Eigen::VectorXd &u, - vortex_msgs::msg::ThrusterForces &msg) { - int r = u.size(); - std::vector u_vec(r); - std::copy_n(u.begin(), r, u_vec.begin()); - msg.thrust = u_vec; +inline void array_eigen_to_msg(const Eigen::VectorXd& u, + vortex_msgs::msg::ThrusterForces& msg) { + int r = u.size(); + std::vector u_vec(r); + std::copy_n(u.begin(), r, u_vec.begin()); + msg.thrust = u_vec; } /** @@ -116,23 +118,25 @@ inline void array_eigen_to_msg(const Eigen::VectorXd &u, * @param cols The number of columns in the resulting Eigen matrix. * @return The resulting Eigen matrix. */ -inline Eigen::MatrixXd -double_array_to_eigen_matrix(const std::vector &matrix, int rows, - int cols) { - return Eigen::Map>(matrix.data(), rows, - cols); +inline Eigen::MatrixXd double_array_to_eigen_matrix( + const std::vector& matrix, + int rows, + int cols) { + return Eigen::Map>( + matrix.data(), rows, cols); } -inline Eigen::Vector3d -double_array_to_eigen_vector3d(const std::vector &vector) { - // Ensure the input vector has exactly three elements - if (vector.size() != 3) { - throw std::invalid_argument("Input vector must have exactly 3 elements"); - } +inline Eigen::Vector3d double_array_to_eigen_vector3d( + const std::vector& vector) { + // Ensure the input vector has exactly three elements + if (vector.size() != 3) { + throw std::invalid_argument( + "Input vector must have exactly 3 elements"); + } - // Map the vector to Eigen::Vector3d - return Eigen::Map(vector.data()); + // Map the vector to Eigen::Vector3d + return Eigen::Map(vector.data()); } -#endif // VORTEX_ALLOCATOR_UTILS_HPP +#endif // VORTEX_ALLOCATOR_UTILS_HPP diff --git a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py index decab0783..7a9c64b3c 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -6,8 +6,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the thrust_allocator_auv_node. + """Generates a launch description for the thrust_allocator_auv_node. This function creates a ROS 2 launch description that includes the thrust_allocator_auv_node. The node is configured with parameters diff --git a/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp b/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp index a8bcb1a9f..47ab046f0 100644 --- a/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp +++ b/motion/thrust_allocator_auv/src/pseudoinverse_allocator.cpp @@ -1,10 +1,10 @@ #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" -PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd &T_pinv) +PseudoinverseAllocator::PseudoinverseAllocator(const Eigen::MatrixXd& T_pinv) : T_pinv(T_pinv) {} -Eigen::VectorXd -PseudoinverseAllocator::calculate_allocated_thrust(const Eigen::VectorXd &tau) { - Eigen::VectorXd u = T_pinv * tau; - return u; +Eigen::VectorXd PseudoinverseAllocator::calculate_allocated_thrust( + const Eigen::VectorXd& tau) { + Eigen::VectorXd u = T_pinv * tau; + return u; } diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp index 1d4046c25..503a2ab36 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_auv_node.cpp @@ -1,11 +1,11 @@ #include "thrust_allocator_auv/thrust_allocator_ros.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto allocator = std::make_shared(); - RCLCPP_INFO(allocator->get_logger(), "Thrust allocator initiated"); - rclcpp::spin(allocator); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto allocator = std::make_shared(); + RCLCPP_INFO(allocator->get_logger(), "Thrust allocator initiated"); + rclcpp::spin(allocator); + rclcpp::shutdown(); + return 0; } diff --git a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp index 0f6ee771a..af835f635 100644 --- a/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp +++ b/motion/thrust_allocator_auv/src/thrust_allocator_ros.cpp @@ -1,7 +1,7 @@ #include "thrust_allocator_auv/thrust_allocator_ros.hpp" +#include #include "thrust_allocator_auv/pseudoinverse_allocator.hpp" #include "thrust_allocator_auv/thrust_allocator_utils.hpp" -#include #include #include @@ -11,99 +11,101 @@ using namespace std::chrono_literals; ThrustAllocator::ThrustAllocator() : Node("thrust_allocator_node"), pseudoinverse_allocator_(Eigen::MatrixXd::Zero(6, 8)) { - declare_parameter("physical.center_of_mass", std::vector{0}); - declare_parameter("propulsion.dimensions.num", 3); - declare_parameter("propulsion.thrusters.num", 8); - declare_parameter("propulsion.thrusters.min", -100); - declare_parameter("propulsion.thrusters.max", 100); - declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); - declare_parameter("propulsion.thrusters.thruster_force_direction", - std::vector{0}); - declare_parameter("propulsion.thrusters.thruster_position", - std::vector{0}); - - center_of_mass_ = double_array_to_eigen_vector3d( - get_parameter("physical.center_of_mass").as_double_array()); - - num_dimensions_ = get_parameter("propulsion.dimensions.num").as_int(); - num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); - min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); - max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); - thrust_update_period_ = std::chrono::milliseconds(static_cast( - 1000 / - get_parameter("propulsion.thrusters.thrust_update_rate").as_double())); - - thruster_force_direction_ = double_array_to_eigen_matrix( - get_parameter("propulsion.thrusters.thruster_force_direction") - .as_double_array(), - num_dimensions_, num_thrusters_); - - thruster_position_ = double_array_to_eigen_matrix( - get_parameter("propulsion.thrusters.thruster_position").as_double_array(), - num_dimensions_, num_thrusters_); - - thrust_configuration_ = calculate_thrust_allocation_matrix( - thruster_force_direction_, thruster_position_, center_of_mass_); - - wrench_subscriber_ = this->create_subscription( - "thrust/wrench_input", 1, - std::bind(&ThrustAllocator::wrench_cb, this, std::placeholders::_1)); - - thruster_forces_publisher_ = - this->create_publisher( - "thrust/thruster_forces", 5); - - calculate_thrust_timer_ = this->create_wall_timer( - thrust_update_period_, - std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this)); - - pseudoinverse_allocator_.T_pinv = - calculate_right_pseudoinverse(thrust_configuration_); - - body_frame_forces_.setZero(); + declare_parameter("physical.center_of_mass", std::vector{0}); + declare_parameter("propulsion.dimensions.num", 3); + declare_parameter("propulsion.thrusters.num", 8); + declare_parameter("propulsion.thrusters.min", -100); + declare_parameter("propulsion.thrusters.max", 100); + declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); + declare_parameter("propulsion.thrusters.thruster_force_direction", + std::vector{0}); + declare_parameter("propulsion.thrusters.thruster_position", + std::vector{0}); + + center_of_mass_ = double_array_to_eigen_vector3d( + get_parameter("physical.center_of_mass").as_double_array()); + + num_dimensions_ = get_parameter("propulsion.dimensions.num").as_int(); + num_thrusters_ = get_parameter("propulsion.thrusters.num").as_int(); + min_thrust_ = get_parameter("propulsion.thrusters.min").as_int(); + max_thrust_ = get_parameter("propulsion.thrusters.max").as_int(); + thrust_update_period_ = std::chrono::milliseconds(static_cast( + 1000 / + get_parameter("propulsion.thrusters.thrust_update_rate").as_double())); + + thruster_force_direction_ = double_array_to_eigen_matrix( + get_parameter("propulsion.thrusters.thruster_force_direction") + .as_double_array(), + num_dimensions_, num_thrusters_); + + thruster_position_ = double_array_to_eigen_matrix( + get_parameter("propulsion.thrusters.thruster_position") + .as_double_array(), + num_dimensions_, num_thrusters_); + + thrust_configuration_ = calculate_thrust_allocation_matrix( + thruster_force_direction_, thruster_position_, center_of_mass_); + + wrench_subscriber_ = this->create_subscription( + "thrust/wrench_input", 1, + std::bind(&ThrustAllocator::wrench_cb, this, std::placeholders::_1)); + + thruster_forces_publisher_ = + this->create_publisher( + "thrust/thruster_forces", 5); + + calculate_thrust_timer_ = this->create_wall_timer( + thrust_update_period_, + std::bind(&ThrustAllocator::calculate_thrust_timer_cb, this)); + + pseudoinverse_allocator_.T_pinv = + calculate_right_pseudoinverse(thrust_configuration_); + + body_frame_forces_.setZero(); } void ThrustAllocator::calculate_thrust_timer_cb() { - Eigen::VectorXd thruster_forces = - pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_); - - if (is_invalid_matrix(thruster_forces)) { - RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); - return; - } - - if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) { - RCLCPP_WARN(get_logger(), "Thruster forces vector required saturation."); - } - - vortex_msgs::msg::ThrusterForces msg_out; - array_eigen_to_msg(thruster_forces, msg_out); - thruster_forces_publisher_->publish(msg_out); + Eigen::VectorXd thruster_forces = + pseudoinverse_allocator_.calculate_allocated_thrust(body_frame_forces_); + + if (is_invalid_matrix(thruster_forces)) { + RCLCPP_ERROR(get_logger(), "ThrusterForces vector invalid"); + return; + } + + if (!saturate_vector_values(thruster_forces, min_thrust_, max_thrust_)) { + RCLCPP_WARN(get_logger(), + "Thruster forces vector required saturation."); + } + + vortex_msgs::msg::ThrusterForces msg_out; + array_eigen_to_msg(thruster_forces, msg_out); + thruster_forces_publisher_->publish(msg_out); } -void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench &msg) { - Eigen::Vector6d msg_vector; - msg_vector(0) = msg.force.x; // surge - msg_vector(1) = msg.force.y; // sway - msg_vector(2) = msg.force.z; // heave - msg_vector(3) = msg.torque.x; // roll - msg_vector(4) = msg.torque.y; // pitch - msg_vector(5) = msg.torque.z; // yaw - - if (!healthy_wrench(msg_vector)) { - RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); - return; - } - std::swap(msg_vector, body_frame_forces_); +void ThrustAllocator::wrench_cb(const geometry_msgs::msg::Wrench& msg) { + Eigen::Vector6d msg_vector; + msg_vector(0) = msg.force.x; // surge + msg_vector(1) = msg.force.y; // sway + msg_vector(2) = msg.force.z; // heave + msg_vector(3) = msg.torque.x; // roll + msg_vector(4) = msg.torque.y; // pitch + msg_vector(5) = msg.torque.z; // yaw + + if (!healthy_wrench(msg_vector)) { + RCLCPP_ERROR(get_logger(), "ASV wrench vector invalid, ignoring."); + return; + } + std::swap(msg_vector, body_frame_forces_); } -bool ThrustAllocator::healthy_wrench(const Eigen::VectorXd &v) const { - if (is_invalid_matrix(v)) - return false; +bool ThrustAllocator::healthy_wrench(const Eigen::VectorXd& v) const { + if (is_invalid_matrix(v)) + return false; - bool within_max_thrust = std::none_of(v.begin(), v.end(), [this](double val) { - return std::abs(val) > max_thrust_; - }); + bool within_max_thrust = std::none_of( + v.begin(), v.end(), + [this](double val) { return std::abs(val) > max_thrust_; }); - return within_max_thrust; + return within_max_thrust; } diff --git a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py index ebb5a7aa2..944164e5c 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -6,8 +6,7 @@ def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the thruster_interface_auv_node. + """Generates a launch description for the thruster_interface_auv_node. This function creates a ROS 2 launch description that includes the thruster_interface_auv_node. The node is configured to output to the screen diff --git a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py index d74713dd5..90feb69b9 100644 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py @@ -49,13 +49,12 @@ def __init__( self.system_operational_voltage = 20 # Get the full path to the ROS2 package this file is located at - self.ros2_package_name_for_thruster_datasheet = ros2_package_name_for_thruster_datasheet + self.ros2_package_name_for_thruster_datasheet = ( + ros2_package_name_for_thruster_datasheet + ) def _interpolate_forces_to_pwm(self, thruster_forces_array: list) -> list: - """ - Takes in Array of forces in Newtosn [N] - takes 8 floats in form of: - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + """Takes in Array of forces in Newtosn [N] takes 8 floats in form of: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. Returns an Array of PWM Gives out 8 ints in form of: @@ -72,8 +71,12 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array: list) -> list: thruster_forces_array[i] = thruster_forces / 9.80665 # Interpolate data - thruster_datasheet_file_forces = thruster_datasheet_file_data[" Force (Kg f)"].values - thruster_datasheet_file_data_pwm = thruster_datasheet_file_data[" PWM (µs)"].values + thruster_datasheet_file_forces = thruster_datasheet_file_data[ + " Force (Kg f)" + ].values + thruster_datasheet_file_data_pwm = thruster_datasheet_file_data[ + " PWM (µs)" + ].values interpolated_pwm = numpy.interp( thruster_forces_array, thruster_datasheet_file_forces, @@ -101,10 +104,7 @@ def _send_data_to_escs(self, thruster_pwm_array: list) -> None: self.bus.write_i2c_block_data(self.pico_i2c_address, 0, i2c_data_array) def drive_thrusters(self, thruster_forces_array: list) -> list: - """ - Takes in Array of forces in Newtosn [N] - takes 8 floats in form of: - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + """Takes in Array of forces in Newtosn [N] takes 8 floats in form of: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. Converts Forces to PWM signals PWM signals sent to PCA9685 module through I2C @@ -116,16 +116,20 @@ def drive_thrusters(self, thruster_forces_array: list) -> list: Gives out 8 ints in form of: [0, 0, 0, 0, 0, 0, 0, 0] """ - # Apply thruster mapping and direction - thruster_forces_array = [thruster_forces_array[i] * self.thruster_direction[i] for i in self.thruster_mapping] + thruster_forces_array = [ + thruster_forces_array[i] * self.thruster_direction[i] + for i in self.thruster_mapping + ] # Convert Forces to PWM thruster_pwm_array = self._interpolate_forces_to_pwm(thruster_forces_array) # Apply thruster offset for esc_channel, thruster_pwm in enumerate(thruster_pwm_array): - thruster_pwm_array[esc_channel] = thruster_pwm + self.thruster_pwm_offset[esc_channel] + thruster_pwm_array[esc_channel] = ( + thruster_pwm + self.thruster_pwm_offset[esc_channel] + ) # Apply thruster offset and limit PWM if needed for esc_channel in enumerate(thruster_pwm_array): diff --git a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py index 6751f82b1..31ba8a5c1 100755 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ b/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py @@ -4,13 +4,14 @@ from ament_index_python.packages import get_package_share_directory from rclpy.node import Node from std_msgs.msg import Int16MultiArray -from thruster_interface_auv.thruster_interface_auv_driver_lib import ( - ThrusterInterfaceAUVDriver, -) # Custom libraries from vortex_msgs.msg import ThrusterForces +from thruster_interface_auv.thruster_interface_auv_driver_lib import ( + ThrusterInterfaceAUVDriver, +) + class ThrusterInterfaceAUVNode(Node): def __init__(self) -> None: @@ -20,13 +21,21 @@ def __init__(self) -> None: # Create a subscriber that takes data from thruster forces # Then convert this Forces into PWM signals and control the thrusters # Publish PWM values as deebuging feature - self.thruster_forces_subscriber = self.create_subscription(ThrusterForces, "thrust/thruster_forces", self._thruster_forces_callback, 10) + self.thruster_forces_subscriber = self.create_subscription( + ThrusterForces, "thrust/thruster_forces", self._thruster_forces_callback, 10 + ) self.thruster_pwm_publisher = self.create_publisher(Int16MultiArray, "pwm", 10) # Get thruster mapping, direction, offset and clamping parameters - self.declare_parameter("propulsion.thrusters.thruster_to_pin_mapping", [7, 6, 5, 4, 3, 2, 1, 0]) - self.declare_parameter("propulsion.thrusters.thruster_direction", [1, 1, 1, 1, 1, 1, 1, 1]) - self.declare_parameter("propulsion.thrusters.thruster_PWM_offset", [0, 0, 0, 0, 0, 0, 0, 0]) + self.declare_parameter( + "propulsion.thrusters.thruster_to_pin_mapping", [7, 6, 5, 4, 3, 2, 1, 0] + ) + self.declare_parameter( + "propulsion.thrusters.thruster_direction", [1, 1, 1, 1, 1, 1, 1, 1] + ) + self.declare_parameter( + "propulsion.thrusters.thruster_PWM_offset", [0, 0, 0, 0, 0, 0, 0, 0] + ) self.declare_parameter( "propulsion.thrusters.thruster_PWM_min", [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], @@ -38,16 +47,30 @@ def __init__(self) -> None: self.declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0) - self.thruster_mapping = self.get_parameter("propulsion.thrusters.thruster_to_pin_mapping").value - self.thruster_direction = self.get_parameter("propulsion.thrusters.thruster_direction").value - self.thruster_pwm_offset = self.get_parameter("propulsion.thrusters.thruster_PWM_offset").value - self.thruster_pwm_min = self.get_parameter("propulsion.thrusters.thruster_PWM_min").value - self.thruster_pwm_max = self.get_parameter("propulsion.thrusters.thruster_PWM_max").value - self.thrust_timer_period = 1.0 / self.get_parameter("propulsion.thrusters.thrust_update_rate").value + self.thruster_mapping = self.get_parameter( + "propulsion.thrusters.thruster_to_pin_mapping" + ).value + self.thruster_direction = self.get_parameter( + "propulsion.thrusters.thruster_direction" + ).value + self.thruster_pwm_offset = self.get_parameter( + "propulsion.thrusters.thruster_PWM_offset" + ).value + self.thruster_pwm_min = self.get_parameter( + "propulsion.thrusters.thruster_PWM_min" + ).value + self.thruster_pwm_max = self.get_parameter( + "propulsion.thrusters.thruster_PWM_max" + ).value + self.thrust_timer_period = ( + 1.0 / self.get_parameter("propulsion.thrusters.thrust_update_rate").value + ) # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( - ros2_package_name_for_thruster_datasheet=get_package_share_directory("thruster_interface_auv"), + ros2_package_name_for_thruster_datasheet=get_package_share_directory( + "thruster_interface_auv" + ), thruster_mapping=self.thruster_mapping, thruster_direction=self.thruster_direction, thruster_pwm_offset=self.thruster_pwm_offset, @@ -71,7 +94,9 @@ def _thruster_forces_callback(self, msg: ThrusterForces) -> None: def _timer_callback(self) -> None: # Send thruster forces to be converted into PWM signal and sent to control the thrusters # PWM signal gets saved and is published in the "/pwm" topic as a debugging feature to see if everything is alright with the PWM signal - thruster_pwm_array = self.thruster_driver.drive_thrusters(self.thruster_forces_array) + thruster_pwm_array = self.thruster_driver.drive_thrusters( + self.thruster_forces_array + ) pwm_message = Int16MultiArray() pwm_message.data = thruster_pwm_array @@ -79,8 +104,7 @@ def _timer_callback(self) -> None: def main(args: list = None) -> None: - """ - Entry point for the thruster interface AUV node. + """Entry point for the thruster interface AUV node. This function initializes the ROS 2 client library, creates an instance of the ThrusterInterfaceAUVNode, and starts spinning the node to process callbacks.