From f317a459b8ae6f5957aed500bf85dd205b58d8c3 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 27 Sep 2024 18:33:09 +0200 Subject: [PATCH 01/54] extracts 3rd approx polynomials from csv data, saved in coeffs.txt --- motion/thruster_interface_auv/analysis.py | 72 +++++++++++++++++++++++ motion/thruster_interface_auv/coeffs.txt | 23 ++++++++ 2 files changed, 95 insertions(+) create mode 100644 motion/thruster_interface_auv/analysis.py create mode 100644 motion/thruster_interface_auv/coeffs.txt diff --git a/motion/thruster_interface_auv/analysis.py b/motion/thruster_interface_auv/analysis.py new file mode 100644 index 000000000..6cfbb9c23 --- /dev/null +++ b/motion/thruster_interface_auv/analysis.py @@ -0,0 +1,72 @@ +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np + +#-------------------------------- +#load data and transform to numpy + +files = [ + "motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv", + "motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv", + "motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv", + "motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv", + "motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv", + "motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv", +] +datas = [] +for f in files: + data = np.genfromtxt(f, delimiter=',', skip_header=1, usecols=(5, 0)) + datas.append(data) + +#-------------------------------- +#plot pwm values vs force +def plot_csvs(datas): + for i, data in enumerate(datas): + plt.plot(data[:,0], data[:,1], label=i*2+10) + + plt.xlabel("force") + plt.ylabel("pwm") + plt.legend() + plt.show() + +#plot_csvs(datas) + +#-------------------------------- +#calculate the two halfs deg-order poly approx and plot them on data + +def plot_single_vs_poly(xdata, ydata, deg): + zero_indices = np.where(xdata == 0.00)[0] + begin = zero_indices[0] + end = zero_indices[-1] + + xL = xdata[:begin] + xR = xdata[end:] + yL = ydata[:begin] + yR = ydata[end:] + + coeffs_L = np.polyfit(xL, yL, deg) + print("LEFT:", coeffs_L) + coeffs_R = np.polyfit(xR, yR, deg) + print("RIGHT:", coeffs_R) + polyL = np.poly1d(coeffs_L) + polyR = np.poly1d(coeffs_R) + + yL_fit = polyL(xL) + yR_fit = polyR(xR) + + rms = np.sqrt(np.mean((yL - yL_fit) ** 2)) + np.sqrt(np.mean((yR - yR_fit) ** 2)) + + plt.scatter(xdata, ydata, s=2) + plt.plot(xL, yL_fit, color='red') + plt.plot(xR, yR_fit, color='red') + plt.title(f"deg: {deg} rms {rms}") + + plt.xlabel("force") + plt.ylabel("pwm") + plt.show() + + return coeffs_L, coeffs_R + +np.set_printoptions(precision=5, suppress=True) +for data in datas: + plot_single_vs_poly(data[:,0], data[:,1], 3) \ No newline at end of file diff --git a/motion/thruster_interface_auv/coeffs.txt b/motion/thruster_interface_auv/coeffs.txt new file mode 100644 index 000000000..8571a842a --- /dev/null +++ b/motion/thruster_interface_auv/coeffs.txt @@ -0,0 +1,23 @@ +#10V +LEFT: [ 14.10463 72.78164 245.73575 1456.5868 ] +RIGHT: [ 7.39517 -47.59238 196.5525 1543.6743 ] + +#12V +LEFT: [ 7.35916 47.58976 199.18339 1460.57483] +RIGHT: [ 3.84262 -31.64647 160.76665 1539.18791] + +#14V +LEFT: [ 14.10463 72.78164 245.73575 1456.5868 ] +RIGHT: [ 7.39517 -47.59238 196.5525 1543.6743 ] + +#16V +LEFT: [ 3.31158 28.90927 152.1417 1467.70348] +RIGHT: [ 1.45002 -16.92386 117.95721 1533.55911] + +#18V +LEFT: [ 2.50017 25.18231 142.66031 1469.6513 ] +RIGHT: [ 1.22234 -16.56764 116.26078 1531.57015] + +#20V +LEFT: [ 2.55792 27.51178 147.67001 1472.23069] +RIGHT: [ 1.33421 -18.75129 121.29079 1528.99886] \ No newline at end of file From a939cec10a871fb748ac1ca2e22582dc4dc2969e Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 6 Oct 2024 18:48:48 +0200 Subject: [PATCH 02/54] rewrited interface in python - we'll move to cpp now --- motion/thruster_interface_auv/CMakeLists.txt | 1 + .../thruster_interface_auv/config/coeffs.yaml | 21 +++++++++ .../launch/thruster_interface_auv.launch.py | 7 ++- .../thruster_interface_auv_driver_lib.py | 34 +++++++++----- .../thruster_interface_auv_node.py | 47 +++++++++++++++++-- 5 files changed, 95 insertions(+), 15 deletions(-) create mode 100644 motion/thruster_interface_auv/config/coeffs.yaml diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index b23fee9e2..691a29f00 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -14,6 +14,7 @@ ament_python_install_package(thruster_interface_auv) install(DIRECTORY launch resources + config DESTINATION share/${PROJECT_NAME} ) diff --git a/motion/thruster_interface_auv/config/coeffs.yaml b/motion/thruster_interface_auv/config/coeffs.yaml new file mode 100644 index 000000000..384555d16 --- /dev/null +++ b/motion/thruster_interface_auv/config/coeffs.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + coeffs: + 10V: + LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] + 12V: + LEFT: [7.35916, 47.58976, 199.18339, 1460.57483] + RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] + 14V: + LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] + 16V: + LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] + RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] + 18V: + LEFT: [2.50017, 25.18231, 142.66031, 1469.6513] + RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] + 20V: + LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] + RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] \ No newline at end of file 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 a16df2483..62965f89d 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -17,7 +17,12 @@ def generate_launch_description(): "config", "robots", "orca.yaml", - ) + ), + path.join( + get_package_share_directory("thruster_interface_auv"), + "config", + "coeffs.yaml", + ), ], ) return LaunchDescription([thruster_interface_auv_node]) 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 9d72334f5..16ad70bb5 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 @@ -3,7 +3,7 @@ import smbus2 import pandas import numpy - +import yaml class ThrusterInterfaceAUVDriver: def __init__( @@ -17,6 +17,7 @@ def __init__( THRUSTER_PWM_OFFSET=[0, 0, 0, 0, 0, 0, 0, 0], PWM_MIN=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], PWM_MAX=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], + coeffs = None, ): # Initialice the I2C comunication self.bus = None @@ -53,6 +54,8 @@ def __init__( ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET ) + self.coeffs = coeffs + def _interpolate_forces_to_pwm(self, thruster_forces_array): """ Takes in Array of forces in Newtosn [N] @@ -73,14 +76,21 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): for i, thruster_forces in enumerate(thruster_forces_array): thruster_forces_array[i] = thruster_forces / 9.80665 - # Interpolate data - thrusterDatasheetFileForces = thrusterDatasheetFileData[" Force (Kg f)"].values - thrusterDatasheetFileDataPWM = thrusterDatasheetFileData[" PWM (µs)"].values - interpolated_pwm = numpy.interp( - thruster_forces_array, - thrusterDatasheetFileForces, - thrusterDatasheetFileDataPWM, - ) + # Select the appropriate pair of coeffs based on the operational voltage + left_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]['LEFT'] + right_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]['RIGHT'] + + # Calculate the interpolated PWM values using the polynomial coefficients + interpolated_pwm = [] + for force in thruster_forces_array: + if force < 0: + pwm = numpy.polyval(left_coeffs, force) + elif abs(force) < 1e-6: + pwm = 1500 - self.THRUSTER_PWM_OFFSET[0] + else: + pwm = numpy.polyval(right_coeffs, force) + + interpolated_pwm.append(pwm) # Convert PWM signal to integers as they are interpolated and can have floats interpolated_pwm = [int(pwm) for pwm in interpolated_pwm] @@ -133,6 +143,8 @@ def drive_thrusters(self, thruster_forces_array): thruster_pwm_array[ESC_channel] = ( thruster_pwm + self.THRUSTER_PWM_OFFSET[ESC_channel] ) + + print(thruster_pwm_array) # Apply thruster offset and limit PWM if needed for ESC_channel in range(len(thruster_pwm_array)): @@ -143,9 +155,9 @@ def drive_thrusters(self, thruster_forces_array): thruster_pwm_array[ESC_channel] = self.PWM_MAX[ESC_channel] # Send data through I2C to the microcontroller that then controls the ESC and extention the thrusters - try: + """ try: self._send_data_to_escs(thruster_pwm_array) except Exception as errorCode: - print(f"ERROR: Failed to send PWM values: {errorCode}") + print(f"ERROR: Failed to send PWM values: {errorCode}") """ return 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 7fb44a573..9a75c76c2 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 @@ -10,7 +10,7 @@ from thruster_interface_auv.thruster_interface_auv_driver_lib import ( ThrusterInterfaceAUVDriver, ) - +from rcl_interfaces.msg import ParameterDescriptor class ThrusterInterfaceAUVNode(Node): def __init__(self): @@ -23,7 +23,7 @@ def __init__(self): 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) + self.thruster_pwm_publisher = self.create_publisher(Int16MultiArray, "pwm_NEW", 10) # Get thruster mapping, direction, offset and clamping parameters self.declare_parameter( @@ -46,6 +46,19 @@ def __init__(self): self.declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0) + self.declare_parameter("coeffs.10V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.10V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.12V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.12V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.14V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.14V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.16V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.16V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.18V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.18V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.20V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) + self.declare_parameter("coeffs.20V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) + self.thruster_mapping = self.get_parameter( "propulsion.thrusters.thruster_to_pin_mapping" ).value @@ -65,6 +78,33 @@ def __init__(self): 1.0 / self.get_parameter("propulsion.thrusters.thrust_update_rate").value ) + coeffs = { + 10: { + "LEFT": self.get_parameter("coeffs.10V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.10V.RIGHT").value, + }, + 12: { + "LEFT": self.get_parameter("coeffs.12V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.12V.RIGHT").value, + }, + 14: { + "LEFT": self.get_parameter("coeffs.14V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.14V.RIGHT").value, + }, + 16: { + "LEFT": self.get_parameter("coeffs.16V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.16V.RIGHT").value, + }, + 18: { + "LEFT": self.get_parameter("coeffs.18V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.18V.RIGHT").value, + }, + 20: { + "LEFT": self.get_parameter("coeffs.20V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.20V.RIGHT").value, + }, + } + # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET=get_package_share_directory( @@ -75,6 +115,7 @@ def __init__(self): THRUSTER_PWM_OFFSET=self.thruster_PWM_offset, PWM_MIN=self.thruster_PWM_min, PWM_MAX=self.thruster_PWM_max, + coeffs=coeffs, ) # Start clock timer for driving thrusters every 0.2 seconds @@ -116,4 +157,4 @@ def main(args=None): if __name__ == "__main__": - main() + main() \ No newline at end of file From 37d50c653b04689e9f44fb50da3551c3c489845d Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 6 Oct 2024 19:55:48 +0200 Subject: [PATCH 03/54] small change --- .../thruster_interface_auv_driver_lib.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 16ad70bb5..591e3ac18 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 @@ -82,11 +82,11 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): # Calculate the interpolated PWM values using the polynomial coefficients interpolated_pwm = [] - for force in thruster_forces_array: + for i,force in enumerate(thruster_forces_array): if force < 0: pwm = numpy.polyval(left_coeffs, force) elif abs(force) < 1e-6: - pwm = 1500 - self.THRUSTER_PWM_OFFSET[0] + pwm = 1500 - self.THRUSTER_PWM_OFFSET[i] else: pwm = numpy.polyval(right_coeffs, force) @@ -143,8 +143,6 @@ def drive_thrusters(self, thruster_forces_array): thruster_pwm_array[ESC_channel] = ( thruster_pwm + self.THRUSTER_PWM_OFFSET[ESC_channel] ) - - print(thruster_pwm_array) # Apply thruster offset and limit PWM if needed for ESC_channel in range(len(thruster_pwm_array)): From d8c5562bc52d22f321502e887a302689902e7ac8 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 11 Oct 2024 17:15:11 +0200 Subject: [PATCH 04/54] updated files with almost all pre-commit fixed --- .editorconfig | 0 .pre-commit-config.yaml | 6 +- motion/thruster_interface_auv/analysis.py | 2 +- motion/thruster_interface_auv/coeffs.txt | 23 ----- .../thruster_interface_auv/config/coeffs.yaml | 2 +- .../thruster_interface_auv_driver_lib.py | 59 +++++------ .../thruster_interface_auv_node.py | 98 +++++++------------ 7 files changed, 68 insertions(+), 122 deletions(-) create mode 100644 .editorconfig delete mode 100644 motion/thruster_interface_auv/coeffs.txt diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000..e69de29bb diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e329ed654..85a6bc9d0 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,14 +18,12 @@ repos: rev: 5.13.2 hooks: - id: isort - language_version: python3.11 # Code Formatters - repo: https://github.com/psf/black rev: 24.8.0 hooks: - id: black - language_version: python3.11 - repo: https://github.com/pre-commit/mirrors-clang-format rev: v19.1.0 @@ -38,6 +36,7 @@ repos: rev: v2.3.0 hooks: - id: codespell + args: ['--write-changes'] # Linters and Static Analysis Tools - repo: https://github.com/pre-commit/mirrors-mypy @@ -45,17 +44,14 @@ repos: hooks: - id: mypy args: ["--explicit-package-bases"] - language_version: python3.11 - repo: https://github.com/PyCQA/pylint rev: v3.3.1 hooks: - id: pylint - language_version: python3.11 - repo: https://github.com/PyCQA/bandit rev: 1.7.10 hooks: - id: bandit - language_version: python3.11 args: ["--severity-level", "medium"] diff --git a/motion/thruster_interface_auv/analysis.py b/motion/thruster_interface_auv/analysis.py index 6cfbb9c23..20ad655d5 100644 --- a/motion/thruster_interface_auv/analysis.py +++ b/motion/thruster_interface_auv/analysis.py @@ -13,7 +13,7 @@ "motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv", "motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv", ] -datas = [] +datas = [] for f in files: data = np.genfromtxt(f, delimiter=',', skip_header=1, usecols=(5, 0)) datas.append(data) diff --git a/motion/thruster_interface_auv/coeffs.txt b/motion/thruster_interface_auv/coeffs.txt deleted file mode 100644 index 8571a842a..000000000 --- a/motion/thruster_interface_auv/coeffs.txt +++ /dev/null @@ -1,23 +0,0 @@ -#10V -LEFT: [ 14.10463 72.78164 245.73575 1456.5868 ] -RIGHT: [ 7.39517 -47.59238 196.5525 1543.6743 ] - -#12V -LEFT: [ 7.35916 47.58976 199.18339 1460.57483] -RIGHT: [ 3.84262 -31.64647 160.76665 1539.18791] - -#14V -LEFT: [ 14.10463 72.78164 245.73575 1456.5868 ] -RIGHT: [ 7.39517 -47.59238 196.5525 1543.6743 ] - -#16V -LEFT: [ 3.31158 28.90927 152.1417 1467.70348] -RIGHT: [ 1.45002 -16.92386 117.95721 1533.55911] - -#18V -LEFT: [ 2.50017 25.18231 142.66031 1469.6513 ] -RIGHT: [ 1.22234 -16.56764 116.26078 1531.57015] - -#20V -LEFT: [ 2.55792 27.51178 147.67001 1472.23069] -RIGHT: [ 1.33421 -18.75129 121.29079 1528.99886] \ No newline at end of file diff --git a/motion/thruster_interface_auv/config/coeffs.yaml b/motion/thruster_interface_auv/config/coeffs.yaml index 384555d16..c8d8aeed4 100644 --- a/motion/thruster_interface_auv/config/coeffs.yaml +++ b/motion/thruster_interface_auv/config/coeffs.yaml @@ -18,4 +18,4 @@ RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] 20V: LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] - RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] \ No newline at end of file + RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] 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 591e3ac18..579a03e4a 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 @@ -1,9 +1,8 @@ #!/usr/bin/env python3 # Import libraries -import smbus2 -import pandas import numpy -import yaml +import smbus2 + class ThrusterInterfaceAUVDriver: def __init__( @@ -17,14 +16,15 @@ def __init__( THRUSTER_PWM_OFFSET=[0, 0, 0, 0, 0, 0, 0, 0], PWM_MIN=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], PWM_MAX=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - coeffs = None, - ): - # Initialice the I2C comunication + coeffs=None, + ) -> None: + + # Initialice the I2C communication self.bus = None try: self.bus = smbus2.SMBus(I2C_BUS) except Exception as errorCode: - print(f"ERROR: Failed connection I2C buss nr {self.bus}: {errorCode}") + print(f"ERROR: Failed connection I2C bus nr {self.bus}: {errorCode}") self.PICO_I2C_ADDRESS = PICO_I2C_ADDRESS # Set mapping, direction and offset for the thrusters @@ -35,7 +35,7 @@ def __init__( self.PWM_MAX = PWM_MAX # Convert SYSTEM_OPERATIONAL_VOLTAGE to a whole even number to work with - # This is because we have multiple files for the behavious of the thrusters depending on the voltage of the drone + # This is because we have multiple files for the behaviour of the thrusters depending on the voltage of the drone if SYSTEM_OPERATIONAL_VOLTAGE < 11.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 10 elif SYSTEM_OPERATIONAL_VOLTAGE < 13.0: @@ -50,13 +50,11 @@ 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 self.coeffs = coeffs - def _interpolate_forces_to_pwm(self, thruster_forces_array): + def _interpolate_forces_to_pwm(self, thruster_forces_array) -> list: """ Takes in Array of forces in Newtosn [N] takes 8 floats in form of: @@ -67,10 +65,10 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): [0, 0, 0, 0, 0, 0, 0, 0] """ # Read the important data from the .csv file - thrusterDatasheetFileData = pandas.read_csv( - f"{self.ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET}/resources/T200-Thrusters-{self.SYSTEM_OPERATIONAL_VOLTAGE}V.csv", - usecols=[" PWM (µs)", " Force (Kg f)"], - ) + # thrusterDatasheetFileData = pandas.read_csv( + # f"{self.ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET}/resources/T200-Thrusters-{self.SYSTEM_OPERATIONAL_VOLTAGE}V.csv", + # usecols=[" PWM (µs)", " Force (Kg f)"], + # ) # Convert Newtons to Kg as Thruster Datasheet is in Kg format for i, thruster_forces in enumerate(thruster_forces_array): @@ -82,14 +80,16 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): # Calculate the interpolated PWM values using the polynomial coefficients interpolated_pwm = [] - for i,force in enumerate(thruster_forces_array): + for i, force in enumerate(thruster_forces_array): if force < 0: + # use the left coefficients pwm = numpy.polyval(left_coeffs, force) - elif abs(force) < 1e-6: + elif force == 0.00: pwm = 1500 - self.THRUSTER_PWM_OFFSET[i] else: + # use the right coefficients pwm = numpy.polyval(right_coeffs, force) - + interpolated_pwm.append(pwm) # Convert PWM signal to integers as they are interpolated and can have floats @@ -97,7 +97,7 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array): return interpolated_pwm - def _send_data_to_escs(self, thruster_pwm_array): + def _send_data_to_escs(self, thruster_pwm_array) -> None: i2c_data_array = [] # Divide data into bytes as I2C only sends bytes @@ -112,7 +112,7 @@ def _send_data_to_escs(self, thruster_pwm_array): # OBS!: Python adds an extra byte at the start that the Microcotroller that is receiving this has to handle self.bus.write_i2c_block_data(self.PICO_I2C_ADDRESS, 0, i2c_data_array) - def drive_thrusters(self, thruster_forces_array): + def drive_thrusters(self, thruster_forces_array) -> list: """ Takes in Array of forces in Newtosn [N] takes 8 floats in form of: @@ -122,7 +122,7 @@ def drive_thrusters(self, thruster_forces_array): PWM signals sent to PCA9685 module through I2C PCA9685 Module sends electrical PWM signals to the individual thruster ESCs The ESCs send corecponding electrical power to the Thrustres - Thrusters then generate thrust acordingly to the Forces sent to this driver + Thrusters then generate thrust accordingly to the Forces sent to this driver Returns an Array of PWM signal for debugging purposes Gives out 8 ints in form of: @@ -130,19 +130,14 @@ def drive_thrusters(self, thruster_forces_array): """ # 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 range(len(thruster_pwm_array)): @@ -152,10 +147,10 @@ def drive_thrusters(self, thruster_forces_array): elif thruster_pwm_array[ESC_channel] > self.PWM_MAX[ESC_channel]: # To big thruster_pwm_array[ESC_channel] = self.PWM_MAX[ESC_channel] - # Send data through I2C to the microcontroller that then controls the ESC and extention the thrusters - """ try: + # Send data through I2C to the microcontroller that then controls the ESC and extension the thrusters + try: self._send_data_to_escs(thruster_pwm_array) except Exception as errorCode: - print(f"ERROR: Failed to send PWM values: {errorCode}") """ + print(f"ERROR: Failed to send PWM values: {errorCode}") return 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 9a75c76c2..63c213fc7 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 @@ -1,40 +1,33 @@ #!/usr/bin/env python3 # ROS2 Libraries import rclpy +from ament_index_python.packages import get_package_share_directory +from rcl_interfaces.msg import ParameterDescriptor from rclpy.node import Node from std_msgs.msg import Int16MultiArray -from ament_index_python.packages import get_package_share_directory - -# Custom libraries -from vortex_msgs.msg import ThrusterForces from thruster_interface_auv.thruster_interface_auv_driver_lib import ( ThrusterInterfaceAUVDriver, ) -from rcl_interfaces.msg import ParameterDescriptor + +# Custom libraries +from vortex_msgs.msg import ThrusterForces + class ThrusterInterfaceAUVNode(Node): - def __init__(self): + def __init__(self) -> None: # Initialize and name the node process running super().__init__("thruster_interface_auv_node") # 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_pwm_publisher = self.create_publisher(Int16MultiArray, "pwm_NEW", 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_APPROX", 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], @@ -59,57 +52,44 @@ def __init__(self): self.declare_parameter("coeffs.20V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) self.declare_parameter("coeffs.20V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - 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 + # dictionary for the coeffs of the poly approx of thruster forces based on OPERATIONAL_VOLTAGE coeffs = { 10: { - "LEFT": self.get_parameter("coeffs.10V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.10V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.10V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.10V.RIGHT").value, }, 12: { - "LEFT": self.get_parameter("coeffs.12V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.12V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.12V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.12V.RIGHT").value, }, 14: { - "LEFT": self.get_parameter("coeffs.14V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.14V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.14V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.14V.RIGHT").value, }, 16: { - "LEFT": self.get_parameter("coeffs.16V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.16V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.16V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.16V.RIGHT").value, }, 18: { - "LEFT": self.get_parameter("coeffs.18V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.18V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.18V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.18V.RIGHT").value, }, 20: { - "LEFT": self.get_parameter("coeffs.20V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.20V.RIGHT").value, + "LEFT": self.get_parameter("coeffs.20V.LEFT").value, + "RIGHT": self.get_parameter("coeffs.20V.RIGHT").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, @@ -127,23 +107,21 @@ def __init__(self): # Debugging self.get_logger().info('"thruster_interface_auv_node" has been started') - def _thruster_forces_callback(self, msg): + def _thruster_forces_callback(self, msg) -> None: # Get data of the forces published self.thruster_forces_array = msg.thrust - def _timer_callback(self): + 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 debuging feature to see if everything is alright with the PWM signal - thruster_pwm_array = self.thruster_driver.drive_thrusters( - self.thruster_forces_array - ) + # 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) pwm_message = Int16MultiArray() pwm_message.data = thruster_pwm_array self.thruster_pwm_publisher.publish(pwm_message) -def main(args=None): +def main(args=None) -> None: # Initialize rclpy.init(args=args) @@ -157,4 +135,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() From 1d4dcdac9c39a22862fb010e64780b4b3359b46f Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 20 Oct 2024 20:36:00 +0200 Subject: [PATCH 05/54] not working but anders want me to push before leaving the lab --- motion/thruster_interface_auv/package.xml | 13 ++- .../resources/thruster_interface_auv | 0 .../thruster_interface_auv_node.py | 2 + .../thruster_interface_auv_cpp/CMakeLists.txt | 65 +++++++++++ .../config/coeffs.yaml | 21 ++++ .../thruster_interface_auv_driver.hpp | 45 ++++++++ .../thruster_interface_auv_ros.hpp | 37 +++++++ .../thruster_interface_auv_cpp.launch.py | 30 +++++ motion/thruster_interface_auv_cpp/package.xml | 28 +++++ .../src/thruster_interface_auv_driver.cpp | 104 ++++++++++++++++++ .../src/thruster_interface_auv_node.cpp | 10 ++ .../src/truster_interface_auv_ros.cpp | 65 +++++++++++ 12 files changed, 414 insertions(+), 6 deletions(-) delete mode 100644 motion/thruster_interface_auv/resources/thruster_interface_auv create mode 100644 motion/thruster_interface_auv_cpp/CMakeLists.txt create mode 100644 motion/thruster_interface_auv_cpp/config/coeffs.yaml create mode 100644 motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp create mode 100644 motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp create mode 100644 motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py create mode 100644 motion/thruster_interface_auv_cpp/package.xml create mode 100644 motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp create mode 100644 motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp create mode 100644 motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index d0dc93e67..6e02d748d 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -2,20 +2,21 @@ thruster_interface_auv - 1.0.0 - Thruster interface to control thrusters through PCA9685 Module - vortex + 0.0.0 + Thruster interface to control thrusters mapping force to pwm values + albertomorselli MIT ament_cmake_python - rclpy - std_msgs + rclcpp + geometry_msgs vortex_msgs + std_msgs ros2launch ament_cmake - + \ No newline at end of file diff --git a/motion/thruster_interface_auv/resources/thruster_interface_auv b/motion/thruster_interface_auv/resources/thruster_interface_auv deleted file mode 100644 index e69de29bb..000000000 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 63c213fc7..df06d8050 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 @@ -87,6 +87,8 @@ def __init__(self) -> None: }, } + print(coeffs) + # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET=get_package_share_directory("thruster_interface_auv"), diff --git a/motion/thruster_interface_auv_cpp/CMakeLists.txt b/motion/thruster_interface_auv_cpp/CMakeLists.txt new file mode 100644 index 000000000..170b5e0bf --- /dev/null +++ b/motion/thruster_interface_auv_cpp/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.5) +project(thruster_interface_auv_cpp) + +# Required dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(Eigen3 REQUIRED) + +# Include directories +include_directories(include ${EIGEN3_INCLUDE_DIR}) + +# Define the driver library +add_library(thruster_interface_auv_driver + src/thruster_interface_auv_driver.cpp +) +target_include_directories(thruster_interface_auv_driver PRIVATE include) +ament_target_dependencies(thruster_interface_auv_driver rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) + +# Define the ROS interface library +add_library(thruster_interface_auv_ros + src/truster_interface_auv_ros.cpp +) +target_link_libraries(thruster_interface_auv_ros thruster_interface_auv_driver) +target_include_directories(thruster_interface_auv_ros PRIVATE include) +ament_target_dependencies(thruster_interface_auv_ros rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) + +# Define the executable for the ROS node +add_executable(thruster_interface_auv_node + src/thruster_interface_auv_node.cpp +) +target_link_libraries(thruster_interface_auv_node thruster_interface_auv_ros) +ament_target_dependencies(thruster_interface_auv_node rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) + +# Install targets +install(TARGETS + thruster_interface_auv_driver + thruster_interface_auv_ros + thruster_interface_auv_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +# Install headers +install(DIRECTORY include/ + DESTINATION include/ +) + +# Install launch files +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/launch +) + +# Install config files (coeffs.yaml) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/config +) + +ament_package() diff --git a/motion/thruster_interface_auv_cpp/config/coeffs.yaml b/motion/thruster_interface_auv_cpp/config/coeffs.yaml new file mode 100644 index 000000000..c8d8aeed4 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/config/coeffs.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + coeffs: + 10V: + LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] + 12V: + LEFT: [7.35916, 47.58976, 199.18339, 1460.57483] + RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] + 14V: + LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] + 16V: + LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] + RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] + 18V: + LEFT: [2.50017, 25.18231, 142.66031, 1469.6513] + RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] + 20V: + LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] + RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp new file mode 100644 index 000000000..dbda26c46 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp @@ -0,0 +1,45 @@ +#ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP +#define THRUSTER_INTERFACE_AUV_DRIVER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ThrusterInterfaceAUVDriver { +public: + explicit ThrusterInterfaceAUVDriver(int I2C_BUS = 1, + int PICO_I2C_ADDRESS = 0x21, + double SYSTEM_OPERATIONAL_VOLTAGE = 16.0, + std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET = "", + std::vector THRUSTER_MAPPING = {7, 6, 5, 4, 3, 2, 1, 0}, + std::vector THRUSTER_DIRECTION = {1, 1, 1, 1, 1, 1, 1, 1}, + std::vector THRUSTER_PWM_OFFSET = {0, 0, 0, 0, 0, 0, 0, 0}, + std::vector PWM_MIN = {1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100}, + std::vector PWM_MAX = {1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900}, + std::map> coeffs = {}); + + std::vector drive_thrusters(const std::vector& thruster_forces_array); + +private: + int i2c_fd; + int PICO_I2C_ADDRESS; + std::vector THRUSTER_MAPPING; + std::vector THRUSTER_DIRECTION; + std::vector THRUSTER_PWM_OFFSET; + std::vector PWM_MIN; + std::vector PWM_MAX; + double SYSTEM_OPERATIONAL_VOLTAGE; + std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET; + std::map> coeffs; + + std::vector _interpolate_forces_to_pwm(const std::vector& thruster_forces_array); + void _send_data_to_escs(const std::vector& thruster_pwm_array); +}; + +#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp new file mode 100644 index 000000000..4a8767420 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp @@ -0,0 +1,37 @@ +#ifndef THRUSTER_INTERFACE_AUV_ROS_HPP +#define THRUSTER_INTERFACE_AUV_ROS_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp" + +class ThrusterInterfaceAUVNode : public rclcpp::Node { +public: + explicit ThrusterInterfaceAUVNode(); + +private: + void thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); + void timer_callback(); + + rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; + rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + std::vector thruster_mapping_; + std::vector thruster_direction_; + std::vector thruster_pwm_offset_; + std::vector pwm_min_; + std::vector pwm_max_; + std::vector thruster_forces_array_; + double thrust_timer_period_; + + ThrusterInterfaceAUVDriver thruster_driver_; +}; + +#endif // THRUSTER_INTERFACE_AUV_ROS_HPP diff --git a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py new file mode 100644 index 000000000..d22ce4ee4 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from os import path + +def generate_launch_description(): + # Path to the YAML file + config = path.join(get_package_share_directory("auv_setup"),'config', 'robots', "orca.yaml") + + thruster_interface_auv_node = Node( + package='thruster_interface_auv_cpp', + executable='thruster_interface_auv_cpp_node', + name='thruster_interface_auv_cpp_node', + output='screen', + parameters=[ + path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + "orca.yaml", + ), + path.join( + get_package_share_directory("thruster_interface_auv_cpp"), + "config", + "coeffs.yaml", + ), + ], + ) + + return LaunchDescription([thruster_interface_auv_node]) \ No newline at end of file diff --git a/motion/thruster_interface_auv_cpp/package.xml b/motion/thruster_interface_auv_cpp/package.xml new file mode 100644 index 000000000..3e1f19439 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/package.xml @@ -0,0 +1,28 @@ + + + + thruster_interface_auv_cpp + 1.0.0 + Thruster interface to control thrusters through PCA9685 Module + vortex + MIT + + ament_cmake + rclcpp + std_msgs + vortex_msgs + Eigen3 + ament_index_cpp + + rclcpp + std_msgs + vortex_msgs + Eigen3 + ament_index_cpp + + ros2launch + + + ament_cmake + + diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp new file mode 100644 index 000000000..06a6f71b4 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -0,0 +1,104 @@ +#include "thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp" + +ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(int I2C_BUS, int PICO_I2C_ADDRESS, double SYSTEM_OPERATIONAL_VOLTAGE, + std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET, + std::vector THRUSTER_MAPPING, std::vector THRUSTER_DIRECTION, + std::vector THRUSTER_PWM_OFFSET, std::vector PWM_MIN, + std::vector PWM_MAX, + std::map> coeffs) + : PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), THRUSTER_MAPPING(THRUSTER_MAPPING), THRUSTER_DIRECTION(THRUSTER_DIRECTION), + THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), PWM_MIN(PWM_MIN), PWM_MAX(PWM_MAX), SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), + ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET(ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET), coeffs(coeffs) { + + // Open I2C device + char i2c_device[20]; + snprintf(i2c_device, 19, "/dev/i2c-%d", I2C_BUS); + i2c_fd = open(i2c_device, O_RDWR); + if (i2c_fd < 0) { + std::cerr << "ERROR: Failed to open I2C bus" << std::endl; + } + + if (ioctl(i2c_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { + std::cerr << "ERROR: Failed to set I2C address" << std::endl; + } + + // Convert SYSTEM_OPERATIONAL_VOLTAGE to the nearest valid value + if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { + SYSTEM_OPERATIONAL_VOLTAGE = 10; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { + SYSTEM_OPERATIONAL_VOLTAGE = 12; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 15.0) { + SYSTEM_OPERATIONAL_VOLTAGE = 14; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 17.0) { + SYSTEM_OPERATIONAL_VOLTAGE = 16; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 19.0) { + SYSTEM_OPERATIONAL_VOLTAGE = 18; + } else { + SYSTEM_OPERATIONAL_VOLTAGE = 20; + } +} + +std::vector ThrusterInterfaceAUVDriver::_interpolate_forces_to_pwm(const std::vector& thruster_forces_array) { + std::vector interpolated_pwm; + Eigen::VectorXd left_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; + Eigen::VectorXd right_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; + + for (size_t i = 0; i < thruster_forces_array.size(); ++i) { + double force = thruster_forces_array[i] / 9.80665; // Convert Newtons to Kg + double pwm = 0; + + if (force < 0) { + pwm = left_coeffs(0) + left_coeffs(1) * force + left_coeffs(2) * std::pow(force,2) + left_coeffs(3) * std::pow(force,3); + } else if (force == 0.0) { + pwm = 1500 - THRUSTER_PWM_OFFSET[i]; + } else { + pwm = right_coeffs(0) + right_coeffs(1) * force + right_coeffs(2) * std::pow(force,2) + right_coeffs(3) * std::pow(force,3); + } + + interpolated_pwm.push_back(static_cast(pwm)); + } + return interpolated_pwm; +} + +void ThrusterInterfaceAUVDriver::_send_data_to_escs(const std::vector& thruster_pwm_array) { + std::vector i2c_data_array; + + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + uint8_t msb = (thruster_pwm_array[i] >> 8) & 0xFF; + uint8_t lsb = thruster_pwm_array[i] & 0xFF; + i2c_data_array.push_back(msb); + i2c_data_array.push_back(lsb); + } + + if (write(i2c_fd, i2c_data_array.data(), i2c_data_array.size()) != static_cast(i2c_data_array.size())) { + std::cerr << "ERROR: Failed to send data via I2C" << std::endl; + } +} + +std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector& thruster_forces_array) { + std::vector thruster_forces_modified(thruster_forces_array.size()); + + for (size_t i = 0; i < thruster_forces_array.size(); ++i) { + thruster_forces_modified[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; + } + + std::vector thruster_pwm_array = _interpolate_forces_to_pwm(thruster_forces_modified); + + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; + + if (thruster_pwm_array[i] < PWM_MIN[i]) { + thruster_pwm_array[i] = PWM_MIN[i]; + } else if (thruster_pwm_array[i] > PWM_MAX[i]) { + thruster_pwm_array[i] = PWM_MAX[i]; + } + } + + try { + _send_data_to_escs(thruster_pwm_array); + } catch (const std::exception& e) { + std::cerr << "ERROR: Failed to send PWM values: " << e.what() << std::endl; + } + + return thruster_pwm_array; +} diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp new file mode 100644 index 000000000..c6200f68d --- /dev/null +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp @@ -0,0 +1,10 @@ +#include "thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_cpp_node"); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp new file mode 100644 index 000000000..332d9b226 --- /dev/null +++ b/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp @@ -0,0 +1,65 @@ +#include "thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() + : Node("thruster_interface_auv_cpp_node"), + thruster_forces_array_(8, 0.0) { + // Subscriber and Publisher setup + thruster_forces_subscriber_ = this->create_subscription( + "thrust/thruster_forces", 10, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); + + thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX", 10); + + // Get parameters + this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping", std::vector{7, 6, 5, 4, 3, 2, 1, 0}); + this->declare_parameter>("propulsion.thrusters.thruster_direction", std::vector{1, 1, 1, 1, 1, 1, 1, 1}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset", std::vector{0, 0, 0, 0, 0, 0, 0, 0}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_min", std::vector{1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_max", std::vector{1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900}); + this->declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); + + this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping", thruster_mapping_); + this->get_parameter("propulsion.thrusters.thruster_direction", thruster_direction_); + this->get_parameter("propulsion.thrusters.thruster_PWM_offset", thruster_pwm_offset_); + this->get_parameter("propulsion.thrusters.thruster_PWM_min", pwm_min_); + this->get_parameter("propulsion.thrusters.thruster_PWM_max", pwm_max_); + this->get_parameter("propulsion.thrusters.thrust_update_rate", thrust_timer_period_); + thrust_timer_period_ = 1.0 / thrust_timer_period_; + + // Get the coefficients for polynomial interpolation of thruster forces + std::map> coeffs; + std::vector voltages = {10, 12, 14, 16, 18, 20}; + for (int voltage : voltages) { + coeffs[voltage]["LEFT"] = Eigen::VectorXd::Map(this->declare_parameter>("coeffs." + std::to_string(voltage) + "V.LEFT", {}).data(), 6); + coeffs[voltage]["RIGHT"] = Eigen::VectorXd::Map(this->declare_parameter>("coeffs." + std::to_string(voltage) + "V.RIGHT", {}).data(), 6); + } + + // Initialize the thruster driver + thruster_driver_ = ThrusterInterfaceAUVDriver( + ament_index_cpp::get_package_share_directory("thruster_interface_auv"), + thruster_mapping_, thruster_direction_, thruster_pwm_offset_, pwm_min_, pwm_max_, coeffs + ); + + // Timer for periodic thruster control updates + timer_ = this->create_wall_timer(std::chrono::duration(thrust_timer_period_), std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); + + // Debugging + RCLCPP_INFO(this->get_logger(), "Thruster Interface AUV Node has been started."); +} + +void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { + thruster_forces_array_ = msg->thrust; +} + +void ThrusterInterfaceAUVNode::timer_callback() { + // Send forces to be converted into PWM and published + std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(thruster_forces_array_); + + // Convert std::vector to std::vector + std::vector thruster_pwm_array_short(thruster_pwm_array.begin(), thruster_pwm_array.end()); + + // Publish PWM values for debugging + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array_short; + thruster_pwm_publisher_->publish(pwm_message); +} \ No newline at end of file From f2e98d4c4301a19aecc65265aa890e29a7d4add9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20H=C3=B8gden?= Date: Sun, 20 Oct 2024 23:30:32 +0200 Subject: [PATCH 06/54] refactor: make code build --- .../thruster_interface_auv_cpp/CMakeLists.txt | 96 +++++++---- .../thruster_interface_auv_driver.hpp | 49 +++--- .../thruster_interface_auv_ros.hpp | 30 ++-- .../src/thruster_interface_auv_driver.cpp | 152 +++++++++++------- .../src/thruster_interface_auv_ros.cpp | 83 ++++++++++ .../src/truster_interface_auv_ros.cpp | 65 -------- 6 files changed, 274 insertions(+), 201 deletions(-) create mode 100644 motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp delete mode 100644 motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp diff --git a/motion/thruster_interface_auv_cpp/CMakeLists.txt b/motion/thruster_interface_auv_cpp/CMakeLists.txt index 170b5e0bf..149c38b0d 100644 --- a/motion/thruster_interface_auv_cpp/CMakeLists.txt +++ b/motion/thruster_interface_auv_cpp/CMakeLists.txt @@ -12,54 +12,80 @@ find_package(Eigen3 REQUIRED) # Include directories include_directories(include ${EIGEN3_INCLUDE_DIR}) -# Define the driver library -add_library(thruster_interface_auv_driver +add_executable(${PROJECT_NAME}_node + src/thruster_interface_auv_node.cpp src/thruster_interface_auv_driver.cpp + src/thruster_interface_auv_ros.cpp ) -target_include_directories(thruster_interface_auv_driver PRIVATE include) -ament_target_dependencies(thruster_interface_auv_driver rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Define the ROS interface library -add_library(thruster_interface_auv_ros - src/truster_interface_auv_ros.cpp +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + vortex_msgs + ament_index_cpp + Eigen3 ) -target_link_libraries(thruster_interface_auv_ros thruster_interface_auv_driver) -target_include_directories(thruster_interface_auv_ros PRIVATE include) -ament_target_dependencies(thruster_interface_auv_ros rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Define the executable for the ROS node -add_executable(thruster_interface_auv_node - src/thruster_interface_auv_node.cpp +# Install launch files. +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} ) -target_link_libraries(thruster_interface_auv_node thruster_interface_auv_ros) -ament_target_dependencies(thruster_interface_auv_node rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Install targets install(TARGETS - thruster_interface_auv_driver - thruster_interface_auv_ros - thruster_interface_auv_node + ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) -install( - DIRECTORY include/ - DESTINATION include -) +# # Define the driver library +# add_library(thruster_interface_auv_driver +# src/thruster_interface_auv_driver.cpp +# ) +# target_include_directories(thruster_interface_auv_driver PRIVATE include) +# ament_target_dependencies(thruster_interface_auv_driver rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Install headers -install(DIRECTORY include/ - DESTINATION include/ -) +# # Define the ROS interface library +# add_library(thruster_interface_auv_ros +# src/truster_interface_auv_ros.cpp +# ) +# target_link_libraries(thruster_interface_auv_ros thruster_interface_auv_driver) +# target_include_directories(thruster_interface_auv_ros PRIVATE include) +# ament_target_dependencies(thruster_interface_auv_ros rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Install launch files -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/launch -) +# # Define the executable for the ROS node +# add_executable(thruster_interface_auv_node +# src/thruster_interface_auv_node.cpp +# ) +# target_link_libraries(thruster_interface_auv_node thruster_interface_auv_ros) +# ament_target_dependencies(thruster_interface_auv_node rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) -# Install config files (coeffs.yaml) -install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}/config -) +# # Install targets +# install(TARGETS +# thruster_interface_auv_driver +# thruster_interface_auv_ros +# thruster_interface_auv_node +# DESTINATION lib/${PROJECT_NAME} +# ) + +# install( +# DIRECTORY include/ +# DESTINATION include +# ) + +# # Install headers +# install(DIRECTORY include/ +# DESTINATION include/ +# ) + +# # Install launch files +# install(DIRECTORY launch +# DESTINATION share/${PROJECT_NAME}/launch +# ) + +# # Install config files (coeffs.yaml) +# install(DIRECTORY config +# DESTINATION share/${PROJECT_NAME}/config +# ) ament_package() diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp index dbda26c46..3c1b8be4c 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp @@ -3,43 +3,44 @@ #include #include -#include -#include -#include -#include -#include #include -#include +#include class ThrusterInterfaceAUVDriver { public: - explicit ThrusterInterfaceAUVDriver(int I2C_BUS = 1, - int PICO_I2C_ADDRESS = 0x21, - double SYSTEM_OPERATIONAL_VOLTAGE = 16.0, - std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET = "", - std::vector THRUSTER_MAPPING = {7, 6, 5, 4, 3, 2, 1, 0}, - std::vector THRUSTER_DIRECTION = {1, 1, 1, 1, 1, 1, 1, 1}, - std::vector THRUSTER_PWM_OFFSET = {0, 0, 0, 0, 0, 0, 0, 0}, - std::vector PWM_MIN = {1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100}, - std::vector PWM_MAX = {1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900}, - std::map> coeffs = {}); - - std::vector drive_thrusters(const std::vector& thruster_forces_array); + ThrusterInterfaceAUVDriver( + int I2C_BUS = 1, + int PICO_I2C_ADDRESS = 0x21, + double SYSTEM_OPERATIONAL_VOLTAGE = 16.0, + const std::string& ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET = "", + const std::vector& THRUSTER_MAPPING = {7,6,5,4,3,2,1,0}, + const std::vector& THRUSTER_DIRECTION = {1,1,1,1,1,1,1,1}, + const std::vector& THRUSTER_PWM_OFFSET = {0,0,0,0,0,0,0,0}, + const std::vector& PWM_MIN = {1100,1100,1100,1100,1100,1100,1100,1100}, + const std::vector& PWM_MAX = {1900,1900,1900,1900,1900,1900,1900,1900}, + const std::map>>& coeffs = {} + ); + + ~ThrusterInterfaceAUVDriver(); + + std::vector drive_thrusters(const std::vector& thruster_forces_array); private: - int i2c_fd; + int bus_fd; int PICO_I2C_ADDRESS; + int SYSTEM_OPERATIONAL_VOLTAGE; + std::vector THRUSTER_MAPPING; std::vector THRUSTER_DIRECTION; std::vector THRUSTER_PWM_OFFSET; std::vector PWM_MIN; std::vector PWM_MAX; - double SYSTEM_OPERATIONAL_VOLTAGE; - std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET; - std::map> coeffs; - std::vector _interpolate_forces_to_pwm(const std::vector& thruster_forces_array); - void _send_data_to_escs(const std::vector& thruster_pwm_array); + std::map>> coeffs; + + std::vector interpolate_forces_to_pwm(const std::vector& thruster_forces_array); + void send_data_to_escs(const std::vector& thruster_pwm_array); + double polyval(const std::vector& coeffs, double x); }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp index 4a8767420..7ed057ba5 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp @@ -1,19 +1,14 @@ -#ifndef THRUSTER_INTERFACE_AUV_ROS_HPP -#define THRUSTER_INTERFACE_AUV_ROS_HPP +#ifndef THRUSTER_INTERFACE_AUV_NODE_HPP +#define THRUSTER_INTERFACE_AUV_NODE_HPP -#include -#include -#include -#include -#include -#include -#include - -#include "thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int16_multi_array.hpp" +#include "vortex_msgs/msg/thruster_forces.hpp" +#include class ThrusterInterfaceAUVNode : public rclcpp::Node { public: - explicit ThrusterInterfaceAUVNode(); + ThrusterInterfaceAUVNode(); private: void thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); @@ -23,15 +18,10 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; rclcpp::TimerBase::SharedPtr timer_; - std::vector thruster_mapping_; - std::vector thruster_direction_; - std::vector thruster_pwm_offset_; - std::vector pwm_min_; - std::vector pwm_max_; + ThrusterInterfaceAUVDriver thruster_driver_; + std::vector thruster_forces_array_; double thrust_timer_period_; - - ThrusterInterfaceAUVDriver thruster_driver_; }; -#endif // THRUSTER_INTERFACE_AUV_ROS_HPP +#endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp index 06a6f71b4..f4db411a5 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -1,92 +1,129 @@ -#include "thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp" - -ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(int I2C_BUS, int PICO_I2C_ADDRESS, double SYSTEM_OPERATIONAL_VOLTAGE, - std::string ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET, - std::vector THRUSTER_MAPPING, std::vector THRUSTER_DIRECTION, - std::vector THRUSTER_PWM_OFFSET, std::vector PWM_MIN, - std::vector PWM_MAX, - std::map> coeffs) - : PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), THRUSTER_MAPPING(THRUSTER_MAPPING), THRUSTER_DIRECTION(THRUSTER_DIRECTION), - THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), PWM_MIN(PWM_MIN), PWM_MAX(PWM_MAX), SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET(ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET), coeffs(coeffs) { - - // Open I2C device - char i2c_device[20]; - snprintf(i2c_device, 19, "/dev/i2c-%d", I2C_BUS); - i2c_fd = open(i2c_device, O_RDWR); - if (i2c_fd < 0) { - std::cerr << "ERROR: Failed to open I2C bus" << std::endl; - } - - if (ioctl(i2c_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { - std::cerr << "ERROR: Failed to set I2C address" << std::endl; +#include +#include +#include +#include +#include +#include +#include + +ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( + int I2C_BUS, + int PICO_I2C_ADDRESS, + double SYSTEM_OPERATIONAL_VOLTAGE, + const std::string& ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET, + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, + const std::vector& THRUSTER_PWM_OFFSET, + const std::vector& PWM_MIN, + const std::vector& PWM_MAX, + const std::map>>& coeffs +) : PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + THRUSTER_MAPPING(THRUSTER_MAPPING), + THRUSTER_DIRECTION(THRUSTER_DIRECTION), + THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), + PWM_MIN(PWM_MIN), + PWM_MAX(PWM_MAX), + coeffs(coeffs) +{ + // Open the I2C bus + std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); + bus_fd = open(i2c_filename.c_str(), O_RDWR); + if (bus_fd < 0) { + std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; } - // Convert SYSTEM_OPERATIONAL_VOLTAGE to the nearest valid value + // Convert SYSTEM_OPERATIONAL_VOLTAGE to a category if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { - SYSTEM_OPERATIONAL_VOLTAGE = 10; + this->SYSTEM_OPERATIONAL_VOLTAGE = 10; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { - SYSTEM_OPERATIONAL_VOLTAGE = 12; + this->SYSTEM_OPERATIONAL_VOLTAGE = 12; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 15.0) { - SYSTEM_OPERATIONAL_VOLTAGE = 14; + this->SYSTEM_OPERATIONAL_VOLTAGE = 14; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 17.0) { - SYSTEM_OPERATIONAL_VOLTAGE = 16; + this->SYSTEM_OPERATIONAL_VOLTAGE = 16; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 19.0) { - SYSTEM_OPERATIONAL_VOLTAGE = 18; + this->SYSTEM_OPERATIONAL_VOLTAGE = 18; } else { - SYSTEM_OPERATIONAL_VOLTAGE = 20; + this->SYSTEM_OPERATIONAL_VOLTAGE = 20; + } +} + +ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { + if (bus_fd >= 0) { + close(bus_fd); } } -std::vector ThrusterInterfaceAUVDriver::_interpolate_forces_to_pwm(const std::vector& thruster_forces_array) { - std::vector interpolated_pwm; - Eigen::VectorXd left_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; - Eigen::VectorXd right_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; +double ThrusterInterfaceAUVDriver::polyval(const std::vector& coeffs, double x) { + double result = 0.0; + for (size_t i = 0; i < coeffs.size(); ++i) { + result += coeffs[i] * std::pow(x, coeffs.size() - i - 1); + } + return result; +} +std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const std::vector& thruster_forces_array) { + // Convert Newtons to Kg (since the thruster datasheet is in Kg) + std::vector forces_in_kg(thruster_forces_array.size()); for (size_t i = 0; i < thruster_forces_array.size(); ++i) { - double force = thruster_forces_array[i] / 9.80665; // Convert Newtons to Kg - double pwm = 0; + forces_in_kg[i] = thruster_forces_array[i] / 9.80665; + } + + // Select the appropriate coefficients based on the operational voltage + auto left_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; + auto right_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; + std::vector interpolated_pwm; + for (size_t i = 0; i < forces_in_kg.size(); ++i) { + double force = forces_in_kg[i]; + double pwm = 0.0; if (force < 0) { - pwm = left_coeffs(0) + left_coeffs(1) * force + left_coeffs(2) * std::pow(force,2) + left_coeffs(3) * std::pow(force,3); + pwm = polyval(left_coeffs, force); } else if (force == 0.0) { pwm = 1500 - THRUSTER_PWM_OFFSET[i]; } else { - pwm = right_coeffs(0) + right_coeffs(1) * force + right_coeffs(2) * std::pow(force,2) + right_coeffs(3) * std::pow(force,3); + pwm = polyval(right_coeffs, force); } - - interpolated_pwm.push_back(static_cast(pwm)); + interpolated_pwm.push_back(static_cast(pwm)); } + return interpolated_pwm; } -void ThrusterInterfaceAUVDriver::_send_data_to_escs(const std::vector& thruster_pwm_array) { - std::vector i2c_data_array; - +void ThrusterInterfaceAUVDriver::send_data_to_escs(const std::vector& thruster_pwm_array) { + uint8_t i2c_data_array[16]; for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - uint8_t msb = (thruster_pwm_array[i] >> 8) & 0xFF; - uint8_t lsb = thruster_pwm_array[i] & 0xFF; - i2c_data_array.push_back(msb); - i2c_data_array.push_back(lsb); + i2c_data_array[2*i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB + i2c_data_array[2*i + 1] = thruster_pwm_array[i] & 0xFF; // LSB } - if (write(i2c_fd, i2c_data_array.data(), i2c_data_array.size()) != static_cast(i2c_data_array.size())) { - std::cerr << "ERROR: Failed to send data via I2C" << std::endl; + // Set the I2C slave address + if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { + std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; + return; } -} -std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector& thruster_forces_array) { - std::vector thruster_forces_modified(thruster_forces_array.size()); + // Write data to the I2C device + if (write(bus_fd, i2c_data_array, 16) != 16) { + std::cerr << "ERROR: Failed to write to I2C device" << std::endl; + } +} - for (size_t i = 0; i < thruster_forces_array.size(); ++i) { - thruster_forces_modified[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; +std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector& thruster_forces_array) { + // Apply thruster mapping and direction + std::vector mapped_forces(thruster_forces_array.size()); + for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { + mapped_forces[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; } - std::vector thruster_pwm_array = _interpolate_forces_to_pwm(thruster_forces_modified); + // Convert forces to PWM + std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); + // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; + // Clamp the PWM signal if (thruster_pwm_array[i] < PWM_MIN[i]) { thruster_pwm_array[i] = PWM_MIN[i]; } else if (thruster_pwm_array[i] > PWM_MAX[i]) { @@ -94,10 +131,11 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vectordeclare_parameter>("propulsion.thrusters.thruster_to_pin_mapping", {7,6,5,4,3,2,1,0}); + this->declare_parameter>("propulsion.thrusters.thruster_direction", {1,1,1,1,1,1,1,1}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset", {0,0,0,0,0,0,0,0}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_min", {1100,1100,1100,1100,1100,1100,1100,1100}); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_max", {1900,1900,1900,1900,1900,1900,1900,1900}); + this->declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); + + // Coefficients parameters + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + + std::map>> coeffs; + std::vector voltage_levels = {10, 12, 14, 16, 18, 20}; + for (int voltage : voltage_levels) { + std::string left_param = "coeffs." + std::to_string(voltage) + "V.LEFT"; + std::string right_param = "coeffs." + std::to_string(voltage) + "V.RIGHT"; + this->declare_parameter(left_param, std::vector{}, descriptor); + this->declare_parameter(right_param, std::vector{}, descriptor); + + auto left_coeffs = this->get_parameter(left_param).as_double_array(); + auto right_coeffs = this->get_parameter(right_param).as_double_array(); + + coeffs[voltage]["LEFT"] = left_coeffs; + coeffs[voltage]["RIGHT"] = right_coeffs; + } + + // Get parameters + auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); + auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); + auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); + auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); + auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); + thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); + + // Initialize thruster driver + thruster_driver_ = ThrusterInterfaceAUVDriver( + 1, 0x21, 16.0, ament_index_cpp::get_package_share_directory("thruster_interface_auv"), + std::vector(thruster_mapping.begin(), thruster_mapping.end()), + std::vector(thruster_direction.begin(), thruster_direction.end()), + std::vector(thruster_PWM_offset.begin(), thruster_PWM_offset.end()), + std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), + std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), + coeffs + ); + + // Initialize thruster forces array + thruster_forces_array_ = std::vector(8, 0.0); + + // Set up subscriber and publisher + thruster_forces_subscriber_ = this->create_subscription( + "thrust/thruster_forces", 10, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1) + ); + + thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX", 10); + + // Set up timer + timer_ = this->create_wall_timer( + std::chrono::duration(thrust_timer_period_), + std::bind(&ThrusterInterfaceAUVNode::timer_callback, this) + ); + + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" has been started"); +} + +void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { + thruster_forces_array_ = msg->thrust; +} + +void ThrusterInterfaceAUVNode::timer_callback() { + // Send thruster forces to be converted into PWM signals + auto thruster_pwm_array = thruster_driver_.drive_thrusters(thruster_forces_array_); + + // Publish PWM values for debugging + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array; + thruster_pwm_publisher_->publish(pwm_message); +} diff --git a/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp deleted file mode 100644 index 332d9b226..000000000 --- a/motion/thruster_interface_auv_cpp/src/truster_interface_auv_ros.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" - -ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() - : Node("thruster_interface_auv_cpp_node"), - thruster_forces_array_(8, 0.0) { - // Subscriber and Publisher setup - thruster_forces_subscriber_ = this->create_subscription( - "thrust/thruster_forces", 10, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); - - thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX", 10); - - // Get parameters - this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping", std::vector{7, 6, 5, 4, 3, 2, 1, 0}); - this->declare_parameter>("propulsion.thrusters.thruster_direction", std::vector{1, 1, 1, 1, 1, 1, 1, 1}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset", std::vector{0, 0, 0, 0, 0, 0, 0, 0}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_min", std::vector{1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_max", std::vector{1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900}); - this->declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); - - this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping", thruster_mapping_); - this->get_parameter("propulsion.thrusters.thruster_direction", thruster_direction_); - this->get_parameter("propulsion.thrusters.thruster_PWM_offset", thruster_pwm_offset_); - this->get_parameter("propulsion.thrusters.thruster_PWM_min", pwm_min_); - this->get_parameter("propulsion.thrusters.thruster_PWM_max", pwm_max_); - this->get_parameter("propulsion.thrusters.thrust_update_rate", thrust_timer_period_); - thrust_timer_period_ = 1.0 / thrust_timer_period_; - - // Get the coefficients for polynomial interpolation of thruster forces - std::map> coeffs; - std::vector voltages = {10, 12, 14, 16, 18, 20}; - for (int voltage : voltages) { - coeffs[voltage]["LEFT"] = Eigen::VectorXd::Map(this->declare_parameter>("coeffs." + std::to_string(voltage) + "V.LEFT", {}).data(), 6); - coeffs[voltage]["RIGHT"] = Eigen::VectorXd::Map(this->declare_parameter>("coeffs." + std::to_string(voltage) + "V.RIGHT", {}).data(), 6); - } - - // Initialize the thruster driver - thruster_driver_ = ThrusterInterfaceAUVDriver( - ament_index_cpp::get_package_share_directory("thruster_interface_auv"), - thruster_mapping_, thruster_direction_, thruster_pwm_offset_, pwm_min_, pwm_max_, coeffs - ); - - // Timer for periodic thruster control updates - timer_ = this->create_wall_timer(std::chrono::duration(thrust_timer_period_), std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - - // Debugging - RCLCPP_INFO(this->get_logger(), "Thruster Interface AUV Node has been started."); -} - -void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - thruster_forces_array_ = msg->thrust; -} - -void ThrusterInterfaceAUVNode::timer_callback() { - // Send forces to be converted into PWM and published - std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(thruster_forces_array_); - - // Convert std::vector to std::vector - std::vector thruster_pwm_array_short(thruster_pwm_array.begin(), thruster_pwm_array.end()); - - // Publish PWM values for debugging - std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array_short; - thruster_pwm_publisher_->publish(pwm_message); -} \ No newline at end of file From f753f5f030b8a7295d5788b49d03e80546bfc723 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Tue, 22 Oct 2024 02:47:13 +0200 Subject: [PATCH 07/54] fixed some gpt-shit and numerically comparing it to python works properly. I still have to figure out how to remove default initialization class field from the header file. Apparently the code was relying on default values for default constructor and so on on multiple levels, including constructor default values, class default values, declare_parameter default values creating a total mess. Removed almost all of them, i'm not able to remove the last one: driver class default params. If I do, i get a compilation error saying that my constructor called from outside doesnt match with the constructor specified in the class? idk. But somehow executes my constructor because substitutes the default values, but if i call only mine, doesnt compile. BOH. Mi sa che vado a letto ciao. --- .../thruster_interface_auv_node.py | 2 +- .../thruster_interface_auv_cpp/CMakeLists.txt | 58 +------------- .../thruster_interface_auv_driver.hpp | 28 ++++--- .../thruster_interface_auv_ros.hpp | 2 +- .../src/thruster_interface_auv_driver.cpp | 69 ++++++++++++---- .../src/thruster_interface_auv_node.cpp | 5 +- .../src/thruster_interface_auv_ros.cpp | 78 +++++++++---------- 7 files changed, 115 insertions(+), 127 deletions(-) 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 df06d8050..3d9515fa6 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 @@ -102,7 +102,7 @@ def __init__(self) -> None: # Start clock timer for driving thrusters every 0.2 seconds # Declare "self.thruster_forces_array" in case no topic comes in at the first possible second - self.thruster_forces_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + self.thruster_forces_array = [2.23] * 8 self.timer = self.create_timer(self.thrust_timer_period, self._timer_callback) diff --git a/motion/thruster_interface_auv_cpp/CMakeLists.txt b/motion/thruster_interface_auv_cpp/CMakeLists.txt index 149c38b0d..aa7e679a8 100644 --- a/motion/thruster_interface_auv_cpp/CMakeLists.txt +++ b/motion/thruster_interface_auv_cpp/CMakeLists.txt @@ -6,24 +6,20 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(Eigen3 REQUIRED) # Include directories -include_directories(include ${EIGEN3_INCLUDE_DIR}) +include_directories(include/${PROJECT_NAME}) add_executable(${PROJECT_NAME}_node src/thruster_interface_auv_node.cpp - src/thruster_interface_auv_driver.cpp src/thruster_interface_auv_ros.cpp + src/thruster_interface_auv_driver.cpp ) ament_target_dependencies(${PROJECT_NAME}_node rclcpp std_msgs vortex_msgs - ament_index_cpp - Eigen3 ) # Install launch files. @@ -38,54 +34,4 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -# # Define the driver library -# add_library(thruster_interface_auv_driver -# src/thruster_interface_auv_driver.cpp -# ) -# target_include_directories(thruster_interface_auv_driver PRIVATE include) -# ament_target_dependencies(thruster_interface_auv_driver rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) - -# # Define the ROS interface library -# add_library(thruster_interface_auv_ros -# src/truster_interface_auv_ros.cpp -# ) -# target_link_libraries(thruster_interface_auv_ros thruster_interface_auv_driver) -# target_include_directories(thruster_interface_auv_ros PRIVATE include) -# ament_target_dependencies(thruster_interface_auv_ros rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) - -# # Define the executable for the ROS node -# add_executable(thruster_interface_auv_node -# src/thruster_interface_auv_node.cpp -# ) -# target_link_libraries(thruster_interface_auv_node thruster_interface_auv_ros) -# ament_target_dependencies(thruster_interface_auv_node rclcpp std_msgs vortex_msgs ament_index_cpp Eigen3) - -# # Install targets -# install(TARGETS -# thruster_interface_auv_driver -# thruster_interface_auv_ros -# thruster_interface_auv_node -# DESTINATION lib/${PROJECT_NAME} -# ) - -# install( -# DIRECTORY include/ -# DESTINATION include -# ) - -# # Install headers -# install(DIRECTORY include/ -# DESTINATION include/ -# ) - -# # Install launch files -# install(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME}/launch -# ) - -# # Install config files (coeffs.yaml) -# install(DIRECTORY config -# DESTINATION share/${PROJECT_NAME}/config -# ) - ament_package() diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp index 3c1b8be4c..e29387522 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp @@ -5,20 +5,26 @@ #include #include #include +#include +#include +#include +#include +#include +#include class ThrusterInterfaceAUVDriver { public: + ThrusterInterfaceAUVDriver( int I2C_BUS = 1, - int PICO_I2C_ADDRESS = 0x21, - double SYSTEM_OPERATIONAL_VOLTAGE = 16.0, - const std::string& ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET = "", - const std::vector& THRUSTER_MAPPING = {7,6,5,4,3,2,1,0}, - const std::vector& THRUSTER_DIRECTION = {1,1,1,1,1,1,1,1}, - const std::vector& THRUSTER_PWM_OFFSET = {0,0,0,0,0,0,0,0}, - const std::vector& PWM_MIN = {1100,1100,1100,1100,1100,1100,1100,1100}, - const std::vector& PWM_MAX = {1900,1900,1900,1900,1900,1900,1900,1900}, - const std::map>>& coeffs = {} + int PICO_I2C_ADDRESS = 0xAA, + double SYSTEM_OPERATIONAL_VOLTAGE = 12.1, + const std::vector& THRUSTER_MAPPING = {}, + const std::vector& THRUSTER_DIRECTION = {}, + const std::vector& THRUSTER_PWM_OFFSET = {}, + const std::vector& PWM_MIN = {}, + const std::vector& PWM_MAX = {}, + const std::map>>& COEFFS = {} ); ~ThrusterInterfaceAUVDriver(); @@ -26,7 +32,9 @@ class ThrusterInterfaceAUVDriver { std::vector drive_thrusters(const std::vector& thruster_forces_array); private: + int bus_fd; + int I2C_BUS; int PICO_I2C_ADDRESS; int SYSTEM_OPERATIONAL_VOLTAGE; @@ -36,7 +44,7 @@ class ThrusterInterfaceAUVDriver { std::vector PWM_MIN; std::vector PWM_MAX; - std::map>> coeffs; + std::map>> COEFFS; std::vector interpolate_forces_to_pwm(const std::vector& thruster_forces_array); void send_data_to_escs(const std::vector& thruster_pwm_array); diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp index 7ed057ba5..aff10b60a 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp @@ -4,7 +4,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/int16_multi_array.hpp" #include "vortex_msgs/msg/thruster_forces.hpp" -#include +#include "thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { public: diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp index f4db411a5..f57c8fc43 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -1,30 +1,71 @@ -#include -#include -#include -#include -#include -#include -#include +#include "thruster_interface_auv_driver.hpp" ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( int I2C_BUS, int PICO_I2C_ADDRESS, double SYSTEM_OPERATIONAL_VOLTAGE, - const std::string& ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET, const std::vector& THRUSTER_MAPPING, const std::vector& THRUSTER_DIRECTION, const std::vector& THRUSTER_PWM_OFFSET, const std::vector& PWM_MIN, const std::vector& PWM_MAX, - const std::map>>& coeffs -) : PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + const std::map>>& COEFFS +) : I2C_BUS(I2C_BUS), + PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), THRUSTER_MAPPING(THRUSTER_MAPPING), THRUSTER_DIRECTION(THRUSTER_DIRECTION), THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), PWM_MIN(PWM_MIN), PWM_MAX(PWM_MAX), - coeffs(coeffs) + COEFFS(COEFFS) { + + + + //DEBUGGING ONLY----------------------------------------------------------------------- + std::cout << "I2C_BUS: " << I2C_BUS << std::endl; + std::cout << "PICO_I2C_ADDRESS: " << PICO_I2C_ADDRESS << std::endl; + std::cout << "SYSTEM_OPERATIONAL_VOLTAGE: " << SYSTEM_OPERATIONAL_VOLTAGE << std::endl; + std::cout << "THRUSTER_MAPPING: "; + for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { + std::cout << THRUSTER_MAPPING[i] << " "; + } + std::cout << std::endl; + std::cout << "THRUSTER_DIRECTION: "; + for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { + std::cout << THRUSTER_DIRECTION[i] << " "; + } + std::cout << std::endl; + std::cout << "THRUSTER_PWM_OFFSET: "; + for (int i = 0; i < THRUSTER_PWM_OFFSET.size(); i++) { + std::cout << THRUSTER_PWM_OFFSET[i] << " "; + } + std::cout << std::endl; + std::cout << "PWM_MIN: "; + for (int i = 0; i < PWM_MIN.size(); i++) { + std::cout << PWM_MIN[i] << " "; + } + std::cout << std::endl; + std::cout << "PWM_MAX: "; + for (int i = 0; i < PWM_MAX.size(); i++) { + std::cout << PWM_MAX[i] << " "; + } + std::cout << std::endl; + for (auto const& [key, val] : COEFFS) { + std::cout << "COEFFS[" << key << "]: " << std::endl; + for (auto const& [key2, val2] : val) { + std::cout << "COEFFS[" << key << "][" << key2 << "]: "; + for (int i = 0; i < val2.size(); i++) { + std::cout << val2[i] << " "; + } + std::cout << std::endl; + } + } + //--------------------------------------------------------------------------------------- + + + // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); bus_fd = open(i2c_filename.c_str(), O_RDWR); @@ -32,7 +73,7 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; } - // Convert SYSTEM_OPERATIONAL_VOLTAGE to a category + // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to integer field variable if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { this->SYSTEM_OPERATIONAL_VOLTAGE = 10; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { @@ -70,8 +111,8 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const } // Select the appropriate coefficients based on the operational voltage - auto left_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; - auto right_coeffs = coeffs[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; + auto left_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; + auto right_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; std::vector interpolated_pwm; for (size_t i = 0; i < forces_in_kg.size(); ++i) { diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp index c6200f68d..91808f769 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp @@ -1,10 +1,9 @@ -#include "thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp" +#include "thruster_interface_auv_ros.hpp" int main(int argc, char** argv) { rclcpp::init(argc, argv); RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_cpp_node"); - auto node = std::make_shared(); - rclcpp::spin(node); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp index b6c73e113..2eac7f3b6 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp @@ -1,45 +1,49 @@ -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp" +#include "thruster_interface_auv_ros.hpp" ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { - // Declare parameters - this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping", {7,6,5,4,3,2,1,0}); - this->declare_parameter>("propulsion.thrusters.thruster_direction", {1,1,1,1,1,1,1,1}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset", {0,0,0,0,0,0,0,0}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_min", {1100,1100,1100,1100,1100,1100,1100,1100}); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_max", {1900,1900,1900,1900,1900,1900,1900,1900}); - this->declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0); - // Coefficients parameters - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; + //create a subscriber that takes data from thruster forces and publisher for debugging + this->thruster_forces_subscriber_ = this->create_subscription( + "thrust/thruster_forces", 10, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1) + ); + this->thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX_cpp", 10); + //declare orca.yaml parameters + this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping"); + this->declare_parameter>("propulsion.thrusters.thruster_direction"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); + this->declare_parameter("propulsion.thrusters.thrust_update_rate"); + + //get orca.yaml parameters + auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); + auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); + auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); + auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); + auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); + this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); + + //coeffs.yaml parameters std::map>> coeffs; std::vector voltage_levels = {10, 12, 14, 16, 18, 20}; for (int voltage : voltage_levels) { std::string left_param = "coeffs." + std::to_string(voltage) + "V.LEFT"; std::string right_param = "coeffs." + std::to_string(voltage) + "V.RIGHT"; - this->declare_parameter(left_param, std::vector{}, descriptor); - this->declare_parameter(right_param, std::vector{}, descriptor); + this->declare_parameter>(left_param); //declare coeffs.10V.LEFT + this->declare_parameter>(right_param); auto left_coeffs = this->get_parameter(left_param).as_double_array(); auto right_coeffs = this->get_parameter(right_param).as_double_array(); - coeffs[voltage]["LEFT"] = left_coeffs; + coeffs[voltage]["LEFT"] = left_coeffs; //save to coeffs[10]["LEFT"] coeffs[voltage]["RIGHT"] = right_coeffs; } - // Get parameters - auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); - auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); - auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); - auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); - auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); - thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); - // Initialize thruster driver - thruster_driver_ = ThrusterInterfaceAUVDriver( - 1, 0x21, 16.0, ament_index_cpp::get_package_share_directory("thruster_interface_auv"), + this->thruster_driver_ = ThrusterInterfaceAUVDriver( + 1, 0x21, 16.0, std::vector(thruster_mapping.begin(), thruster_mapping.end()), std::vector(thruster_direction.begin(), thruster_direction.end()), std::vector(thruster_PWM_offset.begin(), thruster_PWM_offset.end()), @@ -48,35 +52,25 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ coeffs ); - // Initialize thruster forces array - thruster_forces_array_ = std::vector(8, 0.0); - - // Set up subscriber and publisher - thruster_forces_subscriber_ = this->create_subscription( - "thrust/thruster_forces", 10, - std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1) - ); - - thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX", 10); + //Declare "thruster_forces_array" in case no topic comes in at the beginning + this->thruster_forces_array_ = std::vector(8, 2.23); - // Set up timer timer_ = this->create_wall_timer( - std::chrono::duration(thrust_timer_period_), + std::chrono::duration(this->thrust_timer_period_), std::bind(&ThrusterInterfaceAUVNode::timer_callback, this) ); - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" has been started"); + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_cpp_node\" has been started"); } void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - thruster_forces_array_ = msg->thrust; + this->thruster_forces_array_ = msg->thrust; } void ThrusterInterfaceAUVNode::timer_callback() { - // Send thruster forces to be converted into PWM signals - auto thruster_pwm_array = thruster_driver_.drive_thrusters(thruster_forces_array_); + std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(this->thruster_forces_array_); - // Publish PWM values for debugging + //publish PWM values for debugging std_msgs::msg::Int16MultiArray pwm_message; pwm_message.data = thruster_pwm_array; thruster_pwm_publisher_->publish(pwm_message); From e0446c6e3ee625b39e696aa83d7602d4cedd9778 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Tue, 22 Oct 2024 18:52:46 +0200 Subject: [PATCH 08/54] fixed constructor and offset pwm --- .../thruster_interface_auv_driver.hpp | 27 ++++---- .../src/thruster_interface_auv_driver.cpp | 64 +++++++++---------- .../src/thruster_interface_auv_ros.cpp | 2 +- 3 files changed, 47 insertions(+), 46 deletions(-) diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp index e29387522..f8f013d54 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include @@ -15,20 +14,23 @@ class ThrusterInterfaceAUVDriver { public: + //default contructor called when instantiating the object in .hpp file + ThrusterInterfaceAUVDriver(); + ~ThrusterInterfaceAUVDriver(); + + //contructor actually used ThrusterInterfaceAUVDriver( - int I2C_BUS = 1, - int PICO_I2C_ADDRESS = 0xAA, - double SYSTEM_OPERATIONAL_VOLTAGE = 12.1, - const std::vector& THRUSTER_MAPPING = {}, - const std::vector& THRUSTER_DIRECTION = {}, - const std::vector& THRUSTER_PWM_OFFSET = {}, - const std::vector& PWM_MIN = {}, - const std::vector& PWM_MAX = {}, - const std::map>>& COEFFS = {} + int I2C_BUS, + int PICO_I2C_ADDRESS, + double SYSTEM_OPERATIONAL_VOLTAGE, + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, + const std::vector& THRUSTER_PWM_OFFSET, + const std::vector& PWM_MIN, + const std::vector& PWM_MAX, + const std::map>>& COEFFS ); - ~ThrusterInterfaceAUVDriver(); - std::vector drive_thrusters(const std::vector& thruster_forces_array); private: @@ -48,7 +50,6 @@ class ThrusterInterfaceAUVDriver { std::vector interpolate_forces_to_pwm(const std::vector& thruster_forces_array); void send_data_to_escs(const std::vector& thruster_pwm_array); - double polyval(const std::vector& coeffs, double x); }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp index f57c8fc43..8b0a79d79 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -1,5 +1,7 @@ #include "thruster_interface_auv_driver.hpp" +ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(){} + ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( int I2C_BUS, int PICO_I2C_ADDRESS, @@ -12,7 +14,7 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( const std::map>>& COEFFS ) : I2C_BUS(I2C_BUS), PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), + //SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), done after in the if-else THRUSTER_MAPPING(THRUSTER_MAPPING), THRUSTER_DIRECTION(THRUSTER_DIRECTION), THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), @@ -28,36 +30,36 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::cout << "PICO_I2C_ADDRESS: " << PICO_I2C_ADDRESS << std::endl; std::cout << "SYSTEM_OPERATIONAL_VOLTAGE: " << SYSTEM_OPERATIONAL_VOLTAGE << std::endl; std::cout << "THRUSTER_MAPPING: "; - for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { + for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { std::cout << THRUSTER_MAPPING[i] << " "; } std::cout << std::endl; std::cout << "THRUSTER_DIRECTION: "; - for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { + for (size_t i = 0; i < THRUSTER_DIRECTION.size(); ++i) { std::cout << THRUSTER_DIRECTION[i] << " "; } std::cout << std::endl; std::cout << "THRUSTER_PWM_OFFSET: "; - for (int i = 0; i < THRUSTER_PWM_OFFSET.size(); i++) { + for (size_t i = 0; i < THRUSTER_PWM_OFFSET.size(); ++i) { std::cout << THRUSTER_PWM_OFFSET[i] << " "; } std::cout << std::endl; std::cout << "PWM_MIN: "; - for (int i = 0; i < PWM_MIN.size(); i++) { + for (size_t i = 0; i < PWM_MIN.size(); ++i) { std::cout << PWM_MIN[i] << " "; } std::cout << std::endl; std::cout << "PWM_MAX: "; - for (int i = 0; i < PWM_MAX.size(); i++) { + for (size_t i = 0; i < PWM_MAX.size(); ++i) { std::cout << PWM_MAX[i] << " "; } std::cout << std::endl; - for (auto const& [key, val] : COEFFS) { - std::cout << "COEFFS[" << key << "]: " << std::endl; - for (auto const& [key2, val2] : val) { - std::cout << "COEFFS[" << key << "][" << key2 << "]: "; - for (int i = 0; i < val2.size(); i++) { - std::cout << val2[i] << " "; + for (auto const& [voltage, coeffs] : COEFFS) { + std::cout << "Voltage: " << (int)voltage << std::endl; + for (auto const& [direction, coeff] : coeffs) { + std::cout << "Direction: " << direction << std::endl; + for (size_t i = 0; i < coeff.size(); ++i) { + std::cout << coeff[i] << " "; } std::cout << std::endl; } @@ -66,14 +68,7 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - // Open the I2C bus - std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); - bus_fd = open(i2c_filename.c_str(), O_RDWR); - if (bus_fd < 0) { - std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; - } - - // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to integer field variable + // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to assign the integer field variable [one is double, field var is int] if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { this->SYSTEM_OPERATIONAL_VOLTAGE = 10; } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { @@ -87,6 +82,13 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( } else { this->SYSTEM_OPERATIONAL_VOLTAGE = 20; } + + // Open the I2C bus + std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); + bus_fd = open(i2c_filename.c_str(), O_RDWR); + if (bus_fd < 0) { + std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; + } } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { @@ -95,14 +97,6 @@ ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { } } -double ThrusterInterfaceAUVDriver::polyval(const std::vector& coeffs, double x) { - double result = 0.0; - for (size_t i = 0; i < coeffs.size(); ++i) { - result += coeffs[i] * std::pow(x, coeffs.size() - i - 1); - } - return result; -} - std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const std::vector& thruster_forces_array) { // Convert Newtons to Kg (since the thruster datasheet is in Kg) std::vector forces_in_kg(thruster_forces_array.size()); @@ -119,11 +113,17 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const double force = forces_in_kg[i]; double pwm = 0.0; if (force < 0) { - pwm = polyval(left_coeffs, force); + pwm = left_coeffs[0] * std::pow(forces_in_kg[i], 3) + \ + left_coeffs[1] * std::pow(forces_in_kg[i], 2) + \ + left_coeffs[2] * forces_in_kg[i] + \ + left_coeffs[3]; } else if (force == 0.0) { - pwm = 1500 - THRUSTER_PWM_OFFSET[i]; + pwm = 1500; } else { - pwm = polyval(right_coeffs, force); + pwm = right_coeffs[0] * std::pow(forces_in_kg[i], 3) + \ + right_coeffs[1] * std::pow(forces_in_kg[i], 2) + \ + right_coeffs[2] * forces_in_kg[i] + \ + right_coeffs[3]; } interpolated_pwm.push_back(static_cast(pwm)); } @@ -162,7 +162,7 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vect // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; + //thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; // Clamp the PWM signal if (thruster_pwm_array[i] < PWM_MIN[i]) { diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp index 2eac7f3b6..cf61e5e4e 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp @@ -53,7 +53,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ ); //Declare "thruster_forces_array" in case no topic comes in at the beginning - this->thruster_forces_array_ = std::vector(8, 2.23); + this->thruster_forces_array_ = std::vector(8, 0.0); timer_ = this->create_wall_timer( std::chrono::duration(this->thrust_timer_period_), From eba1387fba6f61bc6c5931bac190f0d2ae3bcab2 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Wed, 23 Oct 2024 21:12:03 +0200 Subject: [PATCH 09/54] removed debugging prints --- .../thruster_interface_auv_node.py | 2 +- .../src/thruster_interface_auv_driver.cpp | 45 ------------------- .../src/thruster_interface_auv_ros.cpp | 2 +- 3 files changed, 2 insertions(+), 47 deletions(-) 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 3d9515fa6..262ec948f 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 @@ -102,7 +102,7 @@ def __init__(self) -> None: # Start clock timer for driving thrusters every 0.2 seconds # Declare "self.thruster_forces_array" in case no topic comes in at the first possible second - self.thruster_forces_array = [2.23] * 8 + self.thruster_forces_array = [0.00] * 8 self.timer = self.create_timer(self.thrust_timer_period, self._timer_callback) diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp index 8b0a79d79..7331cb5d3 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -23,51 +23,6 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( COEFFS(COEFFS) { - - - //DEBUGGING ONLY----------------------------------------------------------------------- - std::cout << "I2C_BUS: " << I2C_BUS << std::endl; - std::cout << "PICO_I2C_ADDRESS: " << PICO_I2C_ADDRESS << std::endl; - std::cout << "SYSTEM_OPERATIONAL_VOLTAGE: " << SYSTEM_OPERATIONAL_VOLTAGE << std::endl; - std::cout << "THRUSTER_MAPPING: "; - for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { - std::cout << THRUSTER_MAPPING[i] << " "; - } - std::cout << std::endl; - std::cout << "THRUSTER_DIRECTION: "; - for (size_t i = 0; i < THRUSTER_DIRECTION.size(); ++i) { - std::cout << THRUSTER_DIRECTION[i] << " "; - } - std::cout << std::endl; - std::cout << "THRUSTER_PWM_OFFSET: "; - for (size_t i = 0; i < THRUSTER_PWM_OFFSET.size(); ++i) { - std::cout << THRUSTER_PWM_OFFSET[i] << " "; - } - std::cout << std::endl; - std::cout << "PWM_MIN: "; - for (size_t i = 0; i < PWM_MIN.size(); ++i) { - std::cout << PWM_MIN[i] << " "; - } - std::cout << std::endl; - std::cout << "PWM_MAX: "; - for (size_t i = 0; i < PWM_MAX.size(); ++i) { - std::cout << PWM_MAX[i] << " "; - } - std::cout << std::endl; - for (auto const& [voltage, coeffs] : COEFFS) { - std::cout << "Voltage: " << (int)voltage << std::endl; - for (auto const& [direction, coeff] : coeffs) { - std::cout << "Direction: " << direction << std::endl; - for (size_t i = 0; i < coeff.size(); ++i) { - std::cout << coeff[i] << " "; - } - std::cout << std::endl; - } - } - //--------------------------------------------------------------------------------------- - - - // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to assign the integer field variable [one is double, field var is int] if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { this->SYSTEM_OPERATIONAL_VOLTAGE = 10; diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp index cf61e5e4e..f2bc72108 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp @@ -53,7 +53,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ ); //Declare "thruster_forces_array" in case no topic comes in at the beginning - this->thruster_forces_array_ = std::vector(8, 0.0); + this->thruster_forces_array_ = std::vector(8, 0.00); timer_ = this->create_wall_timer( std::chrono::duration(this->thrust_timer_period_), From 070913ccb87dadd3d68ce3016f2b57ed71366541 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 13:55:48 +0200 Subject: [PATCH 10/54] refactor(workflows): removed old ci workflows and added updated ci workflows --- .github/workflows/bandit.yaml | 23 -------- .github/workflows/black.yaml | 23 -------- .github/workflows/build-test.yaml | 72 ------------------------ .github/workflows/ci-pre-commit.yaml | 11 ++++ .github/workflows/clang-format.yaml | 33 ----------- .github/workflows/docker-publish.yml | 60 -------------------- .github/workflows/isort.yaml | 23 -------- .github/workflows/mypy.yaml | 23 -------- .github/workflows/pylint.yaml | 23 -------- .github/workflows/source-build.yaml | 17 ++++++ .github/workflows/update-pre-commit.yaml | 13 +++++ 11 files changed, 41 insertions(+), 280 deletions(-) delete mode 100644 .github/workflows/bandit.yaml delete mode 100644 .github/workflows/black.yaml delete mode 100644 .github/workflows/build-test.yaml create mode 100644 .github/workflows/ci-pre-commit.yaml delete mode 100644 .github/workflows/clang-format.yaml delete mode 100644 .github/workflows/docker-publish.yml delete mode 100644 .github/workflows/isort.yaml delete mode 100644 .github/workflows/mypy.yaml delete mode 100644 .github/workflows/pylint.yaml create mode 100644 .github/workflows/source-build.yaml create mode 100644 .github/workflows/update-pre-commit.yaml diff --git a/.github/workflows/bandit.yaml b/.github/workflows/bandit.yaml deleted file mode 100644 index b16067449..000000000 --- a/.github/workflows/bandit.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: Bandit Security Scan - -on: [pull_request] - -jobs: - bandit: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - - name: Install bandit - run: | - python -m pip install bandit[toml] - - - name: Run bandit scan - run: | - bandit -c pyproject.toml -r . --severity-level medium diff --git a/.github/workflows/black.yaml b/.github/workflows/black.yaml deleted file mode 100644 index 9f995176f..000000000 --- a/.github/workflows/black.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: Black Code Formatter Check - -on: [pull_request] - -jobs: - black: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - - name: Install black - run: | - python -m pip install black - - - name: Run black - run: | - black --check . diff --git a/.github/workflows/build-test.yaml b/.github/workflows/build-test.yaml deleted file mode 100644 index 5d8e2b5be..000000000 --- a/.github/workflows/build-test.yaml +++ /dev/null @@ -1,72 +0,0 @@ -name: Build and Test - -on: [pull_request] - -jobs: - build: - runs-on: ubuntu-22.04 - - steps: - - name: Checkout Repository - uses: actions/checkout@v4 - - - name: Install ROS 2 Humble - run: | - sudo apt update - sudo apt install -y curl gnupg lsb-release - - # Add the ROS 2 GPG key - sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg - - # Add the ROS 2 repository - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null - - sudo apt update - - # Install ROS 2 Humble desktop version - sudo apt install -y ros-humble-desktop - - - name: Install Build Tools and Dependencies - run: | - sudo apt install -y python3-colcon-common-extensions python3-rosdep - - # Initialize rosdep - sudo rosdep init - rosdep update - - - name: Set up ROS 2 Workspace - run: | - mkdir -p ~/ros2_ws/src - cd ~/ros2_ws/src - - # Copy the repository code to the workspace src folder - cp -r $GITHUB_WORKSPACE/* . - - # Clone vortex-msgs repository - git clone https://github.com/vortexntnu/vortex-msgs.git - - # Go back to workspace root - cd ~/ros2_ws - - - name: Install Package Dependencies with rosdep - run: | - source /opt/ros/humble/setup.bash - cd ~/ros2_ws - rosdep install --from-paths src --ignore-src -r -y - - - name: Build ROS 2 Workspace - run: | - source /opt/ros/humble/setup.bash - cd ~/ros2_ws - colcon build --event-handlers console_cohesion+ - - - name: Run Tests - run: | - source /opt/ros/humble/setup.bash - cd ~/ros2_ws - colcon test --event-handlers console_direct+ - - - name: Test Results Summary - run: | - cd ~/ros2_ws - colcon test-result --verbose diff --git a/.github/workflows/ci-pre-commit.yaml b/.github/workflows/ci-pre-commit.yaml new file mode 100644 index 000000000..642bb7fb4 --- /dev/null +++ b/.github/workflows/ci-pre-commit.yaml @@ -0,0 +1,11 @@ +name: Run Pre-Commit Workflow +# Run pre-commit hooks on a repository +on: + workflow_dispatch: + pull_request: +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-pre-commit.yaml@main + with: + ref: ${{ github.ref }} + python_version: '3.11' diff --git a/.github/workflows/clang-format.yaml b/.github/workflows/clang-format.yaml deleted file mode 100644 index 7e72dc520..000000000 --- a/.github/workflows/clang-format.yaml +++ /dev/null @@ -1,33 +0,0 @@ -name: Clang-Format Code Style Check - -on: [pull_request] - -jobs: - clang-format: - runs-on: ubuntu-latest - - steps: - - name: Checkout code - uses: actions/checkout@v4 - - - name: Install clang-format - run: | - sudo apt-get update - sudo apt-get install -y clang-format - - - name: Run clang-format check - run: | - # Find all C/C++ source and header files - find . -type f \( -name '*.c' -o -name '*.cpp' -o -name '*.cc' -o -name '*.cxx' -o -name '*.h' -o -name '*.hpp' -o -name '*.hh' -o -name '*.hxx' \) -print0 | xargs -0 clang-format -i - - # Check if any files were modified - if [[ $(git status --porcelain) ]]; then - echo "Code is not properly formatted. Please run clang-format." - echo "Modified files:" - git status --porcelain - echo "Differences:" - git --no-pager diff - exit 1 - else - echo "All code is properly formatted." - fi diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml deleted file mode 100644 index 0bb2cce3b..000000000 --- a/.github/workflows/docker-publish.yml +++ /dev/null @@ -1,60 +0,0 @@ -name: Publish Docker image to ghcr - -on: - push: - branches: [development] - -env: - REGISTRY: ghcr.io - IMAGE_NAME: ${{ github.repository }} - -jobs: - build-and-push-image: - runs-on: ubuntu-latest - permissions: - contents: read - packages: write - - steps: - - name: Checkout own repository - uses: actions/checkout@v3 - - # vortex_msgs needs to be in the same location as the rest of the vortex-auv packages - - name: Checkout vortex_msgs - uses: actions/checkout@v3 - with: - repository: "vortexntnu/vortex-msgs" - path: "./vortex-msgs" - - name: Move vortex_msgs - run: mv ./vortex-msgs ../vortex-msgs - - # robot_localization needs to be in the same location as the rest of the vortex-auv packages - - name: Checkout robot_localization - uses: actions/checkout@v3 - with: - repository: "vortexntnu/robot_localization" - path: "./robot_localization" - - name: Move robot_localization - run: mv ./robot_localization ../robot_localization - - - name: Log in to the Container registry - uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 - with: - registry: ${{ env.REGISTRY }} - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract metadata (tags, labels) for Docker - id: meta - uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 - with: - images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} - - - name: Build and push Docker image - uses: docker/build-push-action@ad44023a93711e3deb337508980b4b5e9bcdc5dc - with: - context: .. - file: ./docker/Dockerfile - push: true - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} diff --git a/.github/workflows/isort.yaml b/.github/workflows/isort.yaml deleted file mode 100644 index dd12f04a5..000000000 --- a/.github/workflows/isort.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: isort Import Sorting Check - -on: [pull_request] - -jobs: - isort: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - - name: Install isort - run: | - python -m pip install isort - - - name: Run isort - run: | - isort --check . diff --git a/.github/workflows/mypy.yaml b/.github/workflows/mypy.yaml deleted file mode 100644 index 261452bc0..000000000 --- a/.github/workflows/mypy.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: Mypy Type Checker - -on: [pull_request] - -jobs: - mypy: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - - name: Install mypy - run: | - python -m pip install mypy - - - name: Run mypy - run: | - mypy . --explicit-package-bases diff --git a/.github/workflows/pylint.yaml b/.github/workflows/pylint.yaml deleted file mode 100644 index cc7ca806b..000000000 --- a/.github/workflows/pylint.yaml +++ /dev/null @@ -1,23 +0,0 @@ -name: Pylint Code Linter - -on: [pull_request] - -jobs: - pylint: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - - name: Install pylint - run: | - python -m pip install pylint - - - name: Run pylint - run: | - pylint . diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml new file mode 100644 index 000000000..5fdc08b71 --- /dev/null +++ b/.github/workflows/source-build.yaml @@ -0,0 +1,17 @@ +name: Source Build +# Build the ROS 2 workspace from source code and run tests +on: + push: + workflow_dispatch: + pull_request: + # Runs daily to check for dependency issues or flaking tests + schedule: + - cron: "0 1 * * *" +jobs: + source-build: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-source-build.yaml@main + with: + ros_distro: 'humble' + os_name: 'ubuntu-22.04' + ref: ${{ github.ref_name }} + vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos' diff --git a/.github/workflows/update-pre-commit.yaml b/.github/workflows/update-pre-commit.yaml new file mode 100644 index 000000000..1246b33f2 --- /dev/null +++ b/.github/workflows/update-pre-commit.yaml @@ -0,0 +1,13 @@ +name: Run Pre-Commit Update Workflow +# Update pre-commit config and create a pull request if changes are detected +on: + workflow_dispatch: + schedule: + - cron: "0 0 1 * *" # Run every month at midnight on the first day of the month +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-update-pre-commit.yaml@main + with: + ref: ${{ github.ref }} + python_version: '3.10' + responsible: 'kluge7' From 0a33453216222b74879f6fe3b37c9237c90bde45 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 13:56:20 +0200 Subject: [PATCH 11/54] refactor(config): updated pre commit hooks and removed unused config files --- .codespellignore | 1 - .codespellrc | 6 - .pre-commit-config.yaml | 107 ++++--- .pylintrc | 666 ---------------------------------------- mypy.ini | 30 -- pyproject.toml | 7 - 6 files changed, 66 insertions(+), 751 deletions(-) delete mode 100644 .codespellignore delete mode 100644 .codespellrc delete mode 100644 .pylintrc delete mode 100644 mypy.ini delete mode 100644 pyproject.toml diff --git a/.codespellignore b/.codespellignore deleted file mode 100644 index db80dec81..000000000 --- a/.codespellignore +++ /dev/null @@ -1 +0,0 @@ -theses diff --git a/.codespellrc b/.codespellrc deleted file mode 100644 index e3368847e..000000000 --- a/.codespellrc +++ /dev/null @@ -1,6 +0,0 @@ -[codespell] -# File containing words to ignore during the spell check. -ignore-words = .codespellignore - -# Check file names as well. -check-filenames = true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 85a6bc9d0..a5a292cb2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,57 +1,82 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + repos: - # Formatters and Code Fixers + # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - # Fix end-of-file newlines + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: ["--allow-multiple-documents"] + - id: debug-statements - id: end-of-file-fixer - # Remove trailing whitespaces + - id: mixed-line-ending - id: trailing-whitespace - # Fix byte order marker (BOM) issues + exclude_types: [rst] - id: fix-byte-order-marker - # Ensure consistent line endings - - id: mixed-line-ending - args: ["--fix=lf"] - - # Import Sorting - - repo: https://github.com/pycqa/isort - rev: 5.13.2 + - id: requirements-txt-fixer + # Python hooks + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.7.0 hooks: - - id: isort - - # Code Formatters - - repo: https://github.com/psf/black - rev: 24.8.0 - hooks: - - id: black - + - id: ruff-format + - id: ruff + name: ruff-isort + args: [ + "--select=I", + "--fix" + ] + - id: ruff + name: ruff-pyupgrade + args: [ + "--select=UP", + "--fix" + ] + - id: ruff + name: ruff-pydocstyle + args: [ + "--select=D", + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D213,D205,D401,D415,D400,D417,D404", + "--fix" + ] + stages: [pre-commit] + pass_filenames: true + - id: ruff + name: ruff-check + args: [ + "--select=F,PT,B,C4,T20,S,N", + "--ignore=T201,N806,F841,B006,N803,B020,S101,S607,S603,B007,N815,B005,N816,N999", + "--fix" + ] + stages: [pre-commit] + pass_filenames: true + # C++ hooks - repo: https://github.com/pre-commit/mirrors-clang-format rev: v19.1.0 hooks: - id: clang-format args: [--style=file] - - # Spell Checking + # Spellcheck in comments and docs - repo: https://github.com/codespell-project/codespell rev: v2.3.0 hooks: - id: codespell - args: ['--write-changes'] - - # Linters and Static Analysis Tools - - repo: https://github.com/pre-commit/mirrors-mypy - rev: v1.11.2 - hooks: - - id: mypy - args: ["--explicit-package-bases"] - - - repo: https://github.com/PyCQA/pylint - rev: v3.3.1 - hooks: - - id: pylint - - - repo: https://github.com/PyCQA/bandit - rev: 1.7.10 - hooks: - - id: bandit - args: ["--severity-level", "medium"] + args: ['--write-changes', '--ignore-words-list=theses'] diff --git a/.pylintrc b/.pylintrc deleted file mode 100644 index 32ceb669a..000000000 --- a/.pylintrc +++ /dev/null @@ -1,666 +0,0 @@ -[MAIN] - -# Analyse import fallback blocks. This can be used to support both Python 2 and -# 3 compatible code, which means that the block might have code that exists -# only in one or another interpreter, leading to false positives when analysed. -analyse-fallback-blocks=no - -# Clear in-memory caches upon conclusion of linting. Useful if running pylint -# in a server-like mode. -clear-cache-post-run=no - -# Load and enable all available extensions. Use --list-extensions to see a list -# all available extensions. -#enable-all-extensions= - -# In error mode, messages with a category besides ERROR or FATAL are -# suppressed, and no reports are done by default. Error mode is compatible with -# disabling specific errors. -#errors-only= - -# Always return a 0 (non-error) status code, even if lint errors are found. -# This is primarily useful in continuous integration scripts. -#exit-zero= - -# A comma-separated list of package or module names from where C extensions may -# be loaded. Extensions are loading into the active Python interpreter and may -# run arbitrary code. -extension-pkg-allow-list= - -# A comma-separated list of package or module names from where C extensions may -# be loaded. Extensions are loading into the active Python interpreter and may -# run arbitrary code. (This is an alternative name to extension-pkg-allow-list -# for backward compatibility.) -extension-pkg-whitelist= - -# Return non-zero exit code if any of these messages/categories are detected, -# even if score is above --fail-under value. Syntax same as enable. Messages -# specified are enabled, while categories only check already-enabled messages. -fail-on= - -# Specify a score threshold under which the program will exit with error. -fail-under=10 - -# Interpret the stdin as a python script, whose filename needs to be passed as -# the module_or_package argument. -#from-stdin= - -# Files or directories to be skipped. They should be base names, not paths. -ignore=CVS - -# Add files or directories matching the regular expressions patterns to the -# ignore-list. The regex matches against paths and can be in Posix or Windows -# format. Because '\\' represents the directory delimiter on Windows systems, -# it can't be used as an escape character. -ignore-paths= - -# Files or directories matching the regular expression patterns are skipped. -# The regex matches against base names, not paths. The default value ignores -# Emacs file locks -ignore-patterns=^\.# - -# List of module names for which member attributes should not be checked and -# will not be imported (useful for modules/projects where namespaces are -# manipulated during runtime and thus existing member attributes cannot be -# deduced by static analysis). It supports qualified module names, as well as -# Unix pattern matching. -ignored-modules= - -# Python code to execute, usually for sys.path manipulation such as -# pygtk.require(). -#init-hook= - -# Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the -# number of processors available to use, and will cap the count on Windows to -# avoid hangs. -jobs=1 - -# Control the amount of potential inferred values when inferring a single -# object. This can help the performance when dealing with large functions or -# complex, nested conditions. -limit-inference-results=100 - -# List of plugins (as comma separated values of python module names) to load, -# usually to register additional checkers. -load-plugins= - -# Pickle collected data for later comparisons. -persistent=yes - -# Resolve imports to .pyi stubs if available. May reduce no-member messages and -# increase not-an-iterable messages. -prefer-stubs=no - -# Minimum Python version to use for version dependent checks. Will default to -# the version used to run pylint. -py-version=3.11 - -# Discover python modules and packages in the file system subtree. -recursive=no - -# Add paths to the list of the source roots. Supports globbing patterns. The -# source root is an absolute path or a path relative to the current working -# directory used to determine a package namespace for modules located under the -# source root. -source-roots= - -# When enabled, pylint would attempt to guess common misconfiguration and emit -# user-friendly hints instead of false-positive error messages. -suggestion-mode=yes - -# Allow loading of arbitrary C extensions. Extensions are imported into the -# active Python interpreter and may run arbitrary code. -unsafe-load-any-extension=no - -# In verbose mode, extra non-checker-related info will be displayed. -#verbose= - - -[BASIC] - -# Naming style matching correct argument names. -argument-naming-style=snake_case - -# Regular expression matching correct argument names. Overrides argument- -# naming-style. If left empty, argument names will be checked with the set -# naming style. -#argument-rgx= - -# Naming style matching correct attribute names. -attr-naming-style=snake_case - -# Regular expression matching correct attribute names. Overrides attr-naming- -# style. If left empty, attribute names will be checked with the set naming -# style. -#attr-rgx= - -# Bad variable names which should always be refused, separated by a comma. -bad-names=foo, - bar, - baz, - toto, - tutu, - tata - -# Bad variable names regexes, separated by a comma. If names match any regex, -# they will always be refused -bad-names-rgxs= - -# Naming style matching correct class attribute names. -class-attribute-naming-style=any - -# Regular expression matching correct class attribute names. Overrides class- -# attribute-naming-style. If left empty, class attribute names will be checked -# with the set naming style. -#class-attribute-rgx= - -# Naming style matching correct class constant names. -class-const-naming-style=UPPER_CASE - -# Regular expression matching correct class constant names. Overrides class- -# const-naming-style. If left empty, class constant names will be checked with -# the set naming style. -#class-const-rgx= - -# Naming style matching correct class names. -class-naming-style=PascalCase - -# Regular expression matching correct class names. Overrides class-naming- -# style. If left empty, class names will be checked with the set naming style. -#class-rgx= - -# Naming style matching correct constant names. -const-naming-style=UPPER_CASE - -# Regular expression matching correct constant names. Overrides const-naming- -# style. If left empty, constant names will be checked with the set naming -# style. -#const-rgx= - -# Minimum line length for functions/classes that require docstrings, shorter -# ones are exempt. -docstring-min-length=-1 - -# Naming style matching correct function names. -function-naming-style=snake_case - -# Regular expression matching correct function names. Overrides function- -# naming-style. If left empty, function names will be checked with the set -# naming style. -#function-rgx= - -# Good variable names which should always be accepted, separated by a comma. -good-names=i, - j, - k, - ex, - Run, - _ - -# Good variable names regexes, separated by a comma. If names match any regex, -# they will always be accepted -good-names-rgxs= - -# Include a hint for the correct naming format with invalid-name. -include-naming-hint=no - -# Naming style matching correct inline iteration names. -inlinevar-naming-style=any - -# Regular expression matching correct inline iteration names. Overrides -# inlinevar-naming-style. If left empty, inline iteration names will be checked -# with the set naming style. -#inlinevar-rgx= - -# Naming style matching correct method names. -method-naming-style=snake_case - -# Regular expression matching correct method names. Overrides method-naming- -# style. If left empty, method names will be checked with the set naming style. -#method-rgx= - -# Naming style matching correct module names. -module-naming-style=snake_case - -# Regular expression matching correct module names. Overrides module-naming- -# style. If left empty, module names will be checked with the set naming style. -#module-rgx= - -# Colon-delimited sets of names that determine each other's naming style when -# the name regexes allow several styles. -name-group= - -# Regular expression which should only match function or class names that do -# not require a docstring. -no-docstring-rgx=^_ - -# List of decorators that produce properties, such as abc.abstractproperty. Add -# to this list to register other decorators that produce valid properties. -# These decorators are taken in consideration only for invalid-name. -property-classes=abc.abstractproperty - -# Regular expression matching correct type alias names. If left empty, type -# alias names will be checked with the set naming style. -#typealias-rgx= - -# Regular expression matching correct type variable names. If left empty, type -# variable names will be checked with the set naming style. -#typevar-rgx= - -# Naming style matching correct variable names. -variable-naming-style=snake_case - -# Regular expression matching correct variable names. Overrides variable- -# naming-style. If left empty, variable names will be checked with the set -# naming style. -#variable-rgx= - - -[CLASSES] - -# Warn about protected attribute access inside special methods -check-protected-access-in-special-methods=no - -# List of method names used to declare (i.e. assign) instance attributes. -defining-attr-methods=__init__, - __new__, - setUp, - asyncSetUp, - __post_init__ - -# List of member names, which should be excluded from the protected access -# warning. -exclude-protected=_asdict,_fields,_replace,_source,_make,os._exit - -# List of valid names for the first argument in a class method. -valid-classmethod-first-arg=cls - -# List of valid names for the first argument in a metaclass class method. -valid-metaclass-classmethod-first-arg=mcs - - -[DESIGN] - -# List of regular expressions of class ancestor names to ignore when counting -# public methods (see R0903) -exclude-too-few-public-methods= - -# List of qualified class names to ignore when counting class parents (see -# R0901) -ignored-parents= - -# Maximum number of arguments for function / method. -max-args=21 - -# Maximum number of attributes for a class (see R0902). -max-attributes=22 - -# Maximum number of boolean expressions in an if statement (see R0916). -max-bool-expr=5 - -# Maximum number of branch for function / method body. -max-branches=18 - -# Maximum number of locals for function / method body. -max-locals=24 - -# Maximum number of parents for a class (see R0901). -max-parents=7 - -# Maximum number of positional arguments for function / method. -max-positional-arguments=21 - -# Maximum number of public methods for a class (see R0904). -max-public-methods=20 - -# Maximum number of return / yield for function / method body. -max-returns=6 - -# Maximum number of statements in function / method body. -max-statements=64 - -# Minimum number of public methods for a class (see R0903). -min-public-methods=2 - - -[EXCEPTIONS] - -# Exceptions that will emit a warning when caught. -overgeneral-exceptions=builtins.BaseException,builtins.Exception - - -[FORMAT] - -# Expected format of line ending, e.g. empty (any line ending), LF or CRLF. -expected-line-ending-format= - -# Regexp for a line that is allowed to be longer than the limit. -ignore-long-lines=^\s*(# )??$ - -# Number of spaces of indent required inside a hanging or continued line. -indent-after-paren=4 - -# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 -# tab). -indent-string=' ' - -# Maximum number of characters on a single line. -max-line-length=150 - -# Maximum number of lines in a module. -max-module-lines=1000 - -# Allow the body of a class to be on the same line as the declaration if body -# contains single statement. -single-line-class-stmt=no - -# Allow the body of an if to be on the same line as the test if there is no -# else. -single-line-if-stmt=no - - -[IMPORTS] - -# List of modules that can be imported at any level, not just the top level -# one. -allow-any-import-level= - -# Allow explicit reexports by alias from a package __init__. -allow-reexport-from-package=no - -# Allow wildcard imports from modules that define __all__. -allow-wildcard-with-all=no - -# Deprecated modules which should not be used, separated by a comma. -deprecated-modules= - -# Output a graph (.gv or any supported image format) of external dependencies -# to the given file (report RP0402 must not be disabled). -ext-import-graph= - -# Output a graph (.gv or any supported image format) of all (i.e. internal and -# external) dependencies to the given file (report RP0402 must not be -# disabled). -import-graph= - -# Output a graph (.gv or any supported image format) of internal dependencies -# to the given file (report RP0402 must not be disabled). -int-import-graph= - -# Force import order to recognize a module as part of the standard -# compatibility libraries. -known-standard-library= - -# Force import order to recognize a module as part of a third party library. -known-third-party=enchant - -# Couples of modules and preferred modules, separated by a comma. -preferred-modules= - - -[LOGGING] - -# The type of string formatting that logging methods do. `old` means using % -# formatting, `new` is for `{}` formatting. -logging-format-style=old - -# Logging modules to check that the string format arguments are in logging -# function parameter format. -logging-modules=logging - - -[MESSAGES CONTROL] - -# Only show warnings with the listed confidence levels. Leave empty to show -# all. Valid levels: HIGH, CONTROL_FLOW, INFERENCE, INFERENCE_FAILURE, -# UNDEFINED. -confidence=HIGH, - CONTROL_FLOW, - INFERENCE, - INFERENCE_FAILURE, - UNDEFINED - -# Disable the message, report, category or checker with the given id(s). You -# can either give multiple identifiers separated by comma (,) or put this -# option multiple times (only on the command line, not in the configuration -# file where it should appear only once). You can also use "--disable=all" to -# disable everything first and then re-enable specific checks. For example, if -# you want to run only the similarities checker, you can use "--disable=all -# --enable=similarities". If you want to run only the classes checker, but have -# no Warning level messages displayed, use "--disable=all --enable=classes -# --disable=W". -disable=raw-checker-failed, - # Critical issues related to imports, names, and exceptions - import-error, # [E0401] Unable to import module (usually a missing or incorrect import) - broad-exception-caught, # [W0703] A broad exception (e.g., `except Exception`) is caught without specificity - no-name-in-module, # [F0401] Module has no name attribute (e.g., due to a missing __init__.py file) - - # Issues related to program correctness and return consistency - dangerous-default-value, # [W0102] A mutable default argument (e.g., list, dict) is used in a function - inconsistent-return-statements, # [R1710] Function or method has inconsistent return statements - - # Code duplication and design issues - duplicate-code, # [R0801] Duplicate code detected, recommending refactoring for DRY (Don't Repeat Yourself) - too-few-public-methods, # [R0903] Class has too few public methods (often seen in utility classes) - - # Documentation issues - missing-module-docstring, # [C0114] The module is missing a docstring at the top of the file - missing-class-docstring, # [C0115] The class is missing a docstring explaining its purpose - - # Pylint configuration and suppression handling issues - bad-inline-option, # [E0012] Invalid Pylint inline option is used - locally-disabled, # [I0021] A Pylint rule is disabled locally but not re-enabled later - file-ignored, # [I0020] A file is being ignored by Pylint due to an inline directive - suppressed-message, # [I0022] A Pylint message is being suppressed using inline comments - useless-suppression, # [W0511] A Pylint suppression directive (e.g., # pylint: disable=...) is unnecessary - deprecated-pragma, # [W0104] Usage of deprecated or old-style Pylint pragma (e.g., inline comments) - unknown-option-value, # [E0012] An unknown option value is being used in Pylint configuration or inline option - - - - -# Enable the message, report, category or checker with the given id(s). You can -# either give multiple identifier separated by comma (,) or put this option -# multiple time (only on the command line, not in the configuration file where -# it should appear only once). See also the "--disable" option for examples. -enable= - - -[METHOD_ARGS] - -# List of qualified names (i.e., library.method) which require a timeout -# parameter e.g. 'requests.api.get,requests.api.post' -timeout-methods=requests.api.delete,requests.api.get,requests.api.head,requests.api.options,requests.api.patch,requests.api.post,requests.api.put,requests.api.request - - -[MISCELLANEOUS] - -# List of note tags to take in consideration, separated by a comma. -notes=FIXME, - XXX, - TODO - -# Regular expression of note tags to take in consideration. -notes-rgx= - - -[REFACTORING] - -# Maximum number of nested blocks for function / method body -max-nested-blocks=5 - -# Complete name of functions that never returns. When checking for -# inconsistent-return-statements if a never returning function is called then -# it will be considered as an explicit return statement and no message will be -# printed. -never-returning-functions=sys.exit,argparse.parse_error - -# Let 'consider-using-join' be raised when the separator to join on would be -# non-empty (resulting in expected fixes of the type: ``"- " + " - -# ".join(items)``) -suggest-join-with-non-empty-separator=yes - - -[REPORTS] - -# Python expression which should return a score less than or equal to 10. You -# have access to the variables 'fatal', 'error', 'warning', 'refactor', -# 'convention', and 'info' which contain the number of messages in each -# category, as well as 'statement' which is the total number of statements -# analyzed. This score is used by the global evaluation report (RP0004). -evaluation=max(0, 0 if fatal else 10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10)) - -# Template used to display messages. This is a python new-style format string -# used to format the message information. See doc for all details. -msg-template= - -# Set the output format. Available formats are: text, parseable, colorized, -# json2 (improved json format), json (old json format) and msvs (visual -# studio). You can also give a reporter class, e.g. -# mypackage.mymodule.MyReporterClass. -#output-format= - -# Tells whether to display a full report or only the messages. -reports=no - -# Activate the evaluation score. -score=yes - - -[SIMILARITIES] - -# Comments are removed from the similarity computation -ignore-comments=yes - -# Docstrings are removed from the similarity computation -ignore-docstrings=yes - -# Imports are removed from the similarity computation -ignore-imports=yes - -# Signatures are removed from the similarity computation -ignore-signatures=yes - -# Minimum lines number of a similarity. -min-similarity-lines=4 - - -[SPELLING] - -# Limits count of emitted suggestions for spelling mistakes. -max-spelling-suggestions=4 - -# Spelling dictionary name. No available dictionaries : You need to install -# both the python package and the system dependency for enchant to work. -spelling-dict= - -# List of comma separated words that should be considered directives if they -# appear at the beginning of a comment and should not be checked. -spelling-ignore-comment-directives=fmt: on,fmt: off,noqa:,noqa,nosec,isort:skip,mypy: - -# List of comma separated words that should not be checked. -spelling-ignore-words= - -# A path to a file that contains the private dictionary; one word per line. -spelling-private-dict-file= - -# Tells whether to store unknown words to the private dictionary (see the -# --spelling-private-dict-file option) instead of raising a message. -spelling-store-unknown-words=no - - -[STRING] - -# This flag controls whether inconsistent-quotes generates a warning when the -# character used as a quote delimiter is used inconsistently within a module. -check-quote-consistency=no - -# This flag controls whether the implicit-str-concat should generate a warning -# on implicit string concatenation in sequences defined over several lines. -check-str-concat-over-line-jumps=no - - -[TYPECHECK] - -# List of decorators that produce context managers, such as -# contextlib.contextmanager. Add to this list to register other decorators that -# produce valid context managers. -contextmanager-decorators=contextlib.contextmanager - -# List of members which are set dynamically and missed by pylint inference -# system, and so shouldn't trigger E1101 when accessed. Python regular -# expressions are accepted. -generated-members= - -# Tells whether to warn about missing members when the owner of the attribute -# is inferred to be None. -ignore-none=yes - -# This flag controls whether pylint should warn about no-member and similar -# checks whenever an opaque object is returned when inferring. The inference -# can return multiple potential results while evaluating a Python object, but -# some branches might not be evaluated, which results in partial inference. In -# that case, it might be useful to still emit no-member and other checks for -# the rest of the inferred objects. -ignore-on-opaque-inference=yes - -# List of symbolic message names to ignore for Mixin members. -ignored-checks-for-mixins=no-member, - not-async-context-manager, - not-context-manager, - attribute-defined-outside-init - -# List of class names for which member attributes should not be checked (useful -# for classes with dynamically set attributes). This supports the use of -# qualified names. -ignored-classes=optparse.Values,thread._local,_thread._local,argparse.Namespace - -# Show a hint with possible names when a member name was not found. The aspect -# of finding the hint is based on edit distance. -missing-member-hint=yes - -# The minimum edit distance a name should have in order to be considered a -# similar match for a missing member name. -missing-member-hint-distance=1 - -# The total number of similar names that should be taken in consideration when -# showing a hint for a missing member. -missing-member-max-choices=1 - -# Regex pattern to define which classes are considered mixins. -mixin-class-rgx=.*[Mm]ixin - -# List of decorators that change the signature of a decorated function. -signature-mutators= - - -[VARIABLES] - -# List of additional names supposed to be defined in builtins. Remember that -# you should avoid defining new builtins when possible. -additional-builtins= - -# Tells whether unused global variables should be treated as a violation. -allow-global-unused-variables=yes - -# List of names allowed to shadow builtins -allowed-redefined-builtins= - -# List of strings which can identify a callback function by name. A callback -# name must start or end with one of those strings. -callbacks=cb_, - _cb - -# A regular expression matching the name of dummy variables (i.e. expected to -# not be used). -dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ - -# Argument names that match this expression will be ignored. -ignored-argument-names=_.*|^ignored_|^unused_ - -# Tells whether we should check for unused import in __init__ files. -init-import=no - -# List of qualified module names which can have objects that can redefine -# builtins. -redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io diff --git a/mypy.ini b/mypy.ini deleted file mode 100644 index 1a2b59ed2..000000000 --- a/mypy.ini +++ /dev/null @@ -1,30 +0,0 @@ -# mypy.ini -[mypy] -# Only require type annotations on functions and methods -disallow_untyped_calls = False -disallow_untyped_defs = True -disallow_incomplete_defs = True - -# Ignore missing imports (can cause unnecessary noise) -ignore_missing_imports = True - -# Avoid checking for redefinitions, pylint handles variable consistency -no_implicit_reexport = True - -# Don't enforce type hinting inside functions (just the function signature) -disallow_untyped_decorators = False - -# Allow dynamic typing where it makes sense (e.g., in tests) -allow_untyped_globals = True - -# Avoid overlap with pylint style rules -check_untyped_defs = False -warn_unused_ignores = False -warn_no_return = False - -# Disable specific error codes -disable_error_code = attr-defined, arg-type, misc, call-overload, assignment, return-value, union-attr, var-annotated - -[report] -# Report only type annotation-related errors -show_error_codes = True diff --git a/pyproject.toml b/pyproject.toml deleted file mode 100644 index 833724e03..000000000 --- a/pyproject.toml +++ /dev/null @@ -1,7 +0,0 @@ -[tool.black] -line-length = 150 -target-version = ['py39'] -skip-string-normalization = true - -[tool.isort] -profile = "black" From 0b4e696bb86bee5241bb30728f08670e3841f9c8 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 13:58:04 +0200 Subject: [PATCH 12/54] refactor(formatting): applied formatting changes --- .../acoustics_data_record_lib.py | 15 +- .../acoustics_data_record_node.py | 112 ++++++--- .../launch/acoustics_data_record.launch.py | 7 +- .../utilities/display_acoustics_data_live.py | 76 ++++-- .../acoustics_interface_lib.py | 74 +++--- .../acoustics_interface_node.py | 126 +++++++--- .../launch/acoustics_interface_launch.py | 7 +- auv_setup/launch/orca.launch.py | 12 +- auv_setup/launch/topside.launch.py | 8 +- mission/LCD/sources/ip_lib.py | 4 +- mission/LCD/sources/lcd.py | 8 +- mission/LCD/sources/lcd_lib.py | 24 +- mission/LCD/sources/power_sense_module_lib.py | 25 +- mission/LCD/sources/pressure_sensor_lib.py | 4 +- mission/LCD/sources/temperature_sensor_lib.py | 7 +- .../blackbox/blackbox/blackbox_log_data.py | 56 +++-- mission/blackbox/blackbox/blackbox_node.py | 84 ++++--- mission/blackbox/launch/blackbox.launch.py | 4 +- .../power_sense_module_lib.py | 33 ++- .../power_sense_module_node.py | 59 +++-- .../pressure_sensor_lib.py | 8 +- .../pressure_sensor_node.py | 45 ++-- .../temperature_sensor_lib.py | 11 +- .../temperature_sensor_node.py | 53 ++-- .../launch/internal_status_auv.launch.py | 8 +- .../joystick_interface_auv.py | 83 +++++-- .../launch/joystick_interface_auv.launch.py | 4 +- .../tests/test_joystick_interface_auv.py | 13 +- .../launch/thrust_allocator_auv.launch.py | 4 +- motion/thruster_interface_auv/analysis.py | 41 ++-- .../launch/thruster_interface_auv.launch.py | 4 +- motion/thruster_interface_auv/package.xml | 2 +- .../thruster_interface_auv_driver_lib.py | 25 +- .../thruster_interface_auv_node.py | 109 ++++++--- .../thruster_interface_auv_driver.hpp | 75 +++--- .../thruster_interface_auv_ros.hpp | 20 +- .../thruster_interface_auv_cpp.launch.py | 20 +- .../src/thruster_interface_auv_driver.cpp | 230 +++++++++--------- .../src/thruster_interface_auv_node.cpp | 14 +- .../src/thruster_interface_auv_ros.cpp | 109 ++++----- 40 files changed, 987 insertions(+), 636 deletions(-) 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..f81a03556 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,10 +49,10 @@ 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: + Parameters + ---------- self (object): The instance of the class. hydrophone1 (list, optional): Data from hydrophone 1. Defaults to [0]. hydrophone2 (list, optional): Data from hydrophone 2. Defaults to [0]. @@ -64,12 +66,15 @@ def log_data_to_csv_file( position (list, optional): Position data. Defaults to [0.0]. Writes the current time and provided data to a CSV file located at self.data_file_location. + """ # Get current time in hours, minutes, seconds and milliseconds 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..d8e0fc4bd 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 + "/../../../../" + ) # go back to workspace ros2_package_directory_location = ( - ros2_package_directory_location + "src/vortex-auv/acoustics/acoustics_data_record/" + 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,99 +133,98 @@ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ 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. + """ self.position_data = msg.data # 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 +243,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. @@ -223,6 +254,7 @@ def main() -> None: Returns: None + """ # Initialize ROS2 rclpy.init() diff --git a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py index 085c82c38..17d32772b 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 @@ -16,6 +16,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A launch description containing the node configuration for acoustics_data_record. + """ # Path to the YAML file yaml_file_path = os.path.join( 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..5a1d66ebb 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,15 +32,21 @@ # 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 # 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] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophoneAxis[0] if i else None + ) hydrophoneAxis[i].label_outer() fig.text(0.25, 0.965, "Hydrophone Data", ha="center") @@ -58,21 +68,23 @@ 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") +acousticsCSVFiles = 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) 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. Returns: list: A list of integers extracted from the pandas object. + """ pandas_string = pandas_object.iloc[0].strip("array('i', ").rstrip(")") pandas_int_array = [int(x.strip()) for x in pandas_string.strip("[]").split(",")] @@ -81,24 +93,25 @@ def convert_pandas_object_to_int_array(pandas_object: pd.Series) -> list: 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. 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_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 @@ -122,6 +135,7 @@ def get_acoustics_data() -> list: Raises: Exception: If there is an error reading the acoustics data or processing the DSP data. + """ # Variables that will be filled with latest acoustics data ---------- hydrophone1 = [0] * HYDROPHONE_DATA_SIZE @@ -148,11 +162,21 @@ def get_acoustics_data() -> list: 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 +194,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 +208,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 +247,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. @@ -241,6 +268,7 @@ def display_live_data() -> None: Note: This function assumes that `getAcousticsData`, `hydrophoneAxis`, `filterAxis`, `FFTAxis`, `colorSoftBlue`, `colorSoftGreen`, and `colorSoftPurple` are defined elsewhere in the code. + """ # Get latest acoustics data acoustics_data = get_acoustics_data() @@ -284,7 +312,9 @@ def display_live_data() -> None: 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.plot( + x_filter, filter_data, label="Filter", color=colorSoftGreen, alpha=0.7 + ) filterAxis.legend(loc="upper right", fontsize="xx-small") # Plot FFT data diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index adf1dcfe7..353b3a152 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -1,18 +1,17 @@ # 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 + """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 - Attributes: - ----------- + Attributes + ---------- _TEENSY_IP (string): self-explanatory _TEENSY_PORT (int): teensy's data port _MY_PORT (int): the device's data port @@ -24,8 +23,8 @@ class TeensyCommunicationUDP: _data_target (str): the field of `acoustics_data` that is written to acoustics_data (dict[str, list[int]]): container for data from teensy - Methods: - -------- + Methods + ------- init_communication(frequenciesOfInterest: list[tuple[int, int]]) -> None: Sets up socket for communication with teensy and waits for handshake fetch_data() -> None: @@ -75,13 +74,16 @@ 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: + 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 @@ -111,9 +113,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 +138,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 +154,17 @@ 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) 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,14 +172,16 @@ 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: + Parameters + ---------- is_float (bool): whether _data_string should be seen as a list of floats or ints - Returns: + Returns + ------- The converted list + """ if cls._data_string == "": return @@ -199,9 +199,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,9 +215,7 @@ 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) print("DEBUGGING: Sent acknowledge package") @@ -229,8 +225,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,16 +254,21 @@ 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: + 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: diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 4c2e4f60d..86d366522 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -2,54 +2,85 @@ 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: calls fetch_data() from acoustics_interface data_publisher(self) -> None: publishes data to ROS2 topics + """ 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 +101,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. @@ -80,23 +110,44 @@ def data_update(self) -> None: 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"])) + 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. @@ -104,6 +155,7 @@ def main(args: list = None) -> None: Args: args (list, optional): Command line arguments passed to the ROS 2 client library. Defaults to None. + """ rclpy.init(args=args) diff --git a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py index a283d3b22..87281d9a4 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 @@ -16,6 +16,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A launch description object that includes the acoustics_interface node with the specified parameters. + """ # Path to the YAML file yaml_file_path = os.path.join( diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 13a57acbe..5971f007b 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 @@ -18,9 +17,12 @@ def generate_launch_description() -> LaunchDescription: LaunchDescription: A launch description containing the environment variable setting and the included launch descriptions for the thruster allocator and 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 +47,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..b2c3570c6 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 @@ -18,9 +17,12 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: The launch description containing the environment 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..869b62bb2 100644 --- a/mission/LCD/sources/ip_lib.py +++ b/mission/LCD/sources/ip_lib.py @@ -9,11 +9,11 @@ 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. + """ try: # Run the command without shell=True diff --git a/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 83ae63dbc..5e0c536be 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. @@ -30,10 +29,13 @@ def format_line(value: str, unit: str) -> str: Returns: str: A formatted string that fits within a 16-character limit, with the unit appended. + """ 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) diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index f77484a5c..7b193a640 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 @@ -36,6 +35,7 @@ def write_to_screen(self, line1: str = "", line2: str = "") -> None: Defaults to an empty string. line2 (str): The text to display on the second line of the LCD screen. Defaults to an empty string. + """ self._lcd.clear() @@ -48,8 +48,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. @@ -64,6 +63,7 @@ def fancy_animation(self, animation_speed: float = 0.4) -> None: - Ghost The animation is displayed in two rows of the LCD screen. + """ # Calculate the appropriate animation speed animation_speed = 1 / animation_speed @@ -108,7 +108,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 +123,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 +140,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 +153,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..12cc72cae 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 @@ -44,18 +47,20 @@ def get_voltage(self) -> float: Returns: float: The system voltage if successfully read and converted, otherwise 0.0. + """ # 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, + """Retrieves the current system current by reading from the current channel, applying an offset, and scaling the result. Returns: @@ -63,9 +68,13 @@ def get_current(self) -> float: Raises: 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..39e42bd2e 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -30,11 +30,11 @@ 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. + """ try: pressure = self.channel_pressure.pressure diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index e911fac2c..5d12703aa 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -1,6 +1,5 @@ #!/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 @@ -16,14 +15,14 @@ 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. Raises: Exception: If there is an error reading the temperature sensor file. + """ try: # Read internal temperature on the computer diff --git a/mission/blackbox/blackbox/blackbox_log_data.py b/mission/blackbox/blackbox/blackbox_log_data.py index 0344897c3..34578a7b3 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. @@ -71,16 +74,23 @@ def manage_csv_files(self, max_file_age_in_days: int = 7, max_size_kb: int = 3_0 - The method first deletes files older than the specified number of days. - If the total size of remaining files exceeds the specified limit, it deletes the oldest files until the size is within the limit. - The expected filename format for the CSV files is "blackbox_data_YYYY-MM-DD_HH:MM:SS.csv". + """ # adjust the max size before you start deleting old files (1 000 000 kb = 1 000 mb = 1 gb) current_time = datetime.now() 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 +102,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 +115,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 +157,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,9 +186,10 @@ def log_data_to_csv_file( pwm_7: int = 0, pwm_8: int = 0, ) -> None: - """ - Logs the provided data to a CSV file. - Parameters: + """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. @@ -189,12 +212,15 @@ def log_data_to_csv_file( - pwm_8 (int): The PWM signal for thruster 8. 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. + """ # Get current time in hours, minutes, seconds and milliseconds 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..89a5d1f0c 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 @@ -76,12 +102,12 @@ def psm_current_callback(self, msg: Float32) -> None: Args: msg (std_msgs.msg.Float32): The message containing the current data. + """ 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 @@ -89,12 +115,12 @@ def psm_voltage_callback(self, msg: Float32) -> None: Args: msg (std_msgs.msg.Float32): The message containing the voltage data. + """ 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 @@ -102,12 +128,12 @@ def pressure_callback(self, msg: Float32) -> None: Args: msg (std_msgs.msg.Float32): The message containing the pressure data. + """ 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 @@ -115,12 +141,12 @@ def temperature_callback(self, msg: Float32) -> None: Args: msg (std_msgs.msg.Float32): The message containing the temperature data. + """ 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 @@ -128,24 +154,24 @@ def thruster_forces_callback(self, msg: ThrusterForces) -> None: Args: msg (vortex_msgs.msg.ThrusterForces): The message containing the thruster forces data. + """ 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. Args: msg (std_msgs.msg.Int16MultiArray): The message containing the PWM signals data. + """ 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 +212,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 @@ -195,6 +220,7 @@ def main(args: list = None) -> None: Args: args (list, optional): Command-line arguments passed to the ROS2 initialization. + """ # Initialize ROS2 rclpy.init(args=args) diff --git a/mission/blackbox/launch/blackbox.launch.py b/mission/blackbox/launch/blackbox.launch.py index 5016f49b8..eb8d03fad 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 @@ -15,6 +14,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A launch description object containing the blackbox node. + """ # Path to the YAML file yaml_file_path = os.path.join( 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..eb6d485fb 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,41 +38,46 @@ 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 voltage in volts. - Returns: - -------- + Returns + ------- float The system voltage in volts. If an error occurs during reading, returns 0.0. + """ # 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 current in amperes. - Returns: - -------- + Returns + ------- 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..e3bfdd696 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,17 +106,17 @@ 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 and publishing current and voltage data. Args: - ----- + ---- args : list, optional Arguments passed to the node. Default is None. + """ rclpy.init(args=args) 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..b59dc3274 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,17 +30,17 @@ 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. If an error occurs, an error message is printed and 0.0 is returned. - Returns: - -------- + Returns + ------- float The pressure reading in hPa (hectopascals), or 0.0 if an error occurs. + """ try: pressure = self.channel_pressure.pressure 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..cf5ce7abb 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,28 +73,29 @@ 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 running and publishing pressure data. Args: - ----- + ---- args : list, optional Arguments passed to the node. Default is None. + """ rclpy.init(args=args) 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..7254dcd12 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,6 +1,5 @@ #!/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 @@ -16,16 +15,16 @@ 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. - Returns: - -------- + Returns + ------- float The current temperature in Celsius. If an error occurs, it returns 0.0. + """ try: # Read internal temperature on the computer 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..b8f8f740d 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,28 +77,29 @@ 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 and publishing temperature data. Args: - ----- + ---- args : list, optional Arguments passed to the node. Default is None. + """ rclpy.init(args=args) 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..2b05126f7 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. @@ -17,10 +16,11 @@ def generate_launch_description() -> LaunchDescription: The nodes will be launched with their respective executables and display output on the screen. - Returns: - -------- + Returns + ------- launch.LaunchDescription A LaunchDescription object containing the nodes to be launched. + """ # Path to the YAML file yaml_file_path = os.path.join( 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..3279d17d2 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. @@ -139,6 +146,7 @@ def create_wrench_message( Returns: Wrench: A 2D wrench message with the given values. + """ wrench_msg = Wrench() wrench_msg.force.x = surge @@ -150,24 +158,19 @@ 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 + """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. @@ -177,8 +180,8 @@ 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 +222,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 +261,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 +292,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,8 +316,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: def main() -> None: - """ - Initializes the ROS 2 client library, creates an instance of the JoystickInterface node, + """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. 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..85fc0744b 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 @@ -16,6 +15,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A ROS 2 launch description containing the joystick_interface_auv node. + """ joystick_interface_node = Node( package="joystick_interface_auv", 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..480ecde32 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 @@ -54,6 +52,7 @@ def test_input_from_controller_into_wrench_msg(self) -> None: Note: This test requires the rclpy library and the Joy and JoystickInterface classes to be properly imported and available in the test environment. + """ rclpy.init() joy_msg = Joy() @@ -70,8 +69,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 +104,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/launch/thrust_allocator_auv.launch.py b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py index decab0783..d51184990 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 @@ -18,6 +17,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A ROS 2 LaunchDescription object containing the thrust_allocator_auv_node. + """ thrust_allocator_auv_node = Node( package="thrust_allocator_auv", diff --git a/motion/thruster_interface_auv/analysis.py b/motion/thruster_interface_auv/analysis.py index 20ad655d5..0df68ee09 100644 --- a/motion/thruster_interface_auv/analysis.py +++ b/motion/thruster_interface_auv/analysis.py @@ -1,9 +1,8 @@ -import pandas as pd import matplotlib.pyplot as plt import numpy as np -#-------------------------------- -#load data and transform to numpy +# -------------------------------- +# load data and transform to numpy files = [ "motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv", @@ -13,26 +12,29 @@ "motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv", "motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv", ] -datas = [] +data = [] for f in files: - data = np.genfromtxt(f, delimiter=',', skip_header=1, usecols=(5, 0)) - datas.append(data) + data = np.genfromtxt(f, delimiter=",", skip_header=1, usecols=(5, 0)) + data.append(data) -#-------------------------------- -#plot pwm values vs force -def plot_csvs(datas): - for i, data in enumerate(datas): - plt.plot(data[:,0], data[:,1], label=i*2+10) + +# -------------------------------- +# plot pwm values vs force +def plot_csvs(data): + for i, data in enumerate(data): + plt.plot(data[:, 0], data[:, 1], label=i * 2 + 10) plt.xlabel("force") plt.ylabel("pwm") plt.legend() plt.show() -#plot_csvs(datas) -#-------------------------------- -#calculate the two halfs deg-order poly approx and plot them on data +# plot_csvs(data) + +# -------------------------------- +# calculate the two halves deg-order poly approx and plot them on data + def plot_single_vs_poly(xdata, ydata, deg): zero_indices = np.where(xdata == 0.00)[0] @@ -57,16 +59,17 @@ def plot_single_vs_poly(xdata, ydata, deg): rms = np.sqrt(np.mean((yL - yL_fit) ** 2)) + np.sqrt(np.mean((yR - yR_fit) ** 2)) plt.scatter(xdata, ydata, s=2) - plt.plot(xL, yL_fit, color='red') - plt.plot(xR, yR_fit, color='red') + plt.plot(xL, yL_fit, color="red") + plt.plot(xR, yR_fit, color="red") plt.title(f"deg: {deg} rms {rms}") - + plt.xlabel("force") plt.ylabel("pwm") plt.show() return coeffs_L, coeffs_R + np.set_printoptions(precision=5, suppress=True) -for data in datas: - plot_single_vs_poly(data[:,0], data[:,1], 3) \ No newline at end of file +for data in data: + plot_single_vs_poly(data[:, 0], data[:, 1], 3) 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 e25eba1b6..7309a91a5 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 @@ -17,6 +16,7 @@ def generate_launch_description() -> LaunchDescription: Returns: LaunchDescription: A ROS 2 launch description containing the thruster_interface_auv_node. + """ thruster_interface_auv_node = Node( package="thruster_interface_auv", diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index 6e02d748d..22f7d5554 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -19,4 +19,4 @@ ament_cmake - \ No newline at end of file + 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 579a03e4a..8d7d49d3d 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 @@ -18,7 +18,6 @@ def __init__( PWM_MAX=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], coeffs=None, ) -> None: - # Initialice the I2C communication self.bus = None try: @@ -50,13 +49,14 @@ 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 + ) self.coeffs = coeffs def _interpolate_forces_to_pwm(self, thruster_forces_array) -> list: - """ - Takes in Array of forces in Newtosn [N] + """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] @@ -75,8 +75,8 @@ def _interpolate_forces_to_pwm(self, thruster_forces_array) -> list: thruster_forces_array[i] = thruster_forces / 9.80665 # Select the appropriate pair of coeffs based on the operational voltage - left_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]['LEFT'] - right_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]['RIGHT'] + left_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"] + right_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"] # Calculate the interpolated PWM values using the polynomial coefficients interpolated_pwm = [] @@ -113,8 +113,7 @@ def _send_data_to_escs(self, thruster_pwm_array) -> None: self.bus.write_i2c_block_data(self.PICO_I2C_ADDRESS, 0, i2c_data_array) def drive_thrusters(self, thruster_forces_array) -> list: - """ - Takes in Array of forces in Newtosn [N] + """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] @@ -128,16 +127,20 @@ def drive_thrusters(self, thruster_forces_array) -> 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 range(len(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 262ec948f..cff31d137 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 @@ -5,13 +5,14 @@ from rcl_interfaces.msg import ParameterDescriptor 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: @@ -21,13 +22,23 @@ 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_pwm_publisher = self.create_publisher(Int16MultiArray, "pwm_APPROX", 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_APPROX", 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], @@ -39,25 +50,61 @@ def __init__(self) -> None: self.declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0) - self.declare_parameter("coeffs.10V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.10V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.12V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.12V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.14V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.14V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.16V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.16V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.18V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.18V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.20V.LEFT", None, ParameterDescriptor(dynamic_typing=True)) - self.declare_parameter("coeffs.20V.RIGHT", None, ParameterDescriptor(dynamic_typing=True)) - - 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.declare_parameter( + "coeffs.10V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.10V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.12V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.12V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.14V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.14V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.16V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.16V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.18V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.18V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.20V.LEFT", None, ParameterDescriptor(dynamic_typing=True) + ) + self.declare_parameter( + "coeffs.20V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) + ) + + 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 + ) # dictionary for the coeffs of the poly approx of thruster forces based on OPERATIONAL_VOLTAGE coeffs = { @@ -91,7 +138,9 @@ def __init__(self) -> None: # 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, @@ -116,7 +165,9 @@ def _thruster_forces_callback(self, msg) -> 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 diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp index f8f013d54..b36851320 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp @@ -1,55 +1,52 @@ #ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP #define THRUSTER_INTERFACE_AUV_DRIVER_HPP -#include -#include -#include -#include +#include #include -#include -#include +#include #include -#include +#include +#include +#include +#include +#include class ThrusterInterfaceAUVDriver { public: - - //default contructor called when instantiating the object in .hpp file - ThrusterInterfaceAUVDriver(); - ~ThrusterInterfaceAUVDriver(); - - //contructor actually used - ThrusterInterfaceAUVDriver( - int I2C_BUS, - int PICO_I2C_ADDRESS, - double SYSTEM_OPERATIONAL_VOLTAGE, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, - const std::vector& THRUSTER_PWM_OFFSET, - const std::vector& PWM_MIN, - const std::vector& PWM_MAX, - const std::map>>& COEFFS - ); - - std::vector drive_thrusters(const std::vector& thruster_forces_array); + // default constructor called when instantiating the object in .hpp file + ThrusterInterfaceAUVDriver(); + ~ThrusterInterfaceAUVDriver(); + + // constructor actually used + ThrusterInterfaceAUVDriver( + int I2C_BUS, + int PICO_I2C_ADDRESS, + double SYSTEM_OPERATIONAL_VOLTAGE, + const std::vector &THRUSTER_MAPPING, + const std::vector &THRUSTER_DIRECTION, + const std::vector &THRUSTER_PWM_OFFSET, + const std::vector &PWM_MIN, + const std::vector &PWM_MAX, + const std::map>> &COEFFS); + + std::vector drive_thrusters(const std::vector &thruster_forces_array); private: + int bus_fd; + int I2C_BUS; + int PICO_I2C_ADDRESS; + int SYSTEM_OPERATIONAL_VOLTAGE; - int bus_fd; - int I2C_BUS; - int PICO_I2C_ADDRESS; - int SYSTEM_OPERATIONAL_VOLTAGE; - - std::vector THRUSTER_MAPPING; - std::vector THRUSTER_DIRECTION; - std::vector THRUSTER_PWM_OFFSET; - std::vector PWM_MIN; - std::vector PWM_MAX; + std::vector THRUSTER_MAPPING; + std::vector THRUSTER_DIRECTION; + std::vector THRUSTER_PWM_OFFSET; + std::vector PWM_MIN; + std::vector PWM_MAX; - std::map>> COEFFS; + std::map>> COEFFS; - std::vector interpolate_forces_to_pwm(const std::vector& thruster_forces_array); - void send_data_to_escs(const std::vector& thruster_pwm_array); + std::vector interpolate_forces_to_pwm(const std::vector &thruster_forces_array); + void send_data_to_escs(const std::vector &thruster_pwm_array); }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp index aff10b60a..ec39d766c 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp @@ -3,25 +3,25 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/int16_multi_array.hpp" -#include "vortex_msgs/msg/thruster_forces.hpp" #include "thruster_interface_auv_driver.hpp" +#include "vortex_msgs/msg/thruster_forces.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { public: - ThrusterInterfaceAUVNode(); + ThrusterInterfaceAUVNode(); private: - void thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); - void timer_callback(); + void thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); + void timer_callback(); - rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; - rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; + rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; + rclcpp::TimerBase::SharedPtr timer_; - ThrusterInterfaceAUVDriver thruster_driver_; + ThrusterInterfaceAUVDriver thruster_driver_; - std::vector thruster_forces_array_; - double thrust_timer_period_; + std::vector thruster_forces_array_; + double thrust_timer_period_; }; #endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py index d22ce4ee4..fc86148ba 100644 --- a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py +++ b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py @@ -1,17 +1,21 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from os import path + def generate_launch_description(): # Path to the YAML file - config = path.join(get_package_share_directory("auv_setup"),'config', 'robots', "orca.yaml") + config = path.join( + get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" + ) thruster_interface_auv_node = Node( - package='thruster_interface_auv_cpp', - executable='thruster_interface_auv_cpp_node', - name='thruster_interface_auv_cpp_node', - output='screen', + package="thruster_interface_auv_cpp", + executable="thruster_interface_auv_cpp_node", + name="thruster_interface_auv_cpp_node", + output="screen", parameters=[ path.join( get_package_share_directory("auv_setup"), @@ -27,4 +31,4 @@ def generate_launch_description(): ], ) - return LaunchDescription([thruster_interface_auv_node]) \ No newline at end of file + return LaunchDescription([thruster_interface_auv_node]) diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp index 7331cb5d3..8d068ca68 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp @@ -1,138 +1,136 @@ #include "thruster_interface_auv_driver.hpp" -ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver(){} +ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( int I2C_BUS, int PICO_I2C_ADDRESS, double SYSTEM_OPERATIONAL_VOLTAGE, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, - const std::vector& THRUSTER_PWM_OFFSET, - const std::vector& PWM_MIN, - const std::vector& PWM_MAX, - const std::map>>& COEFFS -) : I2C_BUS(I2C_BUS), - PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - //SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), done after in the if-else - THRUSTER_MAPPING(THRUSTER_MAPPING), - THRUSTER_DIRECTION(THRUSTER_DIRECTION), - THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), - PWM_MIN(PWM_MIN), - PWM_MAX(PWM_MAX), - COEFFS(COEFFS) -{ - - // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to assign the integer field variable [one is double, field var is int] - if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 10; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 12; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 15.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 14; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 17.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 16; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 19.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 18; - } else { - this->SYSTEM_OPERATIONAL_VOLTAGE = 20; - } - - // Open the I2C bus - std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); - bus_fd = open(i2c_filename.c_str(), O_RDWR); - if (bus_fd < 0) { - std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; - } + const std::vector &THRUSTER_MAPPING, + const std::vector &THRUSTER_DIRECTION, + const std::vector &THRUSTER_PWM_OFFSET, + const std::vector &PWM_MIN, + const std::vector &PWM_MAX, + const std::map>> &COEFFS) : I2C_BUS(I2C_BUS), + PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + // SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), done after in the if-else + THRUSTER_MAPPING(THRUSTER_MAPPING), + THRUSTER_DIRECTION(THRUSTER_DIRECTION), + THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), + PWM_MIN(PWM_MIN), + PWM_MAX(PWM_MAX), + COEFFS(COEFFS) { + + // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to assign the integer field variable [one is double, field var is int] + if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { + this->SYSTEM_OPERATIONAL_VOLTAGE = 10; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { + this->SYSTEM_OPERATIONAL_VOLTAGE = 12; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 15.0) { + this->SYSTEM_OPERATIONAL_VOLTAGE = 14; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 17.0) { + this->SYSTEM_OPERATIONAL_VOLTAGE = 16; + } else if (SYSTEM_OPERATIONAL_VOLTAGE < 19.0) { + this->SYSTEM_OPERATIONAL_VOLTAGE = 18; + } else { + this->SYSTEM_OPERATIONAL_VOLTAGE = 20; + } + + // Open the I2C bus + std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); + bus_fd = open(i2c_filename.c_str(), O_RDWR); + if (bus_fd < 0) { + std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; + } } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - if (bus_fd >= 0) { - close(bus_fd); - } + if (bus_fd >= 0) { + close(bus_fd); + } } -std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const std::vector& thruster_forces_array) { - // Convert Newtons to Kg (since the thruster datasheet is in Kg) - std::vector forces_in_kg(thruster_forces_array.size()); - for (size_t i = 0; i < thruster_forces_array.size(); ++i) { - forces_in_kg[i] = thruster_forces_array[i] / 9.80665; - } - - // Select the appropriate coefficients based on the operational voltage - auto left_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; - auto right_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; - - std::vector interpolated_pwm; - for (size_t i = 0; i < forces_in_kg.size(); ++i) { - double force = forces_in_kg[i]; - double pwm = 0.0; - if (force < 0) { - pwm = left_coeffs[0] * std::pow(forces_in_kg[i], 3) + \ - left_coeffs[1] * std::pow(forces_in_kg[i], 2) + \ - left_coeffs[2] * forces_in_kg[i] + \ - left_coeffs[3]; - } else if (force == 0.0) { - pwm = 1500; - } else { - pwm = right_coeffs[0] * std::pow(forces_in_kg[i], 3) + \ - right_coeffs[1] * std::pow(forces_in_kg[i], 2) + \ - right_coeffs[2] * forces_in_kg[i] + \ - right_coeffs[3]; - } - interpolated_pwm.push_back(static_cast(pwm)); +std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const std::vector &thruster_forces_array) { + // Convert Newtons to Kg (since the thruster datasheet is in Kg) + std::vector forces_in_kg(thruster_forces_array.size()); + for (size_t i = 0; i < thruster_forces_array.size(); ++i) { + forces_in_kg[i] = thruster_forces_array[i] / 9.80665; + } + + // Select the appropriate coefficients based on the operational voltage + auto left_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; + auto right_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; + + std::vector interpolated_pwm; + for (size_t i = 0; i < forces_in_kg.size(); ++i) { + double force = forces_in_kg[i]; + double pwm = 0.0; + if (force < 0) { + pwm = left_coeffs[0] * std::pow(forces_in_kg[i], 3) + + left_coeffs[1] * std::pow(forces_in_kg[i], 2) + + left_coeffs[2] * forces_in_kg[i] + + left_coeffs[3]; + } else if (force == 0.0) { + pwm = 1500; + } else { + pwm = right_coeffs[0] * std::pow(forces_in_kg[i], 3) + + right_coeffs[1] * std::pow(forces_in_kg[i], 2) + + right_coeffs[2] * forces_in_kg[i] + + right_coeffs[3]; } + interpolated_pwm.push_back(static_cast(pwm)); + } - return interpolated_pwm; + return interpolated_pwm; } -void ThrusterInterfaceAUVDriver::send_data_to_escs(const std::vector& thruster_pwm_array) { - uint8_t i2c_data_array[16]; - for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - i2c_data_array[2*i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB - i2c_data_array[2*i + 1] = thruster_pwm_array[i] & 0xFF; // LSB - } - - // Set the I2C slave address - if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { - std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; - return; - } - - // Write data to the I2C device - if (write(bus_fd, i2c_data_array, 16) != 16) { - std::cerr << "ERROR: Failed to write to I2C device" << std::endl; - } +void ThrusterInterfaceAUVDriver::send_data_to_escs(const std::vector &thruster_pwm_array) { + uint8_t i2c_data_array[16]; + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + i2c_data_array[2 * i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB + i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB + } + + // Set the I2C slave address + if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { + std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; + return; + } + + // Write data to the I2C device + if (write(bus_fd, i2c_data_array, 16) != 16) { + std::cerr << "ERROR: Failed to write to I2C device" << std::endl; + } } -std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector& thruster_forces_array) { - // Apply thruster mapping and direction - std::vector mapped_forces(thruster_forces_array.size()); - for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { - mapped_forces[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; +std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector &thruster_forces_array) { + // Apply thruster mapping and direction + std::vector mapped_forces(thruster_forces_array.size()); + for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { + mapped_forces[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; + } + + // Convert forces to PWM + std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); + + // Apply thruster offset and limit PWM if needed + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + // thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; + + // Clamp the PWM signal + if (thruster_pwm_array[i] < PWM_MIN[i]) { + thruster_pwm_array[i] = PWM_MIN[i]; + } else if (thruster_pwm_array[i] > PWM_MAX[i]) { + thruster_pwm_array[i] = PWM_MAX[i]; } + } - // Convert forces to PWM - std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); - - // Apply thruster offset and limit PWM if needed - for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - //thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; - - // Clamp the PWM signal - if (thruster_pwm_array[i] < PWM_MIN[i]) { - thruster_pwm_array[i] = PWM_MIN[i]; - } else if (thruster_pwm_array[i] > PWM_MAX[i]) { - thruster_pwm_array[i] = PWM_MAX[i]; - } - } - - // Send data through I2C - try { - send_data_to_escs(thruster_pwm_array); - } catch (...) { - std::cerr << "ERROR: Failed to send PWM values" << std::endl; - } + // Send data through I2C + try { + send_data_to_escs(thruster_pwm_array); + } catch (...) { + std::cerr << "ERROR: Failed to send PWM values" << std::endl; + } - return thruster_pwm_array; + return thruster_pwm_array; } diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp index 91808f769..99448e0bb 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp @@ -1,9 +1,9 @@ #include "thruster_interface_auv_ros.hpp" -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_cpp_node"); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_cpp_node"); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp index f2bc72108..fc737e5d0 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp @@ -2,76 +2,73 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { - //create a subscriber that takes data from thruster forces and publisher for debugging - this->thruster_forces_subscriber_ = this->create_subscription( - "thrust/thruster_forces", 10, - std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1) - ); - this->thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX_cpp", 10); + // create a subscriber that takes data from thruster forces and publisher for debugging + this->thruster_forces_subscriber_ = this->create_subscription( + "thrust/thruster_forces", 10, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); + this->thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX_cpp", 10); - //declare orca.yaml parameters - this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping"); - this->declare_parameter>("propulsion.thrusters.thruster_direction"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); - this->declare_parameter("propulsion.thrusters.thrust_update_rate"); + // declare orca.yaml parameters + this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping"); + this->declare_parameter>("propulsion.thrusters.thruster_direction"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); + this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); + this->declare_parameter("propulsion.thrusters.thrust_update_rate"); - //get orca.yaml parameters - auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); - auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); - auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); - auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); - auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); - this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); + // get orca.yaml parameters + auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); + auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); + auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); + auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); + auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); + this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); - //coeffs.yaml parameters - std::map>> coeffs; - std::vector voltage_levels = {10, 12, 14, 16, 18, 20}; - for (int voltage : voltage_levels) { - std::string left_param = "coeffs." + std::to_string(voltage) + "V.LEFT"; - std::string right_param = "coeffs." + std::to_string(voltage) + "V.RIGHT"; - this->declare_parameter>(left_param); //declare coeffs.10V.LEFT - this->declare_parameter>(right_param); + // coeffs.yaml parameters + std::map>> coeffs; + std::vector voltage_levels = {10, 12, 14, 16, 18, 20}; + for (int voltage : voltage_levels) { + std::string left_param = "coeffs." + std::to_string(voltage) + "V.LEFT"; + std::string right_param = "coeffs." + std::to_string(voltage) + "V.RIGHT"; + this->declare_parameter>(left_param); // declare coeffs.10V.LEFT + this->declare_parameter>(right_param); - auto left_coeffs = this->get_parameter(left_param).as_double_array(); - auto right_coeffs = this->get_parameter(right_param).as_double_array(); + auto left_coeffs = this->get_parameter(left_param).as_double_array(); + auto right_coeffs = this->get_parameter(right_param).as_double_array(); - coeffs[voltage]["LEFT"] = left_coeffs; //save to coeffs[10]["LEFT"] - coeffs[voltage]["RIGHT"] = right_coeffs; - } + coeffs[voltage]["LEFT"] = left_coeffs; // save to coeffs[10]["LEFT"] + coeffs[voltage]["RIGHT"] = right_coeffs; + } - // Initialize thruster driver - this->thruster_driver_ = ThrusterInterfaceAUVDriver( - 1, 0x21, 16.0, - std::vector(thruster_mapping.begin(), thruster_mapping.end()), - std::vector(thruster_direction.begin(), thruster_direction.end()), - std::vector(thruster_PWM_offset.begin(), thruster_PWM_offset.end()), - std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), - std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), - coeffs - ); + // Initialize thruster driver + this->thruster_driver_ = ThrusterInterfaceAUVDriver( + 1, 0x21, 16.0, + std::vector(thruster_mapping.begin(), thruster_mapping.end()), + std::vector(thruster_direction.begin(), thruster_direction.end()), + std::vector(thruster_PWM_offset.begin(), thruster_PWM_offset.end()), + std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), + std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), + coeffs); - //Declare "thruster_forces_array" in case no topic comes in at the beginning - this->thruster_forces_array_ = std::vector(8, 0.00); + // Declare "thruster_forces_array" in case no topic comes in at the beginning + this->thruster_forces_array_ = std::vector(8, 0.00); - timer_ = this->create_wall_timer( - std::chrono::duration(this->thrust_timer_period_), - std::bind(&ThrusterInterfaceAUVNode::timer_callback, this) - ); + timer_ = this->create_wall_timer( + std::chrono::duration(this->thrust_timer_period_), + std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_cpp_node\" has been started"); + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_cpp_node\" has been started"); } void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - this->thruster_forces_array_ = msg->thrust; + this->thruster_forces_array_ = msg->thrust; } void ThrusterInterfaceAUVNode::timer_callback() { - std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(this->thruster_forces_array_); + std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(this->thruster_forces_array_); - //publish PWM values for debugging - std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array; - thruster_pwm_publisher_->publish(pwm_message); + // publish PWM values for debugging + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array; + thruster_pwm_publisher_->publish(pwm_message); } From c2ce10f3fae3def3b08f354fd53756e1f3a6925c Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:09:36 +0200 Subject: [PATCH 13/54] refactor(docs): applied formatting changes to docstrings --- .../acoustics_data_record_lib.py | 19 ---------- .../acoustics_data_record_node.py | 36 ++++++++++++------- .../launch/acoustics_data_record.launch.py | 3 +- .../utilities/display_acoustics_data_live.py | 24 +++---------- .../acoustics_interface_lib.py | 34 ++++-------------- .../acoustics_interface_node.py | 9 +++-- .../launch/acoustics_interface_launch.py | 3 +- auv_setup/launch/orca.launch.py | 3 +- auv_setup/launch/topside.launch.py | 3 +- mission/LCD/sources/ip_lib.py | 3 +- mission/LCD/sources/lcd.py | 3 +- mission/LCD/sources/lcd_lib.py | 6 ++-- mission/LCD/sources/power_sense_module_lib.py | 6 ++-- mission/LCD/sources/pressure_sensor_lib.py | 3 +- mission/LCD/sources/temperature_sensor_lib.py | 6 ++-- .../blackbox/blackbox/blackbox_log_data.py | 31 ++-------------- mission/blackbox/blackbox/blackbox_node.py | 24 ++++++++----- mission/blackbox/launch/blackbox.launch.py | 3 +- .../power_sense_module_lib.py | 6 ++-- .../power_sense_module_node.py | 9 +++-- .../pressure_sensor_lib.py | 3 +- .../pressure_sensor_node.py | 9 +++-- .../temperature_sensor_lib.py | 6 ++-- .../temperature_sensor_node.py | 9 +++-- .../launch/internal_status_auv.launch.py | 3 +- .../joystick_interface_auv.py | 9 +++-- .../launch/joystick_interface_auv.launch.py | 3 +- .../tests/test_joystick_interface_auv.py | 12 ++++--- .../launch/thrust_allocator_auv.launch.py | 3 +- .../launch/thruster_interface_auv.launch.py | 3 +- .../thruster_interface_auv_driver_lib.py | 22 ------------ 31 files changed, 136 insertions(+), 180 deletions(-) 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 f81a03556..4b98b177f 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 @@ -49,25 +49,6 @@ 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. - - Parameters - ---------- - self (object): The instance of the class. - hydrophone1 (list, optional): Data from hydrophone 1. Defaults to [0]. - hydrophone2 (list, optional): Data from hydrophone 2. Defaults to [0]. - hydrophone3 (list, optional): Data from hydrophone 3. Defaults to [0]. - hydrophone4 (list, optional): Data from hydrophone 4. Defaults to [0]. - hydrophone5 (list, optional): Data from hydrophone 5. Defaults to [0]. - filter_response (list, optional): Filter response data. Defaults to [0]. - fft (list, optional): FFT data. Defaults to [0]. - peaks (list, optional): Peaks data. Defaults to [0]. - tdoa (list, optional): Time Difference of Arrival data. Defaults to [0.0]. - position (list, optional): Position data. Defaults to [0.0]. - - Writes the current time and provided data to a CSV file located at self.data_file_location. - - """ # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] 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 d8e0fc4bd..c90cbdd56 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 @@ -133,7 +133,8 @@ 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. @@ -142,7 +143,8 @@ 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. @@ -151,7 +153,8 @@ 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. @@ -160,7 +163,8 @@ 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. @@ -169,7 +173,8 @@ 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. @@ -178,7 +183,8 @@ 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. @@ -187,7 +193,8 @@ 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. @@ -196,7 +203,8 @@ 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. @@ -205,7 +213,8 @@ 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. @@ -214,7 +223,8 @@ 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. @@ -224,7 +234,8 @@ 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. """ @@ -243,7 +254,8 @@ 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 index 17d32772b..cf3b42814 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py @@ -7,7 +7,8 @@ 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 5a1d66ebb..683a5def5 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -77,15 +77,6 @@ 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. - - Args: - pandasObject (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_int_array = [int(x.strip()) for x in pandas_string.strip("[]").split(",")] @@ -93,15 +84,6 @@ def convert_pandas_object_to_int_array(pandas_object: pd.Series) -> list: 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. - - Args: - pandasObject (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(",") @@ -111,7 +93,8 @@ def convert_pandas_object_to_float_array(pandas_object: pd.Series) -> list: 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 @@ -247,7 +230,8 @@ 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. diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 353b3a152..a547c1243 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -5,7 +5,8 @@ class TeensyCommunicationUDP: - """This class is responsible for the RPI side of teensy-RPI UDP communication. It is + """ + 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 @@ -74,13 +75,6 @@ class TeensyCommunicationUDP: @classmethod def init_communication(cls, frequencies_of_interest: list[tuple[int, int]]) -> None: - """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" @@ -154,7 +148,8 @@ 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 @@ -172,17 +167,6 @@ 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 - - Parameters - ---------- - is_float (bool): whether _data_string should be seen as a list of floats or ints - - Returns - ------- - The converted list - - """ if cls._data_string == "": return @@ -225,7 +209,8 @@ 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 @@ -257,13 +242,6 @@ def _check_if_available(cls) -> None: 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 ( diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 86d366522..81cf96f1a 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -9,7 +9,8 @@ class AcousticsInterfaceNode(Node): - """Publishes Acoustics data to ROS2 + """ + Publishes Acoustics data to ROS2 Methods: data_update() -> None: @@ -101,7 +102,8 @@ 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. @@ -147,7 +149,8 @@ def data_publisher(self) -> None: 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 87281d9a4..9cc6d1b23 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -7,7 +7,8 @@ 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 5971f007b..1060c6e6f 100755 --- a/auv_setup/launch/orca.launch.py +++ b/auv_setup/launch/orca.launch.py @@ -7,7 +7,8 @@ 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 diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index b2c3570c6..27a856d25 100755 --- a/auv_setup/launch/topside.launch.py +++ b/auv_setup/launch/topside.launch.py @@ -8,7 +8,8 @@ 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 diff --git a/mission/LCD/sources/ip_lib.py b/mission/LCD/sources/ip_lib.py index 869b62bb2..1f3170999 100644 --- a/mission/LCD/sources/ip_lib.py +++ b/mission/LCD/sources/ip_lib.py @@ -9,7 +9,8 @@ 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 5e0c536be..4ca551e3c 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -21,7 +21,8 @@ # 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. diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index 7b193a640..3d0ba656e 100644 --- a/mission/LCD/sources/lcd_lib.py +++ b/mission/LCD/sources/lcd_lib.py @@ -24,7 +24,8 @@ 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,7 +49,8 @@ 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. diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index 12cc72cae..9b6f7055c 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -38,7 +38,8 @@ 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 @@ -60,7 +61,8 @@ def get_voltage(self) -> float: return 0.0 def get_current(self) -> float: - """Retrieves the current system current by reading from the current channel, + """ + Retrieves the current system current by reading from the current channel, applying an offset, and scaling the result. Returns: diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index 39e42bd2e..55076c74d 100755 --- a/mission/LCD/sources/pressure_sensor_lib.py +++ b/mission/LCD/sources/pressure_sensor_lib.py @@ -30,7 +30,8 @@ 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 5d12703aa..207ffa78f 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -1,5 +1,6 @@ #!/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 @@ -15,7 +16,8 @@ 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 34578a7b3..760607523 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -58,7 +58,8 @@ def __init__(self, ros2_package_directory: str = "") -> None: 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. + """ + 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. @@ -186,34 +187,6 @@ 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. - 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. - - """ # Get current time in hours, minutes, seconds and milliseconds current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3] diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index 89a5d1f0c..c92d7455f 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -94,7 +94,8 @@ 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 @@ -107,7 +108,8 @@ 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 @@ -120,7 +122,8 @@ 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 @@ -133,7 +136,8 @@ 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 @@ -146,7 +150,8 @@ 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 @@ -159,7 +164,8 @@ 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. @@ -171,7 +177,8 @@ 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 @@ -212,7 +219,8 @@ 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 eb8d03fad..e8e2c0e30 100644 --- a/mission/blackbox/launch/blackbox.launch.py +++ b/mission/blackbox/launch/blackbox.launch.py @@ -6,7 +6,8 @@ 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/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 eb6d485fb..39eab8c8b 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 @@ -38,7 +38,8 @@ 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 @@ -61,7 +62,8 @@ def get_voltage(self) -> float: 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 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 e3bfdd696..fde88240f 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 @@ -70,7 +70,8 @@ def __init__(self) -> None: 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. @@ -94,7 +95,8 @@ def read_timer_callback(self) -> None: ) # 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. @@ -106,7 +108,8 @@ 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 b59dc3274..e453ea000 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,7 +30,8 @@ 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 cf5ce7abb..ee17e7c66 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 @@ -59,7 +59,8 @@ def __init__(self) -> None: 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. @@ -73,7 +74,8 @@ 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. @@ -85,7 +87,8 @@ 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 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 7254dcd12..c05dfbfa0 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,5 +1,6 @@ #!/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 @@ -15,7 +16,8 @@ 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 b8f8f740d..8bbb3ebe6 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 @@ -63,7 +63,8 @@ def __init__(self) -> None: 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. @@ -77,7 +78,8 @@ 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. @@ -89,7 +91,8 @@ 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 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 2b05126f7..4f413729d 100644 --- a/mission/internal_status_auv/launch/internal_status_auv.launch.py +++ b/mission/internal_status_auv/launch/internal_status_auv.launch.py @@ -6,7 +6,8 @@ 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 3279d17d2..ca7b66730 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 @@ -134,7 +134,8 @@ 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. @@ -170,7 +171,8 @@ def transition_to_autonomous_mode(self) -> None: self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: - """Callback function that receives joy messages and converts them into + """ + 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. @@ -316,7 +318,8 @@ def joystick_cb(self, msg: Joy) -> Wrench: def main() -> None: - """Initializes the ROS 2 client library, creates an instance of the JoystickInterface node, + """ + 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. 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 85fc0744b..9faf25a99 100755 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -6,7 +6,8 @@ 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 480ecde32..c435c3544 100644 --- a/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/tests/test_joystick_interface_auv.py @@ -6,7 +6,8 @@ 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 @@ -33,7 +34,8 @@ 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 @@ -69,7 +71,8 @@ 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 @@ -104,7 +107,8 @@ 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/launch/thrust_allocator_auv.launch.py b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py index d51184990..978ba8c39 100644 --- a/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py +++ b/motion/thrust_allocator_auv/launch/thrust_allocator_auv.launch.py @@ -6,7 +6,8 @@ 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/thruster_interface_auv/launch/thruster_interface_auv.launch.py b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py index 7309a91a5..a332e8c3c 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -6,7 +6,8 @@ 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 8d7d49d3d..115d3a627 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 @@ -56,14 +56,6 @@ def __init__( self.coeffs = coeffs def _interpolate_forces_to_pwm(self, thruster_forces_array) -> 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] - - Returns an Array of PWM - Gives out 8 ints in form of: - [0, 0, 0, 0, 0, 0, 0, 0] - """ # Read the important data from the .csv file # thrusterDatasheetFileData = pandas.read_csv( # f"{self.ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET}/resources/T200-Thrusters-{self.SYSTEM_OPERATIONAL_VOLTAGE}V.csv", @@ -113,20 +105,6 @@ def _send_data_to_escs(self, thruster_pwm_array) -> None: self.bus.write_i2c_block_data(self.PICO_I2C_ADDRESS, 0, i2c_data_array) def drive_thrusters(self, thruster_forces_array) -> 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] - - Converts Forces to PWM signals - PWM signals sent to PCA9685 module through I2C - PCA9685 Module sends electrical PWM signals to the individual thruster ESCs - The ESCs send corecponding electrical power to the Thrustres - Thrusters then generate thrust accordingly to the Forces sent to this driver - - Returns an Array of PWM signal for debugging purposes - 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] From 52bca93e85d5a85a15aed925645c12942c27e9dc Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:25:29 +0200 Subject: [PATCH 14/54] refactor(docs): applied formatting changes to docstrings --- .../acoustics_interface/acoustics_interface_lib.py | 12 ++++++------ .../acoustics_interface/acoustics_interface_node.py | 6 +++--- mission/LCD/sources/temperature_sensor_lib.py | 2 +- .../internal_status_auv/temperature_sensor_lib.py | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index a547c1243..2936cd013 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -107,7 +107,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: @@ -132,7 +132,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: @@ -149,7 +149,7 @@ 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 @@ -183,7 +183,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: @@ -199,7 +199,7 @@ 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) print("DEBUGGING: Sent acknowledge package") @@ -210,7 +210,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 diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 81cf96f1a..19ef76731 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -10,7 +10,7 @@ class AcousticsInterfaceNode(Node): """ - Publishes Acoustics data to ROS2 + Publishes Acoustics data to ROS2. Methods: data_update() -> None: @@ -21,7 +21,7 @@ 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() @@ -111,7 +111,7 @@ def data_update(self) -> None: TeensyCommunicationUDP.fetch_data() def data_publisher(self) -> None: - """Publishes to topics""" + """Publishes to topics.""" self._hydrophone_1_publisher.publish( Int32MultiArray(data=TeensyCommunicationUDP.acoustics_data["HYDROPHONE_1"]) ) diff --git a/mission/LCD/sources/temperature_sensor_lib.py b/mission/LCD/sources/temperature_sensor_lib.py index 207ffa78f..5b764c422 100755 --- a/mission/LCD/sources/temperature_sensor_lib.py +++ b/mission/LCD/sources/temperature_sensor_lib.py @@ -3,7 +3,7 @@ ! 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 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 c05dfbfa0..4b2eb82a1 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 @@ -3,7 +3,7 @@ ! 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 From de3433f900fcf11257ef300367e416358f46419b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:37:19 +0200 Subject: [PATCH 15/54] refactor(formatting): applied formatting and style changes --- ...nch.py => acoustics_data_record_launch.py} | 0 .../utilities/display_acoustics_data_live.py | 52 +++++++++---------- .../acoustics_interface_lib.py | 20 +++---- mission/LCD/sources/lcd.py | 6 +-- motion/thruster_interface_auv/analysis.py | 8 +-- .../thruster_interface_auv_driver_lib.py | 46 ++++++++-------- .../thruster_interface_auv_node.py | 12 ++--- .../thruster_interface_auv_cpp.launch.py | 6 +-- 8 files changed, 76 insertions(+), 74 deletions(-) rename acoustics/acoustics_data_record/launch/{acoustics_data_record.launch.py => acoustics_data_record_launch.py} (100%) 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 100% rename from acoustics/acoustics_data_record/launch/acoustics_data_record.launch.py rename to acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py 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 683a5def5..7d949ff89 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -40,27 +40,27 @@ 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 + hydrophone_axis[i] = fig.add_subplot( + gs_hydrophone[i, 0], sharex=hydrophone_axis[0] if i else None ) - hydrophoneAxis[i].label_outer() + 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 @@ -68,27 +68,25 @@ ACOUSTICS_CSV_FILE_DIR = PACKAGE_DIR + "/acoustics_data" # List of all the acoustic files -acousticsCSVFiles = csv_files = glob.glob( +acoustics_csv_file = 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_file, key=os.path.getctime) def convert_pandas_object_to_int_array(pandas_object: pd.Series) -> list: - pandas_string = pandas_object.iloc[0].strip("array('i', ").rstrip(")") + pandas_string = pandas_object.iloc[0].removeprefix("array('i', ").removesuffix(")") 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: - pandas_string = pandas_object.iloc[0].strip("array('f', ").rstrip(")") + pandas_string = pandas_object.iloc[0].removeprefix("array('f', ").removesuffix(")") pandas_float_array = [ float(x.strip()) for x in pandas_string.strip("[]").split(",") ] - return pandas_float_array @@ -140,7 +138,7 @@ 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: @@ -280,26 +278,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 + 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 ) - filterAxis.legend(loc="upper right", fontsize="xx-small") + filter_axis.legend(loc="upper right", fontsize="xx-small") # Plot FFT data FFTAxis.clear() @@ -310,7 +310,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 2936cd013..040670975 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -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 = { @@ -84,9 +84,9 @@ def init_communication(cls, frequencies_of_interest: list[tuple[int, int]]) -> N 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() @@ -96,7 +96,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() @@ -156,7 +156,7 @@ def _get_raw_data(cls) -> str | None: """ 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 OSError as e: # `error` is really `socket.error` @@ -201,7 +201,9 @@ def _get_ip(cls) -> None: def _send_acknowledge_signal(cls) -> None: """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") @@ -253,6 +255,6 @@ def _send_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/mission/LCD/sources/lcd.py b/mission/LCD/sources/lcd.py index 4ca551e3c..d84625320 100644 --- a/mission/LCD/sources/lcd.py +++ b/mission/LCD/sources/lcd.py @@ -56,7 +56,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) @@ -65,7 +65,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) @@ -74,7 +74,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/motion/thruster_interface_auv/analysis.py b/motion/thruster_interface_auv/analysis.py index 0df68ee09..2b2ea5a41 100644 --- a/motion/thruster_interface_auv/analysis.py +++ b/motion/thruster_interface_auv/analysis.py @@ -21,8 +21,8 @@ # -------------------------------- # plot pwm values vs force def plot_csvs(data): - for i, data in enumerate(data): - plt.plot(data[:, 0], data[:, 1], label=i * 2 + 10) + for i, single_data in enumerate(data): + plt.plot(single_data[:, 0], single_data[:, 1], label=i * 2 + 10) plt.xlabel("force") plt.ylabel("pwm") @@ -71,5 +71,5 @@ def plot_single_vs_poly(xdata, ydata, deg): np.set_printoptions(precision=5, suppress=True) -for data in data: - plot_single_vs_poly(data[:, 0], data[:, 1], 3) +for single_data in data: + plot_single_vs_poly(single_data[:, 0], single_data[:, 1], 3) 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 115d3a627..d7efd1dae 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 @@ -7,50 +7,50 @@ class ThrusterInterfaceAUVDriver: def __init__( self, - I2C_BUS=1, - PICO_I2C_ADDRESS=0x21, - SYSTEM_OPERATIONAL_VOLTAGE=16.0, - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET="", - THRUSTER_MAPPING=[7, 6, 5, 4, 3, 2, 1, 0], - THRUSTER_DIRECTION=[1, 1, 1, 1, 1, 1, 1, 1], - THRUSTER_PWM_OFFSET=[0, 0, 0, 0, 0, 0, 0, 0], - PWM_MIN=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - PWM_MAX=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], + i2c_bus=1, + pico_i2c_address=0x21, + system_operational_voltage=16.0, + ros2_package_name_for_thruster_datasheet="", + thruster_mapping=[7, 6, 5, 4, 3, 2, 1, 0], + thruster_direction=[1, 1, 1, 1, 1, 1, 1, 1], + thruster_pwm_offset=[0, 0, 0, 0, 0, 0, 0, 0], + pwm_min=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], + pwm_max=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], coeffs=None, ) -> None: # Initialice the I2C communication self.bus = None try: - self.bus = smbus2.SMBus(I2C_BUS) + self.bus = smbus2.SMBus(i2c_bus) except Exception as errorCode: print(f"ERROR: Failed connection I2C bus nr {self.bus}: {errorCode}") - self.PICO_I2C_ADDRESS = PICO_I2C_ADDRESS + self.PICO_I2C_ADDRESS = pico_i2c_address # Set mapping, direction and offset for the thrusters - self.THRUSTER_MAPPING = THRUSTER_MAPPING - self.THRUSTER_DIRECTION = THRUSTER_DIRECTION - self.THRUSTER_PWM_OFFSET = THRUSTER_PWM_OFFSET - self.PWM_MIN = PWM_MIN - self.PWM_MAX = PWM_MAX + self.THRUSTER_MAPPING = thruster_mapping + self.THRUSTER_DIRECTION = thruster_direction + self.THRUSTER_PWM_OFFSET = thruster_pwm_offset + self.PWM_MIN = pwm_min + self.PWM_MAX = pwm_max # Convert SYSTEM_OPERATIONAL_VOLTAGE to a whole even number to work with # This is because we have multiple files for the behaviour of the thrusters depending on the voltage of the drone - if SYSTEM_OPERATIONAL_VOLTAGE < 11.0: + if system_operational_voltage < 11.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 10 - elif SYSTEM_OPERATIONAL_VOLTAGE < 13.0: + elif system_operational_voltage < 13.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 12 - elif SYSTEM_OPERATIONAL_VOLTAGE < 15.0: + elif system_operational_voltage < 15.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 14 - elif SYSTEM_OPERATIONAL_VOLTAGE < 17.0: + elif system_operational_voltage < 17.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 16 - elif SYSTEM_OPERATIONAL_VOLTAGE < 19.0: + elif system_operational_voltage < 19.0: self.SYSTEM_OPERATIONAL_VOLTAGE = 18 - elif SYSTEM_OPERATIONAL_VOLTAGE >= 19.0: + elif system_operational_voltage >= 19.0: 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 + ros2_package_name_for_thruster_datasheet ) self.coeffs = coeffs 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 cff31d137..608af7743 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 @@ -138,14 +138,14 @@ def __init__(self) -> None: # Initialize thruster driver self.thruster_driver = ThrusterInterfaceAUVDriver( - ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET=get_package_share_directory( + 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, - PWM_MIN=self.thruster_PWM_min, - PWM_MAX=self.thruster_PWM_max, + thruster_mapping=self.thruster_mapping, + thruster_direction=self.thruster_direction, + thruster_pwm_offset=self.thruster_PWM_offset, + pwm_min=self.thruster_PWM_min, + pwm_max=self.thruster_PWM_max, coeffs=coeffs, ) diff --git a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py index fc86148ba..de07f9f45 100644 --- a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py +++ b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py @@ -7,9 +7,9 @@ def generate_launch_description(): # Path to the YAML file - config = path.join( - get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" - ) + # config = path.join( + # get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" + # ) thruster_interface_auv_node = Node( package="thruster_interface_auv_cpp", From 05b6eacfad8b439b8654d3c3b8fdc7d25c482d34 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:37:43 +0200 Subject: [PATCH 16/54] refactor(pre-commit): replaced all python hooks with ruff --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a5a292cb2..62ca28bd3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -54,8 +54,8 @@ repos: name: ruff-pydocstyle args: [ "--select=D", - "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D213,D205,D401,D415,D400,D417,D404", - "--fix" + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D205,D212,D401", + "--fix", ] stages: [pre-commit] pass_filenames: true @@ -63,7 +63,7 @@ repos: name: ruff-check args: [ "--select=F,PT,B,C4,T20,S,N", - "--ignore=T201,N806,F841,B006,N803,B020,S101,S607,S603,B007,N815,B005,N816,N999", + "--ignore=T201,B006,S101,S607,S603", "--fix" ] stages: [pre-commit] From ec43ebdee26a5b2cd619be7b03f314133247f10d Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:39:04 +0200 Subject: [PATCH 17/54] refactor(workflow): updated source build to not run on push --- .github/workflows/source-build.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/source-build.yaml b/.github/workflows/source-build.yaml index 5fdc08b71..802f53637 100644 --- a/.github/workflows/source-build.yaml +++ b/.github/workflows/source-build.yaml @@ -1,7 +1,6 @@ name: Source Build # Build the ROS 2 workspace from source code and run tests on: - push: workflow_dispatch: pull_request: # Runs daily to check for dependency issues or flaking tests From bbd67f3db13f90fd7bf9ef0a9c98cd0dc930a512 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Fri, 25 Oct 2024 14:57:12 +0200 Subject: [PATCH 18/54] refactor(submodules): removed submodules --- .gitmodules | 3 --- perception-auv | 1 - 2 files changed, 4 deletions(-) delete mode 100644 .gitmodules delete mode 160000 perception-auv diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 8d1d96bd4..000000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "perception-auv"] - path = perception-auv - url = git@github.com:vortexntnu/perception-auv.git diff --git a/perception-auv b/perception-auv deleted file mode 160000 index 6f91a3113..000000000 --- a/perception-auv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 6f91a3113182bcc3b64d7b6e78eef4e9ea53ca7a From 4fdeccb413f79709cc5e55e81235e9dceb06eb24 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 27 Oct 2024 17:13:22 +0100 Subject: [PATCH 19/54] started the odissey in fixing things --- motion/thruster_interface_auv/.Doxyfile.swp | Bin 0 -> 20480 bytes motion/thruster_interface_auv/CMakeLists.txt | 34 +- motion/thruster_interface_auv/analysis.py | 75 ---- .../thruster_interface_auv/config/coeffs.yaml | 21 - .../config/thruster_interface_auv_config.yaml | 25 ++ .../thruster_interface_auv_driver.hpp | 101 +++++ .../thruster_interface_auv_ros.hpp | 8 +- .../launch/thruster_interface_auv.launch.py | 24 +- motion/thruster_interface_auv/package.xml | 20 +- .../thruster_interface_auv/resources/README | 3 + .../resources/T200-Thrusters-10V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-12V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-14V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-16V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-18V.csv | 404 +++++++++--------- .../resources/T200-Thrusters-20V.csv | 404 +++++++++--------- .../resources/__init__.py | 0 .../src/thruster_interface_auv_driver.cpp | 98 +++-- .../src/thruster_interface_auv_node.cpp | 4 +- .../src/thruster_interface_auv_ros.cpp | 44 +- .../thruster_interface_auv/__init__.py | 0 .../thruster_interface_auv_driver_lib.py | 137 ------ .../thruster_interface_auv_node.py | 191 --------- .../thruster_interface_auv_cpp/CMakeLists.txt | 37 -- .../config/coeffs.yaml | 21 - .../thruster_interface_auv_driver.hpp | 52 --- .../thruster_interface_auv_cpp.launch.py | 34 -- motion/thruster_interface_auv_cpp/package.xml | 28 -- 28 files changed, 1465 insertions(+), 1916 deletions(-) create mode 100644 motion/thruster_interface_auv/.Doxyfile.swp delete mode 100644 motion/thruster_interface_auv/analysis.py delete mode 100644 motion/thruster_interface_auv/config/coeffs.yaml create mode 100644 motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml create mode 100644 motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp rename motion/{thruster_interface_auv_cpp/include/thruster_interface_auv_cpp => thruster_interface_auv/include/thruster_interface_auv}/thruster_interface_auv_ros.hpp (78%) delete mode 100644 motion/thruster_interface_auv/resources/__init__.py rename motion/{thruster_interface_auv_cpp => thruster_interface_auv}/src/thruster_interface_auv_driver.cpp (54%) rename motion/{thruster_interface_auv_cpp => thruster_interface_auv}/src/thruster_interface_auv_node.cpp (72%) rename motion/{thruster_interface_auv_cpp => thruster_interface_auv}/src/thruster_interface_auv_ros.cpp (66%) delete mode 100644 motion/thruster_interface_auv/thruster_interface_auv/__init__.py delete mode 100644 motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py delete mode 100755 motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py delete mode 100644 motion/thruster_interface_auv_cpp/CMakeLists.txt delete mode 100644 motion/thruster_interface_auv_cpp/config/coeffs.yaml delete mode 100644 motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp delete mode 100644 motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py delete mode 100644 motion/thruster_interface_auv_cpp/package.xml diff --git a/motion/thruster_interface_auv/.Doxyfile.swp b/motion/thruster_interface_auv/.Doxyfile.swp new file mode 100644 index 0000000000000000000000000000000000000000..26a44b005ba7d95ae8f19f5c4ad1e09c3ee014b9 GIT binary patch literal 20480 zcmeI44~!hudBE2g$05YH6j6i_<=GC!hwt|8FfO*~HCfy1^M$+H^WC0}3)XgScXoH` z-JSK!?Co7BbtyuKID{gED7A=739=}qlp;hCMaV%2QG^hploCoUN+=k|Ar26SIK=&Z zZ{F`rY2`H}mGb@BP{D-+7{E%fOIYoxCK$*KZ^e|FPw_OIuDEOl&MC z5>{ox_5xQVzw(Rgex*DT{)uU@d(!m+d#c-N1a7YBcl%!6ywznjCcCR{;JCH!pzJmL z!1i)ZjqgP(Z|6k1!JV2eIu$!=G=7aW5NqJGHP8r(U8@(3ue_wUM=oBrbcs6aH^)Ew zzW9n*1F;5T4a6FVH4tkc)`3vUu@5e?S&Cf`i{5>PT(zL&~ zP5vVzA29NxZSuoL9`)~QUzq#+9~$|fx&Pa3@B~o`I*~DcBEB!V~Z~JO+=#Bk(Xh1P{Uk za6jA!``})<2kwTwa2M=>JK+wv9d3hL;T918a5vlpH^L2YJzNL7;99r_rlAQ9@L?yo zP=y`fKnZNfg9SO*4p+f8*b3t?2Ad%RLvRHQKtH5mJ*42P&v) z&;b|0g|G~kzy)wVoCoK^IdC?d1!uxySOllTX|NC$z4Q_>7;AYqjH^Gf?16&W+!7jKK zu7PQ2LIZr*2`*G&2RKjy8}eX54z|Npuno4tIE=w&$be@Bc0m=~e4}dD0!u2sRMV+c z)P${?o)ZLiEvW{I%2J+PK|E=zx)qd_<0qD>Y}rq*~SaZ9c= zC=I!dpxy{n!SU>T;CjsHAQw~bR{ zn-}e-a@k{^s#mD>^!3&XTTw-}ZL{p;%X$;eJv7+ztu~XK4E$cRs?w40D4=m#r5^>i zF;TI*b~fC=_B&OhR^3R|aI8wPTs9ec55Zd8CG$a2BxF#`o4ok6uol`^joW3U#un?4l_#Ot;^*T`Ht4jfoIkH z@baV@vqi5|@&=sR%#p39r|5X{fP9`|MgtFIS~#GSTEonj%#AN?htCsLg|j53y`k!h zZl&TjWlC)ojh1ur*=R;WULRP39G;QW9+%{_Y)l1dYOFLxQWmElXMW5OBSLxl~sglX$c4Qk- zzpOL%nG@JmU*#?KxJ(r-TlKoFSHIqKYfV$1(vr+RlUb#!94%a%!bZ?b&MGobvcbYF zSGN4JTE479Ha>&$*|HuQwfLnfCWiiijw}qPRAado7zQmCJ#-*DLe_%RESEiNevy3}ZZwN$w~kMJqUl{xu>HK} zOqd1i+d&|kR!kF=s?DB>(A=4OK>E{E(N|^O<#f|6b@lcp*Ccx-Y$5)?_&?8q`2QEn zn|wWupZ_h`4RvtgEATJ)`Tqca4+Z!wSOntt|06sKd*CnO>u?Nz|8HRz394UYarG@tbtequ?Aud#2WbZ z)WC*JHn)B-of;k=nF(Vl)u*|QV%bQkWDr%o_7>{{v z=vo`AQ|#lKi_u)GOwTx61>Vs&;Xh(;i@7g;Sk=#q@g3UQ(q`y+gkCo`F$Qep!D|zI z(()X)fm`oc^|I~&aX_)nXAjUs`{<9{&mcN+@I#|o8~L7Hb%`0uZTjy=tC;eH90@k3 zhts2}Y&tiX=En_@re_qoU$SS!h%uq|&qYL7kwTxyp%J zz&ong{8AtkT0%FX;Gyxs>_9G?8tqSK$D&Zga3)+pCsj4W8Bv>r20Hb3LO@o~Ad0|K z3XX3Pn~(@eq*S@MYVANRq6Du|Q$nOPv)S&|J7!@qXQ!ty^5~B<-ZC63%k!#38y|Jq2Ca$Q9>8t#9!uWpXM` z&F%=JPBb?ri;>wlQH?Sm;WCvM0uydicWU<1Z#^{(GKaH~9Ze07jig4?!`YdY`oB0E zo^2JhcRyhjP$g%A(`8j0OM>bZYeE9uMg?|3!gnaw3OA|>b;tOp@kddRjdM&CjK3dW zrnXw1tn#&LgDt+lEPm4rn;{VYUY{bJgk_laaBr{*!T<=fKFvC{!nSA)t(iFt*9{7b z+;{ektdj{VzoVpsY$hc5$xDi*W9KbgXFVD8Xsc5z$ZjjRj7<(W-^O*GJF~N=WbVAS zwv|)WvCXDK!opHIW>)98s|Dn}Qdi3Hk?S8#jcm$oNRMPU$;9?4{wi0_kpJNh8{$N% zHmYGXX07V!L}^pA-|6k?;mtYc6PG2m|365r^d@pL;{QkW{lCQ5{{{RUeg;2<58!*S zA0#HQ6ut-x;YY*(4ujMLjKBgo6%G*tco!rlkbwmJJAVIb@IBZMJ}iTO#qYltz6-nI zo6rY8#OMDj$iS)aZ}|NG1dqdB_zp;3S#q}zk-ysqyWyKq1IbnX4jf>Nk`L2k{q?jF z;!(#Mh&2#vAlAT1)&RMjD4(W{e~fnzrzutRoE;W9vrd)rEV5nlN;<-^YMpAGXMMxL zxZ=n4R$$?Q@k1Gt(ziP?2Ao=$_>j_w*@hzKqt0*ySZ7g1g2`5qH>hL#(o(5`2_mM@ z++;vxqJ)OEahR|Ee*#3q|naS`=E z(r97kmR?icGdP&pnj0S;Ak&%7N$OO)7=3Cu^XVRH=qHS?SlM&wr81&fNxCSXLuSOP z+l5Z;Z`xB?+#|Wem!x{D@P!Pw z7c6#n|kU@C(yY#YTlCz>+H3I-SOf440Ut%8e4~z$(dbZNU3(!=tlW z!JTrCC)1t8O--kO_h#xp#1*us@+@F= zNl%X{`I;$a2q)C7PitLi-OkZWSi}%{WM$ham0KRGE-yRrF{@V5)>d&KWu~HH6J4Vw zt4jhFvc;k^mq3n`sj<6xQ)()>O{dJ6yL_cVt;tNG+q%(#^u};)$#=cJ2zBLmNSTX8 zeVq8y^3;06qew(ouIOJ)Qd(VM+x2jeIwP&C6hu>=uJ@56rj@}CyAhtLd?beL25!Bz zsU+~EgDge`C8_kAbaV*ELIvZxE@!q5!tT#j0;k&~U6-MUr;k<93XYiDFm+_TNX5$Hf2u literal 0 HcmV?d00001 diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index 691a29f00..c8a45fc0d 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -1,26 +1,36 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(thruster_interface_auv) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_python REQUIRED) -find_package(rclpy REQUIRED) +# Required dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) find_package(vortex_msgs REQUIRED) -ament_python_install_package(thruster_interface_auv) +# Include directories +include_directories(include) + +add_executable(${PROJECT_NAME}_node + src/thruster_interface_auv_node.cpp + src/thruster_interface_auv_ros.cpp + src/thruster_interface_auv_driver.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + vortex_msgs +) +# Install launch files. install(DIRECTORY launch - resources config DESTINATION share/${PROJECT_NAME} ) -install(PROGRAMS - thruster_interface_auv/thruster_interface_auv_node.py - thruster_interface_auv/thruster_interface_auv_driver_lib.py +install(TARGETS + ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/motion/thruster_interface_auv/analysis.py b/motion/thruster_interface_auv/analysis.py deleted file mode 100644 index 2b2ea5a41..000000000 --- a/motion/thruster_interface_auv/analysis.py +++ /dev/null @@ -1,75 +0,0 @@ -import matplotlib.pyplot as plt -import numpy as np - -# -------------------------------- -# load data and transform to numpy - -files = [ - "motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv", - "motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv", - "motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv", - "motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv", - "motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv", - "motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv", -] -data = [] -for f in files: - data = np.genfromtxt(f, delimiter=",", skip_header=1, usecols=(5, 0)) - data.append(data) - - -# -------------------------------- -# plot pwm values vs force -def plot_csvs(data): - for i, single_data in enumerate(data): - plt.plot(single_data[:, 0], single_data[:, 1], label=i * 2 + 10) - - plt.xlabel("force") - plt.ylabel("pwm") - plt.legend() - plt.show() - - -# plot_csvs(data) - -# -------------------------------- -# calculate the two halves deg-order poly approx and plot them on data - - -def plot_single_vs_poly(xdata, ydata, deg): - zero_indices = np.where(xdata == 0.00)[0] - begin = zero_indices[0] - end = zero_indices[-1] - - xL = xdata[:begin] - xR = xdata[end:] - yL = ydata[:begin] - yR = ydata[end:] - - coeffs_L = np.polyfit(xL, yL, deg) - print("LEFT:", coeffs_L) - coeffs_R = np.polyfit(xR, yR, deg) - print("RIGHT:", coeffs_R) - polyL = np.poly1d(coeffs_L) - polyR = np.poly1d(coeffs_R) - - yL_fit = polyL(xL) - yR_fit = polyR(xR) - - rms = np.sqrt(np.mean((yL - yL_fit) ** 2)) + np.sqrt(np.mean((yR - yR_fit) ** 2)) - - plt.scatter(xdata, ydata, s=2) - plt.plot(xL, yL_fit, color="red") - plt.plot(xR, yR_fit, color="red") - plt.title(f"deg: {deg} rms {rms}") - - plt.xlabel("force") - plt.ylabel("pwm") - plt.show() - - return coeffs_L, coeffs_R - - -np.set_printoptions(precision=5, suppress=True) -for single_data in data: - plot_single_vs_poly(single_data[:, 0], single_data[:, 1], 3) diff --git a/motion/thruster_interface_auv/config/coeffs.yaml b/motion/thruster_interface_auv/config/coeffs.yaml deleted file mode 100644 index c8d8aeed4..000000000 --- a/motion/thruster_interface_auv/config/coeffs.yaml +++ /dev/null @@ -1,21 +0,0 @@ -/**: - ros__parameters: - coeffs: - 10V: - LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] - RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] - 12V: - LEFT: [7.35916, 47.58976, 199.18339, 1460.57483] - RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] - 14V: - LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] - RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] - 16V: - LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] - RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] - 18V: - LEFT: [2.50017, 25.18231, 142.66031, 1469.6513] - RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] - 20V: - LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] - RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml new file mode 100644 index 000000000..29f7961a1 --- /dev/null +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -0,0 +1,25 @@ +thruster_interface_auv_node: + ros__parameters: + coeffs: + # 10V: + # LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + # RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] + # 12V: + # LEFT: [7.35916, 47.58976, 199.18339, 1460.57483] + # RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] + # 14V: + # LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] + # RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] #14V csv table is wrong (is the same of 10V) and so also these coefficents + 16V: + LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] + RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] + # 18V: + # LEFT: [2.50017, 25.18231, 142.66031, 1469.6513] + # RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] + # 20V: + # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] + # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] + + i2c: + bus: 1 + address: 0x21 \ No newline at end of file diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp new file mode 100644 index 000000000..a3642b02c --- /dev/null +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -0,0 +1,101 @@ +#ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP +#define THRUSTER_INTERFACE_AUV_DRIVER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief class instantiated by ThrusterInterfaceAUVNode to control the thrusters, takes the thruster forces and + * converts them to PWM signals to be sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo Driver) + * + * @details Based on the datasheets found in /resources, approximate the map with a piecewise (>0 and <0) third order polynomial. + * + * @note The coefficients are found in the config.yaml for all the possible SYSTEM.OPERATIONAL_VOLTAGE values, but we use only 16V for now, + * so: removed all the handling of the other voltages to save resources. Could be re-implemented in the future for more flexibility if we ever need it to + * operate at different voltages in different situations. + */ +class ThrusterInterfaceAUVDriver { + +public: + + /** + * @brief empty default constructor called when declaring the object in the .hpp file. Doesn't initialize any params. + */ + ThrusterInterfaceAUVDriver(); + ~ThrusterInterfaceAUVDriver(); + + /** + * @brief actually used constructor. Called from ThrusterInterfaceAUVNode .cpp when instantiating the object, initializes all the params. + * + * @param I2C_BUS bus number used to communicate + * @param PICO_I2C_ADDRESS i2c address of the ESC that drive the thrusters + * @param THRUSTER_MAPPING pin to motor mapping for the thrusters i.e. if thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. + * @param THRUSTER_DIRECTION physical mounting direction of the thrusters, (+-1) + * @param PWM_MIN minimum clamping pwm values + * @param PWM_MAX maximum clamping pwm values + * @param LEFT_COEFFS third order polynomial coefficients when force < 0 + * @param RIGHT_COEFFS third order polynomial coefficients when force > 0 + */ + ThrusterInterfaceAUVDriver( + int I2C_BUS, + int PICO_I2C_ADDRESS, + const std::vector &THRUSTER_MAPPING, + const std::vector &THRUSTER_DIRECTION, + const std::vector &PWM_MIN, + const std::vector &PWM_MAX, + const std::vector &LEFT_COEFFS, + const std::vector &RIGHT_COEFFS); + + + + /** + * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to convert the thruster forces to PWM values + * 2) send_data_to_escs() to send them to the ESCs via I2C + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster + */ + std::vector drive_thrusters(const std::vector &thruster_forces_array); + + + +private: + + int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely identifies the device. -1 if it fails) + int I2C_BUS; + int PICO_I2C_ADDRESS; + + std::vector THRUSTER_MAPPING; + std::vector THRUSTER_DIRECTION; + std::vector PWM_MIN; + std::vector PWM_MAX; + std::vector LEFT_COEFFS; + std::vector RIGHT_COEFFS; + + /** + * @brief [private] method that just take the thruster forces and return PWM values + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster if we want to publish them for debug purposes + */ + std::vector interpolate_forces_to_pwm(const std::vector &thruster_forces_array); + + /** + * @brief [private] method that takes the pwm values computed and sends them to the ESCs via I2C + * + * @param thruster_pwm_array vector of pwm values to send + */ + void send_data_to_escs(const std::vector &thruster_pwm_array); +}; + +#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp similarity index 78% rename from motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp rename to motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index ec39d766c..f78befa02 100644 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,10 +1,10 @@ #ifndef THRUSTER_INTERFACE_AUV_NODE_HPP #define THRUSTER_INTERFACE_AUV_NODE_HPP -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/int16_multi_array.hpp" -#include "thruster_interface_auv_driver.hpp" -#include "vortex_msgs/msg/thruster_forces.hpp" +#include +#include +#include +#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { public: 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 a332e8c3c..dc8220d10 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -5,26 +5,17 @@ from launch_ros.actions import Node -def generate_launch_description() -> LaunchDescription: - """ - Generates a launch description for the thruster_interface_auv_node. +def generate_launch_description(): + # Path to the YAML file + # config = path.join( + # get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" + # ) - This function creates a ROS 2 launch description that includes the - thruster_interface_auv_node. The node is configured to output to the screen - and emulate a TTY. It also loads parameters from the orca.yaml configuration - file located in the auv_setup package. - - Returns: - LaunchDescription: A ROS 2 launch description containing the - thruster_interface_auv_node. - - """ thruster_interface_auv_node = Node( package="thruster_interface_auv", - executable="thruster_interface_auv_node.py", + executable="thruster_interface_auv_node", name="thruster_interface_auv_node", output="screen", - emulate_tty=True, parameters=[ path.join( get_package_share_directory("auv_setup"), @@ -35,8 +26,9 @@ def generate_launch_description() -> LaunchDescription: path.join( get_package_share_directory("thruster_interface_auv"), "config", - "coeffs.yaml", + "thruster_interface_auv_config.yaml", ), ], ) + return LaunchDescription([thruster_interface_auv_node]) diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index 22f7d5554..fcd8a4c16 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -2,17 +2,21 @@ thruster_interface_auv - 0.0.0 - Thruster interface to control thrusters mapping force to pwm values - albertomorselli + 1.0.0 + Thruster interface to control thrusters through PCA9685 Module + vortex MIT - ament_cmake_python + ament_cmake + rclcpp + std_msgs + vortex_msgs + ament_index_cpp - rclcpp - geometry_msgs - vortex_msgs - std_msgs + rclcpp + std_msgs + vortex_msgs + ament_index_cpp ros2launch diff --git a/motion/thruster_interface_auv/resources/README b/motion/thruster_interface_auv/resources/README index acf71934f..0219b2e45 100644 --- a/motion/thruster_interface_auv/resources/README +++ b/motion/thruster_interface_auv/resources/README @@ -1 +1,4 @@ The different .CSV files reflect the different behaviours of the motors according to the voltage in the system + +TODO: 10V and 14V are the same table. Should investigate. +Not critical because we use only 16V for now diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv index de7294093..6689d4df1 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,2662,13.62,10,136.2,-2.31,17.0 -1104,2667,13.63,10,136.3,-2.30,16.9 -1108,2661,13.45,10,134.5,-2.30,17.1 -1112,2627,13.2,10,132,-2.28,17.3 -1116,2617,12.81,10,128.1,-2.22,17.4 -1120,2600,12.5,10,125,-2.18,17.5 -1124,2590,12.14,10,121.4,-2.16,17.8 -1128,2562,11.96,10,119.6,-2.13,17.8 -1132,2535,11.63,10,116.3,-2.11,18.1 -1136,2528,11.32,10,113.2,-2.05,18.2 -1140,2514,11,10,110,-2.04,18.6 -1144,2484,10.8,10,108,-1.99,18.4 -1148,2472,10.45,10,104.5,-1.95,18.7 -1152,2449,10.2,10,102,-1.93,18.9 -1156,2436,9.9,10,99,-1.89,19.1 -1160,2416,9.56,10,95.6,-1.84,19.2 -1164,2395,9.29,10,92.9,-1.81,19.5 -1168,2371,9.1,10,91,-1.79,19.7 -1172,2344,8.8,10,88,-1.76,19.9 -1176,2332,8.5,10,85,-1.71,20.2 -1180,2306,8.3,10,83,-1.68,20.3 -1184,2283,8.05,10,80.5,-1.65,20.5 -1188,2271,7.71,10,77.1,-1.62,21.0 -1192,2241,7.57,10,75.7,-1.59,21.0 -1196,2219,7.3,10,73,-1.56,21.3 -1200,2198,7.1,10,71,-1.54,21.7 -1204,2177,6.82,10,68.2,-1.50,22.0 -1208,2157,6.6,10,66,-1.47,22.3 -1212,2129,6.48,10,64.8,-1.44,22.3 -1216,2106,6.2,10,62,-1.41,22.8 -1220,2094,6,10,60,-1.36,22.7 -1224,2073,5.8,10,58,-1.33,22.9 -1228,2046,5.6,10,56,-1.32,23.6 -1232,2024,5.4,10,54,-1.29,23.9 -1236,2005,5.2,10,52,-1.26,24.2 -1240,1982,5,10,50,-1.22,24.4 -1244,1957,4.84,10,48.4,-1.19,24.6 -1248,1936,4.64,10,46.4,-1.17,25.1 -1252,1907,4.5,10,45,-1.14,25.3 -1256,1885,4.3,10,43,-1.10,25.5 -1260,1864,4.1,10,41,-1.08,26.2 -1264,1834,3.9,10,39,-1.06,27.1 -1268,1816,3.73,10,37.3,-1.02,27.2 -1272,1789,3.6,10,36,-0.99,27.5 -1276,1762,3.5,10,35,-0.97,27.7 -1280,1745,3.3,10,33,-0.93,28.2 -1284,1717,3.2,10,32,-0.91,28.3 -1288,1697,3,10,30,-0.88,29.3 -1292,1671,2.9,10,29,-0.85,29.4 -1296,1643,2.8,10,28,-0.83,29.5 -1300,1615,2.6,10,26,-0.80,30.7 -1304,1591,2.4,10,24,-0.78,32.5 -1308,1562,2.3,10,23,-0.75,32.7 -1312,1539,2.1,10,21,-0.72,34.3 -1316,1516,2,10,20,-0.70,34.9 -1320,1488,1.9,10,19,-0.67,35.3 -1324,1461,1.8,10,18,-0.65,36.3 -1328,1440,1.7,10,17,-0.63,37.1 -1332,1411,1.6,10,16,-0.61,38.3 -1336,1382,1.5,10,15,-0.58,38.7 -1340,1347,1.4,10,14,-0.55,39.5 -1344,1320,1.3,10,13,-0.53,40.8 -1348,1294,1.2,10,12,-0.51,42.7 -1352,1265,1.2,10,12,-0.49,40.4 -1356,1239,1.1,10,11,-0.47,42.5 -1360,1200,1,10,10,-0.44,44.5 -1364,1174,0.9,10,9,-0.42,46.9 -1368,1142,0.8,10,8,-0.40,49.9 -1372,1114,0.8,10,8,-0.38,47.6 -1376,1080,0.7,10,7,-0.35,50.5 -1380,1048,0.7,10,7,-0.34,48.0 -1384,1016,0.6,10,6,-0.31,52.2 -1388,986,0.5,10,5,-0.29,59.0 -1392,948,0.5,10,5,-0.28,55.3 -1396,916,0.4,10,4,-0.25,62.4 -1400,883,0.4,10,4,-0.24,59.0 -1404,845,0.4,10,4,-0.21,53.3 -1408,813,0.3,10,3,-0.20,66.5 -1412,774,0.22,10,2.2,-0.18,82.5 -1416,740,0.2,10,2,-0.16,81.6 -1420,701,0.2,10,2,-0.15,77.1 -1424,664,0.2,10,2,-0.14,68.0 -1428,624,0.2,10,2,-0.12,59.0 -1432,584,0.1,10,1,-0.10,99.8 -1436,545,0.1,10,1,-0.09,86.2 -1440,504,0.1,10,1,-0.07,72.6 -1444,462,0.05,10,0.5,-0.06,127.0 -1448,421,0.05,10,0.5,-0.05,108.9 -1452,377,0.05,10,0.5,-0.05,90.7 -1456,335,0.05,10,0.5,-0.03,63.5 -1460,0,0,10,0,0.00,0.0 -1464,0,0,10,0,0.00,0.0 -1468,0,0,10,0,0.00,0.0 -1472,0,0,10,0,0.00,0.0 -1476,0,0,10,0,0.00,0.0 -1480,0,0,10,0,0.00,0.0 -1484,0,0,10,0,0.00,0.0 -1488,0,0,10,0,0.00,0.0 -1492,0,0,10,0,0.00,0.0 -1496,0,0,10,0,0.00,0.0 -1500,0,0,10,0,0.00,0.0 -1504,0,0,10,0,0.00,0.0 -1508,0,0,10,0,0.00,0.0 -1512,0,0,10,0,0.00,0.0 -1516,0,0,10,0,0.00,0.0 -1520,0,0,10,0,0.00,0.0 -1524,0,0,10,0,0.00,0.0 -1528,0,0,10,0,0.00,0.0 -1532,0,0,10,0,0.00,0.0 -1536,0,0,10,0,0.00,0.0 -1540,0,0,10,0,0.00,0.0 -1544,332,0.05,10,0.5,0.04,81.6 -1548,373,0.05,10,0.5,0.05,108.9 -1552,417,0.05,10,0.5,0.07,136.1 -1556,459,0.1,10,1,0.08,77.1 -1560,500,0.1,10,1,0.09,90.7 -1564,540,0.1,10,1,0.11,108.9 -1568,582,0.1,10,1,0.13,127.0 -1572,621,0.2,10,2,0.15,72.6 -1576,657,0.2,10,2,0.16,81.6 -1580,695,0.2,10,2,0.18,90.7 -1584,733,0.2,10,2,0.20,102.1 -1588,766,0.3,10,3,0.22,74.1 -1592,805,0.3,10,3,0.24,81.6 -1596,836,0.4,10,4,0.26,65.8 -1600,873,0.4,10,4,0.29,72.6 -1604,906,0.47,10,4.7,0.31,66.6 -1608,938,0.5,10,5,0.34,67.1 -1612,974,0.5,10,5,0.36,72.6 -1616,1007,0.6,10,6,0.39,64.3 -1620,1035,0.7,10,7,0.41,59.0 -1624,1066,0.7,10,7,0.44,63.5 -1628,1101,0.8,10,8,0.47,58.4 -1632,1129,0.83,10,8.3,0.49,59.6 -1636,1159,0.94,10,9.4,0.52,55.5 -1640,1188,1,10,10,0.55,54.9 -1644,1219,1.1,10,11,0.58,52.8 -1648,1250,1.2,10,12,0.61,50.7 -1652,1278,1.3,10,13,0.64,49.2 -1656,1303,1.4,10,14,0.68,48.9 -1660,1330,1.5,10,15,0.70,46.9 -1664,1363,1.5,10,15,0.73,48.4 -1668,1392,1.6,10,16,0.76,47.6 -1672,1418,1.8,10,18,0.80,44.4 -1676,1445,1.9,10,19,0.83,43.4 -1680,1473,2,10,20,0.85,42.6 -1684,1496,2.1,10,21,0.89,42.3 -1688,1519,2.2,10,22,0.92,41.9 -1692,1542,2.3,10,23,0.95,41.2 -1696,1571,2.43,10,24.3,0.98,40.1 -1700,1596,2.7,10,27,0.97,36.0 -1704,1622,2.8,10,28,1.05,37.4 -1708,1650,2.9,10,29,1.08,37.1 -1712,1674,3.1,10,31,1.11,35.8 -1716,1696,3.3,10,33,1.17,35.3 -1720,1720,3.4,10,34,1.18,34.8 -1724,1747,3.5,10,35,1.21,34.5 -1728,1765,3.7,10,37,1.24,33.5 -1732,1793,3.84,10,38.4,1.27,33.2 -1736,1819,4,10,40,1.31,32.8 -1740,1833,4.2,10,42,1.36,32.3 -1744,1860,4.4,10,44,1.41,32.1 -1748,1889,4.5,10,45,1.43,31.8 -1752,1911,4.7,10,47,1.48,31.5 -1756,1933,4.9,10,49,1.52,31.1 -1760,1962,5.1,10,51,1.55,30.3 -1764,1984,5.3,10,53,1.58,29.8 -1768,2004,5.5,10,55,1.62,29.5 -1772,2027,5.7,10,57,1.66,29.0 -1776,2052,5.9,10,59,1.69,28.7 -1780,2075,6.09,10,60.9,1.73,28.4 -1784,2093,6.3,10,63,1.77,28.2 -1788,2118,6.5,10,65,1.80,27.7 -1792,2140,6.73,10,67.3,1.85,27.4 -1796,2160,6.9,10,69,1.88,27.2 -1800,2179,7.13,10,71.3,1.91,26.8 -1804,2200,7.4,10,74,1.99,26.9 -1808,2225,7.6,10,76,2.00,26.3 -1812,2245,7.9,10,79,2.06,26.1 -1816,2273,8.1,10,81,2.09,25.8 -1820,2293,8.33,10,83.3,2.12,25.5 -1824,2307,8.64,10,86.4,2.17,25.1 -1828,2333,8.89,10,88.9,2.21,24.9 -1832,2353,9.14,10,91.4,2.27,24.9 -1836,2378,9.4,10,94,2.30,24.5 -1840,2398,9.7,10,97,2.36,24.4 -1844,2410,10,10,100,2.41,24.1 -1848,2436,10.2,10,102,2.45,24.1 -1852,2453,10.5,10,105,2.48,23.6 -1856,2481,10.79,10,107.9,2.55,23.7 -1860,2499,11.04,10,110.4,2.58,23.4 -1864,2522,11.35,10,113.5,2.62,23.1 -1868,2543,11.6,10,116,2.65,22.8 -1872,2558,11.9,10,119,2.69,22.6 -1876,2575,12.28,10,122.8,2.74,22.3 -1880,2602,12.44,10,124.4,2.77,22.3 -1884,2613,12.8,10,128,2.82,22.0 -1888,2633,13.13,10,131.3,2.88,21.9 -1892,2657,13.4,10,134,2.89,21.6 -1896,2665,13.62,10,136.2,2.92,21.4 -1900,2668,13.6,10,136,2.93,21.5 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,2662,13.62,10,136.2,-2.31,17.0 +1104,2667,13.63,10,136.3,-2.30,16.9 +1108,2661,13.45,10,134.5,-2.30,17.1 +1112,2627,13.2,10,132,-2.28,17.3 +1116,2617,12.81,10,128.1,-2.22,17.4 +1120,2600,12.5,10,125,-2.18,17.5 +1124,2590,12.14,10,121.4,-2.16,17.8 +1128,2562,11.96,10,119.6,-2.13,17.8 +1132,2535,11.63,10,116.3,-2.11,18.1 +1136,2528,11.32,10,113.2,-2.05,18.2 +1140,2514,11,10,110,-2.04,18.6 +1144,2484,10.8,10,108,-1.99,18.4 +1148,2472,10.45,10,104.5,-1.95,18.7 +1152,2449,10.2,10,102,-1.93,18.9 +1156,2436,9.9,10,99,-1.89,19.1 +1160,2416,9.56,10,95.6,-1.84,19.2 +1164,2395,9.29,10,92.9,-1.81,19.5 +1168,2371,9.1,10,91,-1.79,19.7 +1172,2344,8.8,10,88,-1.76,19.9 +1176,2332,8.5,10,85,-1.71,20.2 +1180,2306,8.3,10,83,-1.68,20.3 +1184,2283,8.05,10,80.5,-1.65,20.5 +1188,2271,7.71,10,77.1,-1.62,21.0 +1192,2241,7.57,10,75.7,-1.59,21.0 +1196,2219,7.3,10,73,-1.56,21.3 +1200,2198,7.1,10,71,-1.54,21.7 +1204,2177,6.82,10,68.2,-1.50,22.0 +1208,2157,6.6,10,66,-1.47,22.3 +1212,2129,6.48,10,64.8,-1.44,22.3 +1216,2106,6.2,10,62,-1.41,22.8 +1220,2094,6,10,60,-1.36,22.7 +1224,2073,5.8,10,58,-1.33,22.9 +1228,2046,5.6,10,56,-1.32,23.6 +1232,2024,5.4,10,54,-1.29,23.9 +1236,2005,5.2,10,52,-1.26,24.2 +1240,1982,5,10,50,-1.22,24.4 +1244,1957,4.84,10,48.4,-1.19,24.6 +1248,1936,4.64,10,46.4,-1.17,25.1 +1252,1907,4.5,10,45,-1.14,25.3 +1256,1885,4.3,10,43,-1.10,25.5 +1260,1864,4.1,10,41,-1.08,26.2 +1264,1834,3.9,10,39,-1.06,27.1 +1268,1816,3.73,10,37.3,-1.02,27.2 +1272,1789,3.6,10,36,-0.99,27.5 +1276,1762,3.5,10,35,-0.97,27.7 +1280,1745,3.3,10,33,-0.93,28.2 +1284,1717,3.2,10,32,-0.91,28.3 +1288,1697,3,10,30,-0.88,29.3 +1292,1671,2.9,10,29,-0.85,29.4 +1296,1643,2.8,10,28,-0.83,29.5 +1300,1615,2.6,10,26,-0.80,30.7 +1304,1591,2.4,10,24,-0.78,32.5 +1308,1562,2.3,10,23,-0.75,32.7 +1312,1539,2.1,10,21,-0.72,34.3 +1316,1516,2,10,20,-0.70,34.9 +1320,1488,1.9,10,19,-0.67,35.3 +1324,1461,1.8,10,18,-0.65,36.3 +1328,1440,1.7,10,17,-0.63,37.1 +1332,1411,1.6,10,16,-0.61,38.3 +1336,1382,1.5,10,15,-0.58,38.7 +1340,1347,1.4,10,14,-0.55,39.5 +1344,1320,1.3,10,13,-0.53,40.8 +1348,1294,1.2,10,12,-0.51,42.7 +1352,1265,1.2,10,12,-0.49,40.4 +1356,1239,1.1,10,11,-0.47,42.5 +1360,1200,1,10,10,-0.44,44.5 +1364,1174,0.9,10,9,-0.42,46.9 +1368,1142,0.8,10,8,-0.40,49.9 +1372,1114,0.8,10,8,-0.38,47.6 +1376,1080,0.7,10,7,-0.35,50.5 +1380,1048,0.7,10,7,-0.34,48.0 +1384,1016,0.6,10,6,-0.31,52.2 +1388,986,0.5,10,5,-0.29,59.0 +1392,948,0.5,10,5,-0.28,55.3 +1396,916,0.4,10,4,-0.25,62.4 +1400,883,0.4,10,4,-0.24,59.0 +1404,845,0.4,10,4,-0.21,53.3 +1408,813,0.3,10,3,-0.20,66.5 +1412,774,0.22,10,2.2,-0.18,82.5 +1416,740,0.2,10,2,-0.16,81.6 +1420,701,0.2,10,2,-0.15,77.1 +1424,664,0.2,10,2,-0.14,68.0 +1428,624,0.2,10,2,-0.12,59.0 +1432,584,0.1,10,1,-0.10,99.8 +1436,545,0.1,10,1,-0.09,86.2 +1440,504,0.1,10,1,-0.07,72.6 +1444,462,0.05,10,0.5,-0.06,127.0 +1448,421,0.05,10,0.5,-0.05,108.9 +1452,377,0.05,10,0.5,-0.05,90.7 +1456,335,0.05,10,0.5,-0.03,63.5 +1460,0,0,10,0,0.00,0.0 +1464,0,0,10,0,0.00,0.0 +1468,0,0,10,0,0.00,0.0 +1472,0,0,10,0,0.00,0.0 +1476,0,0,10,0,0.00,0.0 +1480,0,0,10,0,0.00,0.0 +1484,0,0,10,0,0.00,0.0 +1488,0,0,10,0,0.00,0.0 +1492,0,0,10,0,0.00,0.0 +1496,0,0,10,0,0.00,0.0 +1500,0,0,10,0,0.00,0.0 +1504,0,0,10,0,0.00,0.0 +1508,0,0,10,0,0.00,0.0 +1512,0,0,10,0,0.00,0.0 +1516,0,0,10,0,0.00,0.0 +1520,0,0,10,0,0.00,0.0 +1524,0,0,10,0,0.00,0.0 +1528,0,0,10,0,0.00,0.0 +1532,0,0,10,0,0.00,0.0 +1536,0,0,10,0,0.00,0.0 +1540,0,0,10,0,0.00,0.0 +1544,332,0.05,10,0.5,0.04,81.6 +1548,373,0.05,10,0.5,0.05,108.9 +1552,417,0.05,10,0.5,0.07,136.1 +1556,459,0.1,10,1,0.08,77.1 +1560,500,0.1,10,1,0.09,90.7 +1564,540,0.1,10,1,0.11,108.9 +1568,582,0.1,10,1,0.13,127.0 +1572,621,0.2,10,2,0.15,72.6 +1576,657,0.2,10,2,0.16,81.6 +1580,695,0.2,10,2,0.18,90.7 +1584,733,0.2,10,2,0.20,102.1 +1588,766,0.3,10,3,0.22,74.1 +1592,805,0.3,10,3,0.24,81.6 +1596,836,0.4,10,4,0.26,65.8 +1600,873,0.4,10,4,0.29,72.6 +1604,906,0.47,10,4.7,0.31,66.6 +1608,938,0.5,10,5,0.34,67.1 +1612,974,0.5,10,5,0.36,72.6 +1616,1007,0.6,10,6,0.39,64.3 +1620,1035,0.7,10,7,0.41,59.0 +1624,1066,0.7,10,7,0.44,63.5 +1628,1101,0.8,10,8,0.47,58.4 +1632,1129,0.83,10,8.3,0.49,59.6 +1636,1159,0.94,10,9.4,0.52,55.5 +1640,1188,1,10,10,0.55,54.9 +1644,1219,1.1,10,11,0.58,52.8 +1648,1250,1.2,10,12,0.61,50.7 +1652,1278,1.3,10,13,0.64,49.2 +1656,1303,1.4,10,14,0.68,48.9 +1660,1330,1.5,10,15,0.70,46.9 +1664,1363,1.5,10,15,0.73,48.4 +1668,1392,1.6,10,16,0.76,47.6 +1672,1418,1.8,10,18,0.80,44.4 +1676,1445,1.9,10,19,0.83,43.4 +1680,1473,2,10,20,0.85,42.6 +1684,1496,2.1,10,21,0.89,42.3 +1688,1519,2.2,10,22,0.92,41.9 +1692,1542,2.3,10,23,0.95,41.2 +1696,1571,2.43,10,24.3,0.98,40.1 +1700,1596,2.7,10,27,0.97,36.0 +1704,1622,2.8,10,28,1.05,37.4 +1708,1650,2.9,10,29,1.08,37.1 +1712,1674,3.1,10,31,1.11,35.8 +1716,1696,3.3,10,33,1.17,35.3 +1720,1720,3.4,10,34,1.18,34.8 +1724,1747,3.5,10,35,1.21,34.5 +1728,1765,3.7,10,37,1.24,33.5 +1732,1793,3.84,10,38.4,1.27,33.2 +1736,1819,4,10,40,1.31,32.8 +1740,1833,4.2,10,42,1.36,32.3 +1744,1860,4.4,10,44,1.41,32.1 +1748,1889,4.5,10,45,1.43,31.8 +1752,1911,4.7,10,47,1.48,31.5 +1756,1933,4.9,10,49,1.52,31.1 +1760,1962,5.1,10,51,1.55,30.3 +1764,1984,5.3,10,53,1.58,29.8 +1768,2004,5.5,10,55,1.62,29.5 +1772,2027,5.7,10,57,1.66,29.0 +1776,2052,5.9,10,59,1.69,28.7 +1780,2075,6.09,10,60.9,1.73,28.4 +1784,2093,6.3,10,63,1.77,28.2 +1788,2118,6.5,10,65,1.80,27.7 +1792,2140,6.73,10,67.3,1.85,27.4 +1796,2160,6.9,10,69,1.88,27.2 +1800,2179,7.13,10,71.3,1.91,26.8 +1804,2200,7.4,10,74,1.99,26.9 +1808,2225,7.6,10,76,2.00,26.3 +1812,2245,7.9,10,79,2.06,26.1 +1816,2273,8.1,10,81,2.09,25.8 +1820,2293,8.33,10,83.3,2.12,25.5 +1824,2307,8.64,10,86.4,2.17,25.1 +1828,2333,8.89,10,88.9,2.21,24.9 +1832,2353,9.14,10,91.4,2.27,24.9 +1836,2378,9.4,10,94,2.30,24.5 +1840,2398,9.7,10,97,2.36,24.4 +1844,2410,10,10,100,2.41,24.1 +1848,2436,10.2,10,102,2.45,24.1 +1852,2453,10.5,10,105,2.48,23.6 +1856,2481,10.79,10,107.9,2.55,23.7 +1860,2499,11.04,10,110.4,2.58,23.4 +1864,2522,11.35,10,113.5,2.62,23.1 +1868,2543,11.6,10,116,2.65,22.8 +1872,2558,11.9,10,119,2.69,22.6 +1876,2575,12.28,10,122.8,2.74,22.3 +1880,2602,12.44,10,124.4,2.77,22.3 +1884,2613,12.8,10,128,2.82,22.0 +1888,2633,13.13,10,131.3,2.88,21.9 +1892,2657,13.4,10,134,2.89,21.6 +1896,2665,13.62,10,136.2,2.92,21.4 +1900,2668,13.6,10,136,2.93,21.5 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv index d33fb8807..847b36b5f 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), -1100,2976,17.03,12,204.4,-2.90,14.2, -1104,2969,17.08,12,205.0,-2.92,14.2, -1108,2971,16.76,12,201.1,-2.89,14.4, -1112,2933,16.52,12,198.2,-2.83,14.3, -1116,2916,16.08,12,193.0,-2.79,14.4, -1120,2898,15.69,12,188.3,-2.76,14.7, -1124,2874,15.31,12,183.7,-2.72,14.8, -1128,2852,15,12,180.0,-2.67,14.8, -1132,2841,14.51,12,174.1,-2.60,14.9, -1136,2817,14.17,12,170.0,-2.59,15.3, -1140,2792,13.82,12,165.8,-2.56,15.5, -1144,2773,13.46,12,161.5,-2.49,15.4, -1148,2755,13.08,12,157.0,-2.44,15.5, -1152,2728,12.8,12,153.6,-2.43,15.8, -1156,2714,12.4,12,148.8,-2.39,16.0, -1160,2689,12,12,144.0,-2.34,16.3, -1164,2676,11.66,12,139.9,-2.30,16.5, -1168,2654,11.31,12,135.7,-2.25,16.6, -1172,2615,11.1,12,133.2,-2.23,16.7, -1176,2610,10.74,12,128.9,-2.18,16.9, -1180,2576,10.5,12,126.0,-2.14,17.0, -1184,2560,10.11,12,121.3,-2.10,17.3, -1188,2533,9.84,12,118.1,-2.07,17.5, -1192,2510,9.5,12,114.0,-2.01,17.6, -1196,2482,9.2,12,110.4,-1.98,18.0, -1200,2457,8.9,12,106.8,-1.95,18.2, -1204,2443,8.6,12,103.2,-1.88,18.2, -1208,2412,8.3,12,99.6,-1.85,18.5, -1212,2383,8,12,96.0,-1.81,18.8, -1216,2365,7.7,12,92.4,-1.78,19.2, -1220,2335,7.4,12,88.8,-1.73,19.5, -1224,2318,7.1,12,85.2,-1.66,19.5, -1228,2284,6.9,12,82.8,-1.65,19.9, -1232,2260,6.6,12,79.2,-1.61,20.3, -1236,2231,6.4,12,76.8,-1.56,20.3, -1240,2208,6.2,12,74.4,-1.53,20.6, -1244,2189,5.99,12,71.9,-1.49,20.8, -1248,2159,5.77,12,69.2,-1.47,21.2, -1252,2140,5.5,12,66.0,-1.44,21.8, -1256,2106,5.32,12,63.8,-1.40,21.9, -1260,2082,5.17,12,62.0,-1.37,22.1, -1264,2057,4.9,12,58.8,-1.33,22.7, -1268,2038,4.7,12,56.4,-1.29,22.9, -1272,2006,4.56,12,54.7,-1.28,23.4, -1276,1982,4.3,12,51.6,-1.22,23.6, -1280,1955,4.1,12,49.2,-1.19,24.2, -1284,1925,3.9,12,46.8,-1.15,24.6, -1288,1898,3.73,12,44.8,-1.12,24.9, -1292,1869,3.6,12,43.2,-1.08,25.1, -1296,1846,3.4,12,40.8,-1.04,25.6, -1300,1815,3.3,12,39.6,-1.02,25.8, -1304,1788,3.1,12,37.2,-0.99,26.7, -1308,1763,2.98,12,35.8,-0.96,26.9, -1312,1737,2.8,12,33.6,-0.93,27.7, -1316,1708,2.7,12,32.4,-0.90,27.9, -1320,1681,2.41,12,28.9,-0.87,30.1, -1324,1651,2.3,12,27.6,-0.83,30.2, -1328,1620,2.1,12,25.2,-0.79,31.5, -1332,1588,2,12,24.0,-0.77,32.1, -1336,1557,1.9,12,22.8,-0.74,32.4, -1340,1529,1.8,12,21.6,-0.72,33.4, -1344,1504,1.7,12,20.4,-0.69,34.0, -1348,1470,1.6,12,19.2,-0.66,34.3, -1352,1439,1.5,12,18.0,-0.64,35.5, -1356,1411,1.31,12,15.7,-0.60,38.4, -1360,1375,1.3,12,15.6,-0.57,36.6, -1364,1338,1.2,12,14.4,-0.54,37.8, -1368,1309,1.1,12,13.2,-0.52,39.2, -1372,1276,1,12,12.0,-0.49,41.2, -1376,1246,0.9,12,10.8,-0.47,43.7, -1380,1211,0.8,12,9.6,-0.44,45.8, -1384,1169,0.8,12,9.6,-0.42,43.5, -1388,1134,0.7,12,8.4,-0.39,46.4, -1392,1099,0.6,12,7.2,-0.37,51.0, -1396,1057,0.5,12,6.0,-0.34,56.7, -1400,1023,0.5,12,6.0,-0.32,52.9, -1404,986,0.41,12,4.9,-0.29,59.9, -1408,944,0.4,12,4.8,-0.27,55.8, -1412,903,0.4,12,4.8,-0.24,51.0, -1416,865,0.3,12,3.6,-0.23,63.0, -1420,820,0.29,12,3.5,-0.20,58.7, -1424,777,0.2,12,2.4,-0.18,73.7, -1428,737,0.2,12,2.4,-0.16,68.0, -1432,690,0.2,12,2.4,-0.15,60.5, -1436,643,0.1,12,1.2,-0.12,102.1, -1440,597,0.1,12,1.2,-0.11,90.7, -1444,549,0.1,12,1.2,-0.09,75.6, -1448,499,0.05,12,0.6,-0.07,121.0, -1452,448,0.05,12,0.6,-0.06,98.3, -1456,400,0.05,12,0.6,-0.05,75.6, -1460,346,0.05,12,0.6,-0.04,60.5, -1464,0,0,12,0.0,0.00,0.0, -1468,0,0,12,0.0,0.00,0.0, -1472,0,0,12,0.0,0.00,0.0, -1476,0,0,12,0.0,0.00,0.0, -1480,0,0,12,0.0,0.00,0.0, -1484,0,0,12,0.0,0.00,0.0, -1488,0,0,12,0.0,0.00,0.0, -1492,0,0,12,0.0,0.00,0.0, -1496,0,0,12,0.0,0.00,0.0, -1500,0,0,12,0.0,0.00,0.0, -1504,0,0,12,0.0,0.00,0.0, -1508,0,0,12,0.0,0.00,0.0, -1512,0,0,12,0.0,0.00,0.0, -1516,0,0,12,0.0,0.00,0.0, -1520,0,0,12,0.0,0.00,0.0, -1524,0,0,12,0.0,0.00,0.0, -1528,0,0,12,0.0,0.00,0.0, -1532,0,0,12,0.0,0.00,0.0, -1536,0,0,12,0.0,0.00,0.0, -1540,342,0.05,12,0.6,0.04,68.0, -1544,395,0.05,12,0.6,0.05,90.7, -1548,444,0.05,12,0.6,0.07,121.0, -1552,494,0.1,12,1.2,0.10,79.4, -1556,542,0.1,12,1.2,0.11,94.5, -1560,592,0.1,12,1.2,0.13,109.6, -1564,634,0.1,12,1.2,0.15,128.5, -1568,680,0.2,12,2.4,0.18,73.7, -1572,726,0.2,12,2.4,0.20,83.2, -1576,768,0.2,12,2.4,0.22,92.6, -1580,812,0.3,12,3.6,0.25,70.6, -1584,851,0.3,12,3.6,0.28,76.9, -1588,892,0.4,12,4.8,0.31,64.3, -1592,931,0.4,12,4.8,0.33,69.0, -1596,971,0.5,12,6.0,0.37,61.2, -1600,1009,0.5,12,6.0,0.39,65.8, -1604,1044,0.6,12,7.2,0.43,59.2, -1608,1084,0.7,12,8.4,0.46,54.5, -1612,1121,0.7,12,8.4,0.49,58.3, -1616,1152,0.8,12,9.6,0.52,53.9, -1620,1186,0.8,12,9.6,0.55,57.2, -1624,1227,1,12,12.0,0.59,49.1, -1628,1263,1,12,12.0,0.63,52.2, -1632,1291,1.1,12,13.2,0.65,49.5, -1636,1322,1.2,12,14.4,0.68,47.6, -1640,1354,1.3,12,15.6,0.71,45.6, -1644,1385,1.4,12,16.8,0.76,45.1, -1648,1424,1.5,12,18.0,0.79,43.8, -1652,1453,1.6,12,19.2,0.83,43.2, -1656,1485,1.7,12,20.4,0.86,42.0, -1660,1515,1.8,12,21.6,0.89,41.4, -1664,1539,2,12,24.0,0.93,38.7, -1668,1567,2.1,12,25.2,0.97,38.3, -1672,1599,2.2,12,26.4,1.00,38.0, -1676,1632,2.3,12,27.6,1.04,37.6, -1680,1653,2.5,12,30.0,1.08,36.1, -1684,1686,2.8,12,33.6,1.14,33.9, -1688,1716,2.9,12,34.8,1.16,33.4, -1692,1741,3,12,36.0,1.20,33.3, -1696,1769,3.2,12,38.4,1.23,32.1, -1700,1796,3.3,12,39.6,1.28,32.3, -1704,1824,3.5,12,42.0,1.31,31.2, -1708,1854,3.67,12,44.0,1.35,30.7, -1712,1880,3.8,12,45.6,1.40,30.7, -1716,1911,4,12,48.0,1.43,29.9, -1720,1931,4.2,12,50.4,1.48,29.4, -1724,1964,4.4,12,52.8,1.53,29.0, -1728,1996,4.6,12,55.2,1.56,28.3, -1732,2017,4.8,12,57.6,1.63,28.3, -1736,2046,5,12,60.0,1.67,27.9, -1740,2070,5.2,12,62.4,1.71,27.4, -1744,2083,5.4,12,64.8,1.77,27.3, -1748,2118,5.69,12,68.3,1.82,26.6, -1752,2143,5.8,12,69.6,1.85,26.5, -1756,2169,6,12,72.0,1.91,26.5, -1760,2190,6.3,12,75.6,1.92,25.4, -1764,2229,6.5,12,78.0,1.96,25.2, -1768,2244,6.7,12,80.4,2.03,25.3, -1772,2269,7,12,84.0,2.09,24.8, -1776,2304,7.2,12,86.4,2.13,24.7, -1780,2330,7.5,12,90.0,2.18,24.2, -1784,2345,7.8,12,93.6,2.24,23.9, -1788,2391,8,12,96.0,2.27,23.7, -1792,2411,8.32,12,99.8,2.33,23.4, -1796,2432,8.64,12,103.7,2.40,23.1, -1800,2456,8.9,12,106.8,2.46,23.1, -1804,2482,9.24,12,110.9,2.51,22.6, -1808,2508,9.5,12,114.0,2.56,22.5, -1812,2535,9.82,12,117.8,2.62,22.2, -1816,2562,10.14,12,121.7,2.65,21.8, -1820,2585,10.45,12,125.4,2.71,21.6, -1824,2613,10.72,12,128.6,2.78,21.6, -1828,2620,11.1,12,133.2,2.84,21.4, -1832,2661,11.32,12,135.8,2.87,21.1, -1836,2681,11.62,12,139.4,2.93,21.0, -1840,2698,12.01,12,144.1,3.01,20.9, -1844,2721,12.37,12,148.4,3.04,20.5, -1848,2751,12.61,12,151.3,3.08,20.4, -1852,2773,13.04,12,156.5,3.16,20.2, -1856,2782,13.44,12,161.3,3.23,20.0, -1860,2808,13.7,12,164.4,3.26,19.8, -1864,2835,14.11,12,169.3,3.32,19.6, -1868,2859,14.4,12,172.8,3.38,19.6, -1872,2879,14.76,12,177.1,3.40,19.2, -1876,2904,15.13,12,181.6,3.45,19.0, -1880,2923,15.52,12,186.2,3.50,18.8, -1884,2937,15.87,12,190.4,3.57,18.7, -1888,2962,16.3,12,195.6,3.64,18.6, -1892,2976,16.74,12,200.9,3.71,18.4, -1896,2994,16.86,12,202.3,3.69,18.2, -1900,2995,16.91,12,202.9,3.71,18.3, + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), +1100,2976,17.03,12,204.4,-2.90,14.2, +1104,2969,17.08,12,205.0,-2.92,14.2, +1108,2971,16.76,12,201.1,-2.89,14.4, +1112,2933,16.52,12,198.2,-2.83,14.3, +1116,2916,16.08,12,193.0,-2.79,14.4, +1120,2898,15.69,12,188.3,-2.76,14.7, +1124,2874,15.31,12,183.7,-2.72,14.8, +1128,2852,15,12,180.0,-2.67,14.8, +1132,2841,14.51,12,174.1,-2.60,14.9, +1136,2817,14.17,12,170.0,-2.59,15.3, +1140,2792,13.82,12,165.8,-2.56,15.5, +1144,2773,13.46,12,161.5,-2.49,15.4, +1148,2755,13.08,12,157.0,-2.44,15.5, +1152,2728,12.8,12,153.6,-2.43,15.8, +1156,2714,12.4,12,148.8,-2.39,16.0, +1160,2689,12,12,144.0,-2.34,16.3, +1164,2676,11.66,12,139.9,-2.30,16.5, +1168,2654,11.31,12,135.7,-2.25,16.6, +1172,2615,11.1,12,133.2,-2.23,16.7, +1176,2610,10.74,12,128.9,-2.18,16.9, +1180,2576,10.5,12,126.0,-2.14,17.0, +1184,2560,10.11,12,121.3,-2.10,17.3, +1188,2533,9.84,12,118.1,-2.07,17.5, +1192,2510,9.5,12,114.0,-2.01,17.6, +1196,2482,9.2,12,110.4,-1.98,18.0, +1200,2457,8.9,12,106.8,-1.95,18.2, +1204,2443,8.6,12,103.2,-1.88,18.2, +1208,2412,8.3,12,99.6,-1.85,18.5, +1212,2383,8,12,96.0,-1.81,18.8, +1216,2365,7.7,12,92.4,-1.78,19.2, +1220,2335,7.4,12,88.8,-1.73,19.5, +1224,2318,7.1,12,85.2,-1.66,19.5, +1228,2284,6.9,12,82.8,-1.65,19.9, +1232,2260,6.6,12,79.2,-1.61,20.3, +1236,2231,6.4,12,76.8,-1.56,20.3, +1240,2208,6.2,12,74.4,-1.53,20.6, +1244,2189,5.99,12,71.9,-1.49,20.8, +1248,2159,5.77,12,69.2,-1.47,21.2, +1252,2140,5.5,12,66.0,-1.44,21.8, +1256,2106,5.32,12,63.8,-1.40,21.9, +1260,2082,5.17,12,62.0,-1.37,22.1, +1264,2057,4.9,12,58.8,-1.33,22.7, +1268,2038,4.7,12,56.4,-1.29,22.9, +1272,2006,4.56,12,54.7,-1.28,23.4, +1276,1982,4.3,12,51.6,-1.22,23.6, +1280,1955,4.1,12,49.2,-1.19,24.2, +1284,1925,3.9,12,46.8,-1.15,24.6, +1288,1898,3.73,12,44.8,-1.12,24.9, +1292,1869,3.6,12,43.2,-1.08,25.1, +1296,1846,3.4,12,40.8,-1.04,25.6, +1300,1815,3.3,12,39.6,-1.02,25.8, +1304,1788,3.1,12,37.2,-0.99,26.7, +1308,1763,2.98,12,35.8,-0.96,26.9, +1312,1737,2.8,12,33.6,-0.93,27.7, +1316,1708,2.7,12,32.4,-0.90,27.9, +1320,1681,2.41,12,28.9,-0.87,30.1, +1324,1651,2.3,12,27.6,-0.83,30.2, +1328,1620,2.1,12,25.2,-0.79,31.5, +1332,1588,2,12,24.0,-0.77,32.1, +1336,1557,1.9,12,22.8,-0.74,32.4, +1340,1529,1.8,12,21.6,-0.72,33.4, +1344,1504,1.7,12,20.4,-0.69,34.0, +1348,1470,1.6,12,19.2,-0.66,34.3, +1352,1439,1.5,12,18.0,-0.64,35.5, +1356,1411,1.31,12,15.7,-0.60,38.4, +1360,1375,1.3,12,15.6,-0.57,36.6, +1364,1338,1.2,12,14.4,-0.54,37.8, +1368,1309,1.1,12,13.2,-0.52,39.2, +1372,1276,1,12,12.0,-0.49,41.2, +1376,1246,0.9,12,10.8,-0.47,43.7, +1380,1211,0.8,12,9.6,-0.44,45.8, +1384,1169,0.8,12,9.6,-0.42,43.5, +1388,1134,0.7,12,8.4,-0.39,46.4, +1392,1099,0.6,12,7.2,-0.37,51.0, +1396,1057,0.5,12,6.0,-0.34,56.7, +1400,1023,0.5,12,6.0,-0.32,52.9, +1404,986,0.41,12,4.9,-0.29,59.9, +1408,944,0.4,12,4.8,-0.27,55.8, +1412,903,0.4,12,4.8,-0.24,51.0, +1416,865,0.3,12,3.6,-0.23,63.0, +1420,820,0.29,12,3.5,-0.20,58.7, +1424,777,0.2,12,2.4,-0.18,73.7, +1428,737,0.2,12,2.4,-0.16,68.0, +1432,690,0.2,12,2.4,-0.15,60.5, +1436,643,0.1,12,1.2,-0.12,102.1, +1440,597,0.1,12,1.2,-0.11,90.7, +1444,549,0.1,12,1.2,-0.09,75.6, +1448,499,0.05,12,0.6,-0.07,121.0, +1452,448,0.05,12,0.6,-0.06,98.3, +1456,400,0.05,12,0.6,-0.05,75.6, +1460,346,0.05,12,0.6,-0.04,60.5, +1464,0,0,12,0.0,0.00,0.0, +1468,0,0,12,0.0,0.00,0.0, +1472,0,0,12,0.0,0.00,0.0, +1476,0,0,12,0.0,0.00,0.0, +1480,0,0,12,0.0,0.00,0.0, +1484,0,0,12,0.0,0.00,0.0, +1488,0,0,12,0.0,0.00,0.0, +1492,0,0,12,0.0,0.00,0.0, +1496,0,0,12,0.0,0.00,0.0, +1500,0,0,12,0.0,0.00,0.0, +1504,0,0,12,0.0,0.00,0.0, +1508,0,0,12,0.0,0.00,0.0, +1512,0,0,12,0.0,0.00,0.0, +1516,0,0,12,0.0,0.00,0.0, +1520,0,0,12,0.0,0.00,0.0, +1524,0,0,12,0.0,0.00,0.0, +1528,0,0,12,0.0,0.00,0.0, +1532,0,0,12,0.0,0.00,0.0, +1536,0,0,12,0.0,0.00,0.0, +1540,342,0.05,12,0.6,0.04,68.0, +1544,395,0.05,12,0.6,0.05,90.7, +1548,444,0.05,12,0.6,0.07,121.0, +1552,494,0.1,12,1.2,0.10,79.4, +1556,542,0.1,12,1.2,0.11,94.5, +1560,592,0.1,12,1.2,0.13,109.6, +1564,634,0.1,12,1.2,0.15,128.5, +1568,680,0.2,12,2.4,0.18,73.7, +1572,726,0.2,12,2.4,0.20,83.2, +1576,768,0.2,12,2.4,0.22,92.6, +1580,812,0.3,12,3.6,0.25,70.6, +1584,851,0.3,12,3.6,0.28,76.9, +1588,892,0.4,12,4.8,0.31,64.3, +1592,931,0.4,12,4.8,0.33,69.0, +1596,971,0.5,12,6.0,0.37,61.2, +1600,1009,0.5,12,6.0,0.39,65.8, +1604,1044,0.6,12,7.2,0.43,59.2, +1608,1084,0.7,12,8.4,0.46,54.5, +1612,1121,0.7,12,8.4,0.49,58.3, +1616,1152,0.8,12,9.6,0.52,53.9, +1620,1186,0.8,12,9.6,0.55,57.2, +1624,1227,1,12,12.0,0.59,49.1, +1628,1263,1,12,12.0,0.63,52.2, +1632,1291,1.1,12,13.2,0.65,49.5, +1636,1322,1.2,12,14.4,0.68,47.6, +1640,1354,1.3,12,15.6,0.71,45.6, +1644,1385,1.4,12,16.8,0.76,45.1, +1648,1424,1.5,12,18.0,0.79,43.8, +1652,1453,1.6,12,19.2,0.83,43.2, +1656,1485,1.7,12,20.4,0.86,42.0, +1660,1515,1.8,12,21.6,0.89,41.4, +1664,1539,2,12,24.0,0.93,38.7, +1668,1567,2.1,12,25.2,0.97,38.3, +1672,1599,2.2,12,26.4,1.00,38.0, +1676,1632,2.3,12,27.6,1.04,37.6, +1680,1653,2.5,12,30.0,1.08,36.1, +1684,1686,2.8,12,33.6,1.14,33.9, +1688,1716,2.9,12,34.8,1.16,33.4, +1692,1741,3,12,36.0,1.20,33.3, +1696,1769,3.2,12,38.4,1.23,32.1, +1700,1796,3.3,12,39.6,1.28,32.3, +1704,1824,3.5,12,42.0,1.31,31.2, +1708,1854,3.67,12,44.0,1.35,30.7, +1712,1880,3.8,12,45.6,1.40,30.7, +1716,1911,4,12,48.0,1.43,29.9, +1720,1931,4.2,12,50.4,1.48,29.4, +1724,1964,4.4,12,52.8,1.53,29.0, +1728,1996,4.6,12,55.2,1.56,28.3, +1732,2017,4.8,12,57.6,1.63,28.3, +1736,2046,5,12,60.0,1.67,27.9, +1740,2070,5.2,12,62.4,1.71,27.4, +1744,2083,5.4,12,64.8,1.77,27.3, +1748,2118,5.69,12,68.3,1.82,26.6, +1752,2143,5.8,12,69.6,1.85,26.5, +1756,2169,6,12,72.0,1.91,26.5, +1760,2190,6.3,12,75.6,1.92,25.4, +1764,2229,6.5,12,78.0,1.96,25.2, +1768,2244,6.7,12,80.4,2.03,25.3, +1772,2269,7,12,84.0,2.09,24.8, +1776,2304,7.2,12,86.4,2.13,24.7, +1780,2330,7.5,12,90.0,2.18,24.2, +1784,2345,7.8,12,93.6,2.24,23.9, +1788,2391,8,12,96.0,2.27,23.7, +1792,2411,8.32,12,99.8,2.33,23.4, +1796,2432,8.64,12,103.7,2.40,23.1, +1800,2456,8.9,12,106.8,2.46,23.1, +1804,2482,9.24,12,110.9,2.51,22.6, +1808,2508,9.5,12,114.0,2.56,22.5, +1812,2535,9.82,12,117.8,2.62,22.2, +1816,2562,10.14,12,121.7,2.65,21.8, +1820,2585,10.45,12,125.4,2.71,21.6, +1824,2613,10.72,12,128.6,2.78,21.6, +1828,2620,11.1,12,133.2,2.84,21.4, +1832,2661,11.32,12,135.8,2.87,21.1, +1836,2681,11.62,12,139.4,2.93,21.0, +1840,2698,12.01,12,144.1,3.01,20.9, +1844,2721,12.37,12,148.4,3.04,20.5, +1848,2751,12.61,12,151.3,3.08,20.4, +1852,2773,13.04,12,156.5,3.16,20.2, +1856,2782,13.44,12,161.3,3.23,20.0, +1860,2808,13.7,12,164.4,3.26,19.8, +1864,2835,14.11,12,169.3,3.32,19.6, +1868,2859,14.4,12,172.8,3.38,19.6, +1872,2879,14.76,12,177.1,3.40,19.2, +1876,2904,15.13,12,181.6,3.45,19.0, +1880,2923,15.52,12,186.2,3.50,18.8, +1884,2937,15.87,12,190.4,3.57,18.7, +1888,2962,16.3,12,195.6,3.64,18.6, +1892,2976,16.74,12,200.9,3.71,18.4, +1896,2994,16.86,12,202.3,3.69,18.2, +1900,2995,16.91,12,202.9,3.71,18.3, diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv index de7294093..6689d4df1 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,2662,13.62,10,136.2,-2.31,17.0 -1104,2667,13.63,10,136.3,-2.30,16.9 -1108,2661,13.45,10,134.5,-2.30,17.1 -1112,2627,13.2,10,132,-2.28,17.3 -1116,2617,12.81,10,128.1,-2.22,17.4 -1120,2600,12.5,10,125,-2.18,17.5 -1124,2590,12.14,10,121.4,-2.16,17.8 -1128,2562,11.96,10,119.6,-2.13,17.8 -1132,2535,11.63,10,116.3,-2.11,18.1 -1136,2528,11.32,10,113.2,-2.05,18.2 -1140,2514,11,10,110,-2.04,18.6 -1144,2484,10.8,10,108,-1.99,18.4 -1148,2472,10.45,10,104.5,-1.95,18.7 -1152,2449,10.2,10,102,-1.93,18.9 -1156,2436,9.9,10,99,-1.89,19.1 -1160,2416,9.56,10,95.6,-1.84,19.2 -1164,2395,9.29,10,92.9,-1.81,19.5 -1168,2371,9.1,10,91,-1.79,19.7 -1172,2344,8.8,10,88,-1.76,19.9 -1176,2332,8.5,10,85,-1.71,20.2 -1180,2306,8.3,10,83,-1.68,20.3 -1184,2283,8.05,10,80.5,-1.65,20.5 -1188,2271,7.71,10,77.1,-1.62,21.0 -1192,2241,7.57,10,75.7,-1.59,21.0 -1196,2219,7.3,10,73,-1.56,21.3 -1200,2198,7.1,10,71,-1.54,21.7 -1204,2177,6.82,10,68.2,-1.50,22.0 -1208,2157,6.6,10,66,-1.47,22.3 -1212,2129,6.48,10,64.8,-1.44,22.3 -1216,2106,6.2,10,62,-1.41,22.8 -1220,2094,6,10,60,-1.36,22.7 -1224,2073,5.8,10,58,-1.33,22.9 -1228,2046,5.6,10,56,-1.32,23.6 -1232,2024,5.4,10,54,-1.29,23.9 -1236,2005,5.2,10,52,-1.26,24.2 -1240,1982,5,10,50,-1.22,24.4 -1244,1957,4.84,10,48.4,-1.19,24.6 -1248,1936,4.64,10,46.4,-1.17,25.1 -1252,1907,4.5,10,45,-1.14,25.3 -1256,1885,4.3,10,43,-1.10,25.5 -1260,1864,4.1,10,41,-1.08,26.2 -1264,1834,3.9,10,39,-1.06,27.1 -1268,1816,3.73,10,37.3,-1.02,27.2 -1272,1789,3.6,10,36,-0.99,27.5 -1276,1762,3.5,10,35,-0.97,27.7 -1280,1745,3.3,10,33,-0.93,28.2 -1284,1717,3.2,10,32,-0.91,28.3 -1288,1697,3,10,30,-0.88,29.3 -1292,1671,2.9,10,29,-0.85,29.4 -1296,1643,2.8,10,28,-0.83,29.5 -1300,1615,2.6,10,26,-0.80,30.7 -1304,1591,2.4,10,24,-0.78,32.5 -1308,1562,2.3,10,23,-0.75,32.7 -1312,1539,2.1,10,21,-0.72,34.3 -1316,1516,2,10,20,-0.70,34.9 -1320,1488,1.9,10,19,-0.67,35.3 -1324,1461,1.8,10,18,-0.65,36.3 -1328,1440,1.7,10,17,-0.63,37.1 -1332,1411,1.6,10,16,-0.61,38.3 -1336,1382,1.5,10,15,-0.58,38.7 -1340,1347,1.4,10,14,-0.55,39.5 -1344,1320,1.3,10,13,-0.53,40.8 -1348,1294,1.2,10,12,-0.51,42.7 -1352,1265,1.2,10,12,-0.49,40.4 -1356,1239,1.1,10,11,-0.47,42.5 -1360,1200,1,10,10,-0.44,44.5 -1364,1174,0.9,10,9,-0.42,46.9 -1368,1142,0.8,10,8,-0.40,49.9 -1372,1114,0.8,10,8,-0.38,47.6 -1376,1080,0.7,10,7,-0.35,50.5 -1380,1048,0.7,10,7,-0.34,48.0 -1384,1016,0.6,10,6,-0.31,52.2 -1388,986,0.5,10,5,-0.29,59.0 -1392,948,0.5,10,5,-0.28,55.3 -1396,916,0.4,10,4,-0.25,62.4 -1400,883,0.4,10,4,-0.24,59.0 -1404,845,0.4,10,4,-0.21,53.3 -1408,813,0.3,10,3,-0.20,66.5 -1412,774,0.22,10,2.2,-0.18,82.5 -1416,740,0.2,10,2,-0.16,81.6 -1420,701,0.2,10,2,-0.15,77.1 -1424,664,0.2,10,2,-0.14,68.0 -1428,624,0.2,10,2,-0.12,59.0 -1432,584,0.1,10,1,-0.10,99.8 -1436,545,0.1,10,1,-0.09,86.2 -1440,504,0.1,10,1,-0.07,72.6 -1444,462,0.05,10,0.5,-0.06,127.0 -1448,421,0.05,10,0.5,-0.05,108.9 -1452,377,0.05,10,0.5,-0.05,90.7 -1456,335,0.05,10,0.5,-0.03,63.5 -1460,0,0,10,0,0.00,0.0 -1464,0,0,10,0,0.00,0.0 -1468,0,0,10,0,0.00,0.0 -1472,0,0,10,0,0.00,0.0 -1476,0,0,10,0,0.00,0.0 -1480,0,0,10,0,0.00,0.0 -1484,0,0,10,0,0.00,0.0 -1488,0,0,10,0,0.00,0.0 -1492,0,0,10,0,0.00,0.0 -1496,0,0,10,0,0.00,0.0 -1500,0,0,10,0,0.00,0.0 -1504,0,0,10,0,0.00,0.0 -1508,0,0,10,0,0.00,0.0 -1512,0,0,10,0,0.00,0.0 -1516,0,0,10,0,0.00,0.0 -1520,0,0,10,0,0.00,0.0 -1524,0,0,10,0,0.00,0.0 -1528,0,0,10,0,0.00,0.0 -1532,0,0,10,0,0.00,0.0 -1536,0,0,10,0,0.00,0.0 -1540,0,0,10,0,0.00,0.0 -1544,332,0.05,10,0.5,0.04,81.6 -1548,373,0.05,10,0.5,0.05,108.9 -1552,417,0.05,10,0.5,0.07,136.1 -1556,459,0.1,10,1,0.08,77.1 -1560,500,0.1,10,1,0.09,90.7 -1564,540,0.1,10,1,0.11,108.9 -1568,582,0.1,10,1,0.13,127.0 -1572,621,0.2,10,2,0.15,72.6 -1576,657,0.2,10,2,0.16,81.6 -1580,695,0.2,10,2,0.18,90.7 -1584,733,0.2,10,2,0.20,102.1 -1588,766,0.3,10,3,0.22,74.1 -1592,805,0.3,10,3,0.24,81.6 -1596,836,0.4,10,4,0.26,65.8 -1600,873,0.4,10,4,0.29,72.6 -1604,906,0.47,10,4.7,0.31,66.6 -1608,938,0.5,10,5,0.34,67.1 -1612,974,0.5,10,5,0.36,72.6 -1616,1007,0.6,10,6,0.39,64.3 -1620,1035,0.7,10,7,0.41,59.0 -1624,1066,0.7,10,7,0.44,63.5 -1628,1101,0.8,10,8,0.47,58.4 -1632,1129,0.83,10,8.3,0.49,59.6 -1636,1159,0.94,10,9.4,0.52,55.5 -1640,1188,1,10,10,0.55,54.9 -1644,1219,1.1,10,11,0.58,52.8 -1648,1250,1.2,10,12,0.61,50.7 -1652,1278,1.3,10,13,0.64,49.2 -1656,1303,1.4,10,14,0.68,48.9 -1660,1330,1.5,10,15,0.70,46.9 -1664,1363,1.5,10,15,0.73,48.4 -1668,1392,1.6,10,16,0.76,47.6 -1672,1418,1.8,10,18,0.80,44.4 -1676,1445,1.9,10,19,0.83,43.4 -1680,1473,2,10,20,0.85,42.6 -1684,1496,2.1,10,21,0.89,42.3 -1688,1519,2.2,10,22,0.92,41.9 -1692,1542,2.3,10,23,0.95,41.2 -1696,1571,2.43,10,24.3,0.98,40.1 -1700,1596,2.7,10,27,0.97,36.0 -1704,1622,2.8,10,28,1.05,37.4 -1708,1650,2.9,10,29,1.08,37.1 -1712,1674,3.1,10,31,1.11,35.8 -1716,1696,3.3,10,33,1.17,35.3 -1720,1720,3.4,10,34,1.18,34.8 -1724,1747,3.5,10,35,1.21,34.5 -1728,1765,3.7,10,37,1.24,33.5 -1732,1793,3.84,10,38.4,1.27,33.2 -1736,1819,4,10,40,1.31,32.8 -1740,1833,4.2,10,42,1.36,32.3 -1744,1860,4.4,10,44,1.41,32.1 -1748,1889,4.5,10,45,1.43,31.8 -1752,1911,4.7,10,47,1.48,31.5 -1756,1933,4.9,10,49,1.52,31.1 -1760,1962,5.1,10,51,1.55,30.3 -1764,1984,5.3,10,53,1.58,29.8 -1768,2004,5.5,10,55,1.62,29.5 -1772,2027,5.7,10,57,1.66,29.0 -1776,2052,5.9,10,59,1.69,28.7 -1780,2075,6.09,10,60.9,1.73,28.4 -1784,2093,6.3,10,63,1.77,28.2 -1788,2118,6.5,10,65,1.80,27.7 -1792,2140,6.73,10,67.3,1.85,27.4 -1796,2160,6.9,10,69,1.88,27.2 -1800,2179,7.13,10,71.3,1.91,26.8 -1804,2200,7.4,10,74,1.99,26.9 -1808,2225,7.6,10,76,2.00,26.3 -1812,2245,7.9,10,79,2.06,26.1 -1816,2273,8.1,10,81,2.09,25.8 -1820,2293,8.33,10,83.3,2.12,25.5 -1824,2307,8.64,10,86.4,2.17,25.1 -1828,2333,8.89,10,88.9,2.21,24.9 -1832,2353,9.14,10,91.4,2.27,24.9 -1836,2378,9.4,10,94,2.30,24.5 -1840,2398,9.7,10,97,2.36,24.4 -1844,2410,10,10,100,2.41,24.1 -1848,2436,10.2,10,102,2.45,24.1 -1852,2453,10.5,10,105,2.48,23.6 -1856,2481,10.79,10,107.9,2.55,23.7 -1860,2499,11.04,10,110.4,2.58,23.4 -1864,2522,11.35,10,113.5,2.62,23.1 -1868,2543,11.6,10,116,2.65,22.8 -1872,2558,11.9,10,119,2.69,22.6 -1876,2575,12.28,10,122.8,2.74,22.3 -1880,2602,12.44,10,124.4,2.77,22.3 -1884,2613,12.8,10,128,2.82,22.0 -1888,2633,13.13,10,131.3,2.88,21.9 -1892,2657,13.4,10,134,2.89,21.6 -1896,2665,13.62,10,136.2,2.92,21.4 -1900,2668,13.6,10,136,2.93,21.5 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,2662,13.62,10,136.2,-2.31,17.0 +1104,2667,13.63,10,136.3,-2.30,16.9 +1108,2661,13.45,10,134.5,-2.30,17.1 +1112,2627,13.2,10,132,-2.28,17.3 +1116,2617,12.81,10,128.1,-2.22,17.4 +1120,2600,12.5,10,125,-2.18,17.5 +1124,2590,12.14,10,121.4,-2.16,17.8 +1128,2562,11.96,10,119.6,-2.13,17.8 +1132,2535,11.63,10,116.3,-2.11,18.1 +1136,2528,11.32,10,113.2,-2.05,18.2 +1140,2514,11,10,110,-2.04,18.6 +1144,2484,10.8,10,108,-1.99,18.4 +1148,2472,10.45,10,104.5,-1.95,18.7 +1152,2449,10.2,10,102,-1.93,18.9 +1156,2436,9.9,10,99,-1.89,19.1 +1160,2416,9.56,10,95.6,-1.84,19.2 +1164,2395,9.29,10,92.9,-1.81,19.5 +1168,2371,9.1,10,91,-1.79,19.7 +1172,2344,8.8,10,88,-1.76,19.9 +1176,2332,8.5,10,85,-1.71,20.2 +1180,2306,8.3,10,83,-1.68,20.3 +1184,2283,8.05,10,80.5,-1.65,20.5 +1188,2271,7.71,10,77.1,-1.62,21.0 +1192,2241,7.57,10,75.7,-1.59,21.0 +1196,2219,7.3,10,73,-1.56,21.3 +1200,2198,7.1,10,71,-1.54,21.7 +1204,2177,6.82,10,68.2,-1.50,22.0 +1208,2157,6.6,10,66,-1.47,22.3 +1212,2129,6.48,10,64.8,-1.44,22.3 +1216,2106,6.2,10,62,-1.41,22.8 +1220,2094,6,10,60,-1.36,22.7 +1224,2073,5.8,10,58,-1.33,22.9 +1228,2046,5.6,10,56,-1.32,23.6 +1232,2024,5.4,10,54,-1.29,23.9 +1236,2005,5.2,10,52,-1.26,24.2 +1240,1982,5,10,50,-1.22,24.4 +1244,1957,4.84,10,48.4,-1.19,24.6 +1248,1936,4.64,10,46.4,-1.17,25.1 +1252,1907,4.5,10,45,-1.14,25.3 +1256,1885,4.3,10,43,-1.10,25.5 +1260,1864,4.1,10,41,-1.08,26.2 +1264,1834,3.9,10,39,-1.06,27.1 +1268,1816,3.73,10,37.3,-1.02,27.2 +1272,1789,3.6,10,36,-0.99,27.5 +1276,1762,3.5,10,35,-0.97,27.7 +1280,1745,3.3,10,33,-0.93,28.2 +1284,1717,3.2,10,32,-0.91,28.3 +1288,1697,3,10,30,-0.88,29.3 +1292,1671,2.9,10,29,-0.85,29.4 +1296,1643,2.8,10,28,-0.83,29.5 +1300,1615,2.6,10,26,-0.80,30.7 +1304,1591,2.4,10,24,-0.78,32.5 +1308,1562,2.3,10,23,-0.75,32.7 +1312,1539,2.1,10,21,-0.72,34.3 +1316,1516,2,10,20,-0.70,34.9 +1320,1488,1.9,10,19,-0.67,35.3 +1324,1461,1.8,10,18,-0.65,36.3 +1328,1440,1.7,10,17,-0.63,37.1 +1332,1411,1.6,10,16,-0.61,38.3 +1336,1382,1.5,10,15,-0.58,38.7 +1340,1347,1.4,10,14,-0.55,39.5 +1344,1320,1.3,10,13,-0.53,40.8 +1348,1294,1.2,10,12,-0.51,42.7 +1352,1265,1.2,10,12,-0.49,40.4 +1356,1239,1.1,10,11,-0.47,42.5 +1360,1200,1,10,10,-0.44,44.5 +1364,1174,0.9,10,9,-0.42,46.9 +1368,1142,0.8,10,8,-0.40,49.9 +1372,1114,0.8,10,8,-0.38,47.6 +1376,1080,0.7,10,7,-0.35,50.5 +1380,1048,0.7,10,7,-0.34,48.0 +1384,1016,0.6,10,6,-0.31,52.2 +1388,986,0.5,10,5,-0.29,59.0 +1392,948,0.5,10,5,-0.28,55.3 +1396,916,0.4,10,4,-0.25,62.4 +1400,883,0.4,10,4,-0.24,59.0 +1404,845,0.4,10,4,-0.21,53.3 +1408,813,0.3,10,3,-0.20,66.5 +1412,774,0.22,10,2.2,-0.18,82.5 +1416,740,0.2,10,2,-0.16,81.6 +1420,701,0.2,10,2,-0.15,77.1 +1424,664,0.2,10,2,-0.14,68.0 +1428,624,0.2,10,2,-0.12,59.0 +1432,584,0.1,10,1,-0.10,99.8 +1436,545,0.1,10,1,-0.09,86.2 +1440,504,0.1,10,1,-0.07,72.6 +1444,462,0.05,10,0.5,-0.06,127.0 +1448,421,0.05,10,0.5,-0.05,108.9 +1452,377,0.05,10,0.5,-0.05,90.7 +1456,335,0.05,10,0.5,-0.03,63.5 +1460,0,0,10,0,0.00,0.0 +1464,0,0,10,0,0.00,0.0 +1468,0,0,10,0,0.00,0.0 +1472,0,0,10,0,0.00,0.0 +1476,0,0,10,0,0.00,0.0 +1480,0,0,10,0,0.00,0.0 +1484,0,0,10,0,0.00,0.0 +1488,0,0,10,0,0.00,0.0 +1492,0,0,10,0,0.00,0.0 +1496,0,0,10,0,0.00,0.0 +1500,0,0,10,0,0.00,0.0 +1504,0,0,10,0,0.00,0.0 +1508,0,0,10,0,0.00,0.0 +1512,0,0,10,0,0.00,0.0 +1516,0,0,10,0,0.00,0.0 +1520,0,0,10,0,0.00,0.0 +1524,0,0,10,0,0.00,0.0 +1528,0,0,10,0,0.00,0.0 +1532,0,0,10,0,0.00,0.0 +1536,0,0,10,0,0.00,0.0 +1540,0,0,10,0,0.00,0.0 +1544,332,0.05,10,0.5,0.04,81.6 +1548,373,0.05,10,0.5,0.05,108.9 +1552,417,0.05,10,0.5,0.07,136.1 +1556,459,0.1,10,1,0.08,77.1 +1560,500,0.1,10,1,0.09,90.7 +1564,540,0.1,10,1,0.11,108.9 +1568,582,0.1,10,1,0.13,127.0 +1572,621,0.2,10,2,0.15,72.6 +1576,657,0.2,10,2,0.16,81.6 +1580,695,0.2,10,2,0.18,90.7 +1584,733,0.2,10,2,0.20,102.1 +1588,766,0.3,10,3,0.22,74.1 +1592,805,0.3,10,3,0.24,81.6 +1596,836,0.4,10,4,0.26,65.8 +1600,873,0.4,10,4,0.29,72.6 +1604,906,0.47,10,4.7,0.31,66.6 +1608,938,0.5,10,5,0.34,67.1 +1612,974,0.5,10,5,0.36,72.6 +1616,1007,0.6,10,6,0.39,64.3 +1620,1035,0.7,10,7,0.41,59.0 +1624,1066,0.7,10,7,0.44,63.5 +1628,1101,0.8,10,8,0.47,58.4 +1632,1129,0.83,10,8.3,0.49,59.6 +1636,1159,0.94,10,9.4,0.52,55.5 +1640,1188,1,10,10,0.55,54.9 +1644,1219,1.1,10,11,0.58,52.8 +1648,1250,1.2,10,12,0.61,50.7 +1652,1278,1.3,10,13,0.64,49.2 +1656,1303,1.4,10,14,0.68,48.9 +1660,1330,1.5,10,15,0.70,46.9 +1664,1363,1.5,10,15,0.73,48.4 +1668,1392,1.6,10,16,0.76,47.6 +1672,1418,1.8,10,18,0.80,44.4 +1676,1445,1.9,10,19,0.83,43.4 +1680,1473,2,10,20,0.85,42.6 +1684,1496,2.1,10,21,0.89,42.3 +1688,1519,2.2,10,22,0.92,41.9 +1692,1542,2.3,10,23,0.95,41.2 +1696,1571,2.43,10,24.3,0.98,40.1 +1700,1596,2.7,10,27,0.97,36.0 +1704,1622,2.8,10,28,1.05,37.4 +1708,1650,2.9,10,29,1.08,37.1 +1712,1674,3.1,10,31,1.11,35.8 +1716,1696,3.3,10,33,1.17,35.3 +1720,1720,3.4,10,34,1.18,34.8 +1724,1747,3.5,10,35,1.21,34.5 +1728,1765,3.7,10,37,1.24,33.5 +1732,1793,3.84,10,38.4,1.27,33.2 +1736,1819,4,10,40,1.31,32.8 +1740,1833,4.2,10,42,1.36,32.3 +1744,1860,4.4,10,44,1.41,32.1 +1748,1889,4.5,10,45,1.43,31.8 +1752,1911,4.7,10,47,1.48,31.5 +1756,1933,4.9,10,49,1.52,31.1 +1760,1962,5.1,10,51,1.55,30.3 +1764,1984,5.3,10,53,1.58,29.8 +1768,2004,5.5,10,55,1.62,29.5 +1772,2027,5.7,10,57,1.66,29.0 +1776,2052,5.9,10,59,1.69,28.7 +1780,2075,6.09,10,60.9,1.73,28.4 +1784,2093,6.3,10,63,1.77,28.2 +1788,2118,6.5,10,65,1.80,27.7 +1792,2140,6.73,10,67.3,1.85,27.4 +1796,2160,6.9,10,69,1.88,27.2 +1800,2179,7.13,10,71.3,1.91,26.8 +1804,2200,7.4,10,74,1.99,26.9 +1808,2225,7.6,10,76,2.00,26.3 +1812,2245,7.9,10,79,2.06,26.1 +1816,2273,8.1,10,81,2.09,25.8 +1820,2293,8.33,10,83.3,2.12,25.5 +1824,2307,8.64,10,86.4,2.17,25.1 +1828,2333,8.89,10,88.9,2.21,24.9 +1832,2353,9.14,10,91.4,2.27,24.9 +1836,2378,9.4,10,94,2.30,24.5 +1840,2398,9.7,10,97,2.36,24.4 +1844,2410,10,10,100,2.41,24.1 +1848,2436,10.2,10,102,2.45,24.1 +1852,2453,10.5,10,105,2.48,23.6 +1856,2481,10.79,10,107.9,2.55,23.7 +1860,2499,11.04,10,110.4,2.58,23.4 +1864,2522,11.35,10,113.5,2.62,23.1 +1868,2543,11.6,10,116,2.65,22.8 +1872,2558,11.9,10,119,2.69,22.6 +1876,2575,12.28,10,122.8,2.74,22.3 +1880,2602,12.44,10,124.4,2.77,22.3 +1884,2613,12.8,10,128,2.82,22.0 +1888,2633,13.13,10,131.3,2.88,21.9 +1892,2657,13.4,10,134,2.89,21.6 +1896,2665,13.62,10,136.2,2.92,21.4 +1900,2668,13.6,10,136,2.93,21.5 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv index a1d7dd854..271f69d89 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,3465,24.30,16,388.8,-4.07,10.5 -1104,3468,24.3,16,388.8,-4.05,10.4 -1108,3449,23.78,16,380.5,-4.02,10.6 -1112,3421,23.25,16,372.0,-3.96,10.7 -1116,3410,22.64,16,362.2,-3.90,10.8 -1120,3395,22.07,16,353.1,-3.87,11.0 -1124,3374,21.46,16,343.4,-3.82,11.1 -1128,3356,20.98,16,335.7,-3.80,11.3 -1132,3323,20.44,16,327.0,-3.75,11.5 -1136,3307,19.9,16,318.4,-3.71,11.7 -1140,3282,19.32,16,309.1,-3.66,11.8 -1144,3248,18.74,16,299.8,-3.59,12.0 -1148,3223,18.11,16,289.8,-3.52,12.1 -1152,3207,17.53,16,280.5,-3.45,12.3 -1156,3177,17,16,272.0,-3.40,12.5 -1160,3153,16.45,16,263.2,-3.31,12.6 -1164,3111,15.96,16,255.4,-3.25,12.7 -1168,3086,15.48,16,247.7,-3.20,12.9 -1172,3061,14.98,16,239.7,-3.11,13.0 -1176,3050,14.43,16,230.9,-3.04,13.2 -1180,3003,14.04,16,224.6,-2.99,13.3 -1184,2988,13.55,16,216.8,-2.94,13.6 -1188,2952,13.1,16,209.6,-2.86,13.7 -1192,2918,12.74,16,203.8,-2.82,13.8 -1196,2896,12.3,16,196.8,-2.77,14.1 -1200,2865,11.93,16,190.9,-2.71,14.2 -1204,2845,11.59,16,185.4,-2.66,14.3 -1208,2821,11.2,16,179.2,-2.58,14.4 -1212,2792,10.8,16,172.8,-2.55,14.8 -1216,2759,10.5,16,168.0,-2.51,14.9 -1220,2736,10.27,16,164.3,-2.45,14.9 -1224,2712,9.9,16,158.4,-2.38,15.0 -1228,2681,9.5,16,152.0,-2.35,15.5 -1232,2661,9.2,16,147.2,-2.28,15.5 -1236,2629,8.9,16,142.4,-2.24,15.7 -1240,2599,8.6,16,137.6,-2.20,16.0 -1244,2576,8.2,16,131.2,-2.12,16.1 -1248,2548,7.9,16,126.4,-2.08,16.5 -1252,2517,7.57,16,121.1,-2.02,16.7 -1256,2482,7.19,16,115.0,-1.98,17.2 -1260,2440,6.89,16,110.2,-1.94,17.6 -1264,2419,6.53,16,104.5,-1.86,17.8 -1268,2369,6.29,16,100.6,-1.81,18.0 -1272,2345,6,16,96.0,-1.76,18.4 -1276,2306,5.75,16,92.0,-1.70,18.5 -1280,2278,5.5,16,88.0,-1.67,19.0 -1284,2248,5.3,16,84.8,-1.61,19.0 -1288,2222,5,16,80.0,-1.56,19.4 -1292,2199,4.8,16,76.8,-1.51,19.7 -1296,2157,4.6,16,73.6,-1.49,20.2 -1300,2140,4.4,16,70.4,-1.44,20.4 -1304,2106,4.2,16,67.2,-1.40,20.8 -1308,2081,3.98,16,63.7,-1.35,21.2 -1312,2040,3.7,16,59.2,-1.30,22.0 -1316,2001,3.58,16,57.3,-1.26,21.9 -1320,1968,3.39,16,54.2,-1.20,22.2 -1324,1936,3.2,16,51.2,-1.16,22.7 -1328,1903,3,16,48.0,-1.12,23.3 -1332,1870,2.84,16,45.4,-1.10,24.3 -1336,1840,2.7,16,43.2,-1.05,24.4 -1340,1807,2.4,16,38.4,-1.02,26.6 -1344,1773,2.3,16,36.8,-0.98,26.6 -1348,1738,2.1,16,33.6,-0.94,28.1 -1352,1705,2,16,32.0,-0.90,28.2 -1356,1667,1.9,16,30.4,-0.87,28.6 -1360,1630,1.7,16,27.2,-0.82,30.2 -1364,1589,1.6,16,25.6,-0.78,30.7 -1368,1554,1.5,16,24.0,-0.74,31.0 -1372,1519,1.4,16,22.4,-0.72,32.0 -1376,1482,1.3,16,20.8,-0.68,32.7 -1380,1448,1.2,16,19.2,-0.65,33.8 -1384,1410,1.1,16,17.6,-0.62,35.1 -1388,1362,1,16,16.0,-0.58,36.3 -1392,1325,0.9,16,14.4,-0.54,37.5 -1396,1285,0.8,16,12.8,-0.51,40.0 -1400,1246,0.7,16,11.2,-0.48,42.9 -1404,1199,0.66,16,10.6,-0.44,42.1 -1408,1153,0.56,16,9.0,-0.42,46.6 -1412,1113,0.5,16,8.0,-0.39,48.2 -1416,1064,0.4,16,6.4,-0.35,55.3 -1420,1016,0.4,16,6.4,-0.32,50.3 -1424,970,0.37,16,5.9,-0.29,49.8 -1428,916,0.3,16,4.8,-0.26,53.9 -1432,869,0.2,16,3.2,-0.24,73.7 -1436,810,0.2,16,3.2,-0.21,65.2 -1440,758,0.2,16,3.2,-0.18,56.7 -1444,697,0.1,16,1.6,-0.15,93.6 -1448,641,0.1,16,1.6,-0.13,79.4 -1452,578,0.1,16,1.6,-0.10,65.2 -1456,513,0.05,16,0.8,-0.09,107.7 -1460,450,0.05,16,0.8,-0.07,85.0 -1464,388,0.05,16,0.8,-0.05,62.4 -1468,317,0.05,16,0.8,-0.04,45.4 -1472,0,0,16,0.0,0.00,0.0 -1476,0,0,16,0.0,0.00,0.0 -1480,0,0,16,0.0,0.00,0.0 -1484,0,0,16,0.0,0.00,0.0 -1488,0,0,16,0.0,0.00,0.0 -1492,0,0,16,0.0,0.00,0.0 -1496,0,0,16,0.0,0.00,0.0 -1500,0,0,16,0.0,0.00,0.0 -1504,0,0,16,0.0,0.00,0.0 -1508,0,0,16,0.0,0.00,0.0 -1512,0,0,16,0.0,0.00,0.0 -1516,0,0,16,0.0,0.00,0.0 -1520,0,0,16,0.0,0.00,0.0 -1524,0,0,16,0.0,0.00,0.0 -1528,0,0,16,0.0,0.00,0.0 -1532,308,0.05,16,0.8,0.04,51.0 -1536,375,0.05,16,0.8,0.05,68.0 -1540,441,0.05,16,0.8,0.08,96.4 -1544,508,0.05,16,0.8,0.10,124.7 -1548,573,0.1,16,1.6,0.13,79.4 -1552,630,0.1,16,1.6,0.15,96.4 -1556,688,0.1,16,1.6,0.18,113.4 -1560,749,0.2,16,3.2,0.22,68.0 -1564,804,0.2,16,3.2,0.25,78.0 -1568,859,0.2,16,3.2,0.29,90.7 -1572,906,0.3,16,4.8,0.32,67.1 -1576,962,0.4,16,6.4,0.36,56.0 -1580,1007,0.4,16,6.4,0.40,62.4 -1584,1057,0.5,16,8.0,0.44,54.4 -1588,1104,0.5,16,8.0,0.47,59.0 -1592,1148,0.6,16,9.6,0.51,53.4 -1596,1195,0.7,16,11.2,0.56,49.8 -1600,1237,0.8,16,12.8,0.60,47.1 -1604,1279,0.8,16,12.8,0.64,50.0 -1608,1319,0.9,16,14.4,0.68,47.2 -1612,1359,1,16,16.0,0.72,45.1 -1616,1409,1.1,16,17.6,0.78,44.1 -1620,1444,1.2,16,19.2,0.82,42.5 -1624,1480,1.3,16,20.8,0.87,41.9 -1628,1516,1.45,16,23.2,0.91,39.1 -1632,1549,1.5,16,24.0,0.95,39.5 -1636,1583,1.6,16,25.6,0.99,38.8 -1640,1629,1.8,16,28.8,1.03,35.9 -1644,1668,2,16,32.0,1.10,34.3 -1648,1699,2.1,16,33.6,1.14,34.0 -1652,1740,2.2,16,35.2,1.18,33.6 -1656,1766,2.4,16,38.4,1.24,32.2 -1660,1802,2.6,16,41.6,1.28,30.7 -1664,1835,2.8,16,44.8,1.33,29.7 -1668,1865,2.9,16,46.4,1.39,29.9 -1672,1901,3.1,16,49.6,1.44,29.0 -1676,1930,3.3,16,52.8,1.48,28.1 -1680,1961,3.5,16,56.0,1.54,27.5 -1684,1994,3.7,16,59.2,1.59,26.8 -1688,2031,3.9,16,62.4,1.65,26.5 -1692,2071,4.1,16,65.6,1.69,25.8 -1696,2106,4.3,16,68.8,1.76,25.6 -1700,2129,4.58,16,73.3,1.82,24.9 -1704,2160,4.7,16,75.2,1.88,25.0 -1708,2190,5,16,80.0,1.93,24.2 -1712,2219,5.2,16,83.2,1.99,23.9 -1716,2244,5.42,16,86.7,2.05,23.6 -1720,2266,5.69,16,91.0,2.12,23.3 -1724,2305,5.9,16,94.4,2.18,23.1 -1728,2342,6.19,16,99.0,2.22,22.4 -1732,2392,6.4,16,102.4,2.28,22.2 -1736,2416,6.7,16,107.2,2.38,22.2 -1740,2446,7,16,112.0,2.43,21.7 -1744,2475,7.4,16,118.4,2.52,21.3 -1748,2498,7.7,16,123.2,2.58,20.9 -1752,2538,8.04,16,128.6,2.65,20.6 -1756,2584,8.4,16,134.4,2.73,20.3 -1760,2609,8.7,16,139.2,2.76,19.8 -1764,2639,9.03,16,144.5,2.84,19.7 -1768,2663,9.34,16,149.4,2.89,19.3 -1772,2688,9.69,16,155.0,2.98,19.2 -1776,2711,10.01,16,160.2,3.05,19.1 -1780,2735,10.4,16,166.4,3.11,18.7 -1784,2766,10.68,16,170.9,3.17,18.6 -1788,2809,10.9,16,174.4,3.22,18.4 -1792,2822,11.3,16,180.8,3.30,18.3 -1796,2854,11.64,16,186.2,3.37,18.1 -1800,2888,11.93,16,190.9,3.42,17.9 -1804,2903,12.38,16,198.1,3.51,17.7 -1808,2934,12.79,16,204.6,3.61,17.6 -1812,2973,13.14,16,210.2,3.68,17.5 -1816,3002,13.54,16,216.6,3.74,17.3 -1820,3042,13.99,16,223.8,3.82,17.1 -1824,3065,14.46,16,231.4,3.89,16.8 -1828,3108,14.86,16,237.8,3.96,16.6 -1832,3125,15.4,16,246.4,4.06,16.5 -1836,3153,15.86,16,253.8,4.15,16.3 -1840,3180,16.41,16,262.6,4.25,16.2 -1844,3204,16.97,16,271.5,4.30,15.9 -1848,3242,17.42,16,278.7,4.38,15.7 -1852,3260,18,16,288.0,4.51,15.7 -1856,3299,18.5,16,296.0,4.53,15.3 -1860,3308,19.13,16,306.1,4.65,15.2 -1864,3351,19.59,16,313.4,4.71,15.0 -1868,3379,20.15,16,322.4,4.79,14.8 -1872,3406,20.67,16,330.7,4.84,14.6 -1876,3428,21.13,16,338.1,4.93,14.6 -1880,3438,21.8,16,348.8,5.01,14.4 -1884,3455,22.38,16,358.1,5.08,14.2 -1888,3494,22.81,16,365.0,5.14,14.1 -1892,3516,23.28,16,372.5,5.18,13.9 -1896,3527,23.9,16,382.4,5.22,13.7 -1900,3533,23.83,16,381.3,5.25,13.8 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,3465,24.30,16,388.8,-4.07,10.5 +1104,3468,24.3,16,388.8,-4.05,10.4 +1108,3449,23.78,16,380.5,-4.02,10.6 +1112,3421,23.25,16,372.0,-3.96,10.7 +1116,3410,22.64,16,362.2,-3.90,10.8 +1120,3395,22.07,16,353.1,-3.87,11.0 +1124,3374,21.46,16,343.4,-3.82,11.1 +1128,3356,20.98,16,335.7,-3.80,11.3 +1132,3323,20.44,16,327.0,-3.75,11.5 +1136,3307,19.9,16,318.4,-3.71,11.7 +1140,3282,19.32,16,309.1,-3.66,11.8 +1144,3248,18.74,16,299.8,-3.59,12.0 +1148,3223,18.11,16,289.8,-3.52,12.1 +1152,3207,17.53,16,280.5,-3.45,12.3 +1156,3177,17,16,272.0,-3.40,12.5 +1160,3153,16.45,16,263.2,-3.31,12.6 +1164,3111,15.96,16,255.4,-3.25,12.7 +1168,3086,15.48,16,247.7,-3.20,12.9 +1172,3061,14.98,16,239.7,-3.11,13.0 +1176,3050,14.43,16,230.9,-3.04,13.2 +1180,3003,14.04,16,224.6,-2.99,13.3 +1184,2988,13.55,16,216.8,-2.94,13.6 +1188,2952,13.1,16,209.6,-2.86,13.7 +1192,2918,12.74,16,203.8,-2.82,13.8 +1196,2896,12.3,16,196.8,-2.77,14.1 +1200,2865,11.93,16,190.9,-2.71,14.2 +1204,2845,11.59,16,185.4,-2.66,14.3 +1208,2821,11.2,16,179.2,-2.58,14.4 +1212,2792,10.8,16,172.8,-2.55,14.8 +1216,2759,10.5,16,168.0,-2.51,14.9 +1220,2736,10.27,16,164.3,-2.45,14.9 +1224,2712,9.9,16,158.4,-2.38,15.0 +1228,2681,9.5,16,152.0,-2.35,15.5 +1232,2661,9.2,16,147.2,-2.28,15.5 +1236,2629,8.9,16,142.4,-2.24,15.7 +1240,2599,8.6,16,137.6,-2.20,16.0 +1244,2576,8.2,16,131.2,-2.12,16.1 +1248,2548,7.9,16,126.4,-2.08,16.5 +1252,2517,7.57,16,121.1,-2.02,16.7 +1256,2482,7.19,16,115.0,-1.98,17.2 +1260,2440,6.89,16,110.2,-1.94,17.6 +1264,2419,6.53,16,104.5,-1.86,17.8 +1268,2369,6.29,16,100.6,-1.81,18.0 +1272,2345,6,16,96.0,-1.76,18.4 +1276,2306,5.75,16,92.0,-1.70,18.5 +1280,2278,5.5,16,88.0,-1.67,19.0 +1284,2248,5.3,16,84.8,-1.61,19.0 +1288,2222,5,16,80.0,-1.56,19.4 +1292,2199,4.8,16,76.8,-1.51,19.7 +1296,2157,4.6,16,73.6,-1.49,20.2 +1300,2140,4.4,16,70.4,-1.44,20.4 +1304,2106,4.2,16,67.2,-1.40,20.8 +1308,2081,3.98,16,63.7,-1.35,21.2 +1312,2040,3.7,16,59.2,-1.30,22.0 +1316,2001,3.58,16,57.3,-1.26,21.9 +1320,1968,3.39,16,54.2,-1.20,22.2 +1324,1936,3.2,16,51.2,-1.16,22.7 +1328,1903,3,16,48.0,-1.12,23.3 +1332,1870,2.84,16,45.4,-1.10,24.3 +1336,1840,2.7,16,43.2,-1.05,24.4 +1340,1807,2.4,16,38.4,-1.02,26.6 +1344,1773,2.3,16,36.8,-0.98,26.6 +1348,1738,2.1,16,33.6,-0.94,28.1 +1352,1705,2,16,32.0,-0.90,28.2 +1356,1667,1.9,16,30.4,-0.87,28.6 +1360,1630,1.7,16,27.2,-0.82,30.2 +1364,1589,1.6,16,25.6,-0.78,30.7 +1368,1554,1.5,16,24.0,-0.74,31.0 +1372,1519,1.4,16,22.4,-0.72,32.0 +1376,1482,1.3,16,20.8,-0.68,32.7 +1380,1448,1.2,16,19.2,-0.65,33.8 +1384,1410,1.1,16,17.6,-0.62,35.1 +1388,1362,1,16,16.0,-0.58,36.3 +1392,1325,0.9,16,14.4,-0.54,37.5 +1396,1285,0.8,16,12.8,-0.51,40.0 +1400,1246,0.7,16,11.2,-0.48,42.9 +1404,1199,0.66,16,10.6,-0.44,42.1 +1408,1153,0.56,16,9.0,-0.42,46.6 +1412,1113,0.5,16,8.0,-0.39,48.2 +1416,1064,0.4,16,6.4,-0.35,55.3 +1420,1016,0.4,16,6.4,-0.32,50.3 +1424,970,0.37,16,5.9,-0.29,49.8 +1428,916,0.3,16,4.8,-0.26,53.9 +1432,869,0.2,16,3.2,-0.24,73.7 +1436,810,0.2,16,3.2,-0.21,65.2 +1440,758,0.2,16,3.2,-0.18,56.7 +1444,697,0.1,16,1.6,-0.15,93.6 +1448,641,0.1,16,1.6,-0.13,79.4 +1452,578,0.1,16,1.6,-0.10,65.2 +1456,513,0.05,16,0.8,-0.09,107.7 +1460,450,0.05,16,0.8,-0.07,85.0 +1464,388,0.05,16,0.8,-0.05,62.4 +1468,317,0.05,16,0.8,-0.04,45.4 +1472,0,0,16,0.0,0.00,0.0 +1476,0,0,16,0.0,0.00,0.0 +1480,0,0,16,0.0,0.00,0.0 +1484,0,0,16,0.0,0.00,0.0 +1488,0,0,16,0.0,0.00,0.0 +1492,0,0,16,0.0,0.00,0.0 +1496,0,0,16,0.0,0.00,0.0 +1500,0,0,16,0.0,0.00,0.0 +1504,0,0,16,0.0,0.00,0.0 +1508,0,0,16,0.0,0.00,0.0 +1512,0,0,16,0.0,0.00,0.0 +1516,0,0,16,0.0,0.00,0.0 +1520,0,0,16,0.0,0.00,0.0 +1524,0,0,16,0.0,0.00,0.0 +1528,0,0,16,0.0,0.00,0.0 +1532,308,0.05,16,0.8,0.04,51.0 +1536,375,0.05,16,0.8,0.05,68.0 +1540,441,0.05,16,0.8,0.08,96.4 +1544,508,0.05,16,0.8,0.10,124.7 +1548,573,0.1,16,1.6,0.13,79.4 +1552,630,0.1,16,1.6,0.15,96.4 +1556,688,0.1,16,1.6,0.18,113.4 +1560,749,0.2,16,3.2,0.22,68.0 +1564,804,0.2,16,3.2,0.25,78.0 +1568,859,0.2,16,3.2,0.29,90.7 +1572,906,0.3,16,4.8,0.32,67.1 +1576,962,0.4,16,6.4,0.36,56.0 +1580,1007,0.4,16,6.4,0.40,62.4 +1584,1057,0.5,16,8.0,0.44,54.4 +1588,1104,0.5,16,8.0,0.47,59.0 +1592,1148,0.6,16,9.6,0.51,53.4 +1596,1195,0.7,16,11.2,0.56,49.8 +1600,1237,0.8,16,12.8,0.60,47.1 +1604,1279,0.8,16,12.8,0.64,50.0 +1608,1319,0.9,16,14.4,0.68,47.2 +1612,1359,1,16,16.0,0.72,45.1 +1616,1409,1.1,16,17.6,0.78,44.1 +1620,1444,1.2,16,19.2,0.82,42.5 +1624,1480,1.3,16,20.8,0.87,41.9 +1628,1516,1.45,16,23.2,0.91,39.1 +1632,1549,1.5,16,24.0,0.95,39.5 +1636,1583,1.6,16,25.6,0.99,38.8 +1640,1629,1.8,16,28.8,1.03,35.9 +1644,1668,2,16,32.0,1.10,34.3 +1648,1699,2.1,16,33.6,1.14,34.0 +1652,1740,2.2,16,35.2,1.18,33.6 +1656,1766,2.4,16,38.4,1.24,32.2 +1660,1802,2.6,16,41.6,1.28,30.7 +1664,1835,2.8,16,44.8,1.33,29.7 +1668,1865,2.9,16,46.4,1.39,29.9 +1672,1901,3.1,16,49.6,1.44,29.0 +1676,1930,3.3,16,52.8,1.48,28.1 +1680,1961,3.5,16,56.0,1.54,27.5 +1684,1994,3.7,16,59.2,1.59,26.8 +1688,2031,3.9,16,62.4,1.65,26.5 +1692,2071,4.1,16,65.6,1.69,25.8 +1696,2106,4.3,16,68.8,1.76,25.6 +1700,2129,4.58,16,73.3,1.82,24.9 +1704,2160,4.7,16,75.2,1.88,25.0 +1708,2190,5,16,80.0,1.93,24.2 +1712,2219,5.2,16,83.2,1.99,23.9 +1716,2244,5.42,16,86.7,2.05,23.6 +1720,2266,5.69,16,91.0,2.12,23.3 +1724,2305,5.9,16,94.4,2.18,23.1 +1728,2342,6.19,16,99.0,2.22,22.4 +1732,2392,6.4,16,102.4,2.28,22.2 +1736,2416,6.7,16,107.2,2.38,22.2 +1740,2446,7,16,112.0,2.43,21.7 +1744,2475,7.4,16,118.4,2.52,21.3 +1748,2498,7.7,16,123.2,2.58,20.9 +1752,2538,8.04,16,128.6,2.65,20.6 +1756,2584,8.4,16,134.4,2.73,20.3 +1760,2609,8.7,16,139.2,2.76,19.8 +1764,2639,9.03,16,144.5,2.84,19.7 +1768,2663,9.34,16,149.4,2.89,19.3 +1772,2688,9.69,16,155.0,2.98,19.2 +1776,2711,10.01,16,160.2,3.05,19.1 +1780,2735,10.4,16,166.4,3.11,18.7 +1784,2766,10.68,16,170.9,3.17,18.6 +1788,2809,10.9,16,174.4,3.22,18.4 +1792,2822,11.3,16,180.8,3.30,18.3 +1796,2854,11.64,16,186.2,3.37,18.1 +1800,2888,11.93,16,190.9,3.42,17.9 +1804,2903,12.38,16,198.1,3.51,17.7 +1808,2934,12.79,16,204.6,3.61,17.6 +1812,2973,13.14,16,210.2,3.68,17.5 +1816,3002,13.54,16,216.6,3.74,17.3 +1820,3042,13.99,16,223.8,3.82,17.1 +1824,3065,14.46,16,231.4,3.89,16.8 +1828,3108,14.86,16,237.8,3.96,16.6 +1832,3125,15.4,16,246.4,4.06,16.5 +1836,3153,15.86,16,253.8,4.15,16.3 +1840,3180,16.41,16,262.6,4.25,16.2 +1844,3204,16.97,16,271.5,4.30,15.9 +1848,3242,17.42,16,278.7,4.38,15.7 +1852,3260,18,16,288.0,4.51,15.7 +1856,3299,18.5,16,296.0,4.53,15.3 +1860,3308,19.13,16,306.1,4.65,15.2 +1864,3351,19.59,16,313.4,4.71,15.0 +1868,3379,20.15,16,322.4,4.79,14.8 +1872,3406,20.67,16,330.7,4.84,14.6 +1876,3428,21.13,16,338.1,4.93,14.6 +1880,3438,21.8,16,348.8,5.01,14.4 +1884,3455,22.38,16,358.1,5.08,14.2 +1888,3494,22.81,16,365.0,5.14,14.1 +1892,3516,23.28,16,372.5,5.18,13.9 +1896,3527,23.9,16,382.4,5.22,13.7 +1900,3533,23.83,16,381.3,5.25,13.8 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv index ee5517736..31efb4baa 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) -1100,3643,28.22,18,507.9,-4.59,9.0 -1104,3643,27.91,18,502.4,-4.54,9.0 -1108,3624,27.31,18,491.6,-4.53,9.2 -1112,3607,26.62,18,479.2,-4.48,9.3 -1116,3578,26.04,18,468.7,-4.42,9.4 -1120,3563,25.38,18,456.8,-4.35,9.5 -1124,3528,24.8,18,446.4,-4.35,9.7 -1128,3520,24.03,18,432.5,-4.17,9.6 -1132,3483,23.53,18,423.5,-4.18,9.9 -1136,3470,22.78,18,410.0,-4.11,10.0 -1140,3441,22.21,18,399.8,-4.04,10.1 -1144,3422,21.5,18,387.0,-3.99,10.3 -1148,3420,20.61,18,371.0,-3.91,10.6 -1152,3375,20.17,18,363.1,-3.86,10.6 -1156,3343,19.51,18,351.2,-3.81,10.8 -1160,3316,18.85,18,339.3,-3.71,10.9 -1164,3273,18.23,18,328.1,-3.68,11.2 -1168,3248,17.57,18,316.3,-3.56,11.2 -1172,3195,17.12,18,308.2,-3.54,11.5 -1176,3193,16.34,18,294.1,-3.42,11.6 -1180,3153,15.8,18,284.4,-3.36,11.8 -1184,3127,15.19,18,273.4,-3.27,11.9 -1188,3087,14.69,18,264.4,-3.19,12.1 -1192,3057,14.16,18,254.9,-3.09,12.1 -1196,3022,13.68,18,246.2,-3.03,12.3 -1200,2985,13.27,18,238.9,-2.96,12.4 -1204,2958,12.83,18,230.9,-2.88,12.5 -1208,2925,12.36,18,222.5,-2.84,12.8 -1212,2896,11.99,18,215.8,-2.78,12.9 -1216,2866,11.58,18,208.4,-2.70,12.9 -1220,2829,11.32,18,203.8,-2.66,13.0 -1224,2806,11,18,198.0,-2.59,13.1 -1228,2785,10.65,18,191.7,-2.53,13.2 -1232,2756,10.27,18,184.9,-2.48,13.4 -1236,2720,9.97,18,179.5,-2.42,13.5 -1240,2695,9.55,18,171.9,-2.36,13.7 -1244,2673,9.2,18,165.6,-2.32,14.0 -1248,2638,8.87,18,159.7,-2.25,14.1 -1252,2603,8.54,18,153.7,-2.21,14.4 -1256,2572,8.19,18,147.4,-2.16,14.6 -1260,2549,7.77,18,139.9,-2.11,15.1 -1264,2514,7.39,18,133.0,-2.04,15.3 -1268,2488,7,18,126.0,-2.00,15.8 -1272,2444,6.65,18,119.7,-1.94,16.2 -1276,2411,6.3,18,113.4,-1.88,16.6 -1280,2379,6,18,108.0,-1.81,16.8 -1284,2343,5.75,18,103.5,-1.76,17.0 -1288,2310,5.5,18,99.0,-1.72,17.4 -1292,2283,5.23,18,94.1,-1.66,17.6 -1296,2247,5,18,90.0,-1.61,17.9 -1300,2218,4.79,18,86.2,-1.56,18.1 -1304,2178,4.59,18,82.6,-1.52,18.4 -1308,2155,4.3,18,77.4,-1.47,18.9 -1312,2123,4.1,18,73.8,-1.42,19.3 -1316,2089,3.9,18,70.2,-1.38,19.6 -1320,2046,3.68,18,66.2,-1.33,20.1 -1324,2013,3.4,18,61.2,-1.27,20.8 -1328,1974,3.3,18,59.4,-1.22,20.6 -1332,1937,3.1,18,55.8,-1.17,21.1 -1336,1896,2.9,18,52.2,-1.13,21.7 -1340,1871,2.73,18,49.1,-1.08,22.1 -1344,1832,2.5,18,45.0,-1.05,23.3 -1348,1796,2.3,18,41.4,-1.01,24.4 -1352,1765,2.14,18,38.5,-0.97,25.2 -1356,1733,2,18,36.0,-0.93,26.0 -1360,1697,1.9,18,34.2,-0.89,26.1 -1364,1659,1.76,18,31.7,-0.86,27.1 -1368,1611,1.6,18,28.8,-0.81,28.2 -1372,1575,1.5,18,27.0,-0.77,28.6 -1376,1539,1.4,18,25.2,-0.74,29.3 -1380,1507,1.3,18,23.4,-0.70,29.9 -1384,1469,1.2,18,21.6,-0.67,31.1 -1388,1428,1.1,18,19.8,-0.64,32.1 -1392,1390,1,18,18.0,-0.60,33.3 -1396,1344,0.8,18,14.4,-0.56,38.7 -1400,1308,0.8,18,14.4,-0.53,36.9 -1404,1270,0.7,18,12.6,-0.49,39.2 -1408,1229,0.63,18,11.3,-0.46,40.8 -1412,1178,0.53,18,9.5,-0.43,45.2 -1416,1136,0.5,18,9.0,-0.40,44.4 -1420,1090,0.4,18,7.2,-0.37,51.0 -1424,1039,0.4,18,7.2,-0.34,46.6 -1428,989,0.3,18,5.4,-0.30,56.3 -1432,935,0.3,18,5.4,-0.27,50.4 -1436,884,0.2,18,3.6,-0.24,66.8 -1440,823,0.2,18,3.6,-0.21,58.0 -1444,760,0.2,18,3.6,-0.18,50.4 -1448,703,0.1,18,1.8,-0.15,85.7 -1452,638,0.1,18,1.8,-0.13,70.6 -1456,575,0.1,18,1.8,-0.10,58.0 -1460,501,0.05,18,0.9,-0.08,85.7 -1464,428,0.05,18,0.9,-0.06,65.5 -1468,357,0.05,18,0.9,-0.05,50.4 -1472,0,0,18,0.0,0.00,0.0 -1476,0,0,18,0.0,0.00,0.0 -1480,0,0,18,0.0,0.00,0.0 -1484,0,0,18,0.0,0.00,0.0 -1488,0,0,18,0.0,0.00,0.0 -1492,0,0,18,0.0,0.00,0.0 -1496,0,0,18,0.0,0.00,0.0 -1500,0,0,18,0.0,0.00,0.0 -1504,0,0,18,0.0,0.00,0.0 -1508,0,0,18,0.0,0.00,0.0 -1512,0,0,18,0.0,0.00,0.0 -1516,0,0,18,0.0,0.00,0.0 -1520,0,0,18,0.0,0.00,0.0 -1524,0,0,18,0.0,0.00,0.0 -1528,0,0,18,0.0,0.00,0.0 -1532,347,0.05,18,0.9,0.05,50.4 -1536,424,0.05,18,0.9,0.07,75.6 -1540,494,0.05,18,0.9,0.09,100.8 -1544,562,0.1,18,1.8,0.12,68.0 -1548,624,0.1,18,1.8,0.15,83.2 -1552,686,0.1,18,1.8,0.18,100.8 -1556,745,0.2,18,3.6,0.21,59.2 -1560,805,0.2,18,3.6,0.25,69.3 -1564,856,0.2,18,3.6,0.29,79.4 -1568,906,0.3,18,5.4,0.32,58.8 -1572,959,0.3,18,5.4,0.36,66.4 -1576,1009,0.4,18,7.2,0.39,54.2 -1580,1052,0.4,18,7.2,0.43,59.2 -1584,1099,0.5,18,9.0,0.47,52.4 -1588,1140,0.5,18,9.0,0.51,56.4 -1592,1184,0.6,18,10.8,0.55,50.8 -1596,1227,0.7,18,12.6,0.59,46.4 -1600,1267,0.8,18,14.4,0.63,43.5 -1604,1301,0.88,18,15.8,0.67,42.1 -1608,1341,1,18,18.0,0.70,38.8 -1612,1377,1,18,18.0,0.76,42.1 -1616,1419,1.2,18,21.6,0.80,37.2 -1620,1456,1.3,18,23.4,0.84,35.9 -1624,1493,1.38,18,24.8,0.88,35.4 -1628,1527,1.5,18,27.0,0.92,33.9 -1632,1564,1.6,18,28.8,0.97,33.5 -1636,1599,1.7,18,30.6,1.02,33.2 -1640,1637,1.83,18,32.9,1.08,32.6 -1644,1679,2,18,36.0,1.13,31.5 -1648,1716,2.1,18,37.8,1.18,31.2 -1652,1747,2.3,18,41.4,1.22,29.6 -1656,1783,2.47,18,44.5,1.28,28.8 -1660,1817,2.7,18,48.6,1.32,27.2 -1664,1851,2.9,18,52.2,1.38,26.5 -1668,1887,3.07,18,55.3,1.42,25.8 -1672,1920,3.24,18,58.3,1.49,25.5 -1676,1952,3.4,18,61.2,1.54,25.1 -1680,1989,3.6,18,64.8,1.60,24.6 -1684,2021,3.8,18,68.4,1.68,24.5 -1688,2066,4.07,18,73.3,1.74,23.8 -1692,2098,4.29,18,77.2,1.79,23.1 -1696,2143,4.52,18,81.4,1.84,22.6 -1700,2171,4.72,18,85.0,1.92,22.6 -1704,2198,4.98,18,89.6,1.97,22.0 -1708,2230,5.2,18,93.6,2.04,21.8 -1712,2265,5.47,18,98.5,2.08,21.1 -1716,2293,5.7,18,102.6,2.17,21.2 -1720,2330,5.97,18,107.5,2.22,20.6 -1724,2351,6.25,18,112.5,2.28,20.3 -1728,2402,6.55,18,117.9,2.36,20.0 -1732,2443,6.87,18,123.7,2.43,19.7 -1736,2474,7.28,18,131.0,2.50,19.1 -1740,2513,7.64,18,137.5,2.59,18.8 -1744,2546,8,18,144.0,2.64,18.4 -1748,2577,8.4,18,151.2,2.74,18.1 -1752,2602,8.8,18,158.4,2.79,17.6 -1756,2627,9.15,18,164.7,2.87,17.4 -1760,2670,9.48,18,170.6,2.93,17.1 -1764,2695,9.9,18,178.2,3.01,16.9 -1768,2731,10.2,18,183.6,3.10,16.9 -1772,2766,10.53,18,189.5,3.15,16.6 -1776,2809,10.81,18,194.6,3.21,16.5 -1780,2823,11.18,18,201.2,3.27,16.2 -1784,2850,11.58,18,208.4,3.39,16.3 -1788,2894,11.88,18,213.8,3.46,16.2 -1792,2914,12.29,18,221.2,3.54,16.0 -1796,2960,12.59,18,226.6,3.63,16.0 -1800,2985,13.12,18,236.2,3.72,15.8 -1804,3026,13.5,18,243.0,3.79,15.6 -1808,3055,14.04,18,252.7,3.92,15.5 -1812,3089,14.49,18,260.8,4.02,15.4 -1816,3124,15.09,18,271.6,4.14,15.2 -1820,3180,15.55,18,279.9,4.21,15.0 -1824,3222,16.14,18,290.5,4.32,14.9 -1828,3265,16.66,18,299.9,4.41,14.7 -1832,3283,17.32,18,311.8,4.54,14.5 -1836,3324,17.89,18,322.0,4.67,14.5 -1840,3375,18.45,18,332.1,4.69,14.1 -1844,3391,19.27,18,346.9,4.86,14.0 -1848,3417,19.72,18,355.0,4.92,13.9 -1852,3446,20.38,18,366.8,5.02,13.7 -1856,3482,21.03,18,378.5,5.12,13.5 -1860,3513,21.6,18,388.8,5.21,13.4 -1864,3542,22.29,18,401.2,5.29,13.2 -1868,3572,22.87,18,411.7,5.38,13.1 -1872,3597,23.55,18,423.9,5.50,13.0 -1876,3619,24.23,18,436.1,5.60,12.8 -1880,3637,24.77,18,445.9,5.67,12.7 -1884,3653,25.48,18,458.6,5.77,12.6 -1888,3687,26.04,18,468.7,5.82,12.4 -1892,3711,26.74,18,481.3,5.90,12.3 -1896,3751,27.22,18,490.0,5.98,12.2 -1900,3742,27.57,18,496.3,6.02,12.1 + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) +1100,3643,28.22,18,507.9,-4.59,9.0 +1104,3643,27.91,18,502.4,-4.54,9.0 +1108,3624,27.31,18,491.6,-4.53,9.2 +1112,3607,26.62,18,479.2,-4.48,9.3 +1116,3578,26.04,18,468.7,-4.42,9.4 +1120,3563,25.38,18,456.8,-4.35,9.5 +1124,3528,24.8,18,446.4,-4.35,9.7 +1128,3520,24.03,18,432.5,-4.17,9.6 +1132,3483,23.53,18,423.5,-4.18,9.9 +1136,3470,22.78,18,410.0,-4.11,10.0 +1140,3441,22.21,18,399.8,-4.04,10.1 +1144,3422,21.5,18,387.0,-3.99,10.3 +1148,3420,20.61,18,371.0,-3.91,10.6 +1152,3375,20.17,18,363.1,-3.86,10.6 +1156,3343,19.51,18,351.2,-3.81,10.8 +1160,3316,18.85,18,339.3,-3.71,10.9 +1164,3273,18.23,18,328.1,-3.68,11.2 +1168,3248,17.57,18,316.3,-3.56,11.2 +1172,3195,17.12,18,308.2,-3.54,11.5 +1176,3193,16.34,18,294.1,-3.42,11.6 +1180,3153,15.8,18,284.4,-3.36,11.8 +1184,3127,15.19,18,273.4,-3.27,11.9 +1188,3087,14.69,18,264.4,-3.19,12.1 +1192,3057,14.16,18,254.9,-3.09,12.1 +1196,3022,13.68,18,246.2,-3.03,12.3 +1200,2985,13.27,18,238.9,-2.96,12.4 +1204,2958,12.83,18,230.9,-2.88,12.5 +1208,2925,12.36,18,222.5,-2.84,12.8 +1212,2896,11.99,18,215.8,-2.78,12.9 +1216,2866,11.58,18,208.4,-2.70,12.9 +1220,2829,11.32,18,203.8,-2.66,13.0 +1224,2806,11,18,198.0,-2.59,13.1 +1228,2785,10.65,18,191.7,-2.53,13.2 +1232,2756,10.27,18,184.9,-2.48,13.4 +1236,2720,9.97,18,179.5,-2.42,13.5 +1240,2695,9.55,18,171.9,-2.36,13.7 +1244,2673,9.2,18,165.6,-2.32,14.0 +1248,2638,8.87,18,159.7,-2.25,14.1 +1252,2603,8.54,18,153.7,-2.21,14.4 +1256,2572,8.19,18,147.4,-2.16,14.6 +1260,2549,7.77,18,139.9,-2.11,15.1 +1264,2514,7.39,18,133.0,-2.04,15.3 +1268,2488,7,18,126.0,-2.00,15.8 +1272,2444,6.65,18,119.7,-1.94,16.2 +1276,2411,6.3,18,113.4,-1.88,16.6 +1280,2379,6,18,108.0,-1.81,16.8 +1284,2343,5.75,18,103.5,-1.76,17.0 +1288,2310,5.5,18,99.0,-1.72,17.4 +1292,2283,5.23,18,94.1,-1.66,17.6 +1296,2247,5,18,90.0,-1.61,17.9 +1300,2218,4.79,18,86.2,-1.56,18.1 +1304,2178,4.59,18,82.6,-1.52,18.4 +1308,2155,4.3,18,77.4,-1.47,18.9 +1312,2123,4.1,18,73.8,-1.42,19.3 +1316,2089,3.9,18,70.2,-1.38,19.6 +1320,2046,3.68,18,66.2,-1.33,20.1 +1324,2013,3.4,18,61.2,-1.27,20.8 +1328,1974,3.3,18,59.4,-1.22,20.6 +1332,1937,3.1,18,55.8,-1.17,21.1 +1336,1896,2.9,18,52.2,-1.13,21.7 +1340,1871,2.73,18,49.1,-1.08,22.1 +1344,1832,2.5,18,45.0,-1.05,23.3 +1348,1796,2.3,18,41.4,-1.01,24.4 +1352,1765,2.14,18,38.5,-0.97,25.2 +1356,1733,2,18,36.0,-0.93,26.0 +1360,1697,1.9,18,34.2,-0.89,26.1 +1364,1659,1.76,18,31.7,-0.86,27.1 +1368,1611,1.6,18,28.8,-0.81,28.2 +1372,1575,1.5,18,27.0,-0.77,28.6 +1376,1539,1.4,18,25.2,-0.74,29.3 +1380,1507,1.3,18,23.4,-0.70,29.9 +1384,1469,1.2,18,21.6,-0.67,31.1 +1388,1428,1.1,18,19.8,-0.64,32.1 +1392,1390,1,18,18.0,-0.60,33.3 +1396,1344,0.8,18,14.4,-0.56,38.7 +1400,1308,0.8,18,14.4,-0.53,36.9 +1404,1270,0.7,18,12.6,-0.49,39.2 +1408,1229,0.63,18,11.3,-0.46,40.8 +1412,1178,0.53,18,9.5,-0.43,45.2 +1416,1136,0.5,18,9.0,-0.40,44.4 +1420,1090,0.4,18,7.2,-0.37,51.0 +1424,1039,0.4,18,7.2,-0.34,46.6 +1428,989,0.3,18,5.4,-0.30,56.3 +1432,935,0.3,18,5.4,-0.27,50.4 +1436,884,0.2,18,3.6,-0.24,66.8 +1440,823,0.2,18,3.6,-0.21,58.0 +1444,760,0.2,18,3.6,-0.18,50.4 +1448,703,0.1,18,1.8,-0.15,85.7 +1452,638,0.1,18,1.8,-0.13,70.6 +1456,575,0.1,18,1.8,-0.10,58.0 +1460,501,0.05,18,0.9,-0.08,85.7 +1464,428,0.05,18,0.9,-0.06,65.5 +1468,357,0.05,18,0.9,-0.05,50.4 +1472,0,0,18,0.0,0.00,0.0 +1476,0,0,18,0.0,0.00,0.0 +1480,0,0,18,0.0,0.00,0.0 +1484,0,0,18,0.0,0.00,0.0 +1488,0,0,18,0.0,0.00,0.0 +1492,0,0,18,0.0,0.00,0.0 +1496,0,0,18,0.0,0.00,0.0 +1500,0,0,18,0.0,0.00,0.0 +1504,0,0,18,0.0,0.00,0.0 +1508,0,0,18,0.0,0.00,0.0 +1512,0,0,18,0.0,0.00,0.0 +1516,0,0,18,0.0,0.00,0.0 +1520,0,0,18,0.0,0.00,0.0 +1524,0,0,18,0.0,0.00,0.0 +1528,0,0,18,0.0,0.00,0.0 +1532,347,0.05,18,0.9,0.05,50.4 +1536,424,0.05,18,0.9,0.07,75.6 +1540,494,0.05,18,0.9,0.09,100.8 +1544,562,0.1,18,1.8,0.12,68.0 +1548,624,0.1,18,1.8,0.15,83.2 +1552,686,0.1,18,1.8,0.18,100.8 +1556,745,0.2,18,3.6,0.21,59.2 +1560,805,0.2,18,3.6,0.25,69.3 +1564,856,0.2,18,3.6,0.29,79.4 +1568,906,0.3,18,5.4,0.32,58.8 +1572,959,0.3,18,5.4,0.36,66.4 +1576,1009,0.4,18,7.2,0.39,54.2 +1580,1052,0.4,18,7.2,0.43,59.2 +1584,1099,0.5,18,9.0,0.47,52.4 +1588,1140,0.5,18,9.0,0.51,56.4 +1592,1184,0.6,18,10.8,0.55,50.8 +1596,1227,0.7,18,12.6,0.59,46.4 +1600,1267,0.8,18,14.4,0.63,43.5 +1604,1301,0.88,18,15.8,0.67,42.1 +1608,1341,1,18,18.0,0.70,38.8 +1612,1377,1,18,18.0,0.76,42.1 +1616,1419,1.2,18,21.6,0.80,37.2 +1620,1456,1.3,18,23.4,0.84,35.9 +1624,1493,1.38,18,24.8,0.88,35.4 +1628,1527,1.5,18,27.0,0.92,33.9 +1632,1564,1.6,18,28.8,0.97,33.5 +1636,1599,1.7,18,30.6,1.02,33.2 +1640,1637,1.83,18,32.9,1.08,32.6 +1644,1679,2,18,36.0,1.13,31.5 +1648,1716,2.1,18,37.8,1.18,31.2 +1652,1747,2.3,18,41.4,1.22,29.6 +1656,1783,2.47,18,44.5,1.28,28.8 +1660,1817,2.7,18,48.6,1.32,27.2 +1664,1851,2.9,18,52.2,1.38,26.5 +1668,1887,3.07,18,55.3,1.42,25.8 +1672,1920,3.24,18,58.3,1.49,25.5 +1676,1952,3.4,18,61.2,1.54,25.1 +1680,1989,3.6,18,64.8,1.60,24.6 +1684,2021,3.8,18,68.4,1.68,24.5 +1688,2066,4.07,18,73.3,1.74,23.8 +1692,2098,4.29,18,77.2,1.79,23.1 +1696,2143,4.52,18,81.4,1.84,22.6 +1700,2171,4.72,18,85.0,1.92,22.6 +1704,2198,4.98,18,89.6,1.97,22.0 +1708,2230,5.2,18,93.6,2.04,21.8 +1712,2265,5.47,18,98.5,2.08,21.1 +1716,2293,5.7,18,102.6,2.17,21.2 +1720,2330,5.97,18,107.5,2.22,20.6 +1724,2351,6.25,18,112.5,2.28,20.3 +1728,2402,6.55,18,117.9,2.36,20.0 +1732,2443,6.87,18,123.7,2.43,19.7 +1736,2474,7.28,18,131.0,2.50,19.1 +1740,2513,7.64,18,137.5,2.59,18.8 +1744,2546,8,18,144.0,2.64,18.4 +1748,2577,8.4,18,151.2,2.74,18.1 +1752,2602,8.8,18,158.4,2.79,17.6 +1756,2627,9.15,18,164.7,2.87,17.4 +1760,2670,9.48,18,170.6,2.93,17.1 +1764,2695,9.9,18,178.2,3.01,16.9 +1768,2731,10.2,18,183.6,3.10,16.9 +1772,2766,10.53,18,189.5,3.15,16.6 +1776,2809,10.81,18,194.6,3.21,16.5 +1780,2823,11.18,18,201.2,3.27,16.2 +1784,2850,11.58,18,208.4,3.39,16.3 +1788,2894,11.88,18,213.8,3.46,16.2 +1792,2914,12.29,18,221.2,3.54,16.0 +1796,2960,12.59,18,226.6,3.63,16.0 +1800,2985,13.12,18,236.2,3.72,15.8 +1804,3026,13.5,18,243.0,3.79,15.6 +1808,3055,14.04,18,252.7,3.92,15.5 +1812,3089,14.49,18,260.8,4.02,15.4 +1816,3124,15.09,18,271.6,4.14,15.2 +1820,3180,15.55,18,279.9,4.21,15.0 +1824,3222,16.14,18,290.5,4.32,14.9 +1828,3265,16.66,18,299.9,4.41,14.7 +1832,3283,17.32,18,311.8,4.54,14.5 +1836,3324,17.89,18,322.0,4.67,14.5 +1840,3375,18.45,18,332.1,4.69,14.1 +1844,3391,19.27,18,346.9,4.86,14.0 +1848,3417,19.72,18,355.0,4.92,13.9 +1852,3446,20.38,18,366.8,5.02,13.7 +1856,3482,21.03,18,378.5,5.12,13.5 +1860,3513,21.6,18,388.8,5.21,13.4 +1864,3542,22.29,18,401.2,5.29,13.2 +1868,3572,22.87,18,411.7,5.38,13.1 +1872,3597,23.55,18,423.9,5.50,13.0 +1876,3619,24.23,18,436.1,5.60,12.8 +1880,3637,24.77,18,445.9,5.67,12.7 +1884,3653,25.48,18,458.6,5.77,12.6 +1888,3687,26.04,18,468.7,5.82,12.4 +1892,3711,26.74,18,481.3,5.90,12.3 +1896,3751,27.22,18,490.0,5.98,12.2 +1900,3742,27.57,18,496.3,6.02,12.1 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv index cc3112f14..165881815 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv @@ -1,202 +1,202 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, -1100,3783,32.17,20,643.5,-5.04,7.8,, -1104,3777,31.37,20,627.4,-4.96,7.9,, -1108,3740,30.6,20,612.0,-4.92,8.0,, -1112,3741,29.67,20,593.4,-4.84,8.2,, -1116,3712,29.04,20,580.8,-4.79,8.3,, -1120,3681,28.26,20,565.2,-4.74,8.4,, -1124,3639,27.56,20,551.2,-4.68,8.5,, -1128,3642,26.74,20,534.8,-4.58,8.6,, -1132,3624,25.82,20,516.4,-4.46,8.6,, -1136,3569,25.16,20,503.2,-4.40,8.8,, -1140,3558,24.45,20,489.0,-4.35,8.9,, -1144,3571,23.54,20,470.8,-4.23,9.0,, -1148,3498,23.08,20,461.6,-4.16,9.0,, -1152,3449,22.46,20,449.2,-4.12,9.2,, -1156,3457,21.63,20,432.6,-4.02,9.3,, -1160,3390,21,20,420.0,-3.96,9.4,, -1164,3385,20.3,20,406.0,-3.89,9.6,, -1168,3335,19.65,20,393.0,-3.81,9.7,, -1172,3315,19,20,380.0,-3.71,9.8,, -1176,3293,18.36,20,367.2,-3.66,10.0,, -1180,3250,17.75,20,355.0,-3.56,10.0,, -1184,3211,17.11,20,342.2,-3.48,10.2,, -1188,3184,16.43,20,328.6,-3.36,10.2,, -1192,3139,15.81,20,316.2,-3.29,10.4,, -1196,3107,15.23,20,304.6,-3.16,10.4,, -1200,3046,14.74,20,294.8,-3.13,10.6,, -1204,3034,14.12,20,282.4,-3.00,10.6,, -1208,2983,13.66,20,273.2,-2.94,10.8,, -1212,2952,13.22,20,264.4,-2.87,10.9,, -1216,2927,12.69,20,253.8,-2.78,10.9,, -1220,2891,12.36,20,247.2,-2.74,11.1,, -1224,2856,12.01,20,240.2,-2.67,11.1,, -1228,2824,11.57,20,231.4,-2.60,11.3,, -1232,2797,11.27,20,225.4,-2.54,11.3,, -1236,2766,10.84,20,216.8,-2.50,11.5,, -1240,2731,10.54,20,210.8,-2.44,11.6,, -1244,2698,10.22,20,204.4,-2.40,11.8,, -1248,2679,9.77,20,195.4,-2.31,11.8,, -1252,2636,9.4,20,188.0,-2.26,12.0,, -1256,2623,8.96,20,179.2,-2.20,12.3,, -1260,2579,8.6,20,172.0,-2.14,12.4,, -1264,2554,8.29,20,165.8,-2.11,12.7,, -1268,2523,7.87,20,157.4,-2.04,13.0,, -1272,2487,7.48,20,149.6,-1.97,13.2,, -1276,2453,7.03,20,140.6,-1.91,13.6,, -1280,2414,6.6,20,132.0,-1.86,14.1,, -1284,2367,6.3,20,126.0,-1.80,14.3,, -1288,2346,5.97,20,119.4,-1.73,14.5,, -1292,2300,5.7,20,114.0,-1.67,14.7,, -1296,2265,5.41,20,108.2,-1.64,15.1,, -1300,2229,5.16,20,103.2,-1.58,15.3,, -1304,2192,4.88,20,97.6,-1.52,15.6,, -1308,2166,4.6,20,92.0,-1.47,16.0,, -1312,2135,4.31,20,86.2,-1.41,16.4,, -1316,2093,4.09,20,81.8,-1.37,16.8,, -1320,2050,3.81,20,76.2,-1.32,17.4,, -1324,2009,3.58,20,71.6,-1.27,17.7,, -1328,1972,3.32,20,66.4,-1.22,18.4,, -1332,1927,3.09,20,61.8,-1.17,18.9,, -1336,1895,2.9,20,58.0,-1.11,19.1,, -1340,1853,2.71,20,54.2,-1.07,19.7,, -1344,1816,2.46,20,49.2,-1.02,20.7,, -1348,1779,2.3,20,46.0,-0.99,21.5,, -1352,1748,2.1,20,42.0,-0.94,22.5,, -1356,1718,2,20,40.0,-0.91,22.7,, -1360,1676,1.84,20,36.8,-0.88,23.8,, -1364,1643,1.7,20,34.0,-0.83,24.4,, -1368,1597,1.59,20,31.8,-0.78,24.5,, -1372,1560,1.5,20,30.0,-0.75,25.1,, -1376,1526,1.3,20,26.0,-0.72,27.6,, -1380,1493,1.29,20,25.8,-0.69,26.7,, -1384,1461,1.2,20,24.0,-0.66,27.4,, -1388,1426,1.05,20,21.0,-0.63,30.0,, -1392,1388,1,20,20.0,-0.59,29.5,, -1396,1349,0.88,20,17.6,-0.56,31.7,, -1400,1315,0.8,20,16.0,-0.53,33.2,, -1404,1277,0.7,20,14.0,-0.50,36.0,, -1408,1246,0.7,20,14.0,-0.47,33.7,, -1412,1201,0.6,20,12.0,-0.44,37.0,, -1416,1159,0.5,20,10.0,-0.41,41.3,, -1420,1120,0.5,20,10.0,-0.39,38.6,, -1424,1077,0.4,20,8.0,-0.36,44.8,, -1428,1024,0.4,20,8.0,-0.32,40.3,, -1432,982,0.3,20,6.0,-0.29,49.1,, -1436,925,0.2,20,4.0,-0.26,65.8,, -1440,876,0.2,20,4.0,-0.24,59.0,, -1444,813,0.2,20,4.0,-0.20,51.0,, -1448,752,0.1,20,2.0,-0.17,86.2,, -1452,688,0.1,20,2.0,-0.15,72.6,, -1456,624,0.1,20,2.0,-0.12,61.2,, -1460,547,0.05,20,1.0,-0.09,90.7,, -1464,471,0.05,20,1.0,-0.07,72.6,, -1468,389,0.05,20,1.0,-0.05,54.4,, -1472,310,0.05,20,1.0,-0.04,36.3,, -1476,0,0,20,0.0,0.00,0.0,, -1480,0,0,20,0.0,0.00,0.0,, -1484,0,0,20,0.0,0.00,0.0,, -1488,0,0,20,0.0,0.00,0.0,, -1492,0,0,20,0.0,0.00,0.0,, -1496,0,0,20,0.0,0.00,0.0,, -1500,0,0,20,0.0,0.00,0.0,, -1504,0,0,20,0.0,0.00,0.0,, -1508,0,0,20,0.0,0.00,0.0,, -1512,0,0,20,0.0,0.00,0.0,, -1516,0,0,20,0.0,0.00,0.0,, -1520,0,0,20,0.0,0.00,0.0,, -1524,0,0,20,0.0,0.00,0.0,, -1528,0,0,20,0.0,0.00,0.0,, -1532,379,0.05,20,1.0,0.06,59.0,, -1536,457,0.05,20,1.0,0.09,86.2,, -1540,532,0.05,20,1.0,0.11,113.4,, -1544,605,0.1,20,2.0,0.15,74.8,, -1548,676,0.1,20,2.0,0.18,90.7,, -1552,735,0.1,20,2.0,0.21,106.6,, -1556,797,0.2,20,4.0,0.25,62.4,, -1560,857,0.2,20,4.0,0.29,71.4,, -1564,905,0.2,20,4.0,0.32,80.5,, -1568,963,0.3,20,6.0,0.36,60.5,, -1572,1001,0.4,20,8.0,0.39,48.8,, -1576,1046,0.4,20,8.0,0.43,53.3,, -1580,1092,0.5,20,10.0,0.47,46.7,, -1584,1129,0.5,20,10.0,0.50,49.9,, -1588,1162,0.6,20,12.0,0.54,44.6,, -1592,1207,0.7,20,14.0,0.57,40.8,, -1596,1240,0.7,20,14.0,0.61,43.4,, -1600,1273,0.8,20,16.0,0.64,40.0,, -1604,1305,0.8,20,16.0,0.66,41.4,, -1608,1334,0.96,20,19.2,0.70,36.4,, -1612,1367,1,20,20.0,0.74,37.0,, -1616,1407,1.1,20,22.0,0.78,35.5,, -1620,1441,1.2,20,24.0,0.82,34.0,, -1624,1472,1.3,20,26.0,0.86,33.1,, -1628,1506,1.4,20,28.0,0.89,31.8,, -1632,1537,1.51,20,30.2,0.93,30.9,, -1636,1571,1.6,20,32.0,0.98,30.5,, -1640,1603,1.79,20,35.8,1.02,28.5,, -1644,1644,1.9,20,38.0,1.06,27.9,, -1648,1685,2.06,20,41.2,1.12,27.2,, -1652,1720,2.2,20,44.0,1.17,26.6,, -1656,1751,2.37,20,47.4,1.22,25.7,, -1660,1797,2.63,20,52.6,1.28,24.3,, -1664,1832,2.82,20,56.4,1.31,23.2,, -1668,1868,3.06,20,61.2,1.38,22.5,, -1672,1911,3.28,20,65.6,1.44,21.9,, -1676,1950,3.49,20,69.8,1.53,21.9,, -1680,1991,3.72,20,74.4,1.58,21.3,, -1684,2040,3.98,20,79.6,1.64,20.6,, -1688,2072,4.26,20,85.2,1.74,20.4,, -1692,2110,4.59,20,91.8,1.80,19.6,, -1696,2145,4.83,20,96.6,1.85,19.1,, -1700,2186,5.08,20,101.6,1.92,18.9,, -1704,2218,5.39,20,107.8,1.98,18.4,, -1708,2242,5.66,20,113.2,2.07,18.3,, -1712,2292,5.9,20,118.0,2.12,18.0,, -1716,2321,6.2,20,124.0,2.20,17.7,, -1720,2371,6.5,20,130.0,2.25,17.3,, -1724,2419,6.87,20,137.4,2.33,16.9,, -1728,2452,7.3,20,146.0,2.44,16.7,, -1732,2493,7.72,20,154.4,2.52,16.3,, -1736,2520,8.14,20,162.8,2.61,16.0,, -1740,2552,8.53,20,170.6,2.66,15.6,, -1744,2593,8.92,20,178.4,2.76,15.5,, -1748,2621,9.29,20,185.8,2.83,15.2,, -1752,2654,9.7,20,194.0,2.91,15.0,, -1756,2696,10,20,200.0,2.99,15.0,, -1760,2723,10.43,20,208.6,3.06,14.7,, -1764,2752,10.77,20,215.4,3.13,14.6,, -1768,2788,11.12,20,222.4,3.22,14.5,, -1772,2820,11.47,20,229.4,3.28,14.3,, -1776,2861,11.78,20,235.6,3.36,14.2,, -1780,2887,12.23,20,244.6,3.44,14.1,, -1784,2928,12.62,20,252.4,3.55,14.1,, -1788,2968,13,20,260.0,3.64,14.0,, -1792,3013,13.47,20,269.4,3.72,13.8,, -1796,3039,14,20,280.0,3.85,13.7,, -1800,3088,14.46,20,289.2,3.96,13.7,, -1804,3122,15.07,20,301.4,4.10,13.6,, -1808,3182,15.64,20,312.8,4.21,13.5,, -1812,3221,16.27,20,325.4,4.33,13.3,, -1816,3268,16.83,20,336.6,4.42,13.1,, -1820,3304,17.51,20,350.2,4.55,13.0,, -1824,3341,18.07,20,361.4,4.70,13.0,, -1828,3380,18.69,20,373.8,4.82,12.9,, -1832,3418,19.3,20,386.0,4.93,12.8,, -1836,3453,19.98,20,399.6,5.02,12.6,, -1840,3481,20.63,20,412.6,5.14,12.5,, -1844,3519,21.21,20,424.2,5.20,12.3,, -1848,3561,21.88,20,437.6,5.39,12.3,, -1852,3593,22.49,20,449.8,5.52,12.3,, -1856,3614,23.2,20,464.0,5.62,12.1,, -1860,3660,23.8,20,476.0,5.71,12.0,, -1864,3684,24.55,20,491.0,5.82,11.8,, -1868,3721,25.19,20,503.8,5.93,11.8,, -1872,3732,25.95,20,519.0,6.06,11.7,, -1876,3778,26.62,20,532.4,6.13,11.5,, -1880,3801,27.55,20,551.0,6.26,11.4,, -1884,3837,28.12,20,562.4,6.36,11.3,, -1888,3855,28.95,20,579.0,6.46,11.2,, -1892,3877,29.74,20,594.8,6.55,11.0,, -1896,3907,30.46,20,609.2,6.62,10.9,, -1900,3931,31.21,20,624.2,6.72,10.8,, + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, +1100,3783,32.17,20,643.5,-5.04,7.8,, +1104,3777,31.37,20,627.4,-4.96,7.9,, +1108,3740,30.6,20,612.0,-4.92,8.0,, +1112,3741,29.67,20,593.4,-4.84,8.2,, +1116,3712,29.04,20,580.8,-4.79,8.3,, +1120,3681,28.26,20,565.2,-4.74,8.4,, +1124,3639,27.56,20,551.2,-4.68,8.5,, +1128,3642,26.74,20,534.8,-4.58,8.6,, +1132,3624,25.82,20,516.4,-4.46,8.6,, +1136,3569,25.16,20,503.2,-4.40,8.8,, +1140,3558,24.45,20,489.0,-4.35,8.9,, +1144,3571,23.54,20,470.8,-4.23,9.0,, +1148,3498,23.08,20,461.6,-4.16,9.0,, +1152,3449,22.46,20,449.2,-4.12,9.2,, +1156,3457,21.63,20,432.6,-4.02,9.3,, +1160,3390,21,20,420.0,-3.96,9.4,, +1164,3385,20.3,20,406.0,-3.89,9.6,, +1168,3335,19.65,20,393.0,-3.81,9.7,, +1172,3315,19,20,380.0,-3.71,9.8,, +1176,3293,18.36,20,367.2,-3.66,10.0,, +1180,3250,17.75,20,355.0,-3.56,10.0,, +1184,3211,17.11,20,342.2,-3.48,10.2,, +1188,3184,16.43,20,328.6,-3.36,10.2,, +1192,3139,15.81,20,316.2,-3.29,10.4,, +1196,3107,15.23,20,304.6,-3.16,10.4,, +1200,3046,14.74,20,294.8,-3.13,10.6,, +1204,3034,14.12,20,282.4,-3.00,10.6,, +1208,2983,13.66,20,273.2,-2.94,10.8,, +1212,2952,13.22,20,264.4,-2.87,10.9,, +1216,2927,12.69,20,253.8,-2.78,10.9,, +1220,2891,12.36,20,247.2,-2.74,11.1,, +1224,2856,12.01,20,240.2,-2.67,11.1,, +1228,2824,11.57,20,231.4,-2.60,11.3,, +1232,2797,11.27,20,225.4,-2.54,11.3,, +1236,2766,10.84,20,216.8,-2.50,11.5,, +1240,2731,10.54,20,210.8,-2.44,11.6,, +1244,2698,10.22,20,204.4,-2.40,11.8,, +1248,2679,9.77,20,195.4,-2.31,11.8,, +1252,2636,9.4,20,188.0,-2.26,12.0,, +1256,2623,8.96,20,179.2,-2.20,12.3,, +1260,2579,8.6,20,172.0,-2.14,12.4,, +1264,2554,8.29,20,165.8,-2.11,12.7,, +1268,2523,7.87,20,157.4,-2.04,13.0,, +1272,2487,7.48,20,149.6,-1.97,13.2,, +1276,2453,7.03,20,140.6,-1.91,13.6,, +1280,2414,6.6,20,132.0,-1.86,14.1,, +1284,2367,6.3,20,126.0,-1.80,14.3,, +1288,2346,5.97,20,119.4,-1.73,14.5,, +1292,2300,5.7,20,114.0,-1.67,14.7,, +1296,2265,5.41,20,108.2,-1.64,15.1,, +1300,2229,5.16,20,103.2,-1.58,15.3,, +1304,2192,4.88,20,97.6,-1.52,15.6,, +1308,2166,4.6,20,92.0,-1.47,16.0,, +1312,2135,4.31,20,86.2,-1.41,16.4,, +1316,2093,4.09,20,81.8,-1.37,16.8,, +1320,2050,3.81,20,76.2,-1.32,17.4,, +1324,2009,3.58,20,71.6,-1.27,17.7,, +1328,1972,3.32,20,66.4,-1.22,18.4,, +1332,1927,3.09,20,61.8,-1.17,18.9,, +1336,1895,2.9,20,58.0,-1.11,19.1,, +1340,1853,2.71,20,54.2,-1.07,19.7,, +1344,1816,2.46,20,49.2,-1.02,20.7,, +1348,1779,2.3,20,46.0,-0.99,21.5,, +1352,1748,2.1,20,42.0,-0.94,22.5,, +1356,1718,2,20,40.0,-0.91,22.7,, +1360,1676,1.84,20,36.8,-0.88,23.8,, +1364,1643,1.7,20,34.0,-0.83,24.4,, +1368,1597,1.59,20,31.8,-0.78,24.5,, +1372,1560,1.5,20,30.0,-0.75,25.1,, +1376,1526,1.3,20,26.0,-0.72,27.6,, +1380,1493,1.29,20,25.8,-0.69,26.7,, +1384,1461,1.2,20,24.0,-0.66,27.4,, +1388,1426,1.05,20,21.0,-0.63,30.0,, +1392,1388,1,20,20.0,-0.59,29.5,, +1396,1349,0.88,20,17.6,-0.56,31.7,, +1400,1315,0.8,20,16.0,-0.53,33.2,, +1404,1277,0.7,20,14.0,-0.50,36.0,, +1408,1246,0.7,20,14.0,-0.47,33.7,, +1412,1201,0.6,20,12.0,-0.44,37.0,, +1416,1159,0.5,20,10.0,-0.41,41.3,, +1420,1120,0.5,20,10.0,-0.39,38.6,, +1424,1077,0.4,20,8.0,-0.36,44.8,, +1428,1024,0.4,20,8.0,-0.32,40.3,, +1432,982,0.3,20,6.0,-0.29,49.1,, +1436,925,0.2,20,4.0,-0.26,65.8,, +1440,876,0.2,20,4.0,-0.24,59.0,, +1444,813,0.2,20,4.0,-0.20,51.0,, +1448,752,0.1,20,2.0,-0.17,86.2,, +1452,688,0.1,20,2.0,-0.15,72.6,, +1456,624,0.1,20,2.0,-0.12,61.2,, +1460,547,0.05,20,1.0,-0.09,90.7,, +1464,471,0.05,20,1.0,-0.07,72.6,, +1468,389,0.05,20,1.0,-0.05,54.4,, +1472,310,0.05,20,1.0,-0.04,36.3,, +1476,0,0,20,0.0,0.00,0.0,, +1480,0,0,20,0.0,0.00,0.0,, +1484,0,0,20,0.0,0.00,0.0,, +1488,0,0,20,0.0,0.00,0.0,, +1492,0,0,20,0.0,0.00,0.0,, +1496,0,0,20,0.0,0.00,0.0,, +1500,0,0,20,0.0,0.00,0.0,, +1504,0,0,20,0.0,0.00,0.0,, +1508,0,0,20,0.0,0.00,0.0,, +1512,0,0,20,0.0,0.00,0.0,, +1516,0,0,20,0.0,0.00,0.0,, +1520,0,0,20,0.0,0.00,0.0,, +1524,0,0,20,0.0,0.00,0.0,, +1528,0,0,20,0.0,0.00,0.0,, +1532,379,0.05,20,1.0,0.06,59.0,, +1536,457,0.05,20,1.0,0.09,86.2,, +1540,532,0.05,20,1.0,0.11,113.4,, +1544,605,0.1,20,2.0,0.15,74.8,, +1548,676,0.1,20,2.0,0.18,90.7,, +1552,735,0.1,20,2.0,0.21,106.6,, +1556,797,0.2,20,4.0,0.25,62.4,, +1560,857,0.2,20,4.0,0.29,71.4,, +1564,905,0.2,20,4.0,0.32,80.5,, +1568,963,0.3,20,6.0,0.36,60.5,, +1572,1001,0.4,20,8.0,0.39,48.8,, +1576,1046,0.4,20,8.0,0.43,53.3,, +1580,1092,0.5,20,10.0,0.47,46.7,, +1584,1129,0.5,20,10.0,0.50,49.9,, +1588,1162,0.6,20,12.0,0.54,44.6,, +1592,1207,0.7,20,14.0,0.57,40.8,, +1596,1240,0.7,20,14.0,0.61,43.4,, +1600,1273,0.8,20,16.0,0.64,40.0,, +1604,1305,0.8,20,16.0,0.66,41.4,, +1608,1334,0.96,20,19.2,0.70,36.4,, +1612,1367,1,20,20.0,0.74,37.0,, +1616,1407,1.1,20,22.0,0.78,35.5,, +1620,1441,1.2,20,24.0,0.82,34.0,, +1624,1472,1.3,20,26.0,0.86,33.1,, +1628,1506,1.4,20,28.0,0.89,31.8,, +1632,1537,1.51,20,30.2,0.93,30.9,, +1636,1571,1.6,20,32.0,0.98,30.5,, +1640,1603,1.79,20,35.8,1.02,28.5,, +1644,1644,1.9,20,38.0,1.06,27.9,, +1648,1685,2.06,20,41.2,1.12,27.2,, +1652,1720,2.2,20,44.0,1.17,26.6,, +1656,1751,2.37,20,47.4,1.22,25.7,, +1660,1797,2.63,20,52.6,1.28,24.3,, +1664,1832,2.82,20,56.4,1.31,23.2,, +1668,1868,3.06,20,61.2,1.38,22.5,, +1672,1911,3.28,20,65.6,1.44,21.9,, +1676,1950,3.49,20,69.8,1.53,21.9,, +1680,1991,3.72,20,74.4,1.58,21.3,, +1684,2040,3.98,20,79.6,1.64,20.6,, +1688,2072,4.26,20,85.2,1.74,20.4,, +1692,2110,4.59,20,91.8,1.80,19.6,, +1696,2145,4.83,20,96.6,1.85,19.1,, +1700,2186,5.08,20,101.6,1.92,18.9,, +1704,2218,5.39,20,107.8,1.98,18.4,, +1708,2242,5.66,20,113.2,2.07,18.3,, +1712,2292,5.9,20,118.0,2.12,18.0,, +1716,2321,6.2,20,124.0,2.20,17.7,, +1720,2371,6.5,20,130.0,2.25,17.3,, +1724,2419,6.87,20,137.4,2.33,16.9,, +1728,2452,7.3,20,146.0,2.44,16.7,, +1732,2493,7.72,20,154.4,2.52,16.3,, +1736,2520,8.14,20,162.8,2.61,16.0,, +1740,2552,8.53,20,170.6,2.66,15.6,, +1744,2593,8.92,20,178.4,2.76,15.5,, +1748,2621,9.29,20,185.8,2.83,15.2,, +1752,2654,9.7,20,194.0,2.91,15.0,, +1756,2696,10,20,200.0,2.99,15.0,, +1760,2723,10.43,20,208.6,3.06,14.7,, +1764,2752,10.77,20,215.4,3.13,14.6,, +1768,2788,11.12,20,222.4,3.22,14.5,, +1772,2820,11.47,20,229.4,3.28,14.3,, +1776,2861,11.78,20,235.6,3.36,14.2,, +1780,2887,12.23,20,244.6,3.44,14.1,, +1784,2928,12.62,20,252.4,3.55,14.1,, +1788,2968,13,20,260.0,3.64,14.0,, +1792,3013,13.47,20,269.4,3.72,13.8,, +1796,3039,14,20,280.0,3.85,13.7,, +1800,3088,14.46,20,289.2,3.96,13.7,, +1804,3122,15.07,20,301.4,4.10,13.6,, +1808,3182,15.64,20,312.8,4.21,13.5,, +1812,3221,16.27,20,325.4,4.33,13.3,, +1816,3268,16.83,20,336.6,4.42,13.1,, +1820,3304,17.51,20,350.2,4.55,13.0,, +1824,3341,18.07,20,361.4,4.70,13.0,, +1828,3380,18.69,20,373.8,4.82,12.9,, +1832,3418,19.3,20,386.0,4.93,12.8,, +1836,3453,19.98,20,399.6,5.02,12.6,, +1840,3481,20.63,20,412.6,5.14,12.5,, +1844,3519,21.21,20,424.2,5.20,12.3,, +1848,3561,21.88,20,437.6,5.39,12.3,, +1852,3593,22.49,20,449.8,5.52,12.3,, +1856,3614,23.2,20,464.0,5.62,12.1,, +1860,3660,23.8,20,476.0,5.71,12.0,, +1864,3684,24.55,20,491.0,5.82,11.8,, +1868,3721,25.19,20,503.8,5.93,11.8,, +1872,3732,25.95,20,519.0,6.06,11.7,, +1876,3778,26.62,20,532.4,6.13,11.5,, +1880,3801,27.55,20,551.0,6.26,11.4,, +1884,3837,28.12,20,562.4,6.36,11.3,, +1888,3855,28.95,20,579.0,6.46,11.2,, +1892,3877,29.74,20,594.8,6.55,11.0,, +1896,3907,30.46,20,609.2,6.62,10.9,, +1900,3931,31.21,20,624.2,6.72,10.8,, diff --git a/motion/thruster_interface_auv/resources/__init__.py b/motion/thruster_interface_auv/resources/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp similarity index 54% rename from motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp rename to motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 8d068ca68..4a21bc428 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,44 +1,60 @@ -#include "thruster_interface_auv_driver.hpp" +#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( int I2C_BUS, int PICO_I2C_ADDRESS, - double SYSTEM_OPERATIONAL_VOLTAGE, const std::vector &THRUSTER_MAPPING, const std::vector &THRUSTER_DIRECTION, - const std::vector &THRUSTER_PWM_OFFSET, const std::vector &PWM_MIN, const std::vector &PWM_MAX, - const std::map>> &COEFFS) : I2C_BUS(I2C_BUS), - PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - // SYSTEM_OPERATIONAL_VOLTAGE(SYSTEM_OPERATIONAL_VOLTAGE), done after in the if-else - THRUSTER_MAPPING(THRUSTER_MAPPING), - THRUSTER_DIRECTION(THRUSTER_DIRECTION), - THRUSTER_PWM_OFFSET(THRUSTER_PWM_OFFSET), - PWM_MIN(PWM_MIN), - PWM_MAX(PWM_MAX), - COEFFS(COEFFS) { - - // Convert SYSTEM_OPERATIONAL_VOLTAGE passed as argument to assign the integer field variable [one is double, field var is int] - if (SYSTEM_OPERATIONAL_VOLTAGE < 11.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 10; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 13.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 12; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 15.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 14; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 17.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 16; - } else if (SYSTEM_OPERATIONAL_VOLTAGE < 19.0) { - this->SYSTEM_OPERATIONAL_VOLTAGE = 18; - } else { - this->SYSTEM_OPERATIONAL_VOLTAGE = 20; + const std::vector &LEFT_COEFFS, + const std::vector &RIGHT_COEFFS) : I2C_BUS(I2C_BUS), + PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + THRUSTER_MAPPING(THRUSTER_MAPPING), + THRUSTER_DIRECTION(THRUSTER_DIRECTION), + PWM_MIN(PWM_MIN), + PWM_MAX(PWM_MAX), + LEFT_COEFFS(LEFT_COEFFS), + RIGHT_COEFFS(RIGHT_COEFFS) +{ + printf("I2C_BUS: %d\n", I2C_BUS); + printf("PICO_I2C_ADDRESS: %d\n", PICO_I2C_ADDRESS); + printf("THRUSTER_MAPPING: "); + for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { + printf("%d ", THRUSTER_MAPPING[i]); } + printf("\n"); + printf("THRUSTER_DIRECTION: "); + for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { + printf("%d ", THRUSTER_DIRECTION[i]); + } + printf("\n"); + printf("PWM_MIN: "); + for (int i = 0; i < PWM_MIN.size(); i++) { + printf("%d ", PWM_MIN[i]); + } + printf("\n"); + printf("PWM_MAX: "); + for (int i = 0; i < PWM_MAX.size(); i++) { + printf("%d ", PWM_MAX[i]); + } + printf("\n"); + printf("LEFT_COEFFS: "); + for (int i = 0; i < LEFT_COEFFS.size(); i++) { + printf("%f ", LEFT_COEFFS[i]); + } + printf("\n"); + printf("RIGHT_COEFFS: "); + for (int i = 0; i < RIGHT_COEFFS.size(); i++) { + printf("%f ", RIGHT_COEFFS[i]); + } + printf("\n"); // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); - bus_fd = open(i2c_filename.c_str(), O_RDWR); + bus_fd = open(i2c_filename.c_str(), O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) if (bus_fd < 0) { std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; } @@ -57,26 +73,22 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const forces_in_kg[i] = thruster_forces_array[i] / 9.80665; } - // Select the appropriate coefficients based on the operational voltage - auto left_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"]; - auto right_coeffs = COEFFS[SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"]; - std::vector interpolated_pwm; for (size_t i = 0; i < forces_in_kg.size(); ++i) { double force = forces_in_kg[i]; double pwm = 0.0; if (force < 0) { - pwm = left_coeffs[0] * std::pow(forces_in_kg[i], 3) + - left_coeffs[1] * std::pow(forces_in_kg[i], 2) + - left_coeffs[2] * forces_in_kg[i] + - left_coeffs[3]; + pwm = LEFT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + + LEFT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + + LEFT_COEFFS[2] * forces_in_kg[i] + + LEFT_COEFFS[3]; } else if (force == 0.0) { pwm = 1500; } else { - pwm = right_coeffs[0] * std::pow(forces_in_kg[i], 3) + - right_coeffs[1] * std::pow(forces_in_kg[i], 2) + - right_coeffs[2] * forces_in_kg[i] + - right_coeffs[3]; + pwm = RIGHT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + + RIGHT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + + RIGHT_COEFFS[2] * forces_in_kg[i] + + RIGHT_COEFFS[3]; } interpolated_pwm.push_back(static_cast(pwm)); } @@ -91,6 +103,13 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs(const std::vector &t i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB } + /* constexpr std::size_t i2c_data_size = 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) + std::uint8_t i2c_data_array[i2c_data_size]; + auto pwm_to_i2c_data = [](std::int16_t pwm) -> std::array { + return {static_cast((pwm >> 8) & 0xFF), static_cast(pwm & 0xFF)}; + }; + std::transform(thruster_pwm_array.begin(), thruster_pwm_array.end(), i2c_data_array, pwm_to_i2c_data); */ + // Set the I2C slave address if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; @@ -115,7 +134,6 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vect // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - // thruster_pwm_array[i] += THRUSTER_PWM_OFFSET[i]; // Clamp the PWM signal if (thruster_pwm_array[i] < PWM_MIN[i]) { diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp similarity index 72% rename from motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp rename to motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp index 99448e0bb..fc7a38690 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_node.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp @@ -1,8 +1,8 @@ -#include "thruster_interface_auv_ros.hpp" +#include "thruster_interface_auv/thruster_interface_auv_ros.hpp" int main(int argc, char **argv) { rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_cpp_node"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_node"); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp similarity index 66% rename from motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp rename to motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index fc737e5d0..ab83aeaca 100644 --- a/motion/thruster_interface_auv_cpp/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -1,4 +1,4 @@ -#include "thruster_interface_auv_ros.hpp" +#include "thruster_interface_auv/thruster_interface_auv_ros.hpp" ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { @@ -6,49 +6,41 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ this->thruster_forces_subscriber_ = this->create_subscription( "thrust/thruster_forces", 10, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); - this->thruster_pwm_publisher_ = this->create_publisher("pwm_APPROX_cpp", 10); + this->thruster_pwm_publisher_ = this->create_publisher("pwm", 10); - // declare orca.yaml parameters + // from orca.yaml parameters this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping"); this->declare_parameter>("propulsion.thrusters.thruster_direction"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset"); this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); this->declare_parameter("propulsion.thrusters.thrust_update_rate"); + //from thruster_interface_auv.yaml parameters + this->declare_parameter("i2c.bus"); + this->declare_parameter("i2c.address"); + this->declare_parameter>("coeffs.16V.LEFT"); + this->declare_parameter>("coeffs.16V.RIGHT"); - // get orca.yaml parameters + int i2c_bus = this->get_parameter("i2c.bus").as_int(); + int i2c_address = this->get_parameter("i2c.address").as_int(); auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); - auto thruster_PWM_offset = this->get_parameter("propulsion.thrusters.thruster_PWM_offset").as_integer_array(); auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); - - // coeffs.yaml parameters - std::map>> coeffs; - std::vector voltage_levels = {10, 12, 14, 16, 18, 20}; - for (int voltage : voltage_levels) { - std::string left_param = "coeffs." + std::to_string(voltage) + "V.LEFT"; - std::string right_param = "coeffs." + std::to_string(voltage) + "V.RIGHT"; - this->declare_parameter>(left_param); // declare coeffs.10V.LEFT - this->declare_parameter>(right_param); - - auto left_coeffs = this->get_parameter(left_param).as_double_array(); - auto right_coeffs = this->get_parameter(right_param).as_double_array(); - - coeffs[voltage]["LEFT"] = left_coeffs; // save to coeffs[10]["LEFT"] - coeffs[voltage]["RIGHT"] = right_coeffs; - } + auto left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); + auto right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); // Initialize thruster driver this->thruster_driver_ = ThrusterInterfaceAUVDriver( - 1, 0x21, 16.0, + i2c_bus, + i2c_address, std::vector(thruster_mapping.begin(), thruster_mapping.end()), std::vector(thruster_direction.begin(), thruster_direction.end()), - std::vector(thruster_PWM_offset.begin(), thruster_PWM_offset.end()), std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), - coeffs); + std::vector(left_coeffs.begin(), left_coeffs.end()), + std::vector(right_coeffs.begin(), right_coeffs.end()) + ); // Declare "thruster_forces_array" in case no topic comes in at the beginning this->thruster_forces_array_ = std::vector(8, 0.00); @@ -57,7 +49,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ std::chrono::duration(this->thrust_timer_period_), std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_cpp_node\" has been started"); + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" has been started"); } void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { diff --git a/motion/thruster_interface_auv/thruster_interface_auv/__init__.py b/motion/thruster_interface_auv/thruster_interface_auv/__init__.py deleted file mode 100644 index e69de29bb..000000000 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 deleted file mode 100644 index d7efd1dae..000000000 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python3 -# Import libraries -import numpy -import smbus2 - - -class ThrusterInterfaceAUVDriver: - def __init__( - self, - i2c_bus=1, - pico_i2c_address=0x21, - system_operational_voltage=16.0, - ros2_package_name_for_thruster_datasheet="", - thruster_mapping=[7, 6, 5, 4, 3, 2, 1, 0], - thruster_direction=[1, 1, 1, 1, 1, 1, 1, 1], - thruster_pwm_offset=[0, 0, 0, 0, 0, 0, 0, 0], - pwm_min=[1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - pwm_max=[1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - coeffs=None, - ) -> None: - # Initialice the I2C communication - self.bus = None - try: - self.bus = smbus2.SMBus(i2c_bus) - except Exception as errorCode: - print(f"ERROR: Failed connection I2C bus nr {self.bus}: {errorCode}") - self.PICO_I2C_ADDRESS = pico_i2c_address - - # Set mapping, direction and offset for the thrusters - self.THRUSTER_MAPPING = thruster_mapping - self.THRUSTER_DIRECTION = thruster_direction - self.THRUSTER_PWM_OFFSET = thruster_pwm_offset - self.PWM_MIN = pwm_min - self.PWM_MAX = pwm_max - - # Convert SYSTEM_OPERATIONAL_VOLTAGE to a whole even number to work with - # This is because we have multiple files for the behaviour of the thrusters depending on the voltage of the drone - if system_operational_voltage < 11.0: - self.SYSTEM_OPERATIONAL_VOLTAGE = 10 - elif system_operational_voltage < 13.0: - self.SYSTEM_OPERATIONAL_VOLTAGE = 12 - elif system_operational_voltage < 15.0: - self.SYSTEM_OPERATIONAL_VOLTAGE = 14 - elif system_operational_voltage < 17.0: - self.SYSTEM_OPERATIONAL_VOLTAGE = 16 - elif system_operational_voltage < 19.0: - self.SYSTEM_OPERATIONAL_VOLTAGE = 18 - elif system_operational_voltage >= 19.0: - 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.coeffs = coeffs - - def _interpolate_forces_to_pwm(self, thruster_forces_array) -> list: - # Read the important data from the .csv file - # thrusterDatasheetFileData = pandas.read_csv( - # f"{self.ROS2_PACKAGE_NAME_FOR_THRUSTER_DATASHEET}/resources/T200-Thrusters-{self.SYSTEM_OPERATIONAL_VOLTAGE}V.csv", - # usecols=[" PWM (µs)", " Force (Kg f)"], - # ) - - # Convert Newtons to Kg as Thruster Datasheet is in Kg format - for i, thruster_forces in enumerate(thruster_forces_array): - thruster_forces_array[i] = thruster_forces / 9.80665 - - # Select the appropriate pair of coeffs based on the operational voltage - left_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]["LEFT"] - right_coeffs = self.coeffs[self.SYSTEM_OPERATIONAL_VOLTAGE]["RIGHT"] - - # Calculate the interpolated PWM values using the polynomial coefficients - interpolated_pwm = [] - for i, force in enumerate(thruster_forces_array): - if force < 0: - # use the left coefficients - pwm = numpy.polyval(left_coeffs, force) - elif force == 0.00: - pwm = 1500 - self.THRUSTER_PWM_OFFSET[i] - else: - # use the right coefficients - pwm = numpy.polyval(right_coeffs, force) - - interpolated_pwm.append(pwm) - - # Convert PWM signal to integers as they are interpolated and can have floats - interpolated_pwm = [int(pwm) for pwm in interpolated_pwm] - - return interpolated_pwm - - def _send_data_to_escs(self, thruster_pwm_array) -> None: - i2c_data_array = [] - - # Divide data into bytes as I2C only sends bytes - # We have 8 values of 16 bits - # Convert them to 16 values of 8 bits (ie 1 byte) - for i in range(len(thruster_pwm_array)): - msb = (thruster_pwm_array[i] >> 8) & 0xFF - lsb = (thruster_pwm_array[i]) & 0xFF - i2c_data_array.extend([msb, lsb]) - - # Send the whole array through I2C - # OBS!: Python adds an extra byte at the start that the Microcotroller that is receiving this has to handle - self.bus.write_i2c_block_data(self.PICO_I2C_ADDRESS, 0, i2c_data_array) - - def drive_thrusters(self, thruster_forces_array) -> list: - # Apply thruster mapping and direction - 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] - ) - - # Apply thruster offset and limit PWM if needed - for ESC_channel in range(len(thruster_pwm_array)): - # Clamping pwm signal in case it is out of range - if thruster_pwm_array[ESC_channel] < self.PWM_MIN[ESC_channel]: # To small - thruster_pwm_array[ESC_channel] = self.PWM_MIN[ESC_channel] - elif thruster_pwm_array[ESC_channel] > self.PWM_MAX[ESC_channel]: # To big - thruster_pwm_array[ESC_channel] = self.PWM_MAX[ESC_channel] - - # Send data through I2C to the microcontroller that then controls the ESC and extension the thrusters - try: - self._send_data_to_escs(thruster_pwm_array) - except Exception as errorCode: - print(f"ERROR: Failed to send PWM values: {errorCode}") - - return 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 deleted file mode 100755 index 608af7743..000000000 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ /dev/null @@ -1,191 +0,0 @@ -#!/usr/bin/env python3 -# ROS2 Libraries -import rclpy -from ament_index_python.packages import get_package_share_directory -from rcl_interfaces.msg import ParameterDescriptor -from rclpy.node import Node -from std_msgs.msg import Int16MultiArray - -# 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: - # Initialize and name the node process running - super().__init__("thruster_interface_auv_node") - - # 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_pwm_publisher = self.create_publisher( - Int16MultiArray, "pwm_APPROX", 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_PWM_min", - [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - ) - self.declare_parameter( - "propulsion.thrusters.thruster_PWM_max", - [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - ) - - self.declare_parameter("propulsion.thrusters.thrust_update_rate", 10.0) - - self.declare_parameter( - "coeffs.10V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.10V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.12V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.12V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.14V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.14V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.16V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.16V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.18V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.18V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.20V.LEFT", None, ParameterDescriptor(dynamic_typing=True) - ) - self.declare_parameter( - "coeffs.20V.RIGHT", None, ParameterDescriptor(dynamic_typing=True) - ) - - 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 - ) - - # dictionary for the coeffs of the poly approx of thruster forces based on OPERATIONAL_VOLTAGE - coeffs = { - 10: { - "LEFT": self.get_parameter("coeffs.10V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.10V.RIGHT").value, - }, - 12: { - "LEFT": self.get_parameter("coeffs.12V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.12V.RIGHT").value, - }, - 14: { - "LEFT": self.get_parameter("coeffs.14V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.14V.RIGHT").value, - }, - 16: { - "LEFT": self.get_parameter("coeffs.16V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.16V.RIGHT").value, - }, - 18: { - "LEFT": self.get_parameter("coeffs.18V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.18V.RIGHT").value, - }, - 20: { - "LEFT": self.get_parameter("coeffs.20V.LEFT").value, - "RIGHT": self.get_parameter("coeffs.20V.RIGHT").value, - }, - } - - print(coeffs) - - # Initialize thruster driver - self.thruster_driver = ThrusterInterfaceAUVDriver( - 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, - pwm_min=self.thruster_PWM_min, - pwm_max=self.thruster_PWM_max, - coeffs=coeffs, - ) - - # Start clock timer for driving thrusters every 0.2 seconds - # Declare "self.thruster_forces_array" in case no topic comes in at the first possible second - self.thruster_forces_array = [0.00] * 8 - - self.timer = self.create_timer(self.thrust_timer_period, self._timer_callback) - - # Debugging - self.get_logger().info('"thruster_interface_auv_node" has been started') - - def _thruster_forces_callback(self, msg) -> None: - # Get data of the forces published - self.thruster_forces_array = msg.thrust - - 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 - ) - - pwm_message = Int16MultiArray() - pwm_message.data = thruster_pwm_array - self.thruster_pwm_publisher.publish(pwm_message) - - -def main(args=None) -> None: - # Initialize - rclpy.init(args=args) - - # Running - thruster_interface_auv_node = ThrusterInterfaceAUVNode() - rclpy.spin(thruster_interface_auv_node) - - # Shutdown - thruster_interface_auv_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/motion/thruster_interface_auv_cpp/CMakeLists.txt b/motion/thruster_interface_auv_cpp/CMakeLists.txt deleted file mode 100644 index aa7e679a8..000000000 --- a/motion/thruster_interface_auv_cpp/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(thruster_interface_auv_cpp) - -# Required dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(vortex_msgs REQUIRED) - -# Include directories -include_directories(include/${PROJECT_NAME}) - -add_executable(${PROJECT_NAME}_node - src/thruster_interface_auv_node.cpp - src/thruster_interface_auv_ros.cpp - src/thruster_interface_auv_driver.cpp -) - -ament_target_dependencies(${PROJECT_NAME}_node - rclcpp - std_msgs - vortex_msgs -) - -# Install launch files. -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME}_node - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/motion/thruster_interface_auv_cpp/config/coeffs.yaml b/motion/thruster_interface_auv_cpp/config/coeffs.yaml deleted file mode 100644 index c8d8aeed4..000000000 --- a/motion/thruster_interface_auv_cpp/config/coeffs.yaml +++ /dev/null @@ -1,21 +0,0 @@ -/**: - ros__parameters: - coeffs: - 10V: - LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] - RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] - 12V: - LEFT: [7.35916, 47.58976, 199.18339, 1460.57483] - RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] - 14V: - LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] - RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] - 16V: - LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] - RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] - 18V: - LEFT: [2.50017, 25.18231, 142.66031, 1469.6513] - RIGHT: [1.22234, -16.56764, 116.26078, 1531.57015] - 20V: - LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] - RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] diff --git a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp deleted file mode 100644 index b36851320..000000000 --- a/motion/thruster_interface_auv_cpp/include/thruster_interface_auv_cpp/thruster_interface_auv_driver.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP -#define THRUSTER_INTERFACE_AUV_DRIVER_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class ThrusterInterfaceAUVDriver { -public: - // default constructor called when instantiating the object in .hpp file - ThrusterInterfaceAUVDriver(); - ~ThrusterInterfaceAUVDriver(); - - // constructor actually used - ThrusterInterfaceAUVDriver( - int I2C_BUS, - int PICO_I2C_ADDRESS, - double SYSTEM_OPERATIONAL_VOLTAGE, - const std::vector &THRUSTER_MAPPING, - const std::vector &THRUSTER_DIRECTION, - const std::vector &THRUSTER_PWM_OFFSET, - const std::vector &PWM_MIN, - const std::vector &PWM_MAX, - const std::map>> &COEFFS); - - std::vector drive_thrusters(const std::vector &thruster_forces_array); - -private: - int bus_fd; - int I2C_BUS; - int PICO_I2C_ADDRESS; - int SYSTEM_OPERATIONAL_VOLTAGE; - - std::vector THRUSTER_MAPPING; - std::vector THRUSTER_DIRECTION; - std::vector THRUSTER_PWM_OFFSET; - std::vector PWM_MIN; - std::vector PWM_MAX; - - std::map>> COEFFS; - - std::vector interpolate_forces_to_pwm(const std::vector &thruster_forces_array); - void send_data_to_escs(const std::vector &thruster_pwm_array); -}; - -#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py b/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py deleted file mode 100644 index de07f9f45..000000000 --- a/motion/thruster_interface_auv_cpp/launch/thruster_interface_auv_cpp.launch.py +++ /dev/null @@ -1,34 +0,0 @@ -from os import path - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - # Path to the YAML file - # config = path.join( - # get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" - # ) - - thruster_interface_auv_node = Node( - package="thruster_interface_auv_cpp", - executable="thruster_interface_auv_cpp_node", - name="thruster_interface_auv_cpp_node", - output="screen", - parameters=[ - path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", - ), - path.join( - get_package_share_directory("thruster_interface_auv_cpp"), - "config", - "coeffs.yaml", - ), - ], - ) - - return LaunchDescription([thruster_interface_auv_node]) diff --git a/motion/thruster_interface_auv_cpp/package.xml b/motion/thruster_interface_auv_cpp/package.xml deleted file mode 100644 index 3e1f19439..000000000 --- a/motion/thruster_interface_auv_cpp/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - thruster_interface_auv_cpp - 1.0.0 - Thruster interface to control thrusters through PCA9685 Module - vortex - MIT - - ament_cmake - rclcpp - std_msgs - vortex_msgs - Eigen3 - ament_index_cpp - - rclcpp - std_msgs - vortex_msgs - Eigen3 - ament_index_cpp - - ros2launch - - - ament_cmake - - From 609edf0c13c08e71f41cba4af1df7671df44caf4 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 27 Oct 2024 17:21:46 +0100 Subject: [PATCH 20/54] try to pass the test --- .../config/thruster_interface_auv_config.yaml | 6 ++--- .../thruster_interface_auv_driver.hpp | 26 +++++++------------ .../thruster_interface_auv_ros.hpp | 2 +- .../resources/T200-Thrusters-10V.csv | 2 +- .../resources/T200-Thrusters-12V.csv | 2 +- .../resources/T200-Thrusters-14V.csv | 2 +- .../resources/T200-Thrusters-16V.csv | 2 +- .../resources/T200-Thrusters-18V.csv | 2 +- .../resources/T200-Thrusters-20V.csv | 2 +- .../src/thruster_interface_auv_driver.cpp | 17 ++++++------ .../src/thruster_interface_auv_ros.cpp | 7 +++-- 11 files changed, 31 insertions(+), 39 deletions(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 29f7961a1..9a73f6462 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -9,7 +9,7 @@ thruster_interface_auv_node: # RIGHT: [3.84262, -31.64647, 160.76665, 1539.18791] # 14V: # LEFT: [14.10463, 72.78164, 245.73575, 1456.5868] - # RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] #14V csv table is wrong (is the same of 10V) and so also these coefficents + # RIGHT: [7.39517, -47.59238, 196.5525, 1543.6743] #14V csv table is wrong (is the same of 10V) and so also these coefficients 16V: LEFT: [3.31158, 28.90927, 152.1417, 1467.70348] RIGHT: [1.45002, -16.92386, 117.95721, 1533.55911] @@ -19,7 +19,7 @@ thruster_interface_auv_node: # 20V: # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] - + i2c: bus: 1 - address: 0x21 \ No newline at end of file + address: 0x21 diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index a3642b02c..7e96fe193 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,6 +1,7 @@ #ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP #define THRUSTER_INTERFACE_AUV_DRIVER_HPP +#include #include #include #include @@ -10,14 +11,13 @@ #include #include #include -#include /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the thrusters, takes the thruster forces and * converts them to PWM signals to be sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo Driver) - * + * * @details Based on the datasheets found in /resources, approximate the map with a piecewise (>0 and <0) third order polynomial. - * + * * @note The coefficients are found in the config.yaml for all the possible SYSTEM.OPERATIONAL_VOLTAGE values, but we use only 16V for now, * so: removed all the handling of the other voltages to save resources. Could be re-implemented in the future for more flexibility if we ever need it to * operate at different voltages in different situations. @@ -25,7 +25,6 @@ class ThrusterInterfaceAUVDriver { public: - /** * @brief empty default constructor called when declaring the object in the .hpp file. Doesn't initialize any params. */ @@ -34,7 +33,7 @@ class ThrusterInterfaceAUVDriver { /** * @brief actually used constructor. Called from ThrusterInterfaceAUVNode .cpp when instantiating the object, initializes all the params. - * + * * @param I2C_BUS bus number used to communicate * @param PICO_I2C_ADDRESS i2c address of the ESC that drive the thrusters * @param THRUSTER_MAPPING pin to motor mapping for the thrusters i.e. if thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. @@ -54,23 +53,18 @@ class ThrusterInterfaceAUVDriver { const std::vector &LEFT_COEFFS, const std::vector &RIGHT_COEFFS); - - /** * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to convert the thruster forces to PWM values * 2) send_data_to_escs() to send them to the ESCs via I2C - * + * * @param thruster_forces_array vector of forces for each thruster - * + * * @return std::vector vector of pwm values sent to each thruster */ std::vector drive_thrusters(const std::vector &thruster_forces_array); - - private: - - int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely identifies the device. -1 if it fails) + int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely identifies the device. -1 if it fails) int I2C_BUS; int PICO_I2C_ADDRESS; @@ -83,16 +77,16 @@ class ThrusterInterfaceAUVDriver { /** * @brief [private] method that just take the thruster forces and return PWM values - * + * * @param thruster_forces_array vector of forces for each thruster - * + * * @return std::vector vector of pwm values sent to each thruster if we want to publish them for debug purposes */ std::vector interpolate_forces_to_pwm(const std::vector &thruster_forces_array); /** * @brief [private] method that takes the pwm values computed and sends them to the ESCs via I2C - * + * * @param thruster_pwm_array vector of pwm values to send */ void send_data_to_escs(const std::vector &thruster_pwm_array); diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index f78befa02..e51f668c5 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,10 +1,10 @@ #ifndef THRUSTER_INTERFACE_AUV_NODE_HPP #define THRUSTER_INTERFACE_AUV_NODE_HPP +#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include #include #include -#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { public: diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv index 6689d4df1..de317f405 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-10V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) 1100,2662,13.62,10,136.2,-2.31,17.0 1104,2667,13.63,10,136.3,-2.30,16.9 1108,2661,13.45,10,134.5,-2.30,17.1 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv index 847b36b5f..ec96f2e7e 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-12V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W), 1100,2976,17.03,12,204.4,-2.90,14.2, 1104,2969,17.08,12,205.0,-2.92,14.2, 1108,2971,16.76,12,201.1,-2.89,14.4, diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv index 6689d4df1..de317f405 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-14V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) 1100,2662,13.62,10,136.2,-2.31,17.0 1104,2667,13.63,10,136.3,-2.30,16.9 1108,2661,13.45,10,134.5,-2.30,17.1 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv index 271f69d89..1a2c5d995 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-16V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) 1100,3465,24.30,16,388.8,-4.07,10.5 1104,3468,24.3,16,388.8,-4.05,10.4 1108,3449,23.78,16,380.5,-4.02,10.6 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv index 31efb4baa..0b06ea0f8 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-18V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W) 1100,3643,28.22,18,507.9,-4.59,9.0 1104,3643,27.91,18,502.4,-4.54,9.0 1108,3624,27.31,18,491.6,-4.53,9.2 diff --git a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv index 165881815..c751d22ef 100644 --- a/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv +++ b/motion/thruster_interface_auv/resources/T200-Thrusters-20V.csv @@ -1,4 +1,4 @@ - PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, + PWM (µs), RPM, Current (A), Voltage (V), Power (W), Force (Kg f), Efficiency (g/W),, 1100,3783,32.17,20,643.5,-5.04,7.8,, 1104,3777,31.37,20,627.4,-4.96,7.9,, 1108,3740,30.6,20,612.0,-4.92,8.0,, diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 4a21bc428..c56eee988 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -10,15 +10,14 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( const std::vector &PWM_MIN, const std::vector &PWM_MAX, const std::vector &LEFT_COEFFS, - const std::vector &RIGHT_COEFFS) : I2C_BUS(I2C_BUS), - PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - THRUSTER_MAPPING(THRUSTER_MAPPING), - THRUSTER_DIRECTION(THRUSTER_DIRECTION), - PWM_MIN(PWM_MIN), - PWM_MAX(PWM_MAX), - LEFT_COEFFS(LEFT_COEFFS), - RIGHT_COEFFS(RIGHT_COEFFS) -{ + const std::vector &RIGHT_COEFFS) : I2C_BUS(I2C_BUS), + PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + THRUSTER_MAPPING(THRUSTER_MAPPING), + THRUSTER_DIRECTION(THRUSTER_DIRECTION), + PWM_MIN(PWM_MIN), + PWM_MAX(PWM_MAX), + LEFT_COEFFS(LEFT_COEFFS), + RIGHT_COEFFS(RIGHT_COEFFS) { printf("I2C_BUS: %d\n", I2C_BUS); printf("PICO_I2C_ADDRESS: %d\n", PICO_I2C_ADDRESS); printf("THRUSTER_MAPPING: "); diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index ab83aeaca..665c9bfc4 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -14,7 +14,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); this->declare_parameter("propulsion.thrusters.thrust_update_rate"); - //from thruster_interface_auv.yaml parameters + // from thruster_interface_auv.yaml parameters this->declare_parameter("i2c.bus"); this->declare_parameter("i2c.address"); this->declare_parameter>("coeffs.16V.LEFT"); @@ -28,7 +28,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); auto left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); - auto right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + auto right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); // Initialize thruster driver this->thruster_driver_ = ThrusterInterfaceAUVDriver( @@ -39,8 +39,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_ std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), std::vector(left_coeffs.begin(), left_coeffs.end()), - std::vector(right_coeffs.begin(), right_coeffs.end()) - ); + std::vector(right_coeffs.begin(), right_coeffs.end())); // Declare "thruster_forces_array" in case no topic comes in at the beginning this->thruster_forces_array_ = std::vector(8, 0.00); From 2b5e2522c9f8f2a23c6cca9ac83c418e4e707eda Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sun, 27 Oct 2024 17:26:24 +0100 Subject: [PATCH 21/54] refactor(clang-format): updated config file to use ibium --- .clang-format | 309 +++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 305 insertions(+), 4 deletions(-) diff --git a/.clang-format b/.clang-format index 3e4a53daa..8340806b9 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,307 @@ --- -IndentWidth: 2 -AccessModifierOffset: -2 +BasedOnStyle: Chromium +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseArrows: false + AlignCaseColons: false +AlignConsecutiveTableGenBreakingDAGArgColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenCondOperatorColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenDefinitionColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowBreakBeforeNoexceptSpecifier: Never +AllowShortBlocksOnASingleLine: Never +AllowShortCaseExpressionOnASingleLine: true +AllowShortCaseLabelsOnASingleLine: false +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: false +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakAfterReturnType: None +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeConceptDeclarations: Always +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakFunctionDefinitionParameters: false +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +BreakTemplateDeclarations: Yes +ColumnLimit: 80 +CommentPragmas: "^ IWYU pragma:" +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: ^ + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.*\.h> + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.* + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: .* + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: ([-_](test|unittest))?$ +IncludeIsMainSourceRegex: "" +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 4 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLines: + AtEndOfFile: false + AtStartOfBlock: false + AtStartOfFile: true +LambdaBodyIndentation: Signature +Language: Cpp +LineEnding: DeriveLF +MacroBlockBegin: "" +MacroBlockEnd: "" +MainIncludeChar: Quote +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: NextLine +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +QualifierAlignment: Leave +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - c++ + - C++ + CanonicalDelimiter: "" + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDeclarationName: false + AfterFunctionDefinitionName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + ExceptDoubleParentheses: false + InConditionalStatements: false + InCStyleCasts: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +TableGenBreakInsideDAGArg: DontBreak UseTab: Never -ColumnLimit: 0 -NamespaceIndentation: All +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE From 4ad238505e76c0f7cc3b9ac4da395a75b2c2ee3f Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Sun, 27 Oct 2024 17:27:00 +0100 Subject: [PATCH 22/54] refactor(motion): applied updated clang format --- .../eigen_vector6d_typedef.hpp | 4 +- .../pseudoinverse_allocator.hpp | 36 +-- .../thrust_allocator_ros.hpp | 88 +++--- .../thrust_allocator_utils.hpp | 128 +++++---- .../src/pseudoinverse_allocator.cpp | 10 +- .../src/thrust_allocator_auv_node.cpp | 14 +- .../src/thrust_allocator_ros.cpp | 176 ++++++------ .../thruster_interface_auv_driver.hpp | 159 ++++++----- .../thruster_interface_auv_ros.hpp | 29 +- .../src/thruster_interface_auv_driver.cpp | 268 +++++++++--------- .../src/thruster_interface_auv_node.cpp | 13 +- .../src/thruster_interface_auv_ros.cpp | 122 ++++---- 12 files changed, 551 insertions(+), 496 deletions(-) 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/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/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 7e96fe193..3625442c1 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,95 +1,112 @@ #ifndef THRUSTER_INTERFACE_AUV_DRIVER_HPP #define THRUSTER_INTERFACE_AUV_DRIVER_HPP +#include +#include +#include +#include #include #include -#include #include -#include #include #include -#include -#include #include /** - * @brief class instantiated by ThrusterInterfaceAUVNode to control the thrusters, takes the thruster forces and - * converts them to PWM signals to be sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo Driver) + * @brief class instantiated by ThrusterInterfaceAUVNode to control the + * thrusters, takes the thruster forces and converts them to PWM signals to be + * sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo + * Driver) * - * @details Based on the datasheets found in /resources, approximate the map with a piecewise (>0 and <0) third order polynomial. + * @details Based on the datasheets found in /resources, approximate the map + * with a piecewise (>0 and <0) third order polynomial. * - * @note The coefficients are found in the config.yaml for all the possible SYSTEM.OPERATIONAL_VOLTAGE values, but we use only 16V for now, - * so: removed all the handling of the other voltages to save resources. Could be re-implemented in the future for more flexibility if we ever need it to + * @note The coefficients are found in the config.yaml for all the possible + * SYSTEM.OPERATIONAL_VOLTAGE values, but we use only 16V for now, so: removed + * all the handling of the other voltages to save resources. Could be + * re-implemented in the future for more flexibility if we ever need it to * operate at different voltages in different situations. */ class ThrusterInterfaceAUVDriver { + public: + /** + * @brief empty default constructor called when declaring the object in the + * .hpp file. Doesn't initialize any params. + */ + ThrusterInterfaceAUVDriver(); + ~ThrusterInterfaceAUVDriver(); -public: - /** - * @brief empty default constructor called when declaring the object in the .hpp file. Doesn't initialize any params. - */ - ThrusterInterfaceAUVDriver(); - ~ThrusterInterfaceAUVDriver(); - - /** - * @brief actually used constructor. Called from ThrusterInterfaceAUVNode .cpp when instantiating the object, initializes all the params. - * - * @param I2C_BUS bus number used to communicate - * @param PICO_I2C_ADDRESS i2c address of the ESC that drive the thrusters - * @param THRUSTER_MAPPING pin to motor mapping for the thrusters i.e. if thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. - * @param THRUSTER_DIRECTION physical mounting direction of the thrusters, (+-1) - * @param PWM_MIN minimum clamping pwm values - * @param PWM_MAX maximum clamping pwm values - * @param LEFT_COEFFS third order polynomial coefficients when force < 0 - * @param RIGHT_COEFFS third order polynomial coefficients when force > 0 - */ - ThrusterInterfaceAUVDriver( - int I2C_BUS, - int PICO_I2C_ADDRESS, - const std::vector &THRUSTER_MAPPING, - const std::vector &THRUSTER_DIRECTION, - const std::vector &PWM_MIN, - const std::vector &PWM_MAX, - const std::vector &LEFT_COEFFS, - const std::vector &RIGHT_COEFFS); + /** + * @brief actually used constructor. Called from ThrusterInterfaceAUVNode + * .cpp when instantiating the object, initializes all the params. + * + * @param I2C_BUS bus number used to communicate + * @param PICO_I2C_ADDRESS i2c address of the ESC that drive the + * thrusters + * @param THRUSTER_MAPPING pin to motor mapping for the thrusters i.e. if + * thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. + * @param THRUSTER_DIRECTION physical mounting direction of the thrusters, + * (+-1) + * @param PWM_MIN minimum clamping pwm values + * @param PWM_MAX maximum clamping pwm values + * @param LEFT_COEFFS third order polynomial coefficients when force + * < 0 + * @param RIGHT_COEFFS third order polynomial coefficients when force + * > 0 + */ + ThrusterInterfaceAUVDriver(int I2C_BUS, + int PICO_I2C_ADDRESS, + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, + const std::vector& PWM_MIN, + const std::vector& PWM_MAX, + const std::vector& LEFT_COEFFS, + const std::vector& RIGHT_COEFFS); - /** - * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to convert the thruster forces to PWM values - * 2) send_data_to_escs() to send them to the ESCs via I2C - * - * @param thruster_forces_array vector of forces for each thruster - * - * @return std::vector vector of pwm values sent to each thruster - */ - std::vector drive_thrusters(const std::vector &thruster_forces_array); + /** + * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to + * convert the thruster forces to PWM values 2) send_data_to_escs() to send + * them to the ESCs via I2C + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster + */ + std::vector drive_thrusters( + const std::vector& thruster_forces_array); -private: - int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely identifies the device. -1 if it fails) - int I2C_BUS; - int PICO_I2C_ADDRESS; + private: + int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely + ///< identifies the device. -1 if it fails) + int I2C_BUS; + int PICO_I2C_ADDRESS; - std::vector THRUSTER_MAPPING; - std::vector THRUSTER_DIRECTION; - std::vector PWM_MIN; - std::vector PWM_MAX; - std::vector LEFT_COEFFS; - std::vector RIGHT_COEFFS; + std::vector THRUSTER_MAPPING; + std::vector THRUSTER_DIRECTION; + std::vector PWM_MIN; + std::vector PWM_MAX; + std::vector LEFT_COEFFS; + std::vector RIGHT_COEFFS; - /** - * @brief [private] method that just take the thruster forces and return PWM values - * - * @param thruster_forces_array vector of forces for each thruster - * - * @return std::vector vector of pwm values sent to each thruster if we want to publish them for debug purposes - */ - std::vector interpolate_forces_to_pwm(const std::vector &thruster_forces_array); + /** + * @brief [private] method that just take the thruster forces and return PWM + * values + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster + * if we want to publish them for debug purposes + */ + std::vector interpolate_forces_to_pwm( + const std::vector& thruster_forces_array); - /** - * @brief [private] method that takes the pwm values computed and sends them to the ESCs via I2C - * - * @param thruster_pwm_array vector of pwm values to send - */ - void send_data_to_escs(const std::vector &thruster_pwm_array); + /** + * @brief [private] method that takes the pwm values computed and sends them + * to the ESCs via I2C + * + * @param thruster_pwm_array vector of pwm values to send + */ + void send_data_to_escs(const std::vector& thruster_pwm_array); }; -#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP +#endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index e51f668c5..7add6bad1 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,27 +1,30 @@ #ifndef THRUSTER_INTERFACE_AUV_NODE_HPP #define THRUSTER_INTERFACE_AUV_NODE_HPP -#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include #include #include +#include "thruster_interface_auv/thruster_interface_auv_driver.hpp" class ThrusterInterfaceAUVNode : public rclcpp::Node { -public: - ThrusterInterfaceAUVNode(); + public: + ThrusterInterfaceAUVNode(); -private: - void thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg); - void timer_callback(); + private: + void thruster_forces_callback( + const vortex_msgs::msg::ThrusterForces::SharedPtr msg); + void timer_callback(); - rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; - rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; - rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr + thruster_forces_subscriber_; + rclcpp::Publisher::SharedPtr + thruster_pwm_publisher_; + rclcpp::TimerBase::SharedPtr timer_; - ThrusterInterfaceAUVDriver thruster_driver_; + ThrusterInterfaceAUVDriver thruster_driver_; - std::vector thruster_forces_array_; - double thrust_timer_period_; + std::vector thruster_forces_array_; + double thrust_timer_period_; }; -#endif // THRUSTER_INTERFACE_AUV_NODE_HPP +#endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index c56eee988..9f0931087 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -5,149 +5,155 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( int I2C_BUS, int PICO_I2C_ADDRESS, - const std::vector &THRUSTER_MAPPING, - const std::vector &THRUSTER_DIRECTION, - const std::vector &PWM_MIN, - const std::vector &PWM_MAX, - const std::vector &LEFT_COEFFS, - const std::vector &RIGHT_COEFFS) : I2C_BUS(I2C_BUS), - PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - THRUSTER_MAPPING(THRUSTER_MAPPING), - THRUSTER_DIRECTION(THRUSTER_DIRECTION), - PWM_MIN(PWM_MIN), - PWM_MAX(PWM_MAX), - LEFT_COEFFS(LEFT_COEFFS), - RIGHT_COEFFS(RIGHT_COEFFS) { - printf("I2C_BUS: %d\n", I2C_BUS); - printf("PICO_I2C_ADDRESS: %d\n", PICO_I2C_ADDRESS); - printf("THRUSTER_MAPPING: "); - for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { - printf("%d ", THRUSTER_MAPPING[i]); - } - printf("\n"); - printf("THRUSTER_DIRECTION: "); - for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { - printf("%d ", THRUSTER_DIRECTION[i]); - } - printf("\n"); - printf("PWM_MIN: "); - for (int i = 0; i < PWM_MIN.size(); i++) { - printf("%d ", PWM_MIN[i]); - } - printf("\n"); - printf("PWM_MAX: "); - for (int i = 0; i < PWM_MAX.size(); i++) { - printf("%d ", PWM_MAX[i]); - } - printf("\n"); - printf("LEFT_COEFFS: "); - for (int i = 0; i < LEFT_COEFFS.size(); i++) { - printf("%f ", LEFT_COEFFS[i]); - } - printf("\n"); - printf("RIGHT_COEFFS: "); - for (int i = 0; i < RIGHT_COEFFS.size(); i++) { - printf("%f ", RIGHT_COEFFS[i]); - } - printf("\n"); - - // Open the I2C bus - std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); - bus_fd = open(i2c_filename.c_str(), O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) - if (bus_fd < 0) { - std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; - } + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, + const std::vector& PWM_MIN, + const std::vector& PWM_MAX, + const std::vector& LEFT_COEFFS, + const std::vector& RIGHT_COEFFS) + : I2C_BUS(I2C_BUS), + PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), + THRUSTER_MAPPING(THRUSTER_MAPPING), + THRUSTER_DIRECTION(THRUSTER_DIRECTION), + PWM_MIN(PWM_MIN), + PWM_MAX(PWM_MAX), + LEFT_COEFFS(LEFT_COEFFS), + RIGHT_COEFFS(RIGHT_COEFFS) { + printf("I2C_BUS: %d\n", I2C_BUS); + printf("PICO_I2C_ADDRESS: %d\n", PICO_I2C_ADDRESS); + printf("THRUSTER_MAPPING: "); + for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { + printf("%d ", THRUSTER_MAPPING[i]); + } + printf("\n"); + printf("THRUSTER_DIRECTION: "); + for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { + printf("%d ", THRUSTER_DIRECTION[i]); + } + printf("\n"); + printf("PWM_MIN: "); + for (int i = 0; i < PWM_MIN.size(); i++) { + printf("%d ", PWM_MIN[i]); + } + printf("\n"); + printf("PWM_MAX: "); + for (int i = 0; i < PWM_MAX.size(); i++) { + printf("%d ", PWM_MAX[i]); + } + printf("\n"); + printf("LEFT_COEFFS: "); + for (int i = 0; i < LEFT_COEFFS.size(); i++) { + printf("%f ", LEFT_COEFFS[i]); + } + printf("\n"); + printf("RIGHT_COEFFS: "); + for (int i = 0; i < RIGHT_COEFFS.size(); i++) { + printf("%f ", RIGHT_COEFFS[i]); + } + printf("\n"); + + // Open the I2C bus + std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); + bus_fd = open(i2c_filename.c_str(), + O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) + if (bus_fd < 0) { + std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; + } } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - if (bus_fd >= 0) { - close(bus_fd); - } + if (bus_fd >= 0) { + close(bus_fd); + } } -std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm(const std::vector &thruster_forces_array) { - // Convert Newtons to Kg (since the thruster datasheet is in Kg) - std::vector forces_in_kg(thruster_forces_array.size()); - for (size_t i = 0; i < thruster_forces_array.size(); ++i) { - forces_in_kg[i] = thruster_forces_array[i] / 9.80665; - } - - std::vector interpolated_pwm; - for (size_t i = 0; i < forces_in_kg.size(); ++i) { - double force = forces_in_kg[i]; - double pwm = 0.0; - if (force < 0) { - pwm = LEFT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + - LEFT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + - LEFT_COEFFS[2] * forces_in_kg[i] + - LEFT_COEFFS[3]; - } else if (force == 0.0) { - pwm = 1500; - } else { - pwm = RIGHT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + - RIGHT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + - RIGHT_COEFFS[2] * forces_in_kg[i] + - RIGHT_COEFFS[3]; - } - interpolated_pwm.push_back(static_cast(pwm)); - } - - return interpolated_pwm; -} +std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( + const std::vector& thruster_forces_array) { + // Convert Newtons to Kg (since the thruster datasheet is in Kg) + std::vector forces_in_kg(thruster_forces_array.size()); + for (size_t i = 0; i < thruster_forces_array.size(); ++i) { + forces_in_kg[i] = thruster_forces_array[i] / 9.80665; + } -void ThrusterInterfaceAUVDriver::send_data_to_escs(const std::vector &thruster_pwm_array) { - uint8_t i2c_data_array[16]; - for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - i2c_data_array[2 * i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB - i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB - } - - /* constexpr std::size_t i2c_data_size = 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) - std::uint8_t i2c_data_array[i2c_data_size]; - auto pwm_to_i2c_data = [](std::int16_t pwm) -> std::array { - return {static_cast((pwm >> 8) & 0xFF), static_cast(pwm & 0xFF)}; - }; - std::transform(thruster_pwm_array.begin(), thruster_pwm_array.end(), i2c_data_array, pwm_to_i2c_data); */ - - // Set the I2C slave address - if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { - std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; - return; - } - - // Write data to the I2C device - if (write(bus_fd, i2c_data_array, 16) != 16) { - std::cerr << "ERROR: Failed to write to I2C device" << std::endl; - } + std::vector interpolated_pwm; + for (size_t i = 0; i < forces_in_kg.size(); ++i) { + double force = forces_in_kg[i]; + double pwm = 0.0; + if (force < 0) { + pwm = LEFT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + + LEFT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + + LEFT_COEFFS[2] * forces_in_kg[i] + LEFT_COEFFS[3]; + } else if (force == 0.0) { + pwm = 1500; + } else { + pwm = RIGHT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + + RIGHT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + + RIGHT_COEFFS[2] * forces_in_kg[i] + RIGHT_COEFFS[3]; + } + interpolated_pwm.push_back(static_cast(pwm)); + } + + return interpolated_pwm; } -std::vector ThrusterInterfaceAUVDriver::drive_thrusters(const std::vector &thruster_forces_array) { - // Apply thruster mapping and direction - std::vector mapped_forces(thruster_forces_array.size()); - for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { - mapped_forces[i] = thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; - } +void ThrusterInterfaceAUVDriver::send_data_to_escs( + const std::vector& thruster_pwm_array) { + uint8_t i2c_data_array[16]; + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + i2c_data_array[2 * i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB + i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB + } - // Convert forces to PWM - std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); + /* constexpr std::size_t i2c_data_size = 8 * 2; // 8 thrusters * (1xMSB + + 1xLSB) std::uint8_t i2c_data_array[i2c_data_size]; auto pwm_to_i2c_data = + [](std::int16_t pwm) -> std::array { return + {static_cast((pwm >> 8) & 0xFF), static_cast(pwm + & 0xFF)}; + }; + std::transform(thruster_pwm_array.begin(), thruster_pwm_array.end(), + i2c_data_array, pwm_to_i2c_data); */ + + // Set the I2C slave address + if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { + std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; + return; + } - // Apply thruster offset and limit PWM if needed - for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + // Write data to the I2C device + if (write(bus_fd, i2c_data_array, 16) != 16) { + std::cerr << "ERROR: Failed to write to I2C device" << std::endl; + } +} + +std::vector ThrusterInterfaceAUVDriver::drive_thrusters( + const std::vector& thruster_forces_array) { + // Apply thruster mapping and direction + std::vector mapped_forces(thruster_forces_array.size()); + for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { + mapped_forces[i] = + thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; + } - // Clamp the PWM signal - if (thruster_pwm_array[i] < PWM_MIN[i]) { - thruster_pwm_array[i] = PWM_MIN[i]; - } else if (thruster_pwm_array[i] > PWM_MAX[i]) { - thruster_pwm_array[i] = PWM_MAX[i]; + // Convert forces to PWM + std::vector thruster_pwm_array = + interpolate_forces_to_pwm(mapped_forces); + + // Apply thruster offset and limit PWM if needed + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + // Clamp the PWM signal + if (thruster_pwm_array[i] < PWM_MIN[i]) { + thruster_pwm_array[i] = PWM_MIN[i]; + } else if (thruster_pwm_array[i] > PWM_MAX[i]) { + thruster_pwm_array[i] = PWM_MAX[i]; + } } - } - // Send data through I2C - try { - send_data_to_escs(thruster_pwm_array); - } catch (...) { - std::cerr << "ERROR: Failed to send PWM values" << std::endl; - } + // Send data through I2C + try { + send_data_to_escs(thruster_pwm_array); + } catch (...) { + std::cerr << "ERROR: Failed to send PWM values" << std::endl; + } - return thruster_pwm_array; + return thruster_pwm_array; } diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp index fc7a38690..903496ace 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_node.cpp @@ -1,9 +1,10 @@ #include "thruster_interface_auv/thruster_interface_auv_ros.hpp" -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started thruster_interface_auv_node"); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Started thruster_interface_auv_node"); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 665c9bfc4..b86e50e5a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -1,65 +1,87 @@ #include "thruster_interface_auv/thruster_interface_auv_ros.hpp" -ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { +ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() + : Node("thruster_interface_auv_node") { + // create a subscriber that takes data from thruster forces and publisher + // for debugging + this->thruster_forces_subscriber_ = + this->create_subscription( + "thrust/thruster_forces", 10, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, + std::placeholders::_1)); + this->thruster_pwm_publisher_ = + this->create_publisher("pwm", 10); - // create a subscriber that takes data from thruster forces and publisher for debugging - this->thruster_forces_subscriber_ = this->create_subscription( - "thrust/thruster_forces", 10, - std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); - this->thruster_pwm_publisher_ = this->create_publisher("pwm", 10); + // from orca.yaml parameters + this->declare_parameter>( + "propulsion.thrusters.thruster_to_pin_mapping"); + this->declare_parameter>( + "propulsion.thrusters.thruster_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_PWM_min"); + this->declare_parameter>( + "propulsion.thrusters.thruster_PWM_max"); + this->declare_parameter("propulsion.thrusters.thrust_update_rate"); + // from thruster_interface_auv.yaml parameters + this->declare_parameter("i2c.bus"); + this->declare_parameter("i2c.address"); + this->declare_parameter>("coeffs.16V.LEFT"); + this->declare_parameter>("coeffs.16V.RIGHT"); - // from orca.yaml parameters - this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping"); - this->declare_parameter>("propulsion.thrusters.thruster_direction"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_min"); - this->declare_parameter>("propulsion.thrusters.thruster_PWM_max"); - this->declare_parameter("propulsion.thrusters.thrust_update_rate"); - // from thruster_interface_auv.yaml parameters - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); - this->declare_parameter>("coeffs.16V.LEFT"); - this->declare_parameter>("coeffs.16V.RIGHT"); + int i2c_bus = this->get_parameter("i2c.bus").as_int(); + int i2c_address = this->get_parameter("i2c.address").as_int(); + auto thruster_mapping = + this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") + .as_integer_array(); + auto thruster_direction = + this->get_parameter("propulsion.thrusters.thruster_direction") + .as_integer_array(); + auto thruster_PWM_min = + this->get_parameter("propulsion.thrusters.thruster_PWM_min") + .as_integer_array(); + auto thruster_PWM_max = + this->get_parameter("propulsion.thrusters.thruster_PWM_max") + .as_integer_array(); + this->thrust_timer_period_ = + 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate") + .as_double(); + auto left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); + auto right_coeffs = + this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - int i2c_bus = this->get_parameter("i2c.bus").as_int(); - int i2c_address = this->get_parameter("i2c.address").as_int(); - auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping").as_integer_array(); - auto thruster_direction = this->get_parameter("propulsion.thrusters.thruster_direction").as_integer_array(); - auto thruster_PWM_min = this->get_parameter("propulsion.thrusters.thruster_PWM_min").as_integer_array(); - auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max").as_integer_array(); - this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate").as_double(); - auto left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); - auto right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + // Initialize thruster driver + this->thruster_driver_ = ThrusterInterfaceAUVDriver( + i2c_bus, i2c_address, + std::vector(thruster_mapping.begin(), thruster_mapping.end()), + std::vector(thruster_direction.begin(), thruster_direction.end()), + std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), + std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), + std::vector(left_coeffs.begin(), left_coeffs.end()), + std::vector(right_coeffs.begin(), right_coeffs.end())); - // Initialize thruster driver - this->thruster_driver_ = ThrusterInterfaceAUVDriver( - i2c_bus, - i2c_address, - std::vector(thruster_mapping.begin(), thruster_mapping.end()), - std::vector(thruster_direction.begin(), thruster_direction.end()), - std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), - std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), - std::vector(left_coeffs.begin(), left_coeffs.end()), - std::vector(right_coeffs.begin(), right_coeffs.end())); + // Declare "thruster_forces_array" in case no topic comes in at the + // beginning + this->thruster_forces_array_ = std::vector(8, 0.00); - // Declare "thruster_forces_array" in case no topic comes in at the beginning - this->thruster_forces_array_ = std::vector(8, 0.00); + timer_ = this->create_wall_timer( + std::chrono::duration(this->thrust_timer_period_), + std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - timer_ = this->create_wall_timer( - std::chrono::duration(this->thrust_timer_period_), - std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" has been started"); + RCLCPP_INFO(this->get_logger(), + "\"thruster_interface_auv_node\" has been started"); } -void ThrusterInterfaceAUVNode::thruster_forces_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - this->thruster_forces_array_ = msg->thrust; +void ThrusterInterfaceAUVNode::thruster_forces_callback( + const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { + this->thruster_forces_array_ = msg->thrust; } void ThrusterInterfaceAUVNode::timer_callback() { - std::vector thruster_pwm_array = thruster_driver_.drive_thrusters(this->thruster_forces_array_); + std::vector thruster_pwm_array = + thruster_driver_.drive_thrusters(this->thruster_forces_array_); - // publish PWM values for debugging - std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array; - thruster_pwm_publisher_->publish(pwm_message); + // publish PWM values for debugging + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array; + thruster_pwm_publisher_->publish(pwm_message); } From ba099989067c618ad634fd5f46365dd3c7cc1bb9 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 27 Oct 2024 18:16:27 +0100 Subject: [PATCH 23/54] fixed more --- .../thruster_interface_auv_driver.hpp | 12 ++++++------ .../thruster_interface_auv_ros.hpp | 1 + .../src/thruster_interface_auv_driver.cpp | 15 +++------------ .../src/thruster_interface_auv_ros.cpp | 19 +++++++++++++------ 4 files changed, 23 insertions(+), 24 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 3625442c1..bc7a987d8 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -54,10 +54,10 @@ class ThrusterInterfaceAUVDriver { * @param RIGHT_COEFFS third order polynomial coefficients when force * > 0 */ - ThrusterInterfaceAUVDriver(int I2C_BUS, + ThrusterInterfaceAUVDriver(short I2C_BUS, int PICO_I2C_ADDRESS, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, const std::vector& PWM_MIN, const std::vector& PWM_MAX, const std::vector& LEFT_COEFFS, @@ -78,11 +78,11 @@ class ThrusterInterfaceAUVDriver { private: int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely ///< identifies the device. -1 if it fails) - int I2C_BUS; + short I2C_BUS; int PICO_I2C_ADDRESS; - std::vector THRUSTER_MAPPING; - std::vector THRUSTER_DIRECTION; + std::vector THRUSTER_MAPPING; + std::vector THRUSTER_DIRECTION; std::vector PWM_MIN; std::vector PWM_MAX; std::vector LEFT_COEFFS; diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 7add6bad1..dec766ed6 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,6 +1,7 @@ #ifndef THRUSTER_INTERFACE_AUV_NODE_HPP #define THRUSTER_INTERFACE_AUV_NODE_HPP +#include #include #include #include diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 9f0931087..5054161e4 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -3,10 +3,10 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - int I2C_BUS, + short I2C_BUS, int PICO_I2C_ADDRESS, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, + const std::vector& THRUSTER_MAPPING, + const std::vector& THRUSTER_DIRECTION, const std::vector& PWM_MIN, const std::vector& PWM_MAX, const std::vector& LEFT_COEFFS, @@ -104,15 +104,6 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB } - /* constexpr std::size_t i2c_data_size = 8 * 2; // 8 thrusters * (1xMSB + - 1xLSB) std::uint8_t i2c_data_array[i2c_data_size]; auto pwm_to_i2c_data = - [](std::int16_t pwm) -> std::array { return - {static_cast((pwm >> 8) & 0xFF), static_cast(pwm - & 0xFF)}; - }; - std::transform(thruster_pwm_array.begin(), thruster_pwm_array.end(), - i2c_data_array, pwm_to_i2c_data); */ - // Set the I2C slave address if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index b86e50e5a..d29043e0c 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -4,13 +4,18 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { // create a subscriber that takes data from thruster forces and publisher // for debugging + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + this->thruster_forces_subscriber_ = this->create_subscription( - "thrust/thruster_forces", 10, + "thrust/thruster_forces", qos_sensor_data, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); this->thruster_pwm_publisher_ = - this->create_publisher("pwm", 10); + this->create_publisher("pwm", + qos_sensor_data); // from orca.yaml parameters this->declare_parameter>( @@ -45,15 +50,17 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate") .as_double(); - auto left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); - auto right_coeffs = + std::vector left_coeffs = + this->get_parameter("coeffs.16V.LEFT").as_double_array(); + std::vector right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); // Initialize thruster driver this->thruster_driver_ = ThrusterInterfaceAUVDriver( i2c_bus, i2c_address, - std::vector(thruster_mapping.begin(), thruster_mapping.end()), - std::vector(thruster_direction.begin(), thruster_direction.end()), + std::vector(thruster_mapping.begin(), thruster_mapping.end()), + std::vector(thruster_direction.begin(), + thruster_direction.end()), std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), std::vector(left_coeffs.begin(), left_coeffs.end()), From 0c853df1f0c554c722520a7d3bd7e8799f4b7570 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:21:15 +0100 Subject: [PATCH 24/54] feat(ruff): added config file and set pydocstyle convention and quote-style --- ruff.toml | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 ruff.toml diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 000000000..bf3554d64 --- /dev/null +++ b/ruff.toml @@ -0,0 +1,6 @@ +[lint.pydocstyle] +convention = "google" + +[format] +# Keep quotes as is +quote-style = "preserve" From adc08a0f3689a3c10b4e91af5dc53a1eecdc58a7 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:22:48 +0100 Subject: [PATCH 25/54] refactor(pre-commit): updated linting rules --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 62ca28bd3..36cb74d32 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -54,7 +54,7 @@ repos: name: ruff-pydocstyle args: [ "--select=D", - "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D205,D212,D401", + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", "--fix", ] stages: [pre-commit] @@ -63,7 +63,7 @@ repos: name: ruff-check args: [ "--select=F,PT,B,C4,T20,S,N", - "--ignore=T201,B006,S101,S607,S603", + "--ignore=T201,N812,B006,S101,S311,S607,S603", "--fix" ] stages: [pre-commit] From 5bdb138085fec2f334fd9c6bd62d30e65fc41b99 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:23:56 +0100 Subject: [PATCH 26/54] refactor(motion): applied formatting changes to docstring --- .../thrust_allocator_auv/launch/thrust_allocator_auv.launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 978ba8c39..d51184990 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 From 730862763e5254c79bd837df3f56a05e3bd4ec4b Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:24:14 +0100 Subject: [PATCH 27/54] refactor(mission): applied formatting changes to docstring --- mission/LCD/sources/ip_lib.py | 3 +-- mission/LCD/sources/lcd.py | 3 +-- mission/LCD/sources/lcd_lib.py | 6 ++--- mission/LCD/sources/power_sense_module_lib.py | 7 ++---- mission/LCD/sources/pressure_sensor_lib.py | 3 +-- mission/LCD/sources/temperature_sensor_lib.py | 13 +++++----- .../blackbox/blackbox/blackbox_log_data.py | 3 +-- mission/blackbox/blackbox/blackbox_node.py | 24 +++++++------------ mission/blackbox/launch/blackbox.launch.py | 3 +-- .../power_sense_module_lib.py | 10 ++++---- .../power_sense_module_node.py | 9 +++---- .../pressure_sensor_lib.py | 5 ++-- .../pressure_sensor_node.py | 9 +++---- .../temperature_sensor_lib.py | 15 ++++++------ .../temperature_sensor_node.py | 9 +++---- .../launch/internal_status_auv.launch.py | 5 ++-- .../joystick_interface_auv.py | 14 +++++------ .../launch/joystick_interface_auv.launch.py | 3 +-- .../tests/test_joystick_interface_auv.py | 12 ++++------ 19 files changed, 58 insertions(+), 98 deletions(-) diff --git a/mission/LCD/sources/ip_lib.py b/mission/LCD/sources/ip_lib.py index 1f3170999..869b62bb2 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 d84625320..65692cb49 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. diff --git a/mission/LCD/sources/lcd_lib.py b/mission/LCD/sources/lcd_lib.py index 3d0ba656e..7b193a640 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 @@ -49,8 +48,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. diff --git a/mission/LCD/sources/power_sense_module_lib.py b/mission/LCD/sources/power_sense_module_lib.py index 9b6f7055c..76556be47 100755 --- a/mission/LCD/sources/power_sense_module_lib.py +++ b/mission/LCD/sources/power_sense_module_lib.py @@ -38,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 @@ -61,9 +60,7 @@ def get_voltage(self) -> float: 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. diff --git a/mission/LCD/sources/pressure_sensor_lib.py b/mission/LCD/sources/pressure_sensor_lib.py index 55076c74d..39e42bd2e 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 5b764c422..bb509af4c 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: -! 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. +"""NOTE: Internal temperature measurement approach. + +For now, we don't have an external sensor to measure the internal temperature. +Instead, we use the internal computer's temperature sensor to approximate the environmental temperature. +Future improvement: Implement an external temperature sensor for a more accurate measurement of the internal temperature 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 760607523..5ea74ced8 100755 --- a/mission/blackbox/blackbox/blackbox_log_data.py +++ b/mission/blackbox/blackbox/blackbox_log_data.py @@ -58,8 +58,7 @@ def __init__(self, ros2_package_directory: str = "") -> None: 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. + """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. diff --git a/mission/blackbox/blackbox/blackbox_node.py b/mission/blackbox/blackbox/blackbox_node.py index c92d7455f..89a5d1f0c 100755 --- a/mission/blackbox/blackbox/blackbox_node.py +++ b/mission/blackbox/blackbox/blackbox_node.py @@ -94,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 @@ -108,8 +107,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 @@ -122,8 +120,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 @@ -136,8 +133,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 @@ -150,8 +146,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 @@ -164,8 +159,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. @@ -177,8 +171,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 @@ -219,8 +212,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 e8e2c0e30..eb8d03fad 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/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 39eab8c8b..262a99f1d 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 @@ -38,14 +38,13 @@ 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 voltage in volts. - Returns + Returns: ------- float The system voltage in volts. If an error occurs during reading, returns 0.0. @@ -62,14 +61,13 @@ def get_voltage(self) -> float: 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 current in amperes. - Returns + Returns: ------- float The current value in amperes. If an error occurs during reading, returns 0.0. 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 fde88240f..e3bfdd696 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 @@ -70,8 +70,7 @@ def __init__(self) -> None: 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. @@ -95,8 +94,7 @@ def read_timer_callback(self) -> None: ) # 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. @@ -108,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 e453ea000..85056fdd8 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,14 +30,13 @@ 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. If an error occurs, an error message is printed and 0.0 is returned. - Returns + Returns: ------- float The pressure reading in hPa (hectopascals), or 0.0 if an error occurs. 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 ee17e7c66..cf5ce7abb 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 @@ -59,8 +59,7 @@ def __init__(self) -> None: 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. @@ -74,8 +73,7 @@ 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. @@ -87,8 +85,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 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 4b2eb82a1..525c847b8 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: -! 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. +"""NOTE: Internal temperature measurement approach. + +For now, we don't have an external sensor to measure the internal temperature. +Instead, we use the internal computer's temperature sensor to approximate the environmental temperature. +Future improvement: Implement an external temperature sensor for a more accurate measurement of the internal temperature of the AUV. """ # Python Libraries @@ -16,13 +16,12 @@ 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. - Returns + Returns: ------- float The current temperature in Celsius. If an error occurs, it returns 0.0. 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 8bbb3ebe6..b8f8f740d 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 @@ -63,8 +63,7 @@ def __init__(self) -> None: 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. @@ -78,8 +77,7 @@ 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. @@ -91,8 +89,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 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 4f413729d..d52e20709 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. @@ -17,7 +16,7 @@ def generate_launch_description() -> LaunchDescription: The nodes will be launched with their respective executables and display output on the screen. - Returns + Returns: ------- launch.LaunchDescription A LaunchDescription object containing the nodes to be launched. 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 ca7b66730..ff1242fbb 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 @@ -134,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. @@ -171,9 +170,8 @@ def transition_to_autonomous_mode(self) -> None: 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. + """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. @@ -318,9 +316,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 + """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 9faf25a99..85fc0744b 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 c435c3544..480ecde32 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 @@ -71,8 +69,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 @@ -107,8 +104,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 From be7f07fffd65ffcfb8bf2e46e10998b4f7e850c7 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:24:33 +0100 Subject: [PATCH 28/54] refactor(acoustics): applied formatting changes to docstring --- .../acoustics_data_record_node.py | 36 +++++++------------ .../launch/acoustics_data_record_launch.py | 3 +- .../utilities/display_acoustics_data_live.py | 6 ++-- .../acoustics_interface_lib.py | 15 ++++---- .../acoustics_interface_node.py | 9 ++--- .../launch/acoustics_interface_launch.py | 3 +- 6 files changed, 25 insertions(+), 47 deletions(-) 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 c90cbdd56..d8e0fc4bd 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 @@ -133,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. @@ -143,8 +142,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. @@ -153,8 +151,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. @@ -163,8 +160,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. @@ -173,8 +169,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. @@ -183,8 +178,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. @@ -193,8 +187,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. @@ -203,8 +196,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. @@ -213,8 +205,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. @@ -223,8 +214,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. @@ -234,8 +224,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. """ @@ -254,8 +243,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 index cf3b42814..17d32772b 100755 --- a/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py +++ b/acoustics/acoustics_data_record/launch/acoustics_data_record_launch.py @@ -7,8 +7,7 @@ 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 7d949ff89..7d370f0d3 100644 --- a/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py +++ b/acoustics/acoustics_data_record/utilities/display_acoustics_data_live.py @@ -91,8 +91,7 @@ def convert_pandas_object_to_float_array(pandas_object: pd.Series) -> list: 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 @@ -228,8 +227,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. diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py index 040670975..c305b7c58 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_lib.py @@ -5,13 +5,12 @@ class TeensyCommunicationUDP: - """ - Class is responsible for the RPI side of teensy-RPI UDP communication. It is - implemented with a singleton pattern for convenience. + """Class is responsible for the RPI side of teensy-RPI UDP communication. + It isimplemented with a singleton pattern for convenience. Note: Private members are denoted by _member_name - Attributes + Attributes: ---------- _TEENSY_IP (string): self-explanatory _TEENSY_PORT (int): teensy's data port @@ -24,7 +23,7 @@ class TeensyCommunicationUDP: _data_target (str): the field of `acoustics_data` that is written to acoustics_data (dict[str, list[int]]): container for data from teensy - Methods + Methods: ------- init_communication(frequenciesOfInterest: list[tuple[int, int]]) -> None: Sets up socket for communication with teensy and waits for handshake @@ -148,8 +147,7 @@ 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 @@ -211,8 +209,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 diff --git a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py index 19ef76731..971e23d9a 100755 --- a/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py +++ b/acoustics/acoustics_interface/acoustics_interface/acoustics_interface_node.py @@ -9,8 +9,7 @@ class AcousticsInterfaceNode(Node): - """ - Publishes Acoustics data to ROS2. + """Publishes Acoustics data to ROS2. Methods: data_update() -> None: @@ -102,8 +101,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. @@ -149,8 +147,7 @@ def data_publisher(self) -> None: 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 9cc6d1b23..87281d9a4 100755 --- a/acoustics/acoustics_interface/launch/acoustics_interface_launch.py +++ b/acoustics/acoustics_interface/launch/acoustics_interface_launch.py @@ -7,8 +7,7 @@ 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 From f54e024fc4efd7b5c6d2abfcb58ce83e0613e024 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud <89779148+kluge7@users.noreply.github.com> Date: Mon, 28 Oct 2024 21:25:43 +0100 Subject: [PATCH 29/54] refactor(auv_setup): applied formatting changes to docstring --- auv_setup/launch/orca.launch.py | 3 +-- auv_setup/launch/topside.launch.py | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/auv_setup/launch/orca.launch.py b/auv_setup/launch/orca.launch.py index 1060c6e6f..5971f007b 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 diff --git a/auv_setup/launch/topside.launch.py b/auv_setup/launch/topside.launch.py index 27a856d25..b2c3570c6 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 From f567689cb0ba2833e9fd01d9e60ea6f0090eb8c1 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 1 Nov 2024 20:13:33 +0100 Subject: [PATCH 30/54] fixed fixed --- .../thruster_interface_auv_driver.hpp | 9 ++++ .../src/thruster_interface_auv_driver.cpp | 45 +++++++++++-------- 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index bc7a987d8..e21539bcc 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -100,6 +100,13 @@ class ThrusterInterfaceAUVDriver { std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); + std::int16_t force_to_pwm(double force, + const std::vector& left_coeffs, + const std::vector& right_coeffs); + + std::int16_t interpolate_pwm(double force, + const std::vector& coeffs); + /** * @brief [private] method that takes the pwm values computed and sends them * to the ESCs via I2C @@ -107,6 +114,8 @@ class ThrusterInterfaceAUVDriver { * @param thruster_pwm_array vector of pwm values to send */ void send_data_to_escs(const std::vector& thruster_pwm_array); + + static constexpr auto to_kg = [](double force) { return force / 9.80665; }; }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 5054161e4..798870352 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -71,31 +71,38 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { // Convert Newtons to Kg (since the thruster datasheet is in Kg) std::vector forces_in_kg(thruster_forces_array.size()); - for (size_t i = 0; i < thruster_forces_array.size(); ++i) { - forces_in_kg[i] = thruster_forces_array[i] / 9.80665; - } + std::transform(thruster_forces_array.begin(), thruster_forces_array.end(), + forces_in_kg.begin(), to_kg); std::vector interpolated_pwm; - for (size_t i = 0; i < forces_in_kg.size(); ++i) { - double force = forces_in_kg[i]; - double pwm = 0.0; - if (force < 0) { - pwm = LEFT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + - LEFT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + - LEFT_COEFFS[2] * forces_in_kg[i] + LEFT_COEFFS[3]; - } else if (force == 0.0) { - pwm = 1500; - } else { - pwm = RIGHT_COEFFS[0] * std::pow(forces_in_kg[i], 3) + - RIGHT_COEFFS[1] * std::pow(forces_in_kg[i], 2) + - RIGHT_COEFFS[2] * forces_in_kg[i] + RIGHT_COEFFS[3]; - } - interpolated_pwm.push_back(static_cast(pwm)); + for (const double force : forces_in_kg) { + interpolated_pwm.push_back( + force_to_pwm(force, LEFT_COEFFS, RIGHT_COEFFS)); } - return interpolated_pwm; } +std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( + double force, + const std::vector& left_coeffs, + const std::vector& right_coeffs) { + if (force < 0) { + return interpolate_pwm(force, left_coeffs); + } else if (force > 0) { + return interpolate_pwm(force, right_coeffs); + } else { + return 1500; + } +} + +std::int16_t ThrusterInterfaceAUVDriver::interpolate_pwm( + double force, + const std::vector& coeffs) { + return static_cast(coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + coeffs[3]); +} + void ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { uint8_t i2c_data_array[16]; From 711bf9d8fc30485c9f1edad7c5878ed722f1a8e2 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 3 Nov 2024 11:48:00 +0100 Subject: [PATCH 31/54] sub and publisher passed from .yaml --- motion/thruster_interface_auv/.Doxyfile.swp | Bin 20480 -> 0 bytes .../config/thruster_interface_auv_config.yaml | 4 ++ .../src/thruster_interface_auv_ros.cpp | 38 +++++++++++------- 3 files changed, 27 insertions(+), 15 deletions(-) delete mode 100644 motion/thruster_interface_auv/.Doxyfile.swp diff --git a/motion/thruster_interface_auv/.Doxyfile.swp b/motion/thruster_interface_auv/.Doxyfile.swp deleted file mode 100644 index 26a44b005ba7d95ae8f19f5c4ad1e09c3ee014b9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 20480 zcmeI44~!hudBE2g$05YH6j6i_<=GC!hwt|8FfO*~HCfy1^M$+H^WC0}3)XgScXoH` z-JSK!?Co7BbtyuKID{gED7A=739=}qlp;hCMaV%2QG^hploCoUN+=k|Ar26SIK=&Z zZ{F`rY2`H}mGb@BP{D-+7{E%fOIYoxCK$*KZ^e|FPw_OIuDEOl&MC z5>{ox_5xQVzw(Rgex*DT{)uU@d(!m+d#c-N1a7YBcl%!6ywznjCcCR{;JCH!pzJmL z!1i)ZjqgP(Z|6k1!JV2eIu$!=G=7aW5NqJGHP8r(U8@(3ue_wUM=oBrbcs6aH^)Ew zzW9n*1F;5T4a6FVH4tkc)`3vUu@5e?S&Cf`i{5>PT(zL&~ zP5vVzA29NxZSuoL9`)~QUzq#+9~$|fx&Pa3@B~o`I*~DcBEB!V~Z~JO+=#Bk(Xh1P{Uk za6jA!``})<2kwTwa2M=>JK+wv9d3hL;T918a5vlpH^L2YJzNL7;99r_rlAQ9@L?yo zP=y`fKnZNfg9SO*4p+f8*b3t?2Ad%RLvRHQKtH5mJ*42P&v) z&;b|0g|G~kzy)wVoCoK^IdC?d1!uxySOllTX|NC$z4Q_>7;AYqjH^Gf?16&W+!7jKK zu7PQ2LIZr*2`*G&2RKjy8}eX54z|Npuno4tIE=w&$be@Bc0m=~e4}dD0!u2sRMV+c z)P${?o)ZLiEvW{I%2J+PK|E=zx)qd_<0qD>Y}rq*~SaZ9c= zC=I!dpxy{n!SU>T;CjsHAQw~bR{ zn-}e-a@k{^s#mD>^!3&XTTw-}ZL{p;%X$;eJv7+ztu~XK4E$cRs?w40D4=m#r5^>i zF;TI*b~fC=_B&OhR^3R|aI8wPTs9ec55Zd8CG$a2BxF#`o4ok6uol`^joW3U#un?4l_#Ot;^*T`Ht4jfoIkH z@baV@vqi5|@&=sR%#p39r|5X{fP9`|MgtFIS~#GSTEonj%#AN?htCsLg|j53y`k!h zZl&TjWlC)ojh1ur*=R;WULRP39G;QW9+%{_Y)l1dYOFLxQWmElXMW5OBSLxl~sglX$c4Qk- zzpOL%nG@JmU*#?KxJ(r-TlKoFSHIqKYfV$1(vr+RlUb#!94%a%!bZ?b&MGobvcbYF zSGN4JTE479Ha>&$*|HuQwfLnfCWiiijw}qPRAado7zQmCJ#-*DLe_%RESEiNevy3}ZZwN$w~kMJqUl{xu>HK} zOqd1i+d&|kR!kF=s?DB>(A=4OK>E{E(N|^O<#f|6b@lcp*Ccx-Y$5)?_&?8q`2QEn zn|wWupZ_h`4RvtgEATJ)`Tqca4+Z!wSOntt|06sKd*CnO>u?Nz|8HRz394UYarG@tbtequ?Aud#2WbZ z)WC*JHn)B-of;k=nF(Vl)u*|QV%bQkWDr%o_7>{{v z=vo`AQ|#lKi_u)GOwTx61>Vs&;Xh(;i@7g;Sk=#q@g3UQ(q`y+gkCo`F$Qep!D|zI z(()X)fm`oc^|I~&aX_)nXAjUs`{<9{&mcN+@I#|o8~L7Hb%`0uZTjy=tC;eH90@k3 zhts2}Y&tiX=En_@re_qoU$SS!h%uq|&qYL7kwTxyp%J zz&ong{8AtkT0%FX;Gyxs>_9G?8tqSK$D&Zga3)+pCsj4W8Bv>r20Hb3LO@o~Ad0|K z3XX3Pn~(@eq*S@MYVANRq6Du|Q$nOPv)S&|J7!@qXQ!ty^5~B<-ZC63%k!#38y|Jq2Ca$Q9>8t#9!uWpXM` z&F%=JPBb?ri;>wlQH?Sm;WCvM0uydicWU<1Z#^{(GKaH~9Ze07jig4?!`YdY`oB0E zo^2JhcRyhjP$g%A(`8j0OM>bZYeE9uMg?|3!gnaw3OA|>b;tOp@kddRjdM&CjK3dW zrnXw1tn#&LgDt+lEPm4rn;{VYUY{bJgk_laaBr{*!T<=fKFvC{!nSA)t(iFt*9{7b z+;{ektdj{VzoVpsY$hc5$xDi*W9KbgXFVD8Xsc5z$ZjjRj7<(W-^O*GJF~N=WbVAS zwv|)WvCXDK!opHIW>)98s|Dn}Qdi3Hk?S8#jcm$oNRMPU$;9?4{wi0_kpJNh8{$N% zHmYGXX07V!L}^pA-|6k?;mtYc6PG2m|365r^d@pL;{QkW{lCQ5{{{RUeg;2<58!*S zA0#HQ6ut-x;YY*(4ujMLjKBgo6%G*tco!rlkbwmJJAVIb@IBZMJ}iTO#qYltz6-nI zo6rY8#OMDj$iS)aZ}|NG1dqdB_zp;3S#q}zk-ysqyWyKq1IbnX4jf>Nk`L2k{q?jF z;!(#Mh&2#vAlAT1)&RMjD4(W{e~fnzrzutRoE;W9vrd)rEV5nlN;<-^YMpAGXMMxL zxZ=n4R$$?Q@k1Gt(ziP?2Ao=$_>j_w*@hzKqt0*ySZ7g1g2`5qH>hL#(o(5`2_mM@ z++;vxqJ)OEahR|Ee*#3q|naS`=E z(r97kmR?icGdP&pnj0S;Ak&%7N$OO)7=3Cu^XVRH=qHS?SlM&wr81&fNxCSXLuSOP z+l5Z;Z`xB?+#|Wem!x{D@P!Pw z7c6#n|kU@C(yY#YTlCz>+H3I-SOf440Ut%8e4~z$(dbZNU3(!=tlW z!JTrCC)1t8O--kO_h#xp#1*us@+@F= zNl%X{`I;$a2q)C7PitLi-OkZWSi}%{WM$ham0KRGE-yRrF{@V5)>d&KWu~HH6J4Vw zt4jhFvc;k^mq3n`sj<6xQ)()>O{dJ6yL_cVt;tNG+q%(#^u};)$#=cJ2zBLmNSTX8 zeVq8y^3;06qew(ouIOJ)Qd(VM+x2jeIwP&C6hu>=uJ@56rj@}CyAhtLd?beL25!Bz zsU+~EgDge`C8_kAbaV*ELIvZxE@!q5!tT#j0;k&~U6-MUr;k<93XYiDFm+_TNX5$Hf2u diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 9a73f6462..13cb55467 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -23,3 +23,7 @@ thruster_interface_auv_node: i2c: bus: 1 address: 0x21 + + topics: + thruster_forces_subscriber: "thrust/thruster_forces" + pwm_publisher: "pwm" diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index d29043e0c..67f8ae659 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -2,21 +2,6 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { - // create a subscriber that takes data from thruster forces and publisher - // for debugging - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - - this->thruster_forces_subscriber_ = - this->create_subscription( - "thrust/thruster_forces", qos_sensor_data, - std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, - std::placeholders::_1)); - this->thruster_pwm_publisher_ = - this->create_publisher("pwm", - qos_sensor_data); - // from orca.yaml parameters this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping"); @@ -33,6 +18,9 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); + this->declare_parameter("topics.thruster_forces_subscriber"); + this->declare_parameter("topics.pwm_publisher"); + int i2c_bus = this->get_parameter("i2c.bus").as_int(); int i2c_address = this->get_parameter("i2c.address").as_int(); auto thruster_mapping = @@ -55,6 +43,26 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::vector right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + std::string subscriber_name = + this->get_parameter("topics.thruster_forces_subscriber").as_string(); + std::string publisher_name = + this->get_parameter("topics.pwm_publisher").as_string(); + + // create a subscriber that takes data from thruster forces and publisher + // for debugging + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + this->thruster_forces_subscriber_ = + this->create_subscription( + subscriber_name, qos_sensor_data, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, + std::placeholders::_1)); + this->thruster_pwm_publisher_ = + this->create_publisher(publisher_name, + qos_sensor_data); + // Initialize thruster driver this->thruster_driver_ = ThrusterInterfaceAUVDriver( i2c_bus, i2c_address, From 89204faa9a0302aca1062d1603c677719788770c Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 3 Nov 2024 14:25:02 +0100 Subject: [PATCH 32/54] lowercased member variables --- .../thruster_interface_auv_driver.hpp | 52 +++++----- .../src/thruster_interface_auv_driver.cpp | 95 ++++++++++--------- 2 files changed, 74 insertions(+), 73 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index e21539bcc..9542c7214 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -40,28 +40,28 @@ class ThrusterInterfaceAUVDriver { * @brief actually used constructor. Called from ThrusterInterfaceAUVNode * .cpp when instantiating the object, initializes all the params. * - * @param I2C_BUS bus number used to communicate - * @param PICO_I2C_ADDRESS i2c address of the ESC that drive the + * @param i2c_bus bus number used to communicate + * @param pico_i2c_address i2c address of the ESC that drive the * thrusters - * @param THRUSTER_MAPPING pin to motor mapping for the thrusters i.e. if + * @param thruster_mapping pin to motor mapping for the thrusters i.e. if * thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. - * @param THRUSTER_DIRECTION physical mounting direction of the thrusters, + * @param thruster_direction physical mounting direction of the thrusters, * (+-1) - * @param PWM_MIN minimum clamping pwm values - * @param PWM_MAX maximum clamping pwm values - * @param LEFT_COEFFS third order polynomial coefficients when force + * @param pwm_min minimum clamping pwm values + * @param pwm_max maximum clamping pwm values + * @param left_coeffs third order polynomial coefficients when force * < 0 - * @param RIGHT_COEFFS third order polynomial coefficients when force + * @param right_coeffs third order polynomial coefficients when force * > 0 */ - ThrusterInterfaceAUVDriver(short I2C_BUS, - int PICO_I2C_ADDRESS, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, - const std::vector& PWM_MIN, - const std::vector& PWM_MAX, - const std::vector& LEFT_COEFFS, - const std::vector& RIGHT_COEFFS); + ThrusterInterfaceAUVDriver(short i2c_bus, + int pico_i2c_address, + const std::vector& thruster_mapping, + const std::vector& thruster_direction, + const std::vector& pwm_min, + const std::vector& pwm_max, + const std::vector& left_coeffs, + const std::vector& right_coeffs); /** * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to @@ -76,17 +76,17 @@ class ThrusterInterfaceAUVDriver { const std::vector& thruster_forces_array); private: - int bus_fd; ///< file descriptor for the I2C bus (integer >0 that uniquely - ///< identifies the device. -1 if it fails) - short I2C_BUS; - int PICO_I2C_ADDRESS; + int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely + ///< identifies the device. -1 if it fails) + short i2c_bus_; + int pico_i2c_address_; - std::vector THRUSTER_MAPPING; - std::vector THRUSTER_DIRECTION; - std::vector PWM_MIN; - std::vector PWM_MAX; - std::vector LEFT_COEFFS; - std::vector RIGHT_COEFFS; + std::vector thruster_mapping_; + std::vector thruster_direction_; + std::vector pwm_min_; + std::vector pwm_max_; + std::vector left_coeffs_; + std::vector right_coeffs_; /** * @brief [private] method that just take the thruster forces and return PWM diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 798870352..8027145d3 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -3,67 +3,68 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - short I2C_BUS, - int PICO_I2C_ADDRESS, - const std::vector& THRUSTER_MAPPING, - const std::vector& THRUSTER_DIRECTION, - const std::vector& PWM_MIN, - const std::vector& PWM_MAX, - const std::vector& LEFT_COEFFS, - const std::vector& RIGHT_COEFFS) - : I2C_BUS(I2C_BUS), - PICO_I2C_ADDRESS(PICO_I2C_ADDRESS), - THRUSTER_MAPPING(THRUSTER_MAPPING), - THRUSTER_DIRECTION(THRUSTER_DIRECTION), - PWM_MIN(PWM_MIN), - PWM_MAX(PWM_MAX), - LEFT_COEFFS(LEFT_COEFFS), - RIGHT_COEFFS(RIGHT_COEFFS) { - printf("I2C_BUS: %d\n", I2C_BUS); - printf("PICO_I2C_ADDRESS: %d\n", PICO_I2C_ADDRESS); + short i2c_bus, + int pico_i2c_address, + const std::vector& thruster_mapping, + const std::vector& thruster_direction, + const std::vector& pwm_min, + const std::vector& pwm_max, + const std::vector& left_coeffs, + const std::vector& right_coeffs) + : i2c_bus_(i2c_bus), + pico_i2c_address_(pico_i2c_address), + thruster_mapping_(thruster_mapping), + thruster_direction_(thruster_direction), + pwm_min_(pwm_min), + pwm_max_(pwm_max), + left_coeffs_(left_coeffs), + right_coeffs_(right_coeffs) { + printf("I2C_BUS: %d\n", i2c_bus_); + printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); printf("THRUSTER_MAPPING: "); - for (int i = 0; i < THRUSTER_MAPPING.size(); i++) { - printf("%d ", THRUSTER_MAPPING[i]); + for (int i = 0; i < thruster_mapping_.size(); i++) { + printf("%d ", thruster_mapping_[i]); } printf("\n"); printf("THRUSTER_DIRECTION: "); - for (int i = 0; i < THRUSTER_DIRECTION.size(); i++) { - printf("%d ", THRUSTER_DIRECTION[i]); + for (int i = 0; i < thruster_direction_.size(); i++) { + printf("%d ", thruster_direction_[i]); } printf("\n"); printf("PWM_MIN: "); - for (int i = 0; i < PWM_MIN.size(); i++) { - printf("%d ", PWM_MIN[i]); + for (int i = 0; i < pwm_min_.size(); i++) { + printf("%d ", pwm_min_[i]); } printf("\n"); printf("PWM_MAX: "); - for (int i = 0; i < PWM_MAX.size(); i++) { - printf("%d ", PWM_MAX[i]); + for (int i = 0; i < pwm_max_.size(); i++) { + printf("%d ", pwm_max_[i]); } printf("\n"); printf("LEFT_COEFFS: "); - for (int i = 0; i < LEFT_COEFFS.size(); i++) { - printf("%f ", LEFT_COEFFS[i]); + for (int i = 0; i < left_coeffs_.size(); i++) { + printf("%f ", left_coeffs_[i]); } printf("\n"); printf("RIGHT_COEFFS: "); - for (int i = 0; i < RIGHT_COEFFS.size(); i++) { - printf("%f ", RIGHT_COEFFS[i]); + for (int i = 0; i < right_coeffs_.size(); i++) { + printf("%f ", right_coeffs_[i]); } printf("\n"); // Open the I2C bus - std::string i2c_filename = "/dev/i2c-" + std::to_string(I2C_BUS); - bus_fd = open(i2c_filename.c_str(), - O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) - if (bus_fd < 0) { - std::cerr << "ERROR: Failed to open I2C bus " << I2C_BUS << std::endl; + std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); + bus_fd_ = + open(i2c_filename.c_str(), + O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) + if (bus_fd_ < 0) { + std::cerr << "ERROR: Failed to open I2C bus " << i2c_bus_ << std::endl; } } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - if (bus_fd >= 0) { - close(bus_fd); + if (bus_fd_ >= 0) { + close(bus_fd_); } } @@ -77,7 +78,7 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( std::vector interpolated_pwm; for (const double force : forces_in_kg) { interpolated_pwm.push_back( - force_to_pwm(force, LEFT_COEFFS, RIGHT_COEFFS)); + force_to_pwm(force, left_coeffs_, right_coeffs_)); } return interpolated_pwm; } @@ -112,13 +113,13 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( } // Set the I2C slave address - if (ioctl(bus_fd, I2C_SLAVE, PICO_I2C_ADDRESS) < 0) { + if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; return; } // Write data to the I2C device - if (write(bus_fd, i2c_data_array, 16) != 16) { + if (write(bus_fd_, i2c_data_array, 16) != 16) { std::cerr << "ERROR: Failed to write to I2C device" << std::endl; } } @@ -127,9 +128,9 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { // Apply thruster mapping and direction std::vector mapped_forces(thruster_forces_array.size()); - for (size_t i = 0; i < THRUSTER_MAPPING.size(); ++i) { - mapped_forces[i] = - thruster_forces_array[THRUSTER_MAPPING[i]] * THRUSTER_DIRECTION[i]; + for (size_t i = 0; i < thruster_mapping_.size(); ++i) { + mapped_forces[i] = thruster_forces_array[thruster_mapping_[i]] * + thruster_direction_[i]; } // Convert forces to PWM @@ -139,10 +140,10 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { // Clamp the PWM signal - if (thruster_pwm_array[i] < PWM_MIN[i]) { - thruster_pwm_array[i] = PWM_MIN[i]; - } else if (thruster_pwm_array[i] > PWM_MAX[i]) { - thruster_pwm_array[i] = PWM_MAX[i]; + if (thruster_pwm_array[i] < pwm_min_[i]) { + thruster_pwm_array[i] = pwm_min_[i]; + } else if (thruster_pwm_array[i] > pwm_max_[i]) { + thruster_pwm_array[i] = pwm_max_[i]; } } From b21588aa14d1f8645c24b136df5279ff49d64ffc Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 3 Nov 2024 14:38:21 +0100 Subject: [PATCH 33/54] fixed contructor with make_unique --- .../thruster_interface_auv/thruster_interface_auv_driver.hpp | 1 - .../thruster_interface_auv/thruster_interface_auv_ros.hpp | 2 +- .../src/thruster_interface_auv_driver.cpp | 2 -- .../thruster_interface_auv/src/thruster_interface_auv_ros.cpp | 4 ++-- 4 files changed, 3 insertions(+), 6 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 9542c7214..5c8075353 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -33,7 +33,6 @@ class ThrusterInterfaceAUVDriver { * @brief empty default constructor called when declaring the object in the * .hpp file. Doesn't initialize any params. */ - ThrusterInterfaceAUVDriver(); ~ThrusterInterfaceAUVDriver(); /** diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index dec766ed6..7a3b471b3 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -22,7 +22,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { thruster_pwm_publisher_; rclcpp::TimerBase::SharedPtr timer_; - ThrusterInterfaceAUVDriver thruster_driver_; + std::unique_ptr thruster_driver_; std::vector thruster_forces_array_; double thrust_timer_period_; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 8027145d3..ed42b893c 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,7 +1,5 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" -ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver() {} - ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( short i2c_bus, int pico_i2c_address, diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 67f8ae659..30f03f87a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -64,7 +64,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() qos_sensor_data); // Initialize thruster driver - this->thruster_driver_ = ThrusterInterfaceAUVDriver( + this->thruster_driver_ = std::make_unique( i2c_bus, i2c_address, std::vector(thruster_mapping.begin(), thruster_mapping.end()), std::vector(thruster_direction.begin(), @@ -93,7 +93,7 @@ void ThrusterInterfaceAUVNode::thruster_forces_callback( void ThrusterInterfaceAUVNode::timer_callback() { std::vector thruster_pwm_array = - thruster_driver_.drive_thrusters(this->thruster_forces_array_); + thruster_driver_->drive_thrusters(this->thruster_forces_array_); // publish PWM values for debugging std_msgs::msg::Int16MultiArray pwm_message; From f0ce55b28ed53e1a8f7c15dfe10da2cc3c3d7acb Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 3 Nov 2024 16:25:56 +0100 Subject: [PATCH 34/54] try except --- .../thruster_interface_auv_driver.hpp | 5 ++++ .../src/thruster_interface_auv_driver.cpp | 25 +++++++++++++------ 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 5c8075353..40b25ab46 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -115,6 +115,11 @@ class ThrusterInterfaceAUVDriver { void send_data_to_escs(const std::vector& thruster_pwm_array); static constexpr auto to_kg = [](double force) { return force / 9.80665; }; + static constexpr auto pwm_to_i2c_data = + [](std::int16_t pwm) -> std::array { + return {static_cast((pwm >> 8) & 0xFF), + static_cast(pwm & 0xFF)}; + }; }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index ed42b893c..fbaf6c17a 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -104,11 +104,17 @@ std::int16_t ThrusterInterfaceAUVDriver::interpolate_pwm( void ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { - uint8_t i2c_data_array[16]; - for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { - i2c_data_array[2 * i] = (thruster_pwm_array[i] >> 8) & 0xFF; // MSB - i2c_data_array[2 * i + 1] = thruster_pwm_array[i] & 0xFF; // LSB - } + constexpr std::size_t i2c_data_size = + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) + std::vector i2c_data_array; + i2c_data_array.reserve(i2c_data_size); + + std::for_each(thruster_pwm_array.begin(), thruster_pwm_array.end(), + [&](std::int16_t pwm) { + std::array bytes = pwm_to_i2c_data(pwm); + std::copy(bytes.begin(), bytes.end(), + std::back_inserter(i2c_data_array)); + }); // Set the I2C slave address if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { @@ -117,7 +123,7 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( } // Write data to the I2C device - if (write(bus_fd_, i2c_data_array, 16) != 16) { + if (write(bus_fd_, i2c_data_array.data(), 16) != 16) { std::cerr << "ERROR: Failed to write to I2C device" << std::endl; } } @@ -145,11 +151,14 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( } } - // Send data through I2C try { send_data_to_escs(thruster_pwm_array); + } catch (const std::exception& e) { + std::cerr << "ERROR: Failed to send PWM values - " << e.what() + << std::endl; } catch (...) { - std::cerr << "ERROR: Failed to send PWM values" << std::endl; + std::cerr << "ERROR: Failed to send PWM values - Unknown exception" + << std::endl; } return thruster_pwm_array; From daec7c10c2da2e220453636d6fe1eb7f229fc8bc Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 3 Nov 2024 17:11:38 +0100 Subject: [PATCH 35/54] added exceptions --- .../thruster_interface_auv_driver.hpp | 2 ++ .../src/thruster_interface_auv_driver.cpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 40b25ab46..34c91257f 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -86,6 +87,7 @@ class ThrusterInterfaceAUVDriver { std::vector pwm_max_; std::vector left_coeffs_; std::vector right_coeffs_; + bool DEBUGGING_; /** * @brief [private] method that just take the thruster forces and return PWM diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index fbaf6c17a..4ff5f32fd 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -48,7 +48,6 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( for (int i = 0; i < right_coeffs_.size(); i++) { printf("%f ", right_coeffs_[i]); } - printf("\n"); // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); @@ -56,7 +55,9 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( open(i2c_filename.c_str(), O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) if (bus_fd_ < 0) { - std::cerr << "ERROR: Failed to open I2C bus " << i2c_bus_ << std::endl; + std::runtime_error("ERROR: Failed to open I2C bus " + + std::to_string(i2c_bus_) + " : " + + std::string(strerror(errno))); } } @@ -118,13 +119,16 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( // Set the I2C slave address if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { - std::cerr << "ERROR: Failed to set I2C slave address" << std::endl; + throw std::runtime_error("Failed to open I2C bus " + + std::to_string(i2c_bus_) + " : " + + std::string(strerror(errno))); return; } // Write data to the I2C device if (write(bus_fd_, i2c_data_array.data(), 16) != 16) { - std::cerr << "ERROR: Failed to write to I2C device" << std::endl; + throw std::runtime_error("ERROR: Failed to write to I2C device : " + + std::string(strerror(errno))); } } From bef931dd99ad8651067fe1302fb5b4e74c3bd1d6 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 8 Nov 2024 16:00:29 +0100 Subject: [PATCH 36/54] args passed by struct --- .../thruster_interface_auv_driver.hpp | 40 +++++------ .../src/thruster_interface_auv_driver.cpp | 72 +++++++++---------- .../src/thruster_interface_auv_ros.cpp | 31 +++++--- 3 files changed, 76 insertions(+), 67 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 34c91257f..b179f7edc 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -13,6 +13,17 @@ #include #include +struct ThrusterParameters { + std::vector mapping; + std::vector direction; + std::vector pwm_min; + std::vector pwm_max; +}; +struct Coeffs { + std::vector left; + std::vector right; +}; + /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be @@ -54,15 +65,11 @@ class ThrusterInterfaceAUVDriver { * @param right_coeffs third order polynomial coefficients when force * > 0 */ - ThrusterInterfaceAUVDriver(short i2c_bus, - int pico_i2c_address, - const std::vector& thruster_mapping, - const std::vector& thruster_direction, - const std::vector& pwm_min, - const std::vector& pwm_max, - const std::vector& left_coeffs, - const std::vector& right_coeffs); - + ThrusterInterfaceAUVDriver( + short i2c_bus, + int pico_i2c_address, + const std::vector& thruster_parameters, + const std::vector& approx_poly_coeffs); /** * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values 2) send_data_to_escs() to send @@ -78,16 +85,11 @@ class ThrusterInterfaceAUVDriver { private: int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely ///< identifies the device. -1 if it fails) + short i2c_bus_; int pico_i2c_address_; - - std::vector thruster_mapping_; - std::vector thruster_direction_; - std::vector pwm_min_; - std::vector pwm_max_; - std::vector left_coeffs_; - std::vector right_coeffs_; - bool DEBUGGING_; + std::vector thruster_parameters_; + std::vector approx_poly_coeffs_; /** * @brief [private] method that just take the thruster forces and return PWM @@ -101,9 +103,7 @@ class ThrusterInterfaceAUVDriver { std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); - std::int16_t force_to_pwm(double force, - const std::vector& left_coeffs, - const std::vector& right_coeffs); + std::int16_t force_to_pwm(double force, const std::vector& coeffs); std::int16_t interpolate_pwm(double force, const std::vector& coeffs); diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 4ff5f32fd..c4aa70a27 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -3,51 +3,46 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( short i2c_bus, int pico_i2c_address, - const std::vector& thruster_mapping, - const std::vector& thruster_direction, - const std::vector& pwm_min, - const std::vector& pwm_max, - const std::vector& left_coeffs, - const std::vector& right_coeffs) + const std::vector& thruster_parameters, + const std::vector& approx_poly_coeffs) : i2c_bus_(i2c_bus), pico_i2c_address_(pico_i2c_address), - thruster_mapping_(thruster_mapping), - thruster_direction_(thruster_direction), - pwm_min_(pwm_min), - pwm_max_(pwm_max), - left_coeffs_(left_coeffs), - right_coeffs_(right_coeffs) { + thruster_parameters_(thruster_parameters), + approx_poly_coeffs_(approx_poly_coeffs) + +{ printf("I2C_BUS: %d\n", i2c_bus_); printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); printf("THRUSTER_MAPPING: "); - for (int i = 0; i < thruster_mapping_.size(); i++) { - printf("%d ", thruster_mapping_[i]); + for (size_t i = 0; i < thruster_parameters[0].mapping.size(); ++i) { + printf("%d ", thruster_parameters[0].mapping[i]); } printf("\n"); printf("THRUSTER_DIRECTION: "); - for (int i = 0; i < thruster_direction_.size(); i++) { - printf("%d ", thruster_direction_[i]); + for (size_t i = 0; i < thruster_parameters[0].direction.size(); ++i) { + printf("%d ", thruster_parameters[0].direction[i]); } printf("\n"); - printf("PWM_MIN: "); - for (int i = 0; i < pwm_min_.size(); i++) { - printf("%d ", pwm_min_[i]); + printf("THRUSTER_PWM_MIN: "); + for (size_t i = 0; i < thruster_parameters[0].pwm_min.size(); ++i) { + printf("%d ", thruster_parameters[0].pwm_min[i]); } printf("\n"); - printf("PWM_MAX: "); - for (int i = 0; i < pwm_max_.size(); i++) { - printf("%d ", pwm_max_[i]); + printf("THRUSTER_PWM_MAX: "); + for (size_t i = 0; i < thruster_parameters[0].pwm_max.size(); ++i) { + printf("%d ", thruster_parameters[0].pwm_max[i]); } printf("\n"); printf("LEFT_COEFFS: "); - for (int i = 0; i < left_coeffs_.size(); i++) { - printf("%f ", left_coeffs_[i]); + for (size_t i = 0; i < approx_poly_coeffs[0].left.size(); ++i) { + printf("%f ", approx_poly_coeffs[0].left[i]); } printf("\n"); printf("RIGHT_COEFFS: "); - for (int i = 0; i < right_coeffs_.size(); i++) { - printf("%f ", right_coeffs_[i]); + for (size_t i = 0; i < approx_poly_coeffs[0].right.size(); ++i) { + printf("%f ", approx_poly_coeffs[0].right[i]); } + printf("\n"); // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); @@ -76,20 +71,18 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( std::vector interpolated_pwm; for (const double force : forces_in_kg) { - interpolated_pwm.push_back( - force_to_pwm(force, left_coeffs_, right_coeffs_)); + interpolated_pwm.push_back(force_to_pwm(force, approx_poly_coeffs_)); } return interpolated_pwm; } std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( double force, - const std::vector& left_coeffs, - const std::vector& right_coeffs) { + const std::vector& coeffs) { if (force < 0) { - return interpolate_pwm(force, left_coeffs); + return interpolate_pwm(force, coeffs[0].left); } else if (force > 0) { - return interpolate_pwm(force, right_coeffs); + return interpolate_pwm(force, coeffs[0].right); } else { return 1500; } @@ -136,9 +129,10 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { // Apply thruster mapping and direction std::vector mapped_forces(thruster_forces_array.size()); - for (size_t i = 0; i < thruster_mapping_.size(); ++i) { - mapped_forces[i] = thruster_forces_array[thruster_mapping_[i]] * - thruster_direction_[i]; + for (size_t i = 0; i < thruster_parameters_[0].mapping.size(); ++i) { + mapped_forces[i] = + thruster_forces_array[thruster_parameters_[0].mapping[i]] * + thruster_parameters_[0].direction[i]; } // Convert forces to PWM @@ -148,10 +142,10 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { // Clamp the PWM signal - if (thruster_pwm_array[i] < pwm_min_[i]) { - thruster_pwm_array[i] = pwm_min_[i]; - } else if (thruster_pwm_array[i] > pwm_max_[i]) { - thruster_pwm_array[i] = pwm_max_[i]; + if (thruster_pwm_array[i] < thruster_parameters_[0].pwm_min[i]) { + thruster_pwm_array[i] = thruster_parameters_[0].pwm_min[i]; + } else if (thruster_pwm_array[i] > thruster_parameters_[0].pwm_max[i]) { + thruster_pwm_array[i] = thruster_parameters_[0].pwm_max[i]; } } diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 30f03f87a..20be21fee 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -63,16 +63,31 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() this->create_publisher(publisher_name, qos_sensor_data); + ThrusterParameters thruster_parameters_struct; + thruster_parameters_struct.mapping = + std::vector(thruster_mapping.begin(), thruster_mapping.end()); + thruster_parameters_struct.direction = std::vector( + thruster_direction.begin(), thruster_direction.end()); + thruster_parameters_struct.pwm_min = + std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()); + thruster_parameters_struct.pwm_max = + std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()); + + Coeffs coeffs_struct; + coeffs_struct.left = + std::vector(left_coeffs.begin(), left_coeffs.end()); + coeffs_struct.right = + std::vector(right_coeffs.begin(), right_coeffs.end()); + + // Create a vector of ThrusterParameters + std::vector thruster_parameters; + thruster_parameters.push_back(thruster_parameters_struct); + std::vector coeffs; + coeffs.push_back(coeffs_struct); + // Initialize thruster driver this->thruster_driver_ = std::make_unique( - i2c_bus, i2c_address, - std::vector(thruster_mapping.begin(), thruster_mapping.end()), - std::vector(thruster_direction.begin(), - thruster_direction.end()), - std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()), - std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()), - std::vector(left_coeffs.begin(), left_coeffs.end()), - std::vector(right_coeffs.begin(), right_coeffs.end())); + i2c_bus, i2c_address, thruster_parameters, coeffs); // Declare "thruster_forces_array" in case no topic comes in at the // beginning From 5a3dae14cf58bdf2e813c153ac08816798748042 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 8 Nov 2024 16:33:53 +0100 Subject: [PATCH 37/54] separate init function that extracts yaml --- .../thruster_interface_auv_driver.hpp | 19 +-- .../thruster_interface_auv_ros.hpp | 17 ++- .../src/thruster_interface_auv_driver.cpp | 32 ++--- .../src/thruster_interface_auv_ros.cpp | 122 +++++++++--------- 4 files changed, 100 insertions(+), 90 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index b179f7edc..63c746485 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -19,7 +19,7 @@ struct ThrusterParameters { std::vector pwm_min; std::vector pwm_max; }; -struct Coeffs { +struct PolyCoeffs { std::vector left; std::vector right; }; @@ -69,7 +69,7 @@ class ThrusterInterfaceAUVDriver { short i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector& approx_poly_coeffs); + const std::vector& approx_poly_coeffs); /** * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values 2) send_data_to_escs() to send @@ -86,10 +86,10 @@ class ThrusterInterfaceAUVDriver { int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely ///< identifies the device. -1 if it fails) - short i2c_bus_; + int i2c_bus_; int pico_i2c_address_; std::vector thruster_parameters_; - std::vector approx_poly_coeffs_; + std::vector poly_coeffs_; /** * @brief [private] method that just take the thruster forces and return PWM @@ -103,7 +103,8 @@ class ThrusterInterfaceAUVDriver { std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); - std::int16_t force_to_pwm(double force, const std::vector& coeffs); + std::int16_t force_to_pwm(double force, + const std::vector& coeffs); std::int16_t interpolate_pwm(double force, const std::vector& coeffs); @@ -116,12 +117,12 @@ class ThrusterInterfaceAUVDriver { */ void send_data_to_escs(const std::vector& thruster_pwm_array); - static constexpr auto to_kg = [](double force) { return force / 9.80665; }; - static constexpr auto pwm_to_i2c_data = - [](std::int16_t pwm) -> std::array { + static constexpr double to_kg(double force) { return force / 9.80665; } + static constexpr std::array pwm_to_i2c_data( + std::int16_t pwm) { return {static_cast((pwm >> 8) & 0xFF), static_cast(pwm & 0xFF)}; - }; + } }; #endif // THRUSTER_INTERFACE_AUV_DRIVER_HPP diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 7a3b471b3..02915cd3c 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -15,17 +15,24 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { void thruster_forces_callback( const vortex_msgs::msg::ThrusterForces::SharedPtr msg); void timer_callback(); + void extract_all_parameters(); + int i2c_bus_; + int i2c_address_; + std::string subscriber_topic_name_; + std::string publisher_topic_name_; + std::vector thruster_parameters_; + std::vector poly_coeffs_; + + std::vector thruster_forces_array_; + double thrust_timer_period_; + + std::unique_ptr thruster_driver_; rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; rclcpp::Publisher::SharedPtr thruster_pwm_publisher_; rclcpp::TimerBase::SharedPtr timer_; - - std::unique_ptr thruster_driver_; - - std::vector thruster_forces_array_; - double thrust_timer_period_; }; #endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index c4aa70a27..0a016a87d 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -4,43 +4,43 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( short i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector& approx_poly_coeffs) + const std::vector& poly_coeffs) : i2c_bus_(i2c_bus), pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), - approx_poly_coeffs_(approx_poly_coeffs) + poly_coeffs_(poly_coeffs) { printf("I2C_BUS: %d\n", i2c_bus_); printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); printf("THRUSTER_MAPPING: "); - for (size_t i = 0; i < thruster_parameters[0].mapping.size(); ++i) { - printf("%d ", thruster_parameters[0].mapping[i]); + for (size_t i = 0; i < thruster_parameters_[0].mapping.size(); ++i) { + printf("%d ", thruster_parameters_[0].mapping[i]); } printf("\n"); printf("THRUSTER_DIRECTION: "); - for (size_t i = 0; i < thruster_parameters[0].direction.size(); ++i) { - printf("%d ", thruster_parameters[0].direction[i]); + for (size_t i = 0; i < thruster_parameters_[0].direction.size(); ++i) { + printf("%d ", thruster_parameters_[0].direction[i]); } printf("\n"); printf("THRUSTER_PWM_MIN: "); - for (size_t i = 0; i < thruster_parameters[0].pwm_min.size(); ++i) { - printf("%d ", thruster_parameters[0].pwm_min[i]); + for (size_t i = 0; i < thruster_parameters_[0].pwm_min.size(); ++i) { + printf("%d ", thruster_parameters_[0].pwm_min[i]); } printf("\n"); printf("THRUSTER_PWM_MAX: "); - for (size_t i = 0; i < thruster_parameters[0].pwm_max.size(); ++i) { - printf("%d ", thruster_parameters[0].pwm_max[i]); + for (size_t i = 0; i < thruster_parameters_[0].pwm_max.size(); ++i) { + printf("%d ", thruster_parameters_[0].pwm_max[i]); } printf("\n"); printf("LEFT_COEFFS: "); - for (size_t i = 0; i < approx_poly_coeffs[0].left.size(); ++i) { - printf("%f ", approx_poly_coeffs[0].left[i]); + for (size_t i = 0; i < poly_coeffs_[0].left.size(); ++i) { + printf("%f ", poly_coeffs_[0].left[i]); } printf("\n"); printf("RIGHT_COEFFS: "); - for (size_t i = 0; i < approx_poly_coeffs[0].right.size(); ++i) { - printf("%f ", approx_poly_coeffs[0].right[i]); + for (size_t i = 0; i < poly_coeffs_[0].right.size(); ++i) { + printf("%f ", poly_coeffs_[0].right[i]); } printf("\n"); @@ -71,14 +71,14 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( std::vector interpolated_pwm; for (const double force : forces_in_kg) { - interpolated_pwm.push_back(force_to_pwm(force, approx_poly_coeffs_)); + interpolated_pwm.push_back(force_to_pwm(force, poly_coeffs_)); } return interpolated_pwm; } std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( double force, - const std::vector& coeffs) { + const std::vector& coeffs) { if (force < 0) { return interpolate_pwm(force, coeffs[0].left); } else if (force > 0) { diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 20be21fee..c62bcf1e1 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -2,6 +2,55 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { + this->extract_all_parameters(); + + // create a subscriber that takes data from thruster forces and publisher + // for debugging + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + this->thruster_forces_subscriber_ = + this->create_subscription( + subscriber_topic_name_, qos_sensor_data, + std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, + std::placeholders::_1)); + this->thruster_pwm_publisher_ = + this->create_publisher( + publisher_topic_name_, qos_sensor_data); + + // Initialize thruster driver + this->thruster_driver_ = std::make_unique( + i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + + // Declare "thruster_forces_array" in case no topic comes in at the + // beginning + this->thruster_forces_array_ = std::vector(8, 0.00); + + timer_ = this->create_wall_timer( + std::chrono::duration(this->thrust_timer_period_), + std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), + "\"thruster_interface_auv_node\" has been started"); +} + +void ThrusterInterfaceAUVNode::thruster_forces_callback( + const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { + this->thruster_forces_array_ = msg->thrust; +} + +void ThrusterInterfaceAUVNode::timer_callback() { + std::vector thruster_pwm_array = + thruster_driver_->drive_thrusters(this->thruster_forces_array_); + + // publish PWM values for debugging + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array; + thruster_pwm_publisher_->publish(pwm_message); +} + +void ThrusterInterfaceAUVNode::extract_all_parameters() { // from orca.yaml parameters this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping"); @@ -13,16 +62,22 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() "propulsion.thrusters.thruster_PWM_max"); this->declare_parameter("propulsion.thrusters.thrust_update_rate"); // from thruster_interface_auv.yaml parameters - this->declare_parameter("i2c.bus"); - this->declare_parameter("i2c.address"); + this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); + this->declare_parameter("i2c.bus"); + this->declare_parameter("i2c.address"); + this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); + this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + this->declare_parameter("topics.thruster_forces_subscriber"); this->declare_parameter("topics.pwm_publisher"); + this->subscriber_topic_name_ = + this->get_parameter("topics.thruster_forces_subscriber").as_string(); + this->publisher_topic_name_ = + this->get_parameter("topics.pwm_publisher").as_string(); - int i2c_bus = this->get_parameter("i2c.bus").as_int(); - int i2c_address = this->get_parameter("i2c.address").as_int(); auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") .as_integer_array(); @@ -43,26 +98,6 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::vector right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - std::string subscriber_name = - this->get_parameter("topics.thruster_forces_subscriber").as_string(); - std::string publisher_name = - this->get_parameter("topics.pwm_publisher").as_string(); - - // create a subscriber that takes data from thruster forces and publisher - // for debugging - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - - this->thruster_forces_subscriber_ = - this->create_subscription( - subscriber_name, qos_sensor_data, - std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, - std::placeholders::_1)); - this->thruster_pwm_publisher_ = - this->create_publisher(publisher_name, - qos_sensor_data); - ThrusterParameters thruster_parameters_struct; thruster_parameters_struct.mapping = std::vector(thruster_mapping.begin(), thruster_mapping.end()); @@ -73,45 +108,12 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() thruster_parameters_struct.pwm_max = std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()); - Coeffs coeffs_struct; + PolyCoeffs coeffs_struct; coeffs_struct.left = std::vector(left_coeffs.begin(), left_coeffs.end()); coeffs_struct.right = std::vector(right_coeffs.begin(), right_coeffs.end()); - // Create a vector of ThrusterParameters - std::vector thruster_parameters; - thruster_parameters.push_back(thruster_parameters_struct); - std::vector coeffs; - coeffs.push_back(coeffs_struct); - - // Initialize thruster driver - this->thruster_driver_ = std::make_unique( - i2c_bus, i2c_address, thruster_parameters, coeffs); - - // Declare "thruster_forces_array" in case no topic comes in at the - // beginning - this->thruster_forces_array_ = std::vector(8, 0.00); - - timer_ = this->create_wall_timer( - std::chrono::duration(this->thrust_timer_period_), - std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - - RCLCPP_INFO(this->get_logger(), - "\"thruster_interface_auv_node\" has been started"); -} - -void ThrusterInterfaceAUVNode::thruster_forces_callback( - const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - this->thruster_forces_array_ = msg->thrust; -} - -void ThrusterInterfaceAUVNode::timer_callback() { - std::vector thruster_pwm_array = - thruster_driver_->drive_thrusters(this->thruster_forces_array_); - - // publish PWM values for debugging - std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array; - thruster_pwm_publisher_->publish(pwm_message); + thruster_parameters_.push_back(thruster_parameters_struct); + poly_coeffs_.push_back(coeffs_struct); } From 8981c0ffec228dbdba415271285e59fdbd072597 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 8 Nov 2024 16:53:06 +0100 Subject: [PATCH 38/54] cleaned a bitttt --- .../thruster_interface_auv_driver.hpp | 3 +- .../src/thruster_interface_auv_driver.cpp | 8 +-- .../src/thruster_interface_auv_ros.cpp | 54 +++++++++++-------- 3 files changed, 39 insertions(+), 26 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 63c746485..6d3f7c49d 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -13,9 +13,10 @@ #include #include +#define IDLE_PWM_VALUE 1500 struct ThrusterParameters { std::vector mapping; - std::vector direction; + std::vector direction; std::vector pwm_min; std::vector pwm_max; }; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 0a016a87d..0e18ececb 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -8,9 +8,8 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( : i2c_bus_(i2c_bus), pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), - poly_coeffs_(poly_coeffs) - -{ + poly_coeffs_(poly_coeffs) { + // TO BE REMOVED---------------------------------------------------- printf("I2C_BUS: %d\n", i2c_bus_); printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); printf("THRUSTER_MAPPING: "); @@ -43,6 +42,7 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( printf("%f ", poly_coeffs_[0].right[i]); } printf("\n"); + //---------------------------------------------------------------- // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); @@ -84,7 +84,7 @@ std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( } else if (force > 0) { return interpolate_pwm(force, coeffs[0].right); } else { - return 1500; + return IDLE_PWM_VALUE; // 1500 } } diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index c62bcf1e1..3a5fb516b 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -2,29 +2,32 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() : Node("thruster_interface_auv_node") { + // extract all .yaml parameters this->extract_all_parameters(); - // create a subscriber that takes data from thruster forces and publisher - // for debugging + // Set up subscriber and publisher rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); this->thruster_forces_subscriber_ = this->create_subscription( - subscriber_topic_name_, qos_sensor_data, + this->subscriber_topic_name_, qos_sensor_data, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); + + // publisher only for debugging this->thruster_pwm_publisher_ = this->create_publisher( - publisher_topic_name_, qos_sensor_data); + this->publisher_topic_name_, qos_sensor_data); - // Initialize thruster driver + // call constructor for thruster_driver_ this->thruster_driver_ = std::make_unique( - i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + this->i2c_bus_, this->i2c_address_, this->thruster_parameters_, + this->poly_coeffs_); - // Declare "thruster_forces_array" in case no topic comes in at the - // beginning + // Declare thruster_forces_array_ in case no topic comes at the + // very beginning this->thruster_forces_array_ = std::vector(8, 0.00); timer_ = this->create_wall_timer( @@ -32,7 +35,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); RCLCPP_INFO(this->get_logger(), - "\"thruster_interface_auv_node\" has been started"); + "\"thruster_interface_auv_node\" correctly initialized"); } void ThrusterInterfaceAUVNode::thruster_forces_callback( @@ -41,17 +44,18 @@ void ThrusterInterfaceAUVNode::thruster_forces_callback( } void ThrusterInterfaceAUVNode::timer_callback() { + // drive thrusters... std::vector thruster_pwm_array = thruster_driver_->drive_thrusters(this->thruster_forces_array_); - // publish PWM values for debugging + //..and publish PWM values for debugging purposes std_msgs::msg::Int16MultiArray pwm_message; pwm_message.data = thruster_pwm_array; thruster_pwm_publisher_->publish(pwm_message); } void ThrusterInterfaceAUVNode::extract_all_parameters() { - // from orca.yaml parameters + // thruster parameters from orca.yaml this->declare_parameter>( "propulsion.thrusters.thruster_to_pin_mapping"); this->declare_parameter>( @@ -61,23 +65,21 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->declare_parameter>( "propulsion.thrusters.thruster_PWM_max"); this->declare_parameter("propulsion.thrusters.thrust_update_rate"); - // from thruster_interface_auv.yaml parameters + // approx poly coeffs for 16V from thruster_interface_auv.yaml this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); + // i2c parameters this->declare_parameter("i2c.bus"); this->declare_parameter("i2c.address"); - this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); - this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + // topics this->declare_parameter("topics.thruster_forces_subscriber"); this->declare_parameter("topics.pwm_publisher"); - this->subscriber_topic_name_ = - this->get_parameter("topics.thruster_forces_subscriber").as_string(); - this->publisher_topic_name_ = - this->get_parameter("topics.pwm_publisher").as_string(); + //----------------------------------------------------------------------- + // get them auto thruster_mapping = this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") .as_integer_array(); @@ -93,15 +95,25 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->thrust_timer_period_ = 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate") .as_double(); + std::vector left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); std::vector right_coeffs = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); + this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); + this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + + this->subscriber_topic_name_ = + this->get_parameter("topics.thruster_forces_subscriber").as_string(); + this->publisher_topic_name_ = + this->get_parameter("topics.pwm_publisher").as_string(); + + // create ThrusterParameters and PolyCoeffs structs ThrusterParameters thruster_parameters_struct; thruster_parameters_struct.mapping = std::vector(thruster_mapping.begin(), thruster_mapping.end()); - thruster_parameters_struct.direction = std::vector( + thruster_parameters_struct.direction = std::vector( thruster_direction.begin(), thruster_direction.end()); thruster_parameters_struct.pwm_min = std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()); @@ -114,6 +126,6 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { coeffs_struct.right = std::vector(right_coeffs.begin(), right_coeffs.end()); - thruster_parameters_.push_back(thruster_parameters_struct); - poly_coeffs_.push_back(coeffs_struct); + this->thruster_parameters_.push_back(thruster_parameters_struct); + this->poly_coeffs_.push_back(coeffs_struct); } From 02d5e30da2846a61df2208f38a91d305e8b3cb87 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 8 Nov 2024 17:30:57 +0100 Subject: [PATCH 39/54] changed structure --- .../thruster_interface_auv_driver.hpp | 20 +++---- .../thruster_interface_auv_ros.hpp | 2 +- .../src/thruster_interface_auv_driver.cpp | 56 ++++++++++--------- .../src/thruster_interface_auv_ros.cpp | 32 +++++------ 4 files changed, 52 insertions(+), 58 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 6d3f7c49d..9add06969 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -15,16 +15,14 @@ #define IDLE_PWM_VALUE 1500 struct ThrusterParameters { - std::vector mapping; - std::vector direction; - std::vector pwm_min; - std::vector pwm_max; -}; -struct PolyCoeffs { - std::vector left; - std::vector right; + uint8_t mapping; + int8_t direction; + uint16_t pwm_min; + uint16_t pwm_max; }; +enum PolySide { LEFT = 0, RIGHT = 1 }; + /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be @@ -70,7 +68,7 @@ class ThrusterInterfaceAUVDriver { short i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector& approx_poly_coeffs); + const std::vector>& approx_poly_coeffs); /** * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values 2) send_data_to_escs() to send @@ -90,7 +88,7 @@ class ThrusterInterfaceAUVDriver { int i2c_bus_; int pico_i2c_address_; std::vector thruster_parameters_; - std::vector poly_coeffs_; + std::vector> poly_coeffs_; /** * @brief [private] method that just take the thruster forces and return PWM @@ -105,7 +103,7 @@ class ThrusterInterfaceAUVDriver { const std::vector& thruster_forces_array); std::int16_t force_to_pwm(double force, - const std::vector& coeffs); + const std::vector>& coeffs); std::int16_t interpolate_pwm(double force, const std::vector& coeffs); diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 02915cd3c..9375074bf 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -22,7 +22,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { std::string subscriber_topic_name_; std::string publisher_topic_name_; std::vector thruster_parameters_; - std::vector poly_coeffs_; + std::vector> poly_coeffs_; std::vector thruster_forces_array_; double thrust_timer_period_; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 0e18ececb..9f92c6384 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -4,7 +4,7 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( short i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector& poly_coeffs) + const std::vector>& poly_coeffs) : i2c_bus_(i2c_bus), pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), @@ -12,34 +12,36 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( // TO BE REMOVED---------------------------------------------------- printf("I2C_BUS: %d\n", i2c_bus_); printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); - printf("THRUSTER_MAPPING: "); - for (size_t i = 0; i < thruster_parameters_[0].mapping.size(); ++i) { - printf("%d ", thruster_parameters_[0].mapping[i]); + + printf("MAPPING: "); + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { + printf("%d ", thruster_parameters_[i].mapping); } printf("\n"); - printf("THRUSTER_DIRECTION: "); - for (size_t i = 0; i < thruster_parameters_[0].direction.size(); ++i) { - printf("%d ", thruster_parameters_[0].direction[i]); + printf("DIRECTION: "); + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { + printf("%d ", thruster_parameters_[i].direction); } printf("\n"); - printf("THRUSTER_PWM_MIN: "); - for (size_t i = 0; i < thruster_parameters_[0].pwm_min.size(); ++i) { - printf("%d ", thruster_parameters_[0].pwm_min[i]); + printf("PWM_MIN: "); + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { + printf("%d ", thruster_parameters_[i].pwm_min); } printf("\n"); - printf("THRUSTER_PWM_MAX: "); - for (size_t i = 0; i < thruster_parameters_[0].pwm_max.size(); ++i) { - printf("%d ", thruster_parameters_[0].pwm_max[i]); + printf("PWM_MAX: "); + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { + printf("%d ", thruster_parameters_[i].pwm_max); } printf("\n"); + printf("LEFT_COEFFS: "); - for (size_t i = 0; i < poly_coeffs_[0].left.size(); ++i) { - printf("%f ", poly_coeffs_[0].left[i]); + for (size_t i = 0; i < poly_coeffs_[LEFT].size(); ++i) { + printf("%f ", poly_coeffs_[LEFT][i]); } printf("\n"); printf("RIGHT_COEFFS: "); - for (size_t i = 0; i < poly_coeffs_[0].right.size(); ++i) { - printf("%f ", poly_coeffs_[0].right[i]); + for (size_t i = 0; i < poly_coeffs_[RIGHT].size(); ++i) { + printf("%f ", poly_coeffs_[RIGHT][i]); } printf("\n"); //---------------------------------------------------------------- @@ -78,11 +80,11 @@ std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( double force, - const std::vector& coeffs) { + const std::vector>& coeffs) { if (force < 0) { - return interpolate_pwm(force, coeffs[0].left); + return interpolate_pwm(force, coeffs[LEFT]); } else if (force > 0) { - return interpolate_pwm(force, coeffs[0].right); + return interpolate_pwm(force, coeffs[RIGHT]); } else { return IDLE_PWM_VALUE; // 1500 } @@ -129,10 +131,10 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { // Apply thruster mapping and direction std::vector mapped_forces(thruster_forces_array.size()); - for (size_t i = 0; i < thruster_parameters_[0].mapping.size(); ++i) { + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { mapped_forces[i] = - thruster_forces_array[thruster_parameters_[0].mapping[i]] * - thruster_parameters_[0].direction[i]; + thruster_forces_array[thruster_parameters_[i].mapping] * + thruster_parameters_[i].direction; } // Convert forces to PWM @@ -142,10 +144,10 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( // Apply thruster offset and limit PWM if needed for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { // Clamp the PWM signal - if (thruster_pwm_array[i] < thruster_parameters_[0].pwm_min[i]) { - thruster_pwm_array[i] = thruster_parameters_[0].pwm_min[i]; - } else if (thruster_pwm_array[i] > thruster_parameters_[0].pwm_max[i]) { - thruster_pwm_array[i] = thruster_parameters_[0].pwm_max[i]; + if (thruster_pwm_array[i] < thruster_parameters_[i].pwm_min) { + thruster_pwm_array[i] = thruster_parameters_[i].pwm_min; + } else if (thruster_pwm_array[i] > thruster_parameters_[i].pwm_max) { + thruster_pwm_array[i] = thruster_parameters_[i].pwm_max; } } diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 3a5fb516b..a9b571b8c 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -109,23 +109,17 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->publisher_topic_name_ = this->get_parameter("topics.pwm_publisher").as_string(); - // create ThrusterParameters and PolyCoeffs structs - ThrusterParameters thruster_parameters_struct; - thruster_parameters_struct.mapping = - std::vector(thruster_mapping.begin(), thruster_mapping.end()); - thruster_parameters_struct.direction = std::vector( - thruster_direction.begin(), thruster_direction.end()); - thruster_parameters_struct.pwm_min = - std::vector(thruster_PWM_min.begin(), thruster_PWM_min.end()); - thruster_parameters_struct.pwm_max = - std::vector(thruster_PWM_max.begin(), thruster_PWM_max.end()); - - PolyCoeffs coeffs_struct; - coeffs_struct.left = - std::vector(left_coeffs.begin(), left_coeffs.end()); - coeffs_struct.right = - std::vector(right_coeffs.begin(), right_coeffs.end()); - - this->thruster_parameters_.push_back(thruster_parameters_struct); - this->poly_coeffs_.push_back(coeffs_struct); + // create and vectors + ThrusterParameters temp; + for (size_t i = 0; i < thruster_mapping.size(); ++i) { + temp.mapping = static_cast(thruster_mapping[i]); + temp.direction = static_cast(thruster_direction[i]); + temp.pwm_min = static_cast(thruster_PWM_min[i]); + temp.pwm_max = static_cast(thruster_PWM_max[i]); + + this->thruster_parameters_.push_back(temp); + } + + this->poly_coeffs_.push_back(left_coeffs); + this->poly_coeffs_.push_back(right_coeffs); } From 160212cf8d476121b1155fe4d6ed587be356b672 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 8 Nov 2024 18:56:38 +0100 Subject: [PATCH 40/54] removed prints --- .../thruster_interface_auv_driver.hpp | 75 ++++++++++++------- .../thruster_interface_auv_ros.hpp | 24 +++++- .../src/thruster_interface_auv_driver.cpp | 43 +---------- 3 files changed, 70 insertions(+), 72 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 9add06969..fdebcc370 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -40,39 +40,28 @@ enum PolySide { LEFT = 0, RIGHT = 1 }; */ class ThrusterInterfaceAUVDriver { public: - /** - * @brief empty default constructor called when declaring the object in the - * .hpp file. Doesn't initialize any params. - */ ~ThrusterInterfaceAUVDriver(); /** - * @brief actually used constructor. Called from ThrusterInterfaceAUVNode - * .cpp when instantiating the object, initializes all the params. + * @brief called from ThrusterInterfaceAUVNode .cpp when instantiating the + * object, initializes all the params. * - * @param i2c_bus bus number used to communicate - * @param pico_i2c_address i2c address of the ESC that drive the - * thrusters - * @param thruster_mapping pin to motor mapping for the thrusters i.e. if - * thruster_to_pin = [7,6 ...] then thruster 0 is pin 1 etc.. - * @param thruster_direction physical mounting direction of the thrusters, - * (+-1) - * @param pwm_min minimum clamping pwm values - * @param pwm_max maximum clamping pwm values - * @param left_coeffs third order polynomial coefficients when force - * < 0 - * @param right_coeffs third order polynomial coefficients when force - * > 0 + * @param i2c_bus bus number used to communicate + * @param pico_i2c_address i2c address of the ESC that drive the + * @param thruster_parameters describe mapping, direction, min and max pwm + * value for each thruster + * @param poly_coeffs LEFT(<0) and RIGHT(>0) third order + * polynomial coefficients, */ ThrusterInterfaceAUVDriver( short i2c_bus, int pico_i2c_address, const std::vector& thruster_parameters, - const std::vector>& approx_poly_coeffs); + const std::vector>& poly_coeffs); /** - * @brief [PUBLIC] method that calls 1) interpolate_forces_to_pwm() to - * convert the thruster forces to PWM values 2) send_data_to_escs() to send - * them to the ESCs via I2C + * @brief calls both 1) interpolate_forces_to_pwm() to + * convert the thruster forces to PWM values and 2) send_data_to_escs() to + * send them to the ESCs via I2C * * @param thruster_forces_array vector of forces for each thruster * @@ -91,8 +80,7 @@ class ThrusterInterfaceAUVDriver { std::vector> poly_coeffs_; /** - * @brief [private] method that just take the thruster forces and return PWM - * values + * @brief only take the thruster forces and return PWM values * * @param thruster_forces_array vector of forces for each thruster * @@ -102,21 +90,52 @@ class ThrusterInterfaceAUVDriver { std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); + /** + * @brief scalar map from force to pwm x->y. Choose coefficients [LEFT] or + * [RIGHT] based on sign(force) + * + * @param force scalar force value + * @param coeffs std::vector> coeffs contains the pair + * of coefficients + * + * @return std::int16_t scalar pwm value + */ std::int16_t force_to_pwm(double force, const std::vector>& coeffs); - std::int16_t interpolate_pwm(double force, - const std::vector& coeffs); + /** + * @brief compute y = a*x^3 + b*x^2 + c*x + d + * @param force x + * @param coeffs a,b,c,d + * + * @return std::int16_t pwm value + */ + std::int16_t calc_poly(double force, const std::vector& coeffs); /** - * @brief [private] method that takes the pwm values computed and sends them + * @brief only takes the pwm values computed and sends them * to the ESCs via I2C * * @param thruster_pwm_array vector of pwm values to send */ void send_data_to_escs(const std::vector& thruster_pwm_array); + /** + * @brief convert Newtons to Kg + * + * @param force Newtons + * + * @return double Kg + */ static constexpr double to_kg(double force) { return force / 9.80665; } + + /** + * @brief convert pwm values to i2c bytes + * + * @param pwm pwm value + * + * @return std::array i2c data + */ static constexpr std::array pwm_to_i2c_data( std::int16_t pwm) { return {static_cast((pwm >> 8) & 0xFF), diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 9375074bf..7b52ec382 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -12,9 +12,22 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { ThrusterInterfaceAUVNode(); private: + /** + * @brief periodically receive thruster forces topic + * + * @param msg ThrusterForces message + */ void thruster_forces_callback( const vortex_msgs::msg::ThrusterForces::SharedPtr msg); + + /** + * @brief periodically and publish and send pwm commands to thrusters + */ void timer_callback(); + + /** + * @brief extract all parameters from the .yaml file + */ void extract_all_parameters(); int i2c_bus_; @@ -27,10 +40,13 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { std::vector thruster_forces_array_; double thrust_timer_period_; - std::unique_ptr thruster_driver_; - rclcpp::Subscription::SharedPtr - thruster_forces_subscriber_; - rclcpp::Publisher::SharedPtr + std::unique_ptr + thruster_driver_; ///<-- pwm driver + rclcpp::Subscription:: + SharedPtr ///<-- thruster forces subscriber + thruster_forces_subscriber_; + rclcpp::Publisher< + std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher thruster_pwm_publisher_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 9f92c6384..56a719536 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -9,43 +9,6 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), poly_coeffs_(poly_coeffs) { - // TO BE REMOVED---------------------------------------------------- - printf("I2C_BUS: %d\n", i2c_bus_); - printf("PICO_I2C_ADDRESS: %d\n", pico_i2c_address_); - - printf("MAPPING: "); - for (size_t i = 0; i < thruster_parameters_.size(); ++i) { - printf("%d ", thruster_parameters_[i].mapping); - } - printf("\n"); - printf("DIRECTION: "); - for (size_t i = 0; i < thruster_parameters_.size(); ++i) { - printf("%d ", thruster_parameters_[i].direction); - } - printf("\n"); - printf("PWM_MIN: "); - for (size_t i = 0; i < thruster_parameters_.size(); ++i) { - printf("%d ", thruster_parameters_[i].pwm_min); - } - printf("\n"); - printf("PWM_MAX: "); - for (size_t i = 0; i < thruster_parameters_.size(); ++i) { - printf("%d ", thruster_parameters_[i].pwm_max); - } - printf("\n"); - - printf("LEFT_COEFFS: "); - for (size_t i = 0; i < poly_coeffs_[LEFT].size(); ++i) { - printf("%f ", poly_coeffs_[LEFT][i]); - } - printf("\n"); - printf("RIGHT_COEFFS: "); - for (size_t i = 0; i < poly_coeffs_[RIGHT].size(); ++i) { - printf("%f ", poly_coeffs_[RIGHT][i]); - } - printf("\n"); - //---------------------------------------------------------------- - // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); bus_fd_ = @@ -82,15 +45,15 @@ std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( double force, const std::vector>& coeffs) { if (force < 0) { - return interpolate_pwm(force, coeffs[LEFT]); + return calc_poly(force, coeffs[LEFT]); } else if (force > 0) { - return interpolate_pwm(force, coeffs[RIGHT]); + return calc_poly(force, coeffs[RIGHT]); } else { return IDLE_PWM_VALUE; // 1500 } } -std::int16_t ThrusterInterfaceAUVDriver::interpolate_pwm( +std::int16_t ThrusterInterfaceAUVDriver::calc_poly( double force, const std::vector& coeffs) { return static_cast(coeffs[0] * std::pow(force, 3) + From 19e437772336673238c62e13dae3338add1813a4 Mon Sep 17 00:00:00 2001 From: Andreas Kluge Svendsrud Date: Sat, 9 Nov 2024 11:29:37 +0100 Subject: [PATCH 41/54] refactor(thruster_interface_auv): removed outdated python implementation --- .../thruster_interface_auv_driver_lib.py | 148 ------------------ .../thruster_interface_auv_node.py | 129 --------------- 2 files changed, 277 deletions(-) delete mode 100644 motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py delete mode 100755 motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py 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 deleted file mode 100644 index 90feb69b9..000000000 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_driver_lib.py +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/env python3 -# Import libraries -import numpy -import pandas -import smbus2 - - -class ThrusterInterfaceAUVDriver: - def __init__( - self, - i2c_bus: int = 1, - pico_i2c_address: int = 0x21, - system_operational_voltage: float = 16.0, - ros2_package_name_for_thruster_datasheet: str = "", - thruster_mapping: list[int] = [7, 6, 5, 4, 3, 2, 1, 0], - thruster_direction: list[int] = [1, 1, 1, 1, 1, 1, 1, 1], - thruster_pwm_offset: list[int] = [0, 0, 0, 0, 0, 0, 0, 0], - pwm_min: list[int] = [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - pwm_max: list[int] = [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - ) -> None: - # Initialice the I2C communication - self.bus = None - try: - self.bus = smbus2.SMBus(i2c_bus) - except Exception as error_code: - print(f"ERROR: Failed connection I2C bus nr {self.bus}: {error_code}") - self.pico_i2c_address = pico_i2c_address - - # Set mapping, direction and offset for the thrusters - self.thruster_mapping = thruster_mapping - self.thruster_direction = thruster_direction - self.thruster_pwm_offset = thruster_pwm_offset - self.pwm_min = pwm_min - self.pwm_max = pwm_max - - # Convert SYSTEM_OPERATIONAL_VOLTAGE to a whole even number to work with - # This is because we have multiple files for the behaviours of the thrusters depending on the voltage of the drone - if system_operational_voltage < 11.0: - self.system_operational_voltage = 10 - elif system_operational_voltage < 13.0: - self.system_operational_voltage = 12 - elif system_operational_voltage < 15.0: - self.system_operational_voltage = 14 - elif system_operational_voltage < 17.0: - self.system_operational_voltage = 16 - elif system_operational_voltage < 19.0: - self.system_operational_voltage = 18 - elif system_operational_voltage >= 19.0: - 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 - ) - - 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]. - - Returns an Array of PWM - Gives out 8 ints in form of: - [0, 0, 0, 0, 0, 0, 0, 0] - """ - # Read the important data from the .csv file - thruster_datasheet_file_data = pandas.read_csv( - f"{self.ros2_package_name_for_thruster_datasheet}/resources/T200-Thrusters-{self.system_operational_voltage}V.csv", - usecols=[" PWM (µs)", " Force (Kg f)"], - ) - - # Convert Newtons to Kg as Thruster Datasheet is in Kg format - for i, thruster_forces in enumerate(thruster_forces_array): - 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 - interpolated_pwm = numpy.interp( - thruster_forces_array, - thruster_datasheet_file_forces, - thruster_datasheet_file_data_pwm, - ) - - # Convert PWM signal to integers as they are interpolated and can have floats - interpolated_pwm = [int(pwm) for pwm in interpolated_pwm] - - return interpolated_pwm - - def _send_data_to_escs(self, thruster_pwm_array: list) -> None: - i2c_data_array = [] - - # Divide data into bytes as I2C only sends bytes - # We have 8 values of 16 bits - # Convert them to 16 values of 8 bits (ie 1 byte) - for i in enumerate(thruster_pwm_array): - msb = (thruster_pwm_array[i] >> 8) & 0xFF - lsb = (thruster_pwm_array[i]) & 0xFF - i2c_data_array.extend([msb, lsb]) - - # Send the whole array through I2C - # OBS!: Python adds an extra byte at the start that the Microcotroller that is receiving this has to handle - 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]. - - Converts Forces to PWM signals - PWM signals sent to PCA9685 module through I2C - PCA9685 Module sends electrical PWM signals to the individual thruster ESCs - The ESCs send corecponding electrical power to the Thrustres - Thrusters then generate thrust accordingly to the Forces sent to this driver - - Returns an Array of PWM signal for debugging purposes - 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 - ] - - # 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] - ) - - # Apply thruster offset and limit PWM if needed - for esc_channel in enumerate(thruster_pwm_array): - # Clamping pwm signal in case it is out of range - if thruster_pwm_array[esc_channel] < self.pwm_min[esc_channel]: # To small - thruster_pwm_array[esc_channel] = self.pwm_min[esc_channel] - elif thruster_pwm_array[esc_channel] > self.pwm_max[esc_channel]: # To big - thruster_pwm_array[esc_channel] = self.pwm_max[esc_channel] - - # Send data through I2C to the microcontroller that then controls the ESC and extension the thrusters - try: - self._send_data_to_escs(thruster_pwm_array) - except Exception as error_code: - print(f"ERROR: Failed to send PWM values: {error_code}") - - return 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 deleted file mode 100755 index f785b39a4..000000000 --- a/motion/thruster_interface_auv/thruster_interface_auv/thruster_interface_auv_node.py +++ /dev/null @@ -1,129 +0,0 @@ -#!/usr/bin/env python3 -# ROS2 Libraries -import rclpy -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 - - -class ThrusterInterfaceAUVNode(Node): - def __init__(self) -> None: - # Initialize and name the node process running - super().__init__("thruster_interface_auv_node") - - # 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_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_PWM_min", - [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100], - ) - self.declare_parameter( - "propulsion.thrusters.thruster_PWM_max", - [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900], - ) - - 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 - ) - - # Initialize thruster driver - self.thruster_driver = ThrusterInterfaceAUVDriver( - 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, - pwm_min=self.thruster_pwm_min, - pwm_max=self.thruster_pwm_max, - ) - - # Start clock timer for driving thrusters every 0.2 seconds - # Declare "self.thruster_forces_array" in case no topic comes in at the first possible second - self.thruster_forces_array = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - self.timer = self.create_timer(self.thrust_timer_period, self._timer_callback) - - # Debugging - self.get_logger().info('"thruster_interface_auv_node" has been started') - - def _thruster_forces_callback(self, msg: ThrusterForces) -> None: - # Get data of the forces published - self.thruster_forces_array = msg.thrust - - 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 - ) - - pwm_message = Int16MultiArray() - pwm_message.data = thruster_pwm_array - self.thruster_pwm_publisher.publish(pwm_message) - - -def main(args: list = None) -> None: - """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. - Upon shutdown, it destroys the node and shuts down the ROS 2 client library. - - Args: - args (list, optional): Command line arguments passed to the ROS 2 client library. - Defaults to None. - """ - # Initialize - rclpy.init(args=args) - - # Running - thruster_interface_auv_node = ThrusterInterfaceAUVNode() - rclpy.spin(thruster_interface_auv_node) - - # Shutdown - thruster_interface_auv_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() From e9aacb3e76337a36ee94ea7a0beaf2177af338d7 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 12:32:37 +0100 Subject: [PATCH 42/54] document --- motion/thruster_interface_auv/README.md | 37 +++++++++++++++++++ motion/thruster_interface_auv/package.xml | 14 ++----- .../resources/{README => README.md} | 3 +- 3 files changed, 42 insertions(+), 12 deletions(-) create mode 100644 motion/thruster_interface_auv/README.md rename motion/thruster_interface_auv/resources/{README => README.md} (52%) diff --git a/motion/thruster_interface_auv/README.md b/motion/thruster_interface_auv/README.md new file mode 100644 index 000000000..e83bd6965 --- /dev/null +++ b/motion/thruster_interface_auv/README.md @@ -0,0 +1,37 @@ +# Thruster Interface AUV + +This package provides an interface for controlling the thrusters of orca converting forces values to pwm values. The mapping is based on a piecewise third order polynomial approximating the datasheet .csv table found in /resources. Values send via i2c protocol. + +*NOTE: Possibility to extend the handling based on the current operating voltage exists, not implemented as for now.* + +## Usage + +```sh +source install/setup.bash +ros2 launch thruster_interface_auv thruster_interface_auv.launch.py +``` + +## Structure + +### Nodes + +1. `thruster_interface_auv_node.cpp` contains the main loop of the node + +2. `thruster_interface_auv_ros.cpp` contains the implementation of the node dependent on ros2 creating a subscriber for thruster forces, a publisher for pwm values, and a driver for handling the mapping. Initialize everything extracting all the parameters from .yaml file found in `../auv_setup/config/robots/orca.yaml` and `/config/thruster_interface_auv_config.yaml`. + +3. `thruster_interface_auv_driver.cpp` contains the pure .cpp implementation for the mapping, conversion for i2c data format, and sending those values. + +### Topics + +- *subscribe to:* `/thrust/thruster_forces [vortex_msgs/msg/ThrusterForce]` -- array of forces to apply to each thruster. + +- *publish:* `/pwm [std_msgs/msg/Int16MultiArray]` -- pwm command value to apply that force. + +## Config + +1. Edit `../auv_setup/config/robots/orca.yaml` for thruster parameters like mapping, direction, pwm_min and max. +2. Edit `/config/thruster_interface_auv_config.yaml` for i2c bus and address, topic names and polynomial coefficients for the mapping. + +## Contact + +For questions or support, please contact [albertomorselli00@gmail.com](mailto:albertomorselli00@gmail.com). diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index fcd8a4c16..7f936bae3 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -8,17 +8,11 @@ MIT ament_cmake - rclcpp - std_msgs - vortex_msgs - ament_index_cpp - rclcpp - std_msgs - vortex_msgs - ament_index_cpp - - ros2launch + rclcpp + std_msgs + vortex_msgs + ament_index_cpp ament_cmake diff --git a/motion/thruster_interface_auv/resources/README b/motion/thruster_interface_auv/resources/README.md similarity index 52% rename from motion/thruster_interface_auv/resources/README rename to motion/thruster_interface_auv/resources/README.md index 0219b2e45..07d935ce5 100644 --- a/motion/thruster_interface_auv/resources/README +++ b/motion/thruster_interface_auv/resources/README.md @@ -1,4 +1,3 @@ The different .CSV files reflect the different behaviours of the motors according to the voltage in the system -TODO: 10V and 14V are the same table. Should investigate. -Not critical because we use only 16V for now +*PLEASE NOTE: 10V and 14V are the same table. For now we're only using 16V.* From 4b4113d0649ee5b33aaebee6a5054bf8f06d08d6 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 14:35:03 +0100 Subject: [PATCH 43/54] debug flag for publisher --- .../config/thruster_interface_auv_config.yaml | 3 ++ .../thruster_interface_auv_driver.hpp | 4 ++- .../thruster_interface_auv_ros.hpp | 8 ++--- .../launch/thruster_interface_auv.launch.py | 33 +++++++++---------- .../src/thruster_interface_auv_ros.cpp | 31 ++++++++--------- 5 files changed, 41 insertions(+), 38 deletions(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 13cb55467..5ad3d9c79 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -27,3 +27,6 @@ thruster_interface_auv_node: topics: thruster_forces_subscriber: "thrust/thruster_forces" pwm_publisher: "pwm" + + debug: + flag: True diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index fdebcc370..d0f61b58b 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -6,7 +6,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -51,7 +53,7 @@ class ThrusterInterfaceAUVDriver { * @param thruster_parameters describe mapping, direction, min and max pwm * value for each thruster * @param poly_coeffs LEFT(<0) and RIGHT(>0) third order - * polynomial coefficients, + * polynomial coefficients */ ThrusterInterfaceAUVDriver( short i2c_bus, diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 7b52ec382..1f5ab1fc2 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -21,9 +21,10 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { const vortex_msgs::msg::ThrusterForces::SharedPtr msg); /** - * @brief periodically and publish and send pwm commands to thrusters + * @brief publish and send pwm commands to thrusters. Sinchronous with + * thruster_forces_callback */ - void timer_callback(); + void pwm_callback(); /** * @brief extract all parameters from the .yaml file @@ -38,7 +39,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { std::vector> poly_coeffs_; std::vector thruster_forces_array_; - double thrust_timer_period_; + bool debug_flag_; std::unique_ptr thruster_driver_; ///<-- pwm driver @@ -48,7 +49,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { rclcpp::Publisher< std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher thruster_pwm_publisher_; - rclcpp::TimerBase::SharedPtr timer_; }; #endif // THRUSTER_INTERFACE_AUV_NODE_HPP 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 dc8220d10..8e1fb4bdc 100644 --- a/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py +++ b/motion/thruster_interface_auv/launch/thruster_interface_auv.launch.py @@ -5,30 +5,27 @@ from launch_ros.actions import Node -def generate_launch_description(): - # Path to the YAML file - # config = path.join( - # get_package_share_directory("auv_setup"), "config", "robots", "orca.yaml" - # ) +def generate_launch_description() -> LaunchDescription: + config = [ + path.join( + get_package_share_directory(package_name="auv_setup"), + "config", + "robots", + "orca.yaml", + ), + path.join( + get_package_share_directory(package_name="thruster_interface_auv"), + "config", + "thruster_interface_auv_config.yaml", + ), + ] thruster_interface_auv_node = Node( package="thruster_interface_auv", executable="thruster_interface_auv_node", name="thruster_interface_auv_node", output="screen", - parameters=[ - path.join( - get_package_share_directory("auv_setup"), - "config", - "robots", - "orca.yaml", - ), - path.join( - get_package_share_directory("thruster_interface_auv"), - "config", - "thruster_interface_auv_config.yaml", - ), - ], + parameters=config, ) return LaunchDescription([thruster_interface_auv_node]) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index a9b571b8c..ab187ca24 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -17,9 +17,11 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::placeholders::_1)); // publisher only for debugging - this->thruster_pwm_publisher_ = - this->create_publisher( - this->publisher_topic_name_, qos_sensor_data); + if (this->debug_flag_) { + this->thruster_pwm_publisher_ = + this->create_publisher( + this->publisher_topic_name_, qos_sensor_data); + } // call constructor for thruster_driver_ this->thruster_driver_ = std::make_unique( @@ -30,10 +32,6 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() // very beginning this->thruster_forces_array_ = std::vector(8, 0.00); - timer_ = this->create_wall_timer( - std::chrono::duration(this->thrust_timer_period_), - std::bind(&ThrusterInterfaceAUVNode::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" correctly initialized"); } @@ -41,17 +39,20 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() void ThrusterInterfaceAUVNode::thruster_forces_callback( const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { this->thruster_forces_array_ = msg->thrust; + this->pwm_callback(); } -void ThrusterInterfaceAUVNode::timer_callback() { +void ThrusterInterfaceAUVNode::pwm_callback() { // drive thrusters... std::vector thruster_pwm_array = thruster_driver_->drive_thrusters(this->thruster_forces_array_); //..and publish PWM values for debugging purposes - std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array; - thruster_pwm_publisher_->publish(pwm_message); + if (this->debug_flag_) { + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = thruster_pwm_array; + thruster_pwm_publisher_->publish(pwm_message); + } } void ThrusterInterfaceAUVNode::extract_all_parameters() { @@ -64,7 +65,6 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { "propulsion.thrusters.thruster_PWM_min"); this->declare_parameter>( "propulsion.thrusters.thruster_PWM_max"); - this->declare_parameter("propulsion.thrusters.thrust_update_rate"); // approx poly coeffs for 16V from thruster_interface_auv.yaml this->declare_parameter>("coeffs.16V.LEFT"); @@ -78,6 +78,8 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->declare_parameter("topics.thruster_forces_subscriber"); this->declare_parameter("topics.pwm_publisher"); + this->declare_parameter("debug.flag"); + //----------------------------------------------------------------------- // get them auto thruster_mapping = @@ -92,9 +94,6 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { auto thruster_PWM_max = this->get_parameter("propulsion.thrusters.thruster_PWM_max") .as_integer_array(); - this->thrust_timer_period_ = - 1.0 / this->get_parameter("propulsion.thrusters.thrust_update_rate") - .as_double(); std::vector left_coeffs = this->get_parameter("coeffs.16V.LEFT").as_double_array(); @@ -109,6 +108,8 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->publisher_topic_name_ = this->get_parameter("topics.pwm_publisher").as_string(); + this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + // create and vectors ThrusterParameters temp; for (size_t i = 0; i < thruster_mapping.size(); ++i) { From 2a6262a6a7dbfdd62650004a4149c885eab90517 Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 10 Nov 2024 15:20:30 +0100 Subject: [PATCH 44/54] added param event callback for debug flag --- .../thruster_interface_auv_ros.hpp | 29 +++++++++++++++++++ .../src/thruster_interface_auv_ros.cpp | 25 ++++++++++++++++ 2 files changed, 54 insertions(+) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 1f5ab1fc2..c846b1286 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -6,6 +6,7 @@ #include #include #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" +#include class ThrusterInterfaceAUVNode : public rclcpp::Node { public: @@ -31,6 +32,34 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { */ void extract_all_parameters(); + /** + * @brief Initialize the parameter handler and a parameter event callback. + * + */ + void initialize_parameter_handler(); + + /** + * @brief Manages parameter events for the node. + * + * This handle is used to set up a mechanism to listen for and react to changes in parameters. + * Parameters can be used to configure the node's operational behavior dynamically, + * allowing adjustments without altering the code. The `param_handler_` is responsible for + * registering callbacks that are triggered on parameter changes, providing a centralized + * management system within the node for such events. + */ + std::shared_ptr param_handler_; + + /** + * @brief Handle to the registration of the parameter event callback. + * + * Represents a token or reference to the specific callback registration made with + * the parameter event handler (`param_handler_`). This handle allows for management + * of the lifecycle of the callback, such as removing the callback if it's no longer needed. + * It ensures that the node can respond to parameter changes with the registered callback + * in an efficient and controlled manner. + */ + rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; + int i2c_bus_; int i2c_address_; std::string subscriber_topic_name_; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index ab187ca24..96e2060a8 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -32,6 +32,8 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() // very beginning this->thruster_forces_array_ = std::vector(8, 0.00); + initialize_parameter_handler(); + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" correctly initialized"); } @@ -55,6 +57,29 @@ void ThrusterInterfaceAUVNode::pwm_callback() { } } +void ThrusterInterfaceAUVNode::initialize_parameter_handler() { + param_handler_ = std::make_shared(this); + + // Register the parameter event callback directly in the lambda expression + param_cb_handle_ = param_handler_->add_parameter_event_callback( + [this](const rcl_interfaces::msg::ParameterEvent & event) { + auto node_name = this->get_fully_qualified_name(); + + // Filter out events not related to this node + if (event.node != node_name) { + return; // Early return if the event is not from this node + } + + RCLCPP_INFO(this->get_logger(), "Received parameter event"); + for (const auto& changed_parameter : event.changed_parameters) { + if (changed_parameter.name.find("debug.flag") == 0) { + this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + } + } + } + ); +} + void ThrusterInterfaceAUVNode::extract_all_parameters() { // thruster parameters from orca.yaml this->declare_parameter>( From 2bce3de0d2526a9b9965a09a430bc4e8db0f1974 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 15:44:43 +0100 Subject: [PATCH 45/54] precommitfix --- .../thruster_interface_auv_ros.hpp | 26 ++++++++++--------- .../src/thruster_interface_auv_ros.cpp | 12 ++++----- 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index c846b1286..4a1026ae8 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -1,12 +1,12 @@ #ifndef THRUSTER_INTERFACE_AUV_NODE_HPP #define THRUSTER_INTERFACE_AUV_NODE_HPP +#include #include #include #include #include #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" -#include class ThrusterInterfaceAUVNode : public rclcpp::Node { public: @@ -34,17 +34,18 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { /** * @brief Initialize the parameter handler and a parameter event callback. - * - */ + * + */ void initialize_parameter_handler(); /** * @brief Manages parameter events for the node. * - * This handle is used to set up a mechanism to listen for and react to changes in parameters. - * Parameters can be used to configure the node's operational behavior dynamically, - * allowing adjustments without altering the code. The `param_handler_` is responsible for - * registering callbacks that are triggered on parameter changes, providing a centralized + * This handle is used to set up a mechanism to listen for and react to + * changes in parameters. Parameters can be used to configure the node's + * operational behavior dynamically, allowing adjustments without altering + * the code. The `param_handler_` is responsible for registering callbacks + * that are triggered on parameter changes, providing a centralized * management system within the node for such events. */ std::shared_ptr param_handler_; @@ -52,11 +53,12 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { /** * @brief Handle to the registration of the parameter event callback. * - * Represents a token or reference to the specific callback registration made with - * the parameter event handler (`param_handler_`). This handle allows for management - * of the lifecycle of the callback, such as removing the callback if it's no longer needed. - * It ensures that the node can respond to parameter changes with the registered callback - * in an efficient and controlled manner. + * Represents a token or reference to the specific callback registration + * made with the parameter event handler (`param_handler_`). This handle + * allows for management of the lifecycle of the callback, such as removing + * the callback if it's no longer needed. It ensures that the node can + * respond to parameter changes with the registered callback in an efficient + * and controlled manner. */ rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 96e2060a8..536ba1787 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -62,22 +62,22 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { // Register the parameter event callback directly in the lambda expression param_cb_handle_ = param_handler_->add_parameter_event_callback( - [this](const rcl_interfaces::msg::ParameterEvent & event) { + [this](const rcl_interfaces::msg::ParameterEvent& event) { auto node_name = this->get_fully_qualified_name(); // Filter out events not related to this node if (event.node != node_name) { - return; // Early return if the event is not from this node + return; // Early return if the event is not from this node } - + RCLCPP_INFO(this->get_logger(), "Received parameter event"); for (const auto& changed_parameter : event.changed_parameters) { if (changed_parameter.name.find("debug.flag") == 0) { - this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + this->debug_flag_ = + this->get_parameter("debug.flag").as_bool(); } } - } - ); + }); } void ThrusterInterfaceAUVNode::extract_all_parameters() { From a826463d02661229d0e0875959df51127e12a28e Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 15:52:57 +0100 Subject: [PATCH 46/54] param-handler --- .../src/thruster_interface_auv_ros.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 536ba1787..a47698d84 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -75,6 +75,11 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { if (changed_parameter.name.find("debug.flag") == 0) { this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + if (this->debug_flag_ == 0) { + // kill the pub + } else { + // reinitialize the pub + } } } }); From a5b1d560087a6ab9ea15cad7744bf8dc05c89afc Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 10 Nov 2024 16:06:02 +0100 Subject: [PATCH 47/54] kill debug pub when flag turned off --- .../thruster_interface_auv_ros.hpp | 2 ++ .../src/thruster_interface_auv_ros.cpp | 28 ++++++++++++++----- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 4a1026ae8..5b1576ba1 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -38,6 +38,8 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { */ void initialize_parameter_handler(); + void check_and_initialize_debug_publisher(); + /** * @brief Manages parameter events for the node. * diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index a47698d84..15be4432d 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -73,18 +73,32 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { RCLCPP_INFO(this->get_logger(), "Received parameter event"); for (const auto& changed_parameter : event.changed_parameters) { if (changed_parameter.name.find("debug.flag") == 0) { - this->debug_flag_ = - this->get_parameter("debug.flag").as_bool(); - if (this->debug_flag_ == 0) { - // kill the pub - } else { - // reinitialize the pub - } + check_and_initialize_debug_publisher(); } } }); } +void ThrusterInterfaceAUVNode::check_and_initialize_debug_publisher() { + bool debug_flag = this->get_parameter("debug.flag").as_bool(); + if (debug_flag_ != debug_flag) { + if (debug_flag) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + thruster_pwm_publisher_ = + this->create_publisher( + publisher_topic_name_, qos_sensor_data); + RCLCPP_INFO(this->get_logger(), "Initialized debug publisher"); + } else { + thruster_pwm_publisher_.reset(); + RCLCPP_INFO(this->get_logger(), "Killed debug publisher"); + } + debug_flag_ = debug_flag; + } + +} + void ThrusterInterfaceAUVNode::extract_all_parameters() { // thruster parameters from orca.yaml this->declare_parameter>( From 6a1a7edfc2906a18bac9c04755a8f9ccf2f56583 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 17:48:31 +0100 Subject: [PATCH 48/54] not working --- .../config/thruster_interface_auv_config.yaml | 2 +- .../thruster_interface_auv_ros.hpp | 44 ++++++++++--------- .../src/thruster_interface_auv_ros.cpp | 43 +++++++----------- 3 files changed, 41 insertions(+), 48 deletions(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 5ad3d9c79..e384cbb73 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -29,4 +29,4 @@ thruster_interface_auv_node: pwm_publisher: "pwm" debug: - flag: True + flag: False diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 5b1576ba1..0459d6abc 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -38,7 +38,29 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { */ void initialize_parameter_handler(); - void check_and_initialize_debug_publisher(); + /** + * @brief create/reinitialize publisher if flag=1, kill it if flag=0 + */ + void set_publisher(); + + int i2c_bus_; + int i2c_address_; + std::string subscriber_topic_name_; + std::string publisher_topic_name_; + std::vector thruster_parameters_; + std::vector> poly_coeffs_; + + std::vector thruster_forces_array_; + bool debug_flag_; + + std::unique_ptr + thruster_driver_; ///<-- pwm driver + rclcpp::Subscription:: + SharedPtr ///<-- thruster forces subscriber + thruster_forces_subscriber_; + rclcpp::Publisher< + std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher + thruster_pwm_publisher_; /** * @brief Manages parameter events for the node. @@ -51,7 +73,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { * management system within the node for such events. */ std::shared_ptr param_handler_; - /** * @brief Handle to the registration of the parameter event callback. * @@ -63,25 +84,6 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { * and controlled manner. */ rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; - - int i2c_bus_; - int i2c_address_; - std::string subscriber_topic_name_; - std::string publisher_topic_name_; - std::vector thruster_parameters_; - std::vector> poly_coeffs_; - - std::vector thruster_forces_array_; - bool debug_flag_; - - std::unique_ptr - thruster_driver_; ///<-- pwm driver - rclcpp::Subscription:: - SharedPtr ///<-- thruster forces subscriber - thruster_forces_subscriber_; - rclcpp::Publisher< - std_msgs::msg::Int16MultiArray>::SharedPtr ///<-- pwm publisher - thruster_pwm_publisher_; }; #endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 15be4432d..eefd5f697 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -17,11 +17,8 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::placeholders::_1)); // publisher only for debugging - if (this->debug_flag_) { - this->thruster_pwm_publisher_ = - this->create_publisher( - this->publisher_topic_name_, qos_sensor_data); - } + set_publisher(); + initialize_parameter_handler(); // call constructor for thruster_driver_ this->thruster_driver_ = std::make_unique( @@ -32,8 +29,6 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() // very beginning this->thruster_forces_array_ = std::vector(8, 0.00); - initialize_parameter_handler(); - RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" correctly initialized"); } @@ -73,30 +68,26 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { RCLCPP_INFO(this->get_logger(), "Received parameter event"); for (const auto& changed_parameter : event.changed_parameters) { if (changed_parameter.name.find("debug.flag") == 0) { - check_and_initialize_debug_publisher(); + debug_flag_ = this->get_parameter("debug.flag").as_bool(); + RCLCPP_INFO(this->get_logger(), "%d", debug_flag_); + // RCLCPP_INFO(this->get_logger(), "MODYF debug publisher"); + // if (debug_flag_ != debug_flag) { + // debug_flag_ = debug_flag; + // //set_publisher(); + // } } } }); } -void ThrusterInterfaceAUVNode::check_and_initialize_debug_publisher() { - bool debug_flag = this->get_parameter("debug.flag").as_bool(); - if (debug_flag_ != debug_flag) { - if (debug_flag) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - thruster_pwm_publisher_ = - this->create_publisher( - publisher_topic_name_, qos_sensor_data); - RCLCPP_INFO(this->get_logger(), "Initialized debug publisher"); - } else { - thruster_pwm_publisher_.reset(); - RCLCPP_INFO(this->get_logger(), "Killed debug publisher"); - } - debug_flag_ = debug_flag; - } - +void ThrusterInterfaceAUVNode::set_publisher() { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + thruster_pwm_publisher_ = + this->create_publisher( + publisher_topic_name_, qos_sensor_data); + RCLCPP_INFO(this->get_logger(), "Initialized debug publisher"); } void ThrusterInterfaceAUVNode::extract_all_parameters() { From fdd6d69a67401f2c9374ff02cc120a649c6f92f0 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 10 Nov 2024 18:35:52 +0100 Subject: [PATCH 49/54] fixed paramhandler --- .../thruster_interface_auv_ros.hpp | 7 ++- .../src/thruster_interface_auv_ros.cpp | 43 +++++-------------- 2 files changed, 16 insertions(+), 34 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 0459d6abc..49cc2dbf6 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -83,7 +83,12 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { * respond to parameter changes with the registered callback in an efficient * and controlled manner. */ - rclcpp::ParameterEventCallbackHandle::SharedPtr param_cb_handle_; + rclcpp::ParameterCallbackHandle::SharedPtr debug_flag_parameter_cb; + + /** + * callback for updating debug_flag. + */ + void update_debug_flag(const rclcpp::Parameter& p); }; #endif // THRUSTER_INTERFACE_AUV_NODE_HPP diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index eefd5f697..89eb21a1b 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -16,9 +16,9 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); - // publisher only for debugging - set_publisher(); - initialize_parameter_handler(); + this->thruster_pwm_publisher_ = + this->create_publisher( + publisher_topic_name_, qos_sensor_data); // call constructor for thruster_driver_ this->thruster_driver_ = std::make_unique( @@ -29,6 +29,8 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() // very beginning this->thruster_forces_array_ = std::vector(8, 0.00); + initialize_parameter_handler(); + RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" correctly initialized"); } @@ -56,38 +58,13 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { param_handler_ = std::make_shared(this); // Register the parameter event callback directly in the lambda expression - param_cb_handle_ = param_handler_->add_parameter_event_callback( - [this](const rcl_interfaces::msg::ParameterEvent& event) { - auto node_name = this->get_fully_qualified_name(); - - // Filter out events not related to this node - if (event.node != node_name) { - return; // Early return if the event is not from this node - } - - RCLCPP_INFO(this->get_logger(), "Received parameter event"); - for (const auto& changed_parameter : event.changed_parameters) { - if (changed_parameter.name.find("debug.flag") == 0) { - debug_flag_ = this->get_parameter("debug.flag").as_bool(); - RCLCPP_INFO(this->get_logger(), "%d", debug_flag_); - // RCLCPP_INFO(this->get_logger(), "MODYF debug publisher"); - // if (debug_flag_ != debug_flag) { - // debug_flag_ = debug_flag; - // //set_publisher(); - // } - } - } - }); + debug_flag_parameter_cb = param_handler_->add_parameter_callback( + "debug.flag", std::bind(&ThrusterInterfaceAUVNode::update_debug_flag, + this, std::placeholders::_1)); } -void ThrusterInterfaceAUVNode::set_publisher() { - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos_sensor_data = rclcpp::QoS( - rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - thruster_pwm_publisher_ = - this->create_publisher( - publisher_topic_name_, qos_sensor_data); - RCLCPP_INFO(this->get_logger(), "Initialized debug publisher"); +void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { + this->debug_flag_ = p.get_value(); } void ThrusterInterfaceAUVNode::extract_all_parameters() { From 28a67557d8dc6aeb21fd17b9e2afe47f6ee631a5 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Fri, 15 Nov 2024 13:33:24 +0100 Subject: [PATCH 50/54] cleaned up a bit --- .../thruster_interface_auv_driver.hpp | 11 +++++++-- .../thruster_interface_auv_ros.hpp | 17 ++++--------- motion/thruster_interface_auv/package.xml | 1 - .../src/thruster_interface_auv_ros.cpp | 24 ++++++++++--------- 4 files changed, 27 insertions(+), 26 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index d0f61b58b..17bbd2c06 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -15,7 +15,11 @@ #include #include -#define IDLE_PWM_VALUE 1500 +#define IDLE_PWM_VALUE 1500 ///<-- pwm value to output when force = 0.00 + +/** + * @brief struct to hold the parameters for a single thruster + */ struct ThrusterParameters { uint8_t mapping; int8_t direction; @@ -23,7 +27,10 @@ struct ThrusterParameters { uint16_t pwm_max; }; -enum PolySide { LEFT = 0, RIGHT = 1 }; +enum PolySide { + LEFT = 0, + RIGHT = 1 +}; // vector index for the position of the coefficients in the coeff vector /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index 49cc2dbf6..8d4021193 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -65,28 +65,21 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { /** * @brief Manages parameter events for the node. * - * This handle is used to set up a mechanism to listen for and react to - * changes in parameters. Parameters can be used to configure the node's - * operational behavior dynamically, allowing adjustments without altering - * the code. The `param_handler_` is responsible for registering callbacks - * that are triggered on parameter changes, providing a centralized - * management system within the node for such events. + * This handler is used to set up a mechanism to listen for and react to + * changes in parameters via terminal at runtime. */ std::shared_ptr param_handler_; + /** * @brief Handle to the registration of the parameter event callback. * * Represents a token or reference to the specific callback registration - * made with the parameter event handler (`param_handler_`). This handle - * allows for management of the lifecycle of the callback, such as removing - * the callback if it's no longer needed. It ensures that the node can - * respond to parameter changes with the registered callback in an efficient - * and controlled manner. + * made with the parameter event handler (`param_handler_`). */ rclcpp::ParameterCallbackHandle::SharedPtr debug_flag_parameter_cb; /** - * callback for updating debug_flag. + * specific callback for updating debug_flag. */ void update_debug_flag(const rclcpp::Parameter& p); }; diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index 7f936bae3..d1446b23b 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -12,7 +12,6 @@ rclcpp std_msgs vortex_msgs - ament_index_cpp ament_cmake diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 89eb21a1b..0bd2bacd3 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -10,26 +10,25 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() auto qos_sensor_data = rclcpp::QoS( rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); - this->thruster_forces_subscriber_ = + thruster_forces_subscriber_ = this->create_subscription( - this->subscriber_topic_name_, qos_sensor_data, + subscriber_topic_name_, qos_sensor_data, std::bind(&ThrusterInterfaceAUVNode::thruster_forces_callback, this, std::placeholders::_1)); - this->thruster_pwm_publisher_ = + thruster_pwm_publisher_ = this->create_publisher( publisher_topic_name_, qos_sensor_data); // call constructor for thruster_driver_ - this->thruster_driver_ = std::make_unique( - this->i2c_bus_, this->i2c_address_, this->thruster_parameters_, - this->poly_coeffs_); + thruster_driver_ = std::make_unique( + i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); // Declare thruster_forces_array_ in case no topic comes at the // very beginning - this->thruster_forces_array_ = std::vector(8, 0.00); + thruster_forces_array_ = std::vector(8, 0.00); - initialize_parameter_handler(); + this->initialize_parameter_handler(); RCLCPP_INFO(this->get_logger(), "\"thruster_interface_auv_node\" correctly initialized"); @@ -37,7 +36,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode() void ThrusterInterfaceAUVNode::thruster_forces_callback( const vortex_msgs::msg::ThrusterForces::SharedPtr msg) { - this->thruster_forces_array_ = msg->thrust; + thruster_forces_array_ = msg->thrust; this->pwm_callback(); } @@ -47,7 +46,7 @@ void ThrusterInterfaceAUVNode::pwm_callback() { thruster_driver_->drive_thrusters(this->thruster_forces_array_); //..and publish PWM values for debugging purposes - if (this->debug_flag_) { + if (debug_flag_) { std_msgs::msg::Int16MultiArray pwm_message; pwm_message.data = thruster_pwm_array; thruster_pwm_publisher_->publish(pwm_message); @@ -64,7 +63,10 @@ void ThrusterInterfaceAUVNode::initialize_parameter_handler() { } void ThrusterInterfaceAUVNode::update_debug_flag(const rclcpp::Parameter& p) { - this->debug_flag_ = p.get_value(); + debug_flag_ = p.get_value(); + RCLCPP_INFO(this->get_logger(), + "Received parameter event: debug.flag updated to: %s", + debug_flag_ ? "true" : "false"); } void ThrusterInterfaceAUVNode::extract_all_parameters() { From 10d54c7142d110627d8a29236cc237daae5979d0 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 17 Nov 2024 13:14:44 +0100 Subject: [PATCH 51/54] minor changes --- .../thruster_interface_auv_driver.hpp | 26 ++++++++-------- motion/thruster_interface_auv/package.xml | 4 +-- .../src/thruster_interface_auv_driver.cpp | 31 ++++++++++--------- .../src/thruster_interface_auv_ros.cpp | 5 +-- 4 files changed, 35 insertions(+), 31 deletions(-) diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 17bbd2c06..35c60d5e2 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -15,8 +15,6 @@ #include #include -#define IDLE_PWM_VALUE 1500 ///<-- pwm value to output when force = 0.00 - /** * @brief struct to hold the parameters for a single thruster */ @@ -74,9 +72,9 @@ class ThrusterInterfaceAUVDriver { * * @param thruster_forces_array vector of forces for each thruster * - * @return std::vector vector of pwm values sent to each thruster + * @return std::vector vector of pwm values sent to each thruster */ - std::vector drive_thrusters( + std::vector drive_thrusters( const std::vector& thruster_forces_array); private: @@ -88,15 +86,17 @@ class ThrusterInterfaceAUVDriver { std::vector thruster_parameters_; std::vector> poly_coeffs_; + uint16_t idle_pwm_value_; ///< pwm value when force = 0.00 + /** * @brief only take the thruster forces and return PWM values * * @param thruster_forces_array vector of forces for each thruster * - * @return std::vector vector of pwm values sent to each thruster + * @return std::vector vector of pwm values sent to each thruster * if we want to publish them for debug purposes */ - std::vector interpolate_forces_to_pwm( + std::vector interpolate_forces_to_pwm( const std::vector& thruster_forces_array); /** @@ -107,19 +107,19 @@ class ThrusterInterfaceAUVDriver { * @param coeffs std::vector> coeffs contains the pair * of coefficients * - * @return std::int16_t scalar pwm value + * @return std::uint16_t scalar pwm value */ - std::int16_t force_to_pwm(double force, - const std::vector>& coeffs); + std::uint16_t force_to_pwm(double force, + const std::vector>& coeffs); /** * @brief compute y = a*x^3 + b*x^2 + c*x + d * @param force x * @param coeffs a,b,c,d * - * @return std::int16_t pwm value + * @return std::uint16_t pwm value */ - std::int16_t calc_poly(double force, const std::vector& coeffs); + std::uint16_t calc_poly(double force, const std::vector& coeffs); /** * @brief only takes the pwm values computed and sends them @@ -127,7 +127,7 @@ class ThrusterInterfaceAUVDriver { * * @param thruster_pwm_array vector of pwm values to send */ - void send_data_to_escs(const std::vector& thruster_pwm_array); + void send_data_to_escs(const std::vector& thruster_pwm_array); /** * @brief convert Newtons to Kg @@ -146,7 +146,7 @@ class ThrusterInterfaceAUVDriver { * @return std::array i2c data */ static constexpr std::array pwm_to_i2c_data( - std::int16_t pwm) { + std::uint16_t pwm) { return {static_cast((pwm >> 8) & 0xFF), static_cast(pwm & 0xFF)}; } diff --git a/motion/thruster_interface_auv/package.xml b/motion/thruster_interface_auv/package.xml index d1446b23b..14771b560 100644 --- a/motion/thruster_interface_auv/package.xml +++ b/motion/thruster_interface_auv/package.xml @@ -3,8 +3,8 @@ thruster_interface_auv 1.0.0 - Thruster interface to control thrusters through PCA9685 Module - vortex + Thruster interface to control thrusters through PCA9685 Module via i2c, mapping force values to pwm values + vortex MIT ament_cmake diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 56a719536..c7d99b0d1 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -9,7 +9,6 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( pico_i2c_address_(pico_i2c_address), thruster_parameters_(thruster_parameters), poly_coeffs_(poly_coeffs) { - // Open the I2C bus std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); bus_fd_ = open(i2c_filename.c_str(), @@ -19,6 +18,10 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( std::to_string(i2c_bus_) + " : " + std::string(strerror(errno))); } + + idle_pwm_value_ = + (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / + 2; } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { @@ -27,21 +30,21 @@ ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { } } -std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( +std::vector ThrusterInterfaceAUVDriver::interpolate_forces_to_pwm( const std::vector& thruster_forces_array) { // Convert Newtons to Kg (since the thruster datasheet is in Kg) std::vector forces_in_kg(thruster_forces_array.size()); std::transform(thruster_forces_array.begin(), thruster_forces_array.end(), forces_in_kg.begin(), to_kg); - std::vector interpolated_pwm; + std::vector interpolated_pwm; for (const double force : forces_in_kg) { interpolated_pwm.push_back(force_to_pwm(force, poly_coeffs_)); } return interpolated_pwm; } -std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( +std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm( double force, const std::vector>& coeffs) { if (force < 0) { @@ -49,27 +52,27 @@ std::int16_t ThrusterInterfaceAUVDriver::force_to_pwm( } else if (force > 0) { return calc_poly(force, coeffs[RIGHT]); } else { - return IDLE_PWM_VALUE; // 1500 + return idle_pwm_value_; // 1500 } } -std::int16_t ThrusterInterfaceAUVDriver::calc_poly( +std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( double force, const std::vector& coeffs) { - return static_cast(coeffs[0] * std::pow(force, 3) + - coeffs[1] * std::pow(force, 2) + - coeffs[2] * force + coeffs[3]); + return static_cast(coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + coeffs[3]); } void ThrusterInterfaceAUVDriver::send_data_to_escs( - const std::vector& thruster_pwm_array) { + const std::vector& thruster_pwm_array) { constexpr std::size_t i2c_data_size = 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) std::vector i2c_data_array; i2c_data_array.reserve(i2c_data_size); std::for_each(thruster_pwm_array.begin(), thruster_pwm_array.end(), - [&](std::int16_t pwm) { + [&](std::uint16_t pwm) { std::array bytes = pwm_to_i2c_data(pwm); std::copy(bytes.begin(), bytes.end(), std::back_inserter(i2c_data_array)); @@ -84,13 +87,13 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( } // Write data to the I2C device - if (write(bus_fd_, i2c_data_array.data(), 16) != 16) { + if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { throw std::runtime_error("ERROR: Failed to write to I2C device : " + std::string(strerror(errno))); } } -std::vector ThrusterInterfaceAUVDriver::drive_thrusters( +std::vector ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { // Apply thruster mapping and direction std::vector mapped_forces(thruster_forces_array.size()); @@ -101,7 +104,7 @@ std::vector ThrusterInterfaceAUVDriver::drive_thrusters( } // Convert forces to PWM - std::vector thruster_pwm_array = + std::vector thruster_pwm_array = interpolate_forces_to_pwm(mapped_forces); // Apply thruster offset and limit PWM if needed diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 0bd2bacd3..fccdccd69 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -42,13 +42,14 @@ void ThrusterInterfaceAUVNode::thruster_forces_callback( void ThrusterInterfaceAUVNode::pwm_callback() { // drive thrusters... - std::vector thruster_pwm_array = + std::vector thruster_pwm_array = thruster_driver_->drive_thrusters(this->thruster_forces_array_); //..and publish PWM values for debugging purposes if (debug_flag_) { std_msgs::msg::Int16MultiArray pwm_message; - pwm_message.data = thruster_pwm_array; + pwm_message.data = std::vector(thruster_pwm_array.begin(), + thruster_pwm_array.end()); thruster_pwm_publisher_->publish(pwm_message); } } From 03efc1dd693620eb5ba4a9439930264ea944d469 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 17 Nov 2024 13:59:25 +0100 Subject: [PATCH 52/54] a --- .../src/thruster_interface_auv_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index c7d99b0d1..1965c66f7 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -87,7 +87,7 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( } // Write data to the I2C device - if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { + if (write(bus_fd_, i2c_data_array.data(), 16) != 16) { throw std::runtime_error("ERROR: Failed to write to I2C device : " + std::string(strerror(errno))); } From f34357d4f6e54491a590631df9cb60ca1a9c96d7 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 17 Nov 2024 14:57:19 +0100 Subject: [PATCH 53/54] fckg starting byte --- .../src/thruster_interface_auv_driver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 1965c66f7..6155a7e96 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -67,9 +67,10 @@ std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( void ThrusterInterfaceAUVDriver::send_data_to_escs( const std::vector& thruster_pwm_array) { constexpr std::size_t i2c_data_size = - 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) + 1 + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) std::vector i2c_data_array; i2c_data_array.reserve(i2c_data_size); + i2c_data_array.push_back(0x00); // Start byte std::for_each(thruster_pwm_array.begin(), thruster_pwm_array.end(), [&](std::uint16_t pwm) { @@ -87,7 +88,7 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( } // Write data to the I2C device - if (write(bus_fd_, i2c_data_array.data(), 16) != 16) { + if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { throw std::runtime_error("ERROR: Failed to write to I2C device : " + std::string(strerror(errno))); } From 4a7cd2b1b69e3cd1634a61807f7b8c38ffb8d683 Mon Sep 17 00:00:00 2001 From: Alberto Morselli Date: Sun, 17 Nov 2024 15:13:58 +0100 Subject: [PATCH 54/54] xmas push --- .../src/thruster_interface_auv_driver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 6155a7e96..70a4309bb 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -26,6 +26,8 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { if (bus_fd_ >= 0) { + send_data_to_escs(std::vector(thruster_parameters_.size(), + idle_pwm_value_)); close(bus_fd_); } } @@ -70,8 +72,8 @@ void ThrusterInterfaceAUVDriver::send_data_to_escs( 1 + 8 * 2; // 8 thrusters * (1xMSB + 1xLSB) std::vector i2c_data_array; i2c_data_array.reserve(i2c_data_size); - i2c_data_array.push_back(0x00); // Start byte + i2c_data_array.push_back(0x00); // Start byte std::for_each(thruster_pwm_array.begin(), thruster_pwm_array.end(), [&](std::uint16_t pwm) { std::array bytes = pwm_to_i2c_data(pwm);